erlang-Astart算法

%%%-------------------------------------------------------------------
%%% Module  : astar_trace
%%% Author  : 
%%% Description : A星寻路算法
%%%-------------------------------------------------------------------
-module(astar_trace).

-export([generate_route/8,generate_route/7,generate_route/5])。

-define(EXPAND_POINTS(X,Y),[{X-1,Y},{X-1,Y-1},{X ,Y-1},{X+1,Y-1},{X+1,Y},{X+1,Y+1},{X,Y+1},{X-1,Y+1}]).	%% 扩展查找的点

-record(point, {x = 0, y = 0, g = 0, h = 0, f = 0, p_x = 0, p_y = 0}).

generate_route(StartX,StartY,TargetX,TargetY,SceneId) -> 
    generate_route(StartX,StartY,TargetX,TargetY,SceneId,50,50).
generate_route(StartX,StartY,TargetX,TargetY,SceneId,MaxTraceRangeX,MaxTraceRangeY) -> 
    generate_route(StartX,StartY,TargetX,TargetY,SceneId,MaxTraceRangeX,MaxTraceRangeY,0).
generate_route(StartX,StartY,TargetX,TargetY,SceneId,MaxTraceRangeX,MaxTraceRangeY,Flying) ->
	%% 初始化开始点坐标
	CostH = calculate_cost(StartX,StartY,TargetX,TargetY),
	CostG = 0,
    OpenPoints = [#point{x=StartX, y=StartY, g=CostG,h=CostH,f = CostG + CostH}],
	
	%% 目标点坐标
	case search(OpenPoints,[],#point{x = TargetX, y = TargetY},SceneId,MaxTraceRangeX,MaxTraceRangeY,Flying) of
		false ->
			false;
		{Point,ResultOpenPoints,ResultClosePoints} ->
		    PosList = [{P#point.x, P#point.y}||P <- generate_solution(Point,[],ResultClosePoints ++ ResultOpenPoints)],
		    PosList
	end.
	
%% 查找目标点
%% param: OpenPoints 扩充列表,closePoints 已遍历列表,TargetPoint 目标点
search([],_ClosePoints,_TargetPoint,_SceneId,_MaxTraceRangeX,_MaxTraceRangeY,_Flying)->
    false;

search(OpenPoints,ClosePoints,TargetPoint,SceneId,MaxTraceRangeX,MaxTraceRangeY,Flying)->
    [Point | NewOpenPoints] = lists:keysort(#point.f, OpenPoints),
    NewClosePoints = [Point | ClosePoints],
    case is_target(Point,TargetPoint) of
        true->
            {Point,NewOpenPoints,NewClosePoints};
         false ->
            %% 未找到，
            AddOpenPoints = expand(NewOpenPoints,NewClosePoints,Point,TargetPoint,SceneId,MaxTraceRangeX,MaxTraceRangeY,Flying),	%% 坐标点扩充
            search(AddOpenPoints ++ NewOpenPoints,NewClosePoints,TargetPoint,SceneId,MaxTraceRangeX,MaxTraceRangeY,Flying)
	end.

%% 坐标点扩充
expand(OpenPoints,ClosePoints,#point{x=X, y=Y, g=G},#point{x = TargetX,y = TargetY},SceneId,MaxTraceRangeX,MaxTraceRangeY,Flying) ->
    
    CostG = G + 1,
	
	F = fun({X1,Y1},AccIn) ->
				case is_blocked(X1,Y1,TargetX,TargetY,SceneId,MaxTraceRangeX,MaxTraceRangeY,Flying) of
					true ->
						AccIn;
					false ->
                        case [true || #point{x = OpentX,y = OpentY} <- OpenPoints,OpentX == X1, OpentY == Y1] of
                            [] ->
                                case [true || #point{x = CloseX,y = CloseY} <- ClosePoints , CloseX == X1, CloseY == Y1]  of
                                    [] ->
						                CostH = calculate_cost(X1,Y1,TargetX,TargetY),
                                        [#point{x = X1,y = Y1,g = CostG,h = CostH,f = CostH + CostG,p_x = X,p_y = Y} | AccIn];
                                    _ ->
                                        %% 已在 close 列表 中不加
                                        AccIn
                                end;
                            _ ->	
                                %% 已在 opent 列表 中不加
                                AccIn
                        end
                end
		end,
    lists:foldl(F,[],?EXPAND_POINTS(X,Y)).

  
%% 生成结果列表
generate_solution(#point{p_x = X,p_y = Y} = Point,SolutionPoints,Points)->
    case [Info || #point{x = X2,y = Y2} = Info <- Points, X2 == X, Y2 == Y] of
        [] ->
            SolutionPoints;
        [NewPoint | _] ->
			generate_solution(NewPoint,[Point | SolutionPoints],lists:delete(NewPoint,Points))
    end.
            

%% 权重计算
calculate_cost(X1,Y1,X2,Y2) ->
	((X1-X2)*(X1-X2)+(Y1-Y2)*(Y1-Y2)).

%% 是否到达目标点了,到达目标周围点就可以了
is_target(#point{x = StartX,y = StartY}, #point{x = TargetX, y = TargetY}) ->
    StartX == TargetX andalso StartY == TargetY.
%%	abs(StartX - TargetX) =< 1 andalso abs(StartY - TargetY) =<1.


%% check is block
is_blocked(X,Y,TargetX,TargetY,SceneId,MaxTraceRangeX,MaxTraceRangeY,_Flying)->
	case abs(TargetX - X) > MaxTraceRangeX orelse abs(TargetY - Y) > MaxTraceRangeY of
		true ->		%% 在查找范围外，作为阻档处理
			true;	
		false ->	%% 在查找范围内
           not is_block(SceneId, X, Y)
    end.
	
is_block(_SceneId, _X, _Y) ->
	false.