笔记

#ifndef _FINDPATH_
#define _FINDPATH_

// 按512*512大小的地图，算法消耗24*512*512的内存，约为6M
struct _AstarNode
{
 unsigned int F, G;     // F, G值
 struct _AstarNode *Father;   // 父亲指针
 struct _AstarNode *Next;   // OpenList中下一个节点
 unsigned short X, Y;    // 在地图上的XY坐标
 unsigned char Value;    // 格子值, 越大表示越难于通行
 unsigned char Status;    // 0:Init  1:Opened  2:Clsoed
};

// 在地图更新时，建立新的Astar寻路用的地图寻路格子信息
struct _AstarNode *NewAstarMap(int W, int H, unsigned char *BlockValue);

// 每开始一次新的寻路，初始化Astar地图信息
void InitAstarMap(struct _AstarNode *AstarMap);

// 寻路, 返回的是路径的开始点，然后Next，Next一路找下去
struct _AstarNode *FindPath(int SrcX, int SrcY, int DstX, int DstY, struct _AstarNode *AstarMap);

#endif

---------------------------------------------------------------------

#include <stdlib.h>
#include <stdio.h>
#include <memory.h>
#include "conio.h"

#include "FindPath.h"

static unsigned int MapWidth;     // 地图宽
static unsigned int MapHeight;     // 地图高

// 8个方向邻居跟自己位置偏移，从12点方向开始，顺时针
const int DIR_XY[8][2] = {{0, -1}, {1, -1}, {1, 0}, {1, 1}, {0, 1},{-1, 1}, {-1, 0}, {-1, -1}};
const int DIR_G[8] = {10, 14, 10, 14, 10, 14, 10, 14};

// 在地图更新时，建立新的Astar寻路用的地图寻路格子信息
// BlockValue 就是地图格子上定义好的阻挡信息，'*' 表示可以通行，其他不能通行
struct _AstarNode *NewAstarMap(int W, int H, unsigned char *BlockValue)
{
 struct _AstarNode *AstarMap;
 
 AstarMap = (struct _AstarNode *)malloc(W * H * sizeof(struct _AstarNode));
 if (AstarMap == NULL)
 {
  return(NULL);
 }

 memset(AstarMap, 0, W * H * sizeof(struct _AstarNode));
 
 MapWidth = W;
 MapHeight = H;

 unsigned char *P = BlockValue;
 struct _AstarNode *Node = AstarMap;  
 for(int y=0; y<H; y++)
 {
  unsigned char *P = BlockValue + y*W;
  for(int x=0; x<W; x++)
  {
   Node->X = x;
   Node->Y = y;

   if (*P == '*')
   {
    Node->Value = 0;
   }
   else
   {
    Node->Value = 1;
   }
   
   P ++;
   Node ++;
  }
 }
 
 return(AstarMap);
}

// 每开始一次新的寻路，初始化Astar地图信息
void InitAstarMap(struct _AstarNode *AstarMap)
{
 struct _AstarNode *Node = AstarMap;

 for(int y=0; y<MapHeight; y++)
 {
  for(int x=0; x<MapWidth; x++)
  {
   Node->F = 0;
   Node->G = 0;
   Node->Father = NULL;
   Node->Status = 0;
   Node->Next = NULL;

   Node ++;
  }
 }
 return;  
}

// 在邻居中中找小等于自己的最大F值所在的节点, 它还要必须在OpenList中
// 后面要用它来加快在OpenList中的定位
struct _AstarNode *FindNearNode(struct _AstarNode *AstarNode)
{
 struct _AstarNode *Node;
 struct _AstarNode *Max_Node = NULL;
 unsigned int Max_F = 0;

 for(int i=0; i<8; i++)
 {
  // 计算邻居的XY
  unsigned int _X = AstarNode->X + DIR_XY[i][0];
  unsigned int _Y = AstarNode->Y + DIR_XY[i][1];
   
  // 检查是否已经超过地图界限
  if (_X < 0 || _X >= MapWidth || _Y < 0 || _Y >= MapHeight)
  {
   continue;
  }

  // 通过自己和邻居的相对位于来计算它的地址
  Node = AstarNode + DIR_XY[i][1] * MapWidth + DIR_XY[i][0];
  
  // 不在OpenList中，下一个
  if (Node->Status != 1)
  {
   continue;
  }

  // 大于自己的F，下一个
  if (Node->F > AstarNode->F)
  {
   continue;
  }

  if (Node->F > Max_F)
  {
   Max_F = Node->F;
   Max_Node = Node;
  } 
 }

 return(Max_Node);
}

// 插入一个新的OpenList节点
// 暂时采用简单优化的遍历方法
void InsertNode(struct _AstarNode *AstarNode, struct _AstarNode *OpenList)
{
 struct _AstarNode *Node = OpenList->Next;
 struct _AstarNode *PreNode = OpenList;

 // 如果邻居中F值比自己小，而且在OpenList中，以它为起点去扫描
 // 为了最大化效率，这个邻居的F值还是最接近自己的F
 struct _AstarNode *NearNode;
 NearNode = FindNearNode(AstarNode);
 if (NearNode != NULL)
 {
  Node = NearNode;
 }

 AstarNode->Status = 1;  // Opened

 while(Node != NULL)
 {
  if (Node->F > AstarNode->F)
  {
   PreNode->Next = AstarNode;
   AstarNode->Next = Node;
   return;
  }
 
  PreNode = Node;
  Node = Node->Next; 
 }
 
 // 到了这里，表示AstarNode应该插到OpenList的最后
 PreNode->Next = AstarNode;
 AstarNode->Next = NULL;
 
 return;  
}

// 摘取出OpenList的第一个节点
struct _AstarNode *Pop(struct _AstarNode *OpenList)
{
 struct _AstarNode *P = OpenList->Next;
 
 OpenList->Next = P->Next;
 
 P->Next = NULL;
 P->Status = 2;  // Closed

 return(P);
}

// 寻路
// 返回的是路径的开始点，然后Next，Next一路找下去
struct _AstarNode *FindPath(int SrcX, int SrcY, int DstX, int DstY, struct _AstarNode *AstarMap)
{
 bool IsFounded = false;
 
 struct _AstarNode *StartNode = AstarMap + SrcY * MapWidth + SrcX; 
 struct _AstarNode *StopNode = AstarMap + DstY * MapWidth + DstX;
 struct _AstarNode *CurNode, *ChildNode;
 int G, F, H;

 struct _AstarNode OpenList;
 OpenList.Next = NULL;

 // 如果源本身就是一个阻隔点，直接返回StopNode作为路径的开始点
 if (StartNode->Value > 0)
 {
  return(NULL); 
 }

 // 如果目标本身就是一个阻隔点, 直接返回失败
 if (StopNode->Value > 0)
 {
  return(NULL); 
 }

 // 源和目标一样，直接返回StartNode作为路径的开始点
 if (StartNode->X == StopNode->X && StartNode->Y == StopNode->Y)
 {
  StartNode->Next = NULL;
  return(StartNode);
 }
 
 InsertNode(StartNode, &OpenList);
 
 while(IsFounded == false)
 {
  // 找F值最小的节点作为当前节点
  CurNode = Pop(&OpenList);
  if (CurNode == NULL)  // 如果为NULL，表示 OpenList 已经为空，寻路结束
  {
   break;
  }
  
  // 检查当前节点八个方向的邻居
  // 路径平滑处理，引入DirCount，是为了让保证原方向优先，降低W型路径的出现机会
  static unsigned int DirCount = 0;
  for(int i=0; i<8; i++, DirCount++)
  {
   // 计算邻居的XY
   unsigned int DD = DirCount%8;
   unsigned int _X = CurNode->X + DIR_XY[DD][0];
   unsigned int _Y = CurNode->Y + DIR_XY[DD][1];
   
   // 检查是否已经超过地图界限
   if (_X < 0 || _X >= MapWidth || _Y < 0 || _Y >= MapHeight)
   {
    continue;
   }
   
   ChildNode = AstarMap + _Y * MapWidth + _X;
   
   // 邻居是不可通行格子，下一个。（暂时只支持0,1模式）
   if (ChildNode->Value > 0)
   {
    continue;
   }
   
   // 邻居为 Closed，下一个
   if (ChildNode->Status == 2)
   {
    continue;
   }
   
   // 计算邻居的G、H、F值
   G = CurNode->G + DIR_G[DD];
   H = abs(SrcX - DstX)*10 + abs(SrcY - DstY)*10;
   F = H + G; 
   
   if (ChildNode->Status != 1)
   {
    // 不在OpenList，插入
    ChildNode->Father = CurNode;
    ChildNode->G = G;
    ChildNode->F = F;

    InsertNode(ChildNode, &OpenList);
    
    // 检查是否为 目标点，是就表示找到，结束寻路
    if (ChildNode->X == StopNode->X && ChildNode->Y == StopNode->Y)
    {
     IsFounded = true;
     break;
    }
   }
   else
   {
    // 邻居在 OpenList 中，
    // 如果邻居现在的G值比在OpenList的G值小，更新它
    // 下面这段代码不能提前，否则会导致G提前被修改
    if (ChildNode->G > G)
    {
     ChildNode->G = G;
     ChildNode->F = F;
     ChildNode->Father = CurNode;
    }
   }

  } // end For
  
 } // end While

 // 路径存在，从 目标点 回溯到 开始点，产生正向路径
 // 从开始点开始，借用Next来保存路径
 if (IsFounded == true)
 {
  struct _AstarNode *P = StopNode;
  struct _AstarNode *Father;
  
  P->Next = NULL;    // 设置路径最后标志

  while(P != NULL)
  {
   Father = P->Father;

   if (Father == NULL)  // 异常检查，保持不会出现Core
   {
    return(NULL);
   }

   Father->Next = P;
   if (Father->X == StartNode->X && Father->Y == StartNode->Y)
   {
    break;
   }
   else
   {
    P = Father;
   }
  }

  return(Father);
 }
 else
 {
  return(NULL);
 } 
}

const unsigned char MapBlocks[16][16] = {
{"               "},
{"  *****        "},
{"  *****        "},
{"    ****       "},
{"      ****     "},
{"          **** "},
{"     ******   *"},
{"     ***      *"},
{"     *        *"},
{"    *         *"},
{"   *           "},
{"  *************"},
{"    *          "},
{"   *****       "},
{"        *      "},
{" ********      "}};      

 

int main(void)
{
 struct _AstarNode *AStarMap = NewAstarMap(16, 16, (unsigned char *)MapBlocks);
 if (AStarMap == NULL)
 {
  printf("Map Init Error\n");
  return(0);
 }

 printf("MapBlocks: [*] OK \n");
 for(int i=0; i<16; i++)
 {
  printf("%02d:|%s|\n", i, (unsigned char *)MapBlocks+i*16);
 }
 
 InitAstarMap(AStarMap);

 struct _AstarNode *Path = FindPath(2, 1, 2, 15, AStarMap);
 if (Path == NULL)
 {
  printf("Path Not Found\n");
  return(0);
 }
 

 printf("RoutePoint: ");
 while(Path->Next != NULL)
 {
  printf("[%d,%d]->", Path->X, Path->Y);

  Path = Path->Next;
 }

 printf("[%d,%d]  End! ", Path->X, Path->Y);

 getch();

 delete(AStarMap);
}