A*算法寻路算法(最短路径???)代码文件2

// find_path.cpp: implementation of the find_path class.
//
//

#include "stdafx.h"
#include "find_path.h"

//
// Construction/Destruction
//

find_path::find_path()
{
 _pf_go_test = NULL;

 node_close = NULL;
 node_open = NULL;
 node_next = NULL;

 path_p = NULL;
 node_map = NULL;
}

find_path::~find_path()
{
 node_close = NULL;
 node_open = NULL;
 node_next = NULL;

 if (NULL != path_p)
 {
  delete []path_p;
  path_p = NULL;
 }
 
 if (NULL != node_map)
 {
  delete []node_map;
  node_map = NULL;
 }
}

void find_path::reset()
{
 path_t = 0;
 memset(path_p, 0, path_max*sizeof(path_z));

 memset(node_map, 0, path_max*sizeof(path_node));
 for (int j = 0; j < h; j++)
 {
  for (int i = 0; i < w; i++)
  {
   T * p = node_map + w*j + i;
   p->x = i;
   p->y = j;
  }
 }
}

void find_path::init(int width, int height, pf_go_test pf)
{
 set_pf(pf);

 w = width;
 h = height;
 
 path_max = w*h;
 
 path_p = new path_z[path_max];
 node_map = new path_node[path_max];
 
 //reset();
}

//--返回找到的最短路径步数
//--返回-1表示无法到达目的地
//--并将找到的最短路径存储在path_p
int find_path::go(int x0, int y0, int x1, int y1, pf_go_test pf)
{
 reset();
 set_pf(pf);
 
 //reset();
 node_open = (T*)(node_map) + w*y0 + x0;
 
 T * root;
 //root = node_open;

 int xx, yy;//--决定x轴或y轴各自的优先寻路方向
 int dx, dy;//--决定x轴与y轴的优先寻路方向
 bool find = false;
 while (NULL != node_open)
 {
  root = GetFromOpenQueue();
  if (NULL == root) break;
  
  T & t = *root;
  
  if (x1 == t.x && y1 == t.y)
  {
   find = true;
   break; // 达到目的地成功返回
  }
  
  //--决定优先寻路方向
  if (x1 > t.x)
  {
   xx = 1;
   dx = x1 - t.x;
  }
  else
  {
   xx = 0;
   dx = t.x - x1;
  }
  //--
  if (y1 > t.y)
  {
   yy = 1;
   dy = y1 - t.y;
  }
  else
  {
   yy = 0;
   dy = t.y - y1;
  }
  
  //--后面的优先
  
  //--y方向优先
  if (0 == dx
   || (0 != dy && dx > dy))
  {
   //--x方向+优先
   if (1 == xx)
   {
    TryTile(t.x - 1, t.y, root);
    TryTile(t.x + 1, t.y, root);
   }
   //--x方向-优先
   else
   {
    TryTile(t.x + 1, t.y, root);
    TryTile(t.x - 1, t.y, root);
   }
   
   //--y方向+优先
   if (1 == yy)
   {
    TryTile(t.x, t.y - 1, root);
    TryTile(t.x, t.y + 1, root);
   }
   //--y方向-优先
   else
   {
    TryTile(t.x, t.y + 1, root);
    TryTile(t.x, t.y - 1, root);
   }
  }
  //--x方向优先
  else
  {
   //--y方向+优先
   if (1 == yy)
   {
    TryTile(t.x, t.y - 1, root);
    TryTile(t.x, t.y + 1, root);
   }
   //--y方向-优先
   else
   {
    TryTile(t.x, t.y + 1, root);
    TryTile(t.x, t.y - 1, root);
   }
   
   //--x方向+优先
   if (1 == xx)
   {
    TryTile(t.x - 1, t.y, root);
    TryTile(t.x + 1, t.y, root);
   }
   //--x方向-优先
   else
   {
    TryTile(t.x + 1, t.y, root);
    TryTile(t.x - 1, t.y, root);
   }
  }
 };
 if (false == find)
 {
  //--无法到达目的地
  path_t = -1;
  return path_t;
 }

 //--回溯树
 //--将求出的最佳路径(逆向)保存在path_p
 for (path_t = 0; root; path_t++)
 {
  path_p[path_t].x = root->x;
  path_p[path_t].y = root->y;
  
  root = root->parent;
 }
 if (path_t > 0)
 {
  path_t--;
 
  //--路径反转
  int t = path_t/2;
  for (int i = 0; i < t; i++)
  {
   path_z z = path_p[i];
   path_p[i] = path_p[path_t - i];
   path_p[path_t - i] = z;
  }
 }
 
 return path_t;
}