AStar A* 算法的Erlang实现

%% @author rolong@vip.qq.com
%% 本代码来自 瑞仙的Erlang开发博客
%% http://blog.csdn.net/zhongruixian

-module(astar1).

-export([
         find_path/2
         ,test/2
        ]).

-record(state, {
          open_trees %% 开启列表
          ,close_sets %% 关闭列表
          ,parents_trees %% 父节点列表
          ,xy %% 目标点
         }).

-record(point, {
          xy = {0, 0}
          ,g = 0
          ,h = 0
          ,f = 0
         }).

%% 1为向上，按顺时针依次为1~8
-define(DIRS, [1,2,3,4,5,6,7,8]).


%%' API
test(XY1, XY2) ->
    Path = find_path(XY1, XY2),
    io:format("Path:~n~p~n", [Path]),
    show_path(Path).

find_path(XY1, XY2) ->
    State = #state{
               open_trees = gb_trees:empty()
               ,close_sets = gb_sets:new()
               ,xy = XY2
               ,parents_trees = gb_trees:empty()
              },
    StartPoint = #point{xy = XY1},
    OpenTrees = gb_trees:enter(XY1, {0, 0, 0}, State#state.open_trees),
    State1 = State#state{open_trees = OpenTrees},
    find_next_point(StartPoint, State1).
%%.

%%' priv

-spec walkable({X, Y}) -> true | false when
      X :: integer(),
      Y :: integer().

%%'walkable函数根据具体点阵实现
%% 示例如下：

walkable({X, Y}) ->
    {Row, Col} = map(size),
    case X >= 0 andalso Y >= 0 andalso X < Row andalso Y < Col of
        true ->
            Block = map(block),
            Nth = X + Y * Row + 1,
            case string:substr(Block, Nth, 1) of
                "1" -> true;
                "0" -> false
            end;
        false -> false
    end;
walkable(_) -> false.

map(size) ->
    {20, 10};
map(block) ->
    %01234567890123456789
    "11111111111111111111" % 0
    "11111111111111111111" % 1
    "11111111111111111111" % 2
    "11111111000111111111" % 3
    "11111111000111111111" % 4
    "11111111000111111111" % 5
    "11111111000111111111" % 6
    "11111111000111111111" % 7
    "11111111000111111111" % 8
    "11111111111111111111" % 9
    .
%%.

find_next_point(#point{xy = XY}, #state{xy = XY} = State) ->
    get_path(XY, State#state.parents_trees, [XY]);

find_next_point(#point{xy = XY} = CurPoint, State) ->
    State1 = open2close(CurPoint, State),
    %% 将周围点添加到开放列表中
    AroundPoints = find_around_points(CurPoint, State1),
    State2 = add_open_trees(AroundPoints, XY, State1),
    case find_min_f_point(State2) of
        none ->
            io:format("Error coord:~w~n", [CurPoint#point.xy]),
            [];
        NextPoint ->
            find_next_point(NextPoint, State2)
    end.

find_min_f_point(State) ->
    Iter = gb_trees:iterator(State#state.open_trees),
    find_min_f_point(Iter, -1, none).

find_min_f_point(Iter, F, Point) ->
    case gb_trees:next(Iter) of
        none -> Point;
        {XY, {G1, H1, F1}, Iter1} ->
            case F1 < F orelse F =:= -1 of
                true ->
                    Point1 = #point{
                                xy = XY
                                ,g = G1
                                ,h = H1
                                ,f = F1
                               },
                    find_min_f_point(Iter1, F1, Point1);
                false ->
                    find_min_f_point(Iter1, F, Point)
            end
    end.

xy2point({CurX, CurY}, ParentPoint, {DstX, DstY}) ->
    #point{
       xy = {X, Y}
       ,g = G
      } = ParentPoint,
    AddG = if
               CurX =:= X -> 10;
               CurY =:= Y -> 10;
               true -> 14
           end,
    CurH = (erlang:abs(CurX - DstX) + erlang:abs(CurY - DstY)) * 10,
    CurG = G + AddG,
    #point{
       xy = {CurX, CurY}
       ,g = CurG
       ,h = CurH
       ,f = CurG + CurH
      }.

%% 找出周围点
find_around_points(Point, State) ->
    #state{
       close_sets = CloseSets
      } = State,
    #point{
       xy = {X, Y}
      } = Point,
    F = fun(Dir, Acc) ->
                XY1 = get_next_coord(Dir, X, Y),
                case walkable(XY1) of
                    true ->
                        case gb_sets:is_element(XY1, CloseSets) of
                            true -> Acc;
                            false ->
                                Point1  = xy2point(XY1, Point, State#state.xy),
                                [Point1 | Acc]
                        end;
                    false -> Acc
                end
        end,
    lists:foldl(F, [], ?DIRS).

add_open_trees([Point | Tail], ParentXY, State) ->
    case gb_trees:lookup(Point#point.xy, State#state.open_trees) of
        {_XY, {G, _H, _F}} ->
            case Point#point.g < G of
                true ->
                    State1 = add_open_trees1(Point, ParentXY, State),
                    add_open_trees(Tail, ParentXY, State1);
                false ->
                    add_open_trees(Tail, ParentXY, State)
            end;
        none ->
            State1 = add_open_trees1(Point, ParentXY, State),
            add_open_trees(Tail, ParentXY, State1)
    end;
add_open_trees([], _ParentXY, State) ->
    State.


add_open_trees1(Point, ParentXY, State) ->
    #point{xy = XY, g = G, h = H, f = F} = Point,
    OpenTrees1 = gb_trees:enter(XY, {G, H, F}, State#state.open_trees),
    ParentsTrees1 = gb_trees:enter(XY, ParentXY, State#state.parents_trees),
    State#state{
      open_trees = OpenTrees1
      ,parents_trees = ParentsTrees1
     }.

open2close(Point, State) ->
    OpenTrees = gb_trees:delete(Point#point.xy, State#state.open_trees),
    CloseSets = gb_sets:add(Point#point.xy, State#state.close_sets),
    State#state{
      open_trees = OpenTrees
      ,close_sets = CloseSets
     }.

get_next_coord(1,X,Y)->
	{X,Y-1};
get_next_coord(2,X,Y)->
	{X+1,Y-1};
get_next_coord(3,X,Y)->
	{X+1,Y};
get_next_coord(4,X,Y)->
	{X+1,Y+1};
get_next_coord(5,X,Y)->
	{X,Y+1};
get_next_coord(6,X,Y)->
	{X-1,Y+1};
get_next_coord(7,X,Y)->
	{X-1,Y};
get_next_coord(8,X,Y)->
	{X-1,Y-1}.

get_path(XY, ParentsTrees, Acc) ->
    case gb_trees:lookup(XY, ParentsTrees) of
        none -> Acc;
        {value, XY1} ->
            get_path(XY1, ParentsTrees, [XY1 | Acc])
    end.

%%'用于测试打印
show_path(XYs) ->
    Block = map(block),
    {Row, Col} = map(size),
    Len = Row * Col,
    show_path(XYs, Row, Col, Len, Block).

show_path([{X, Y} | XYs], Row, Col, Len, Block) ->
    LeftLen = X + Y * Row,
    RightLen = Len - LeftLen - 1,
    Left = string:left(Block, LeftLen),
    Right = string:right(Block, RightLen),
    Block1 = Left ++ "*" ++ Right,
    show_path(XYs, Row, Col, Len, Block1);
show_path([], Row, _Col, Len, Block) ->
    show_path1(Row, Len, Block, "").

show_path1(_Row, Len, _Block, Acc) when Len =< 0 ->
    io:format("~n~s~n", [Acc]);
show_path1(Row, Len, Block, Acc) ->
    Len1 = Len - Row,
    Left = string:left(Block, Row),
    Block1 = string:right(Block, Len1),
    Acc1 = Acc ++ Left ++ "\n",
    show_path1(Row, Len1, Block1, Acc1).
%%.
%%.

%%% vim: set foldmethod=marker filetype=erlang foldmarker=%%',%%.: