a*寻路

#include <list>
#include <math.h>
#include <map>
#include "iostream"

using namespace std;

 


// 坐标点
typedef struct  _point
{
 int timelist[1000];
 bool isWall;
 int x;
 int y;
 int isblock;
 _point()
 { 
  for(int i = 0; i < 1000; i++)
   timelist[i] = 0;
  isWall = false;
  x = 0;
  y = 0;
 }

 _point(int i)
 {
  isblock = i;
  for(int i = 0; i < 1000; i++)
   timelist[i] = 0;
  isWall = false;
  x = 0;
  y = 0;
 }
 _point(int _x, int _y)
 {
  x = _x;
  y = _y;
 }
}point;
//结点
typedef struct _Node
{
 point pt;
 int f, g, h;
 struct _Node *preNode;//父节点
}Node;
/************************************************************************/
/* A 星算法                                                                     */
/************************************************************************/
class AStar{
public:
 AStar(void);
 ~AStar(void);
public:
 int table_c[6][6];

 static const int SPEND_H = 10;//H值耗费
 static const int SPEND_G_FAR = 14;//远的G值耗费
 static const int SPEND_G_NEAR = 10;//近的G值耗费
 list<Node*> psOpenList; //开放列表
 list<Node*> psCloseList;//封闭列表
 Node *sStartNode; //起始节点
 Node *sEndNode; //终止节点
 Node *pcurrNode;//是否是终点? NOT NULL : NULL

 map<int,point> path; //用于存放寻路后的数据
 map<int,point>::iterator it;
public:
 void SetTables(int *);
 void SetStartNode(point st){sStartNode->pt.x = st.x;sStartNode->pt.y = st.y;};//设定起始节点
 void SetEndNode(point et){sEndNode->pt.x = et.x;sEndNode->pt.y = et.y;};//设定结束节点
 void Search();//主搜索
 void AddToOpenList(Node *src);//添加到开放列表
 void GetNodeByPoint(Node *pareNode, int _x, int _y);//通过坐标添加到开放列表
 bool IsClosed(Node *src);//坐标点是否在封闭列表中
 bool IsInOpenList(Node *src);//坐标点是否在开放列表中
 bool IsBlock(Node *src);//坐标点是否是阻挡点
 void SetH(Node *node);//设定H的耗费
 void SetG(Node *node);//设定G的耗费
 void SetF(Node *node);//设定F的耗费
 Node* GetBestNode(); //得到最优的节点(F值最低的节点)
 bool isPath(list<point> *path, int i, int j);//测试方法-用于输出
};

AStar::AStar()
{
 sStartNode = new Node();
 sEndNode = new Node();
 pcurrNode = NULL;
}
AStar::~AStar(void)
{
 delete sStartNode;
 delete sEndNode;
 delete pcurrNode;
}
void AStar::SetTables(int *tables)
{
 if (tables != NULL)
 {
  for (int i = 0; i < 6; i++)
  {
   for (int j = 0; j < 6; j++)
   {
    table_c[i][j] = tables[i*6+j];
   }  
  }
 }
}
void AStar::Search()
{
 Node *currentNode;//当前节点
 currentNode = sStartNode;
 currentNode->preNode = NULL;//初始节点的父节点为空
 currentNode->g = 0;
 SetH(currentNode);
 SetF(currentNode);
 psOpenList.push_front(sStartNode);//添加起始点到开放列表
 do
 {
  currentNode = GetBestNode();//从开放列表得到最优节点
  //遍历当前结点周围的结点并加入开放列表,当前节点从开放列表中移到封闭列表中
  AddToOpenList(currentNode);

  psCloseList.push_front(currentNode);//添加到关闭列表中
  if(psOpenList.size() < 1 || pcurrNode)break; //如果目标节点已经存在于开放列表或开放列表是空则退出
 } while (true);
 //*******************************测试方法Start*******************************//
 // list<point> path; //用于存放寻路后的数据

 do
 {
  //path.push_front(currentNode->pt);
  path.insert(map<int,point>::value_type(path.size()+1,currentNode->pt));
  //cout<<currentNode->pt.x<<"/t"<<currentNode->pt.y<<endl;
  if(currentNode->pt.x == sStartNode->pt.x && currentNode->pt.y == sStartNode->pt.y)break;
  currentNode = currentNode->preNode;

 } while (true);

 /* //输出模拟的地图和寻路后的路径
 for(int j=0;j<6;++j)
 {
 for (int i=0;i<6;++i)
 {
 if(isPath(&path,i,j))
 cout<<"/033[1;37m o"<<"/t";
 else
 cout<<"/033[0;32;31m ×"<<"/t";
 }
 cout<<endl;
 }*/
 //*******************************测试方法End*******************************//

}
//*******************************测试方法Start*******************************//
bool AStar::isPath(list<point> *path, int i, int j)
{
 for(list<point>::iterator openIterator = path->begin(); openIterator != path->end(); ++openIterator)
 {
  if((*openIterator).x == i && (*openIterator).y == j){return true;}
 }
 return false;
}
//*******************************测试方法End*******************************//
//添加到开放列表
void AStar::AddToOpenList(Node *src){
 //添加当前坐标的周围坐标(一共8个方向)
 GetNodeByPoint(src, src->pt.x, src->pt.y + 1);//上
 GetNodeByPoint(src, src->pt.x, src->pt.y - 1);//下
 GetNodeByPoint(src, src->pt.x - 1, src->pt.y);//左
 GetNodeByPoint(src, src->pt.x + 1, src->pt.y);//右
 GetNodeByPoint(src, src->pt.x - 1, src->pt.y + 1);//左上
 GetNodeByPoint(src, src->pt.x - 1, src->pt.y - 1);//左下
 GetNodeByPoint(src, src->pt.x + 1, src->pt.y + 1);//右上
 GetNodeByPoint(src, src->pt.x + 1, src->pt.y - 1);//右下
}
void AStar::GetNodeByPoint(Node *pareNode, int _x, int _y)
{
 //如果坐标已经越界则不添加---具体参数读配置文件
 if(_x < 0 || _y< 0 || _x >10 || _y>10)
  return ;
 Node *sNode = new Node();
 //设定坐标值
 sNode->pt.x = _x;
 sNode->pt.y = _y;
 //如果此坐标存在于封闭列表或是阻挡物的话,不进行添加
 if(IsClosed(sNode) || IsBlock(sNode))
 {
  return;
 }
 else
 {
  //设定父结点
  sNode->preNode = pareNode;
  SetG(sNode);SetH(sNode);SetF(sNode);//设定各种耗费
  psOpenList.push_front(sNode);//添加到开放列表
  if(sNode->pt.x == sEndNode->pt.x && sNode->pt.y == sEndNode->pt.y)
   pcurrNode = sNode;//终点坐标已经存在于开放列表
 }
}
//是否存在于封闭列表
bool AStar::IsClosed(Node *src)
{
 for(list<Node*>::iterator openIterator = psCloseList.begin(); openIterator != psCloseList.end(); ++openIterator)
 {
  if((*openIterator)->pt.x == src->pt.x && (*openIterator)->pt.y == src->pt.y){return true;}
 }
 return false;
}
//是否存在于开放列表
bool AStar::IsInOpenList(Node *src)
{
 for(list<Node*>::iterator openIterator = psOpenList.begin(); openIterator != psOpenList.end(); ++openIterator)
 {
  if((*openIterator)->pt.x == src->pt.x && (*openIterator)->pt.y == src->pt.y){return true;}
 }
 return false;
}
//是否是阻挡坐标
bool AStar::IsBlock(Node *src)
{
 //下面是测试数据,真实的数据去读配置文件
 if (table_c[src->pt.x][src->pt.y])
 {
  return true;
 }
 
// if(!tables[src->pt.y][src->pt.x])return true;
 //if(src->pt.isWall)return true;
 /* if(src->pt.x == 0 && src->pt.y == 4)return true;
 if(src->pt.x == 1 && src->pt.y == 4)return true;
 if(src->pt.x == 2 && src->pt.y == 9)return true;
 if(src->pt.x == 3 && src->pt.y == 4)return true;
 if(src->pt.x == 4 && src->pt.y == 4)return true;
 //if(src->pt.x == 5 && src->pt.y == 4)return true;
 if(src->pt.x == 6 && src->pt.y == 4)return true;
 if(src->pt.x == 7 && src->pt.y == 4)return true;
 if(src->pt.x == 8 && src->pt.y == 4)return true;
 if(src->pt.x == 9 && src->pt.y == 4)return true;
 if(src->pt.x == 10 && src->pt.y == 4)return true;*/
 return false;
}
//计算从B点(某点)到终点的耗费
void AStar::SetH(Node *node)
{
 node->h = (abs(node->pt.x - sEndNode->pt.x) + abs(node->pt.y - sEndNode->pt.y)) * SPEND_H;
}
//计算从起点到B点(某点)的耗费
void AStar::SetG(Node *node)
{
 if(node->pt.x != node->preNode->pt.x && node->pt.y != node->preNode->pt.y)
  node->g = node->preNode->g + SPEND_G_FAR;
 else
  node->g = node->preNode->g + SPEND_G_NEAR;
}
//计算总体耗费 F = G + H
void AStar::SetF(Node *node)
{
 node->f = node->g + node->h;
}
//从开放列表中得到最优值(F值最低)
Node* AStar::GetBestNode()
{
 Node* bNode;
 int bestF = 9999999;
 list<Node*>::iterator iterT;//记录最优值的位置方便删除
 for(list<Node*>::iterator openIterator = psOpenList.begin(); openIterator != psOpenList.end(); ++openIterator)
 {
  if(bestF > (*openIterator)->f){
   bestF = (*openIterator)->f; bNode = *openIterator;
   iterT = openIterator;
  }
 }
 if(bNode)
  psOpenList.erase(iterT);//找到最优值后从开放列表中删除
 return bNode;
}

 


/*
int tables[6][6] = {{1,1,1,0,1,1},
     {1,1,1,0,1,1},
     {1,0,1,0,1,1},
     {1,1,1,0,1,1},
     {0,1,1,1,1,1},
     {0,1,1,0,0,1}};
*/

int tables_ta[36] = {1,1,1,0,1,1,1,1,1,0,1,1,1,0,1,0,1,1,1,1,1,0,1,1,0,1,1,1,1,1,0,1,1,0,0,1};

struct Wizard
{
 int id;
 int begintime;
 int speed;

 point nowPoint;
 point goPoint;

 AStar astar;
 Wizard()
 {
  id = -1;
  begintime = 1;
  speed = 1;

  nowPoint.x = 0;
  nowPoint.y = 0;

  goPoint.x = 0;
  goPoint.y = 0;

  point tp;
  tp.x = nowPoint.x;
  tp.y = nowPoint.y;
  path.insert(map<int,point>::value_type(path.size()+1,tp));
 }

 map<int,point> path; //用于存放寻路后的数据
 map<int,point>::iterator it;
};


struct  node
{
 bool bIsWall;
 map<int,int> nodelist;
 map<int,int>::iterator it;

 node()
 {
  bIsWall = false;
 }
};

 

void getway_t(Wizard *t_Wizard, node table_table[][6])
{
 // 1.路径查找
 int tabless[6][6];
 for (int i = 0; i<6; i++)
 {
  for (int j = 0; j < 6; j++)
  {
   tabless[i][j] = 0;
  }  
 }
 
 bool bIsOut = false;
 for (int i = 0; i < 4; i++)
 {
  int po = 1;// 时间位置

  while (!bIsOut)
  {
   t_Wizard->it = t_Wizard->path.find(po);
   // 得到时间片的 地图信息
   for(int m = 0; m < 6; m++)
   {
    for (int n = 0; n < 6; n++)
    {
     if(table_table[m][n].bIsWall)
     {
      tabless[m][n] = -1;
     }else if (m <= t_Wizard->it->second.x+1 && m >= t_Wizard->it->second.x-1 && n <= t_Wizard->it->second.y+1 && n >= t_Wizard->it->second.y-1)// 周围8个方向位置
     {
      table_table[m][n].it = table_table[m][n].nodelist.find(po+t_Wizard->speed);// 往后推他停留时间 相对最小时间倍数
      if (table_table[m][n].it == table_table[m][n].nodelist.end())
      {
       tabless[m][n] = 0;
      }else
      {
       tabless[m][n] = table_table[m][n].it->second;
      } 
     }else
     {
      tabless[m][n] = 0;
     }     
    }
   }

   AStar astar;// 设置地图
   if ()
   {
    astar.SetTables(tables_ta);
   }else
   {
    astar.SetTables(tables_ta);
   }
   

   t_Wizard->it = t_Wizard->path.find(po);

   astar.SetStartNode(t_Wizard->it->second);
   astar.SetEndNode(t_Wizard->goPoint);
   astar.Search();

   astar.it = astar.path.find(2);
   point pttp;
   pttp.x = astar.it->second.x;
   pttp.y = astar.it->second.y;
   t_Wizard->path.insert(map<int,point>::value_type(t_Wizard->path.size()+1,pttp));

   if (astar.it->second.x == t_Wizard->goPoint.x && astar.it->second.y == t_Wizard->goPoint.y)
   {
    bIsOut = true;
   }   

   po++;
  }  
 }
 // 2.路径登记
 int popp = 0;
 t_Wizard->it = t_Wizard->path.begin();
 for (;t_Wizard->it != t_Wizard->path.end();t_Wizard->it++)
 {
  for (int cnn = 0; cnn < t_Wizard->speed; cnn++)
  {
   table_table[t_Wizard->it->second.x][t_Wizard->it->second.y].it = table_table[t_Wizard->it->second.x][t_Wizard->it->second.y].nodelist.find(t_Wizard->speed*popp+t_Wizard->begintime);
   if (table_table[t_Wizard->it->second.x][t_Wizard->it->second.y].it != table_table[t_Wizard->it->second.x][t_Wizard->it->second.y].nodelist.end())
   {
    table_table[t_Wizard->it->second.x][t_Wizard->it->second.y].it->second = t_Wizard->id;
   }else
   {
    table_table[t_Wizard->it->second.x][t_Wizard->it->second.y].nodelist.insert(map<int,int>::value_type(table_table[t_Wizard->it->second.x][t_Wizard->it->second.y].nodelist.size()+1,t_Wizard->id));
   }   
  } 
  
  popp++;
 } 
}


node table_table[6][6];// 全局桌面 存放所有登记信息
//int tabless[6][6];

/************************************************************************/
/* A 星算法测试                                                                     */
/************************************************************************/
int main(int argc, char* argv[])
{
// node table_table[6][6];// 全局桌面 存放所有登记信息
// int tabless[6][6];
// for ()
// {
// }
 

 Wizard a_Wizard1;
 a_Wizard1.id = 1;
 a_Wizard1.begintime = 0;
 a_Wizard1.speed = 4;
 a_Wizard1.nowPoint.x = 0;
 a_Wizard1.nowPoint.y = 0;
 a_Wizard1.goPoint.x = 5;
 a_Wizard1.goPoint.y = 5;

 Wizard a_Wizard2;
 a_Wizard2.id = 2;
 a_Wizard2.begintime = 1;
 a_Wizard2.speed = 3;
 a_Wizard2.nowPoint.x = 0;
 a_Wizard2.nowPoint.y = 0;
 a_Wizard1.goPoint.x = 5;
 a_Wizard1.goPoint.y = 5;

 Wizard a_Wizard3;
 a_Wizard2.id = 3;
 a_Wizard2.begintime = 2;
 a_Wizard2.speed = 2;
 a_Wizard2.nowPoint.x = 0;
 a_Wizard2.nowPoint.y = 0;
 a_Wizard1.goPoint.x = 5;
 a_Wizard1.goPoint.y = 5;

 Wizard a_Wizard4;
 a_Wizard2.id = 4;
 a_Wizard2.begintime = 3;
 a_Wizard2.speed = 1;
 a_Wizard2.nowPoint.x = 0;
 a_Wizard2.nowPoint.y = 0;
 a_Wizard1.goPoint.x = 5;
 a_Wizard1.goPoint.y = 5;

 getway_t(&a_Wizard1,table_table);

 getway_t(&a_Wizard2,table_table);

 getway_t(&a_Wizard3,table_table);

 getway_t(&a_Wizard4,table_table);

 getchar();
 return 0;
}

 

/*
#include <sys/time.h>
struct timeval
{
 long tv_sec;
 long tv_usec;
};

timeval start;
timeval end;

gettimeofday(&start,NULL);
gettimeofday(&end,NULL);

int sec = (start.tv_usec-end.tv_usec)*1000000;
*/

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值