erlang之A*寻路

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).

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值