1. search.erl
%%%===================模块描述(开始)=====================
%%
%%%===================模块描述(结束)=====================
-module(search).
%%%=======================STATEMENT====================
-time("2018/8/24 10:45").
%%%=======================EXPORT=======================
-export([search/5, printf/6]).
%%%=======================INCLUDE======================
-include("search.hrl").
%%%=======================RECORD=======================
%%%=======================DEFINE=======================
-define(DEBUG, false).
%%%=================EXPORTED FUNCTIONS=================
%% ----------------------------------------------------
%% @doc
%%
%% @end
%% ----------------------------------------------------
search(X, Y, TargetX1, TargetY1, {M, F, A}) ->
Obstacles = M:F(A),
if
?DEBUG ->
{ok, IO} = file:open("Obstacles.log", write),
io:format(IO, "~w", [Obstacles]),
file:close(IO);
true ->
ok
end,
ObEts = ets:new(?MODULE, []),
[ets:insert(ObEts, {P, 1}) || P <- Obstacles],
case search_lib:check_target(TargetX1, TargetY1, ?X_MIN, ?Y_MIN, ?X_MAX, ?Y_MAX, ObEts) of
none ->
"all_obstacles";
{TargetX, TargetY} ->
Start = search_lib:init_point(X, Y, -1, -1),%%初始化开始坐标点
case search_lib:sub_points(X, Y, ?X_MIN, ?X_MAX, ?Y_MIN, ?Y_MAX, ObEts) of
[] ->
"no_path";
Subs ->
Open = ets:new(?MODULE, [{keypos, #point.point}]),
[begin
Point = search_lib:init_point(PointX, PointY, X, Y),
H = search_h:get_h(PointX, PointY, TargetX, TargetY),
ets:insert(Open, Point#point{f = H + Weight, h = H, g = Weight})
end || {{PointX, PointY}, Weight} <- Subs],
Close = ets:new(?MODULE, [{keypos, #point.point}]),
ets:insert(Close, Start),
search_(TargetX, TargetY, Open, Close, ObEts)
end
end.
%%%===================LOCAL FUNCTIONS==================
%% ----------------------------------------------------
%% @doc
%%
%% @end
%% ----------------------------------------------------
search_(TargetX, TargetY, Open, Close, ObEts) ->
H = search_lib:get_min(Open),
{HX, HY} = H#point.point,
SubPoint = search_lib:sub_points(HX, HY, 0, ?X_MAX, 0, ?Y_MAX, ObEts),%%获得所有能用的子节点
case check(TargetX, TargetY, H, SubPoint, Open, Close) of
{'end', Point} ->
Path = back(Point, Close, []),
if
?DEBUG ->
printf(TargetX, TargetY, Path, ObEts, 0, ?Y_MAX);
true ->
Path
end;
ok ->
case ets:info(Open, size) of
N when N > 0 ->
ets:insert(Close, H),
search_(TargetX, TargetY, Open, Close, ObEts);
_ ->
Path = back(H, Close, []),
if
?DEBUG ->
printf(TargetX, TargetY, Path, ObEts, 0, ?Y_MAX);
true ->
Path
end
end
end.
back(Point, Close, Result) ->
if
Point#point.parent =:= {-1, -1} ->
Result;
true ->
[Parent] = ets:lookup(Close, Point#point.parent),
back(Parent, Close, [Point#point.point | Result])
end.
check(_TargetX, _TargetY, _Parent, [], _Open, _Close) ->
ok;
check(TargetX, TargetY, Parent, [{{TargetX, TargetY}, _Weight} | _SubPoints], _Open, _Close) ->
{'end', Parent};
check(TargetX, TargetY, Parent, [{{X, Y}, Weight} | SubPoints], Open, Close) ->
case ets:lookup(Close, {X, Y}) of
[] ->
{ParentX, ParentY} = Parent#point.point,
case ets:lookup(Open, {X, Y}) of
[] ->%%不在,计算GHF,存入开启列表中
Point = search_lib:init_point(X, Y, ParentX, ParentY),
G = Parent#point.f + Weight,
H = search_h:get_h(X, Y, TargetX, TargetY),
if
H == 0 ->
{'end', Parent};
true ->
ets:insert(Open, Point#point{f = H + G, h = H, g = G}),
check(TargetX, TargetY, Parent, SubPoints, Open, Close)
end;
[OldPoint] ->%%在开启列表中,检查以当前节点为父节点,F是否更优
G = Parent#point.f + Weight,
H = search_h:get_h(X, Y, TargetX, TargetY),
if
H == 0 ->
{'end', Parent};
H < OldPoint#point.h ->
ets:insert(Open, OldPoint#point{f = H + G, h = H, g = G, parent = {ParentX, ParentY}}),
check(TargetX, TargetY, Parent, SubPoints, Open, Close);
true ->
check(TargetX, TargetY, Parent, SubPoints, Open, Close)
end
end;
_ ->%%关闭列表中忽略
check(TargetX, TargetY, Parent, SubPoints, Open, Close)
end.
printf(TX, TY, Path, ObEts, X, Y) when Y =:= -1 ->
if
X =:= 0 ->
io:format(" * ", []);
X < 10 ->
io:format(" ~p ", [X - 1]);
X < 100 ->
io:format(" ~p", [X - 1]);
true ->
io:format("~p", [X - 1])
end,
if
X > ?X_MAX ->
io:format("~n", []);
true ->
printf(TX, TY, Path, ObEts, X + 1, Y)
end;
printf(_TX, _TY, _Path, _Obstacles, _X, Y) when Y < 0 ->
ok;
printf(TX, TY, Path, ObEts, X, Y) when X > ?X_MAX ->
io:format("~n", []),
if
Y =:= 0 ->
ok;
Y < 10 ->
io:format(" ~p ", [Y - 1]);
Y < 100 ->
io:format(" ~p", [Y - 1])
end,
printf(TX, TY, Path, ObEts, 0, Y - 1);
printf(TX, TY, Path, ObEts, X, Y) ->
if
Y =:= ?Y_MAX andalso X =:= 0 ->
io:format(" ~p", [?Y_MAX]);
true ->
ok
end,
if
TX =:= X andalso TY =:= Y ->
io:format("$=$", []);
true ->
case ets:lookup(ObEts, {X, Y}) of
[_] ->
io:format(" / ", []);
_ ->
case lists:member({X, Y}, Path) of
true ->
io:format(" * ", []);
_ ->
io:format(" - ", [])
end
end
end,
printf(TX, TY, Path, ObEts, X + 1, Y).
io(Key, Content) ->
if
?DEBUG ->
io:format("~p~n", [{Key, Content}]),
io:get_line("next");
true ->
ok
end.
2. search_h.erl
%%%===================模块描述(开始)=====================
%%
%%%===================模块描述(结束)=====================
-module(search_h).
%%%=======================STATEMENT====================
-time("2018/8/24 10:54").
%%%=======================EXPORT=======================
-export([get_h/4]).
%%%=======================INCLUDE======================
%%%=======================RECORD=======================
%%%=======================DEFINE=======================
%%%=================EXPORTED FUNCTIONS=================
%% ----------------------------------------------------
%% @doc
%% 计算h值(扩大一百倍除去小数返回)
%% @end
%% ----------------------------------------------------
get_h(X, Y, X, TargetY) ->
abs(Y - TargetY);
get_h(X, Y, TargetX, Y) ->
abs(X - TargetX);
get_h(X, Y, TargetX, TargetY) ->
XV = abs(X - TargetX),
YV = abs(Y - TargetY),
math:sqrt(XV * XV + YV * YV)*1.5.
%% trunc(math:sqrt(XV * XV + YV * YV)*100).
%%%===================LOCAL FUNCTIONS==================
%% ----------------------------------------------------
%% @doc
%%
%% @end
%% ----------------------------------------------------
3. search_lib.erl
%%%===================模块描述(开始)=====================
%%
%%%===================模块描述(结束)=====================
-module(search_lib).
%%%=======================STATEMENT====================
-time("2018/8/24 10:45").
%%%=======================EXPORT=======================
-export([sub_points/7, init_point/4, init_obstacles/1, obstacles_list/1, check_target/7,
read_obstacles/1, get_min/1]).
%%%=======================INCLUDE======================
-include("search.hrl").
%%%=======================RECORD=======================
%%%=======================DEFINE=======================
-define(LEFT, 1).
-define(TOP, 2).
-define(RIGHT, 3).
-define(BOTTOM, 4).
-define(MAX_LOOP, 100).
%%%=================EXPORTED FUNCTIONS=================
%% ----------------------------------------------------
%% @doc
%% 获得一个节点的所有可用子节点及其权重
%% ==>[{{X,Y},Weight},....]
%% @end
%% ----------------------------------------------------
sub_points(X, Y, XMin, XMax, YMin, YMax, ObEts) ->
X1 = max(XMin, X - 1),
X2 = min(XMax, X + 1),
Y1 = max(YMin, Y - 1),
Y2 = min(YMax, Y + 1),
lists:keydelete({X, Y}, 1, sub_points_(X + Y, X1, X2, Y1, Y1, Y2, ObEts, [])).
sub_points_(_Sum, X1, X2, _CurrntY, _Y, _YMax, _Obstacles, Result) when X1 > X2 ->
Result;
sub_points_(Sum, X1, X2, CurrntY, Y, YMax, ObEts, Result) when CurrntY > YMax ->
sub_points_(Sum, X1 + 1, X2, Y, Y, YMax, ObEts, Result);
sub_points_(Sum, X1, X2, CurrntY, Y, YMax, ObEts, Result) ->
case ets:lookup(ObEts, {X1, CurrntY}) of
[] ->
Weight = if
abs(Sum - (X1 + CurrntY)) =:= 1 ->
1.0;
true ->
1.4142135623730951
end,
sub_points_(Sum, X1, X2, CurrntY + 1, Y, YMax, ObEts, [{{X1, CurrntY}, Weight} | Result]);
_ ->
sub_points_(Sum, X1, X2, CurrntY + 1, Y, YMax, ObEts, Result)
end.
%%初始化一个坐标点
init_point(X, Y, ParentX, ParentY) ->
#point{f = 0,
h = 0,
g = 0,
point = {X, Y},
parent = {ParentX, ParentY}
}.
%%生成障碍物
init_obstacles(Ratio) ->
init_obstacles(Ratio, 0, ?Y_MAX, []).
init_obstacles(_Ratio, _X, Y, Result) when Y < 0 ->
Result;
init_obstacles(Ratio, X, Y, Result) when X > ?X_MAX ->
init_obstacles(Ratio, 0, Y - 1, Result);
init_obstacles(Ratio, X, Y, Result) ->
case z_lib:random(1, 100) < Ratio of
true ->
init_obstacles(Ratio, X + 1, Y, [{X, Y} | Result]);
_ ->
init_obstacles(Ratio, X + 1, Y, Result)
end.
%%障碍物列表
obstacles_list(List) ->
List.
read_obstacles(FileName) ->
{ok, [Obs]} = file:consult(FileName),
Obs.
%%检查目标点是否是障碍物
%%如果不是直接返回目标点,如果是循环查找目标点周围不是障碍物的点
%%寻找指定次数之后全是障碍物,返回none
check_target(X, Y, XMin, YMin, XMax, YMax, ObEts) ->
case ets:lookup(ObEts, {X, Y}) of
[] ->
{X, Y};
_ ->
check_target_(0, X - 1, Y - 1, XMin, YMin, XMax, YMax, ObEts, ?LEFT, 1, 1)
end.
check_target_(LoopTimes, _X, _Y, _XMin, _YMin, _XMax, _YMax, _Obstacles, _Direction, _Num, _Circle) when LoopTimes > ?MAX_LOOP ->%%最大循环查找次数
none;
check_target_(LoopTimes, X, Y, XMin, YMin, XMax, YMax, Obstacles, Direction, _Num, Circle) when Direction > ?BOTTOM ->%%换圈层
check_target_(LoopTimes + 1, X - 1, Y - 1, XMin, YMin, XMax, YMax, Obstacles, 1, 0, Circle + 1);
check_target_(LoopTimes, X, Y, XMin, YMin, XMax, YMax, ObEts, Direction, Num, Circle) ->
case X < XMin orelse X > XMax orelse Y < YMin orelse Y > YMax orelse ets:lookup(ObEts, {X, Y}) =/= [] of
true ->
if
Num >= Circle + 2 ->
check_target_(LoopTimes + 1, X, Y, XMin, YMin, XMax, YMax, ObEts, Direction + 1, 1, Circle);
Direction =:= ?LEFT ->
check_target_(LoopTimes + 1, X, Y + 1, XMin, YMin, XMax, YMax, ObEts, Direction, Num + 1, Circle);
Direction =:= ?TOP ->
check_target_(LoopTimes + 1, X + 1, Y, XMin, YMin, XMax, YMax, ObEts, Direction, Num + 1, Circle);
Direction =:= ?RIGHT ->
check_target_(LoopTimes + 1, X, Y - 1, XMin, YMin, XMax, YMax, ObEts, Direction, Num + 1, Circle);
true ->
check_target_(LoopTimes + 1, X - 1, Y, XMin, YMin, XMax, YMax, ObEts, Direction, Num + 1, Circle)
end;
_ ->
{X, Y}
end.
get_min(Ets) ->
case ets:first(Ets) of
'$end_of_table' ->
none;
F ->
K = get_min(Ets, F, hd(ets:lookup(Ets,F))),
ets:delete(Ets, K#point.point),
K
end.
get_min(_Ets, '$end_of_table', Point) ->
Point;
get_min(Ets, Key, Point) ->
P = ets:lookup(Ets, Key),
if
P#point.f < Point#point.f ->
get_min(Ets, ets:next(Ets, Key), P);
true ->
get_min(Ets, ets:next(Ets, Key), Point)
end.
%%%===================LOCAL FUNCTIONS==================
%% ----------------------------------------------------
%% @doc
%%
%% @end
%% ----------------------------------------------------
4. search_h.hrl
%%%-------------------------------------------------------------------
%%% @author Administrator
%%% @copyright (C) 2018, <COMPANY>
%%% @doc
%%%
%%% @end
%%% Created : 28. 八月 2018 14:15
%%%-------------------------------------------------------------------
-author("Administrator").
-record(point, {f, h, g, point, parent}).
%%坐标系统的最大值最小值
-define(X_MAX, 60).
-define(Y_MAX, 50).
-define(X_MIN, 0).
-define(Y_MIN, 0).