a*算法初步（源代码）

2019年11月4日 256次阅读

关于a*算法的文章和代码有很多，这里是我整理的一份源代码，从别人的例子中修改过来的。在这个代码示例中加入了地图信息对寻址结果的影响，可以用在包含不同种类的地形信息的地图中寻址。由于代码可以调试，你可以看清楚每一个步骤是怎么做的，在这里我就不多做讲解了。
#include <windows.h>
#include <stdio.h>

#define COLS 3 //地图
#define ROWS 3
#define TOTAL_TILES 9

#define TileNum( x, y ) ( y * COLS + x + 1 ) //判断是地图上的第几个格子

struct NODE {
 long f,h,i; //在这里，我增加了一个i函数，主要用于地图信息
 int g;
 int x,y;
 int NodeNum;
 struct NODE *Parent;
 struct NODE *Child[8];       /* a node may have upto 8+(NULL) children. */
 struct NODE *NextNode;       /* for filing purposes */
};

struct NODE *OPEN;
struct NODE *CLOSED;

int TileMap[TOTAL_TILES] = { /  
1, 1, 1, /
1, 2, 1, /
1, 1, 1
};
//地图信息，目前由(0,0)点到(2,2)点的寻址结果是(0,0)->(1,1)->(2,2)。如果中间的那个2修改成4，表明不能行走或者是沼泽路面行走困难，寻址结果是(0,0)->(0,1)->(1,2)->(2,2)。

struct STACK {
    struct NODE *NodePtr;
    struct STACK *NextStackPtr;
};
struct STACK *Stack;

void Push(struct NODE *Node)
{
    struct STACK *tmp;
    tmp = ( struct STACK *)calloc(1,sizeof(struct STACK));
    tmp->NodePtr = Node;
    tmp->NextStackPtr = Stack->NextStackPtr;
    Stack->NextStackPtr = tmp;
}

struct NODE *Pop()
{
    struct NODE *tmp;
    struct STACK *tmpSTK;
    tmpSTK = Stack->NextStackPtr;
    tmp = tmpSTK->NodePtr;
    Stack->NextStackPtr = tmpSTK->NextStackPtr;
    free( tmpSTK );
    return( tmp );
}

struct NODE *CheckOPEN(int tilenum)
{
 struct NODE *tmp = OPEN->NextNode;
 while( tmp ) {
  if( tmp->NodeNum == tilenum )
   return( tmp );
  else
   tmp = tmp->NextNode;
 }
 return( NULL );
}

struct NODE *CheckCLOSED(int tilenum)
{
 struct NODE *tmp = CLOSED->NextNode;
 while( tmp ) {
  if( tmp->NodeNum == tilenum )
   return( tmp );
  else
   tmp = tmp->NextNode;
 }
 return( NULL );
}

void PropagateDown(struct NODE *Old)
{
 int c,g;
 struct NODE *Child, *Father;

 g = Old->g;
 for( c = 0; c < 8; c ++ ) {
  if( ( Child = Old->Child[c] ) == NULL ) break;
  if( g + 1 < Child->g ) {
   Child->g = g + 1;
   Child->f = Child->g + Child->h + Child->i;
   //Child->f = Child->g + Child->h;
   Child->Parent = Old;
   Push( Child );
  }
 }
 while( Stack->NextStackPtr != NULL ) {
  Father = Pop();
  for( c = 0; c < 8; c ++ ) {
   if( ( Child = Father->Child[c] ) == NULL ) break;
   if( Father->g + 1 < Child->g ) {
    Child->g = Father->g + 1;
    Child->f = Child->g + Child->h + Child->i;
    //Child->f = Child->g + Child->h;
    Child->Parent = Father;
    Push( Child );
   }
  }
 }
}

void Insert(struct NODE *Successor)
{
 struct NODE *tmp1,*tmp2;
 long f;

 if(OPEN->NextNode == NULL) {
  OPEN->NextNode = Successor;
  return;
 }

 f = Successor->f;
 tmp1 = OPEN;
 tmp2 = OPEN->NextNode;
 while( ( tmp2 != NULL ) && ( tmp2->f < f ) ) {
  tmp1 = tmp2;
  tmp2 = tmp2->NextNode;
 }
 Successor->NextNode = tmp2;
 tmp1->NextNode = Successor;
}

void GenerateSucc(struct NODE *BestNode,long x, long y, long dx, long dy)
{
 int g, TileNumS, c;
 struct NODE *Old,*Successor;

 TileNumS = TileNum( x, y );
 //g = BestNode->g + TileMap[TileNumS];
 g = BestNode->g + 1;

 if( ( Old = CheckOPEN(TileNumS) ) != NULL ) {
  for( c = 0; c < 8; c ++ ) {
   if( BestNode->Child[c] == NULL ) break;
  }
  BestNode->Child[c] = Old;
  if( g < Old->g ) {
   Old->Parent = BestNode;
   Old->g = g;
   Old->f = g + Old->h + Old->i;
   //Old->f = g + Old->h;
  }
 }
 else if( ( Old = CheckCLOSED(TileNumS) ) != NULL ) {
  for( c = 0; c < 8; c ++ ) {
   if( BestNode->Child[c] == NULL ) break;
  }
  BestNode->Child[c] = Old;
  if( g < Old->g ) {
   Old->Parent = BestNode;
   Old->g = g;
   //Old->f = g + Old->h;
   Old->f = g + Old->h + Old->i;
   PropagateDown( Old );
  }
 }
 else {
  Successor = (struct NODE *)calloc(1,sizeof( struct NODE ));
  Successor->Parent = BestNode;
  Successor->NodeNum = TileNumS;
  Successor->g = g;
  //Successor->h = abs( x – dx ) + abs( y – dy );
  Successor->h = ( x – dx ) * ( x – dx ) + ( y – dy ) * ( y – dy );
  Successor->i = TileMap[Successor->NodeNum-1];
  Successor->f = g + Successor->h + Successor->i;
  //Successor->f = g + Successor->h;
  Successor->x = x;
  Successor->y = y;
  Insert(Successor);
  for( c = 0; c < 8; c ++ ) {
   if( BestNode->Child[c] == NULL ) break;
  }
  BestNode->Child[c]=Successor;
 }
}

void GenerateSuccessors(struct NODE *BestNode,long dx,long dy)
{
 long x,y;

 /* Upper-Left  */
 x = BestNode->x – 1; y = BestNode->y – 1;
 if( x >= 0 && y >= 0 )
  GenerateSucc(BestNode,x,y,dx,dy);

 /* Upper       */
 x = BestNode->x; y = BestNode->y – 1;
 if( y >= 0 )
  GenerateSucc(BestNode,x,y,dx,dy);

 /* Upper-Right */
 x = BestNode->x + 1; y = BestNode->y – 1;
 if( x < COLS && y >= 0 )
  GenerateSucc(BestNode,x,y,dx,dy);

 /* Right       */
 x = BestNode->x + 1; y = BestNode->y;
 if( x < COLS )
  GenerateSucc(BestNode,x,y,dx,dy);

 /* Lower-Right */
 x = BestNode->x + 1; y = BestNode->y + 1;
 if( x < COLS && y < ROWS )
  GenerateSucc(BestNode,x,y,dx,dy);

 /* Lower       */
 x = BestNode->x; y = BestNode->y + 1;
 if( y < ROWS )
  GenerateSucc(BestNode,x,y,dx,dy);

 /* Lower-Left  */
 x = BestNode->x – 1; y = BestNode->y + 1;
 if( x >= 0 && y < ROWS )
  GenerateSucc(BestNode,x,y,dx,dy);

 /* Left        */
 x = BestNode->x – 1; y = BestNode->y;
 if( x >= 0 )
  GenerateSucc(BestNode,x,y,dx,dy);
}

struct NODE *ReturnBestNode(void)
{
 struct NODE *tmp;
 tmp = OPEN->NextNode;
 OPEN->NextNode = tmp->NextNode;
 tmp->NextNode = CLOSED->NextNode;
 CLOSED->NextNode = tmp;
 return(tmp);
}

struct NODE *FindPath(long sx, long sy, long dx, long dy)
{
 struct NODE *Node, *BestNode;
 int TileNumDest = TileNum(dx,dy);
 OPEN = (struct NODE *)calloc(1,sizeof( struct NODE ));
 CLOSED = (struct NODE *)calloc(1,sizeof( struct NODE ));

 Node=(struct NODE *)calloc(1,sizeof( struct NODE ));
 Node->NodeNum = TileNum(sx,sy);
 Node->g = 0;
 //Node->h = abs( sx – dx ) + abs( sy – dy );
 Node->h = ( sx – dx ) * ( sx – dx ) + ( sy – dy ) * ( sy – dy );
 Node->i = TileMap[Node->NodeNum-1];
 Node->f = Node->g + Node->h + Node->i;
 //Node->f = Node->g + Node->h;
 Node->x = sx;
 Node->y = sy;

 OPEN->NextNode=Node;
 for (;;) {
  BestNode = (struct NODE *)ReturnBestNode();
  if( BestNode->NodeNum == TileNumDest ) break;
  GenerateSuccessors( BestNode, dx, dy );
 }
 return BestNode;
};

INT WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPTSTR lpCmdLine, int nCmdShow)
{
 Stack=( struct STACK *)calloc(1,sizeof(struct STACK));
 struct NODE * Path = FindPath( 0, 0, 2, 2 );
 return 0;
}

虽然用上面的算法可以找到目的地址，但是目前有几个问题：
1）效率问题：当起点到终点的路径不存在的时候，a*的花费比较大，应该加如预寻址算法或者改进当前算法。
2）当地图中不能走的地方边边角角较多的时候，寻址的结果中折线较多，此时可以寻求其次优解，将路径美化。
3）寻址方向目前有8个，对应于2D的八个方向，感觉应该能扩展到3D空间当中，做一个3D的a*寻址。3D寻址的时候应该是26个方向。

点赞