当前位置：首页 → 问答吧 → 发个A*寻径算法....

发个A*寻径算法....

时间：2011-10-19

来源：互联网

刚刚看到有位同学找类似的算法，就把它发出来，很久以前写的....
Delphi(Pascal) code


unit Formmain;

interface

uses
  Windows, Messages, SysUtils, Variants, Classes, Graphics, Controls, Forms,
  Dialogs, StdCtrls, Grids,XPMan;
type
  //运动方向定义
  TComPassirection = (cdNorth,cdNorthEast,cdEast,cdSouthEast,cdSouth,cdSouthWest,cdWest,cdNorthWest);
                     //北,    东北        东     东南        南        西南        西    西北
  //              (北)
  //               |
  //               |
  //  (西)---------|------------- (东)
  //               |
  //               |
  //               |
  //              (南)
  //为某个方向上定义相对于当前点的偏移量
  TDirectionOffset = array[TComPassirection] of TPoint;
  //记录经过的点
  TNode = record
    Direction : TComPassirection;
    GridPt : TPoint;
  end;
  PNode = ^TNode;

type
  TfrmMain = class(TForm)
    strngrdGridPath: TStringGrid;
    btnClearMap: TButton;
    btnFindPath: TButton;
    btnSetStart: TButton;
    lbl1: TLabel;
    procedure btnClearMapClick(Sender: TObject);
    procedure FormCreate(Sender: TObject);
    procedure FormDestroy(Sender: TObject);
    procedure strngrdGridPathMouseDown(Sender: TObject;
      Button: TMouseButton; Shift: TShiftState; X, Y: Integer);
    procedure strngrdGridPathDrawCell(Sender: TObject; ACol, ARow: Integer;
      Rect: TRect; State: TGridDrawState);
    procedure btnFindPathClick(Sender: TObject);
    procedure btnSetStartClick(Sender: TObject);
  private
    { Private declarations }
  public
    { Public declarations }
    procedure ClearPathQueue;
  end;

var
  frmMain: TfrmMain;
  StartPt,Endpt     : TPoint;                          //起始点和终点
  SetStart          : Boolean;                         //开始移动 
  PathQueue         : TList;                           //路径节点记录
  MapGrid           : array[0..20,0..30] of Byte ;     //地图坐标 0 表示可访问, 1 表示为障碍物
  VistedNotes       : array[0..20,0..30] of Boolean;   //记录哪些节点已经被访问过
const
  DirectionOffset   : TDirectionOffset = (             //为某个方向上定义相对于当前点的实际偏移量
                                         (X : 0; y : -1),(X : 1; y : -1),(X : 1; y : 0),
                                         (X : 1; y : 1),(X : 0; y : 1),(X : -1; y : 1),
                                         (X : -1; y : 0),(X : -1; y : -1));
  //定义每种节点类型
  NODECLEAR        = '';
  NODEOBSTACLE     = '1';
  NODESTART        = '2';
  NODEEND          = '3';
  NODEPATH         = '4';
  NODEVISITED      = '5';
  
implementation

{$R *.dfm}

procedure TfrmMain.btnClearMapClick(Sender: TObject);
  var i , j : Integer;
begin
  for i := 0 to 20 do
  begin
    for j := 0 to 30 do
     strngrdGridPath.Cells[i,j] := '';
  end;
end;

procedure TfrmMain.FormCreate(Sender: TObject);
begin
  StartPt := Point(-1,-1);
  Endpt   := Point(-1,-1);
  //默认情况下用鼠标左键设置起点
  SetStart := True;
  PathQueue := TList.Create;
end;

procedure TfrmMain.FormDestroy(Sender: TObject);
begin
  ClearPathQueue;
  PathQueue.Free;
end;

procedure TfrmMain.strngrdGridPathMouseDown(Sender: TObject;
  Button: TMouseButton; Shift: TShiftState; X, Y: Integer);
  var aRow,aCol : Integer;
begin
  strngrdGridPath.MouseToCell(x,y,aCol,aRow);
  if Button = mbright then
  begin
    //设置或清除障碍点
    if strngrdGridPath.Cells[aCol,aRow] = NODEOBSTACLE then
      strngrdGridPath.Cells[aCol,aRow] := NODECLEAR
    else
      strngrdGridPath.Cells[aCol,aRow] := NODEOBSTACLE;
  end
  else
  //设置起始和结束点
  if SetStart then
  begin
    //如果用户再次设置起始点,则清除原来的起点
    if StartPt.X <> - 1 then
      strngrdGridPath.Cells[StartPt.X,StartPt.Y] := NODECLEAR;
    //设置新起点
    strngrdGridPath.Cells[aCol,aRow] := NODESTART;
    StartPt := Point(aCol,aRow);
  end
  else
  begin
    //如果用户再次设置终点,则清除原来的终点
    if Endpt.X <> - 1 then
      strngrdGridPath.Cells[Endpt.X,Endpt.Y] := NODECLEAR;
    //设置新终点
    strngrdGridPath.Cells[aCol,aRow] := NODEEND;
    Endpt := Point(aCol,aRow);
  end; 
end;

procedure TfrmMain.strngrdGridPathDrawCell(Sender: TObject; ACol,
  ARow: Integer; Rect: TRect; State: TGridDrawState);
begin
  with strngrdGridPath do
  begin
    Canvas.Brush.Color := clWhite;
    //根据方格类型决定其颜色
    if Cells[ACol,ARow] = NODEOBSTACLE then
      Canvas.Brush.Color := clBlack;
    if Cells[ACol,ARow] = NODESTART then
      Canvas.Brush.Color := clBlue;
    if Cells[ACol,ARow] = NODEEND then
      Canvas.Brush.Color := clRed;
    if Cells[ACol,ARow] = NODEPATH   then
      Canvas.Brush.Color := clPurple;
    if Cells[ACol,ARow] = NODEVISITED then
      Canvas.Brush.Color := clGreen;
    Canvas.FillRect(Rect);
  end;
end;

procedure TfrmMain.ClearPathQueue;
  var aCount : Integer;
begin
  for aCount := 0 to PathQueue.Count - 1 do
  begin
    FreeMem(PathQueue[aCount],SizeOf(TNode));
  end;
  PathQueue.Clear;
end;
//A*寻径搜索算法原理:
// 判断本身与目标之间的方向,先选择一个方向,然后移动到该方向上的下一个点,同时计算该点不同方向上的下一个点离终点的距离,
//移动到最近的一个点上,若下一个点是障碍,则回退到该点,再次检查并将刚刚的点视为障碍物
procedure TfrmMain.btnFindPathClick(Sender: TObject);
  var iCount,iCount2 : Integer;
      Curpt,EvalPt,NewPt : TPoint;
      TempNode : PNode;
      Dist,EvalDist : DWORD;
      Dir,NewDir : TComPassirection;
      SearchDirs : array[0..2] of TComPassirection;
begin
  if (StartPt.X = -1) or (Endpt.X = -1) then Exit;
  //清除已经访问节点的数组
  FillChar(VistedNotes,SizeOf(VistedNotes),0);
  //设置障碍
  for iCount := 0 to 20 do
  begin
    for iCount2 := 0 to 30 do
    begin
      if strngrdGridPath.Cells[iCount,iCount2] = NODEOBSTACLE then
        MapGrid[iCount,iCount2] := 1
      else
        MapGrid[iCount,iCount2] := 0;
    end;
  end;
  //删除当前路径
  ClearPathQueue;
  //初始化跟踪变量
  Curpt :=  StartPt;
  VistedNotes[Curpt.X,Curpt.Y] := True;
  //决定起始方向 终点在起始方向左边
  if Endpt.X < StartPt.X then
  begin
    if Endpt.Y > StartPt.Y then      //西南
      Dir := cdSouthWest
    else if Endpt.Y < StartPt.Y then //西北
      Dir := cdNorthWest
    else
      dir := cdWest;                 //西边
  end
  else if Endpt.X > StartPt.X then
  begin
    if Endpt.Y > StartPt.Y then      //东南
      Dir := cdSouthEast
    else if Endpt.Y < StartPt.Y then
      Dir := cdNorthEast            //东北
    else
      Dir := cdEast;                //西
  end
  else  //正上方或正下方
  if Endpt.Y > StartPt.Y then
    Dir := cdSouth                  //北
  else if Endpt.Y < StartPt.Y then
    Dir := cdNorth;                 //南
  GetMem(TempNode,SizeOf(TNode));
  //用当前节点的信息初始化节点对象
  TempNode^.Direction := Dir;
  TempNode^.GridPt.X := Curpt.X;
  TempNode^.GridPt.Y := Curpt.Y;
  //将该节点添加到路径中
  PathQueue.Add(TempNode);
  //开始搜索路径,直到找到为止
  while(Curpt.X <> Endpt.X) or (Curpt.Y <> Endpt.Y) do
  begin
    //重置新坐标,表明未找到
    NewPt := Point(-1,-1);
    //将距离设为可能的最大值
    Dist := $FFFFFFFF;
    //确定3个搜索方向
    SearchDirs[0] := Pred(Dir);
    if Ord(SearchDirs[0]) < Ord(cdNorth) then
      SearchDirs[0] := cdNorthWest;
    SearchDirs[1] := Dir;
    SearchDirs[2] := Succ(Dir);
    if Ord(SearchDirs[2]) > Ord(cdNorthWest) then
      SearchDirs[2] := cdNorth;
    //估计3个方向上的网格位置
    for iCount := 0 to 2 do
    begin
      //根据当前面对的方向,获取相对于当前节点的下一个即将要检查的点的坐标
      EvalPt.X := Curpt.X + DirectionOffset[SearchDirs[iCount]].X;
      EvalPt.Y := Curpt.Y + DirectionOffset[SearchDirs[iCount]].Y;
      //确保该节点在地图范围内
      if (EvalPt.X > - 1) and (EvalPt.X < 20) and (EvalPt.Y > -1) and (EvalPt.Y < 30) then
      begin     //该节点没有被访问过
        if not VistedNotes[EvalPt.X,EvalPt.Y] then
        begin   //该节点不是障碍
          if MapGrid[EvalPt.X,EvalPt.Y] = 0 then
          begin
            EvalDist := (Endpt.X - EvalPt.X) * (Endpt.X - EvalPt.X) + (Endpt.Y - EvalPt.Y) * (Endpt.Y - EvalPt.Y);
            //如果发现某个节点的距离更近,则将该节点置为当前节点
            if EvalDist < Dist then
            begin
              //记录新的节点和距离
              Dist := EvalDist;
              NewPt := EvalPt;
              NewDir := SearchDirs[icount];
            end;
          end;
        end;
      end;

    end;
    //此时如果newpt仍是(-1,-1) 则说明遇到障碍物,故要回退一步重新查找,否则将该点添加到路径中
    if NewPt.X <> - 1 then
    begin
      //将该节点设为新节点
      Curpt := NewPt;
      //将该节点的方向设为新节点的方向
      Dir := NewDir;
      //设置节点为已经访问
      VistedNotes[Curpt.X,Curpt.Y] := True;
      //创建一个节点对象
      GetMem(TempNode,SizeOf(TNode));
      //用新的节点信息初始化节点
      TempNode^.Direction := Dir;
      TempNode^.GridPt.X := Curpt.X;
      TempNode^.GridPt.Y := Curpt.Y;
      //保存路径
      PathQueue.Add(TempNode);
      if PathQueue.Count > 100 then Break;
    end
    else  //已经退回到不可退回的节点,表明该方向无法找到路径,改善算法,重新计算起始方向并再次搜索路径,直到搜索完所有可能的方向
    begin
      if PathQueue.Count = 1 then Break;
      //设置为上一节点的方向 (回退)
      dir := TNode(PathQueue[PathQueue.Count - 2]^).Direction;
      //检索上一节点的坐标,并将其置为当前节点
      Curpt := TNode(PathQueue[PathQueue.Count - 2]^).GridPt;
//      MapGrid[TNode(PathQueue[PathQueue.Count - 2]^).GridPt.X,TNode(PathQueue[PathQueue.Count - 2]^).GridPt.Y] := 1;
      //释放并清除列表中最后一个节点
      FreeMem(PathQueue[PathQueue.Count - 1],SizeOf(TNode));
      PathQueue.Delete(PathQueue.Count - 1);
    end;
    //指定路径上的节点
    for iCount := 0 to PathQueue.Count - 1 do
    begin
      strngrdGridPath.Cells[TNode(PathQueue[iCount]^).GridPt.X,TNode(PathQueue[iCount]^).GridPt.Y] := NODEPATH;
    end;
    strngrdGridPath.Cells[StartPt.X,StartPt.Y] := NODESTART;
    strngrdGridPath.Cells[Endpt.X,Endpt.Y] := NODEEND;
  end;
end;

procedure TfrmMain.btnSetStartClick(Sender: TObject);
begin
  SetStart := not SetStart;
  if SetStart then
    btnSetStart.Caption := '设置起点'
  else
    btnSetStart.Caption := '设置终点'; 
end;

end.

作者: mdejtod 发布时间: 2011-10-19

窗体文件：
object frmMain: TfrmMain
Left = 312
Top = 144
BorderStyle = bsDialog
Caption = 'A*'#26368#30701#36335#24452#23547#24452#31639#27861
ClientHeight = 651
ClientWidth = 952
Color = clBtnFace
Font.Charset = DEFAULT_CHARSET
Font.Color = clWindowText
Font.Height = -11
Font.Name = 'MS Sans Serif'
Font.Style = []
OldCreateOrder = False
OnCreate = FormCreate
OnDestroy = FormDestroy
PixelsPerInch = 96
TextHeight = 13
object lbl1: TLabel
Left = 392
Top = 614
Width = 185
Height = 13
AutoSize = False
Caption = #40736#26631#22312#34920#26684#19978#21491#38190#35774#32622#38556#30861#28857
end
object strngrdGridPath: TStringGrid
Left = 8
Top = 16
Width = 929
Height = 577
ColCount = 15
DefaultColWidth = 60
FixedCols = 0
RowCount = 21
FixedRows = 0
TabOrder = 0
OnDrawCell = strngrdGridPathDrawCell
OnMouseDown = strngrdGridPathMouseDown
end
object btnClearMap: TButton
Left = 280
Top = 608
Width = 75
Height = 25
Caption = #28165#31354#22320#22270
TabOrder = 1
OnClick = btnClearMapClick
end
object btnFindPath: TButton
Left = 168
Top = 608
Width = 75
Height = 25
Caption = #25628#32034#36335#24452
TabOrder = 2
OnClick = btnFindPathClick
end
object btnSetStart: TButton
Left = 56
Top = 608
Width = 75
Height = 25
Caption = #35774#32622#36215#28857
TabOrder = 3
OnClick = btnSetStartClick
end
end

作者: mdejtod 发布时间: 2011-10-19

顶楼主. 算法要支持

作者: warrially 发布时间: 2011-10-19

csdn 图片显示这个问题，一直没改正啊。。。鄙视一下

作者: mdejtod 发布时间: 2011-10-19