A*寻路算法的C++简单实现

2007/8/13 17:26:59

#include <iostream>
#include <ctime>
#include <list>
#include <algorithm>
#include <cassert>

using namespace std;

class AStarPos
{
public:
//点的x,y坐标;前一个连接点的x,y坐标;
//评估成本，起点到该点的成本
int _x, _y, _px, _py, _cost, _base_cost;
AStarPos()
{
_x = 0;
_y = 0;
_px = 0;
_py = 0;
_cost = 1000000; //应取超过实际问题中的最大成本的值
_base_cost = 1000000; //应取超过实际问题中的最大成本的值
}

AStarPos(int x, int y, int px, int py, int cost, int base_cost)
{
_x = x;
_y = y;
_px = px;
_py = py;
_cost = cost;
_base_cost = base_cost;
}

//注意声明格式,否则泛型算法没法编译通过

bool operator== (const AStarPos &p) const
{
return ((this->_x == p._x) && (this->_y == p._y));
}

//注意声明格式,否则泛型算法没法编译通过

bool operator< (AStarPos &p)
{
return this->_cost < p._cost;
}

~AStarPos()
{
}

};

int const _max = 10; //A* 寻路地图大小,此例中地图是正方形，但是也可以是矩形。

//判断当点探索点是否在指定的列表里面。
bool contains(list<AStarPos>, AStarPos&);
//寻找列表中成本最低的点
AStarPos& min_cost(list<AStarPos>);
//在列表中查找点(x,y)的信息
AStarPos& find_pos(list<AStarPos>, int x, int y);

int main()
{
int map[_max][_max], cost[_max][_max] = {0}; //地图数组
::srand((unsigned int) time(NULL));
for(int i = 0; i<_max; i++)
{
for(int j = 0; j < _max; j++)
{
if(rand() % 100 < 30){
map[i][j] = 1;
}
else
{
map[i][j] = 0;
}
}
}

int o_x = ::rand() % _max, o_y = ::rand() % _max;
int d_x = ::rand() % _max, d_y = ::rand() % _max;

stringstream ss;
for(int i = 0; i< _max; i++)
{
for(int j = 0; j < _max; j++)
{
ss<<map[i][j]<<",";
}
ss<<endl;
}
ss<<endl;
ss<<"("<<o_x<<","<<o_y<<"),("<<d_x<<","<<d_y<<")"<<endl;

cout<<ss.str()<<endl;

list<AStarPos> open, closed;
open.push_back(AStarPos(o_x, o_y, o_x, o_y, (o_x - d_x) * (o_x - d_x) + (o_y - d_y) * ( o_y - d_y), 0));
//TODO
//需要判断起点和终点是否为一个点。

int pos_x, pos_y;
while(!open.empty())
{
AStarPos curr_pos = min_cost(open);

closed.push_back(curr_pos);
open.remove(curr_pos);

//以下寻路策略认为不能斜行，可以增加45度方向的定点来实现斜形
if((curr_pos._x - 1 >= 0)){ //左侧
pos_x = curr_pos._x - 1 ;
pos_y = curr_pos._y;

//到达目标
if(pos_x == d_x && pos_y == d_y)
{
AStarPos temp_pos(pos_x,pos_y, curr_pos._x, curr_pos._y,
curr_pos._base_cost + 1,
curr_pos._base_cost + 1);
closed.push_back(temp_pos);
break;
}

if(map[pos_x][pos_y] != 1) //不是障碍物
{
AStarPos temp_pos(pos_x,pos_y, curr_pos._x, curr_pos._y,
curr_pos._base_cost + 1 + (pos_x - d_x) * (pos_x - d_x) + (pos_y - d_y) * ( pos_y - d_y),
curr_pos._base_cost + 1);
if(!contains(open, temp_pos) && !contains(closed, temp_pos)) //该点不在列表中
{
open.push_back(temp_pos);
cost[temp_pos._x][temp_pos._y] = temp_pos._base_cost;
}
}
}

if(curr_pos._x + 1 < _max) //右侧
{
pos_x = curr_pos._x + 1 ;
pos_y = curr_pos._y;

//到达目标
if(pos_x == d_x && pos_y == d_y)
{
AStarPos temp_pos(pos_x,pos_y, curr_pos._x, curr_pos._y,
curr_pos._base_cost + 1,
curr_pos._base_cost + 1);
closed.push_back(temp_pos);
break;
}

if(map[pos_x][pos_y] != 1) //不是障碍物
{
AStarPos temp_pos(pos_x,pos_y, curr_pos._x, curr_pos._y,
curr_pos._base_cost + 1 + (pos_x - d_x) * (pos_x - d_x) + (pos_y - d_y) * ( pos_y - d_y),
curr_pos._base_cost + 1);
if(!contains(open, temp_pos) && !contains(closed, temp_pos)) //该点不在列表中
{
open.push_back(temp_pos);
cost[temp_pos._x][temp_pos._y] = temp_pos._base_cost;
}
}
}

if(curr_pos._y - 1 >= 0) //上侧
{
pos_x = curr_pos._x ;
pos_y = curr_pos._y - 1;

//到达目标
if(pos_x == d_x && pos_y == d_y)
{
AStarPos temp_pos(pos_x,pos_y, curr_pos._x, curr_pos._y,
curr_pos._base_cost + 1,
curr_pos._base_cost + 1);
closed.push_back(temp_pos);
break;
}

if(map[pos_x][pos_y] != 1) //不是障碍物
{
AStarPos temp_pos(pos_x,pos_y, curr_pos._x, curr_pos._y,
curr_pos._base_cost + 1 + (pos_x - d_x) * (pos_x - d_x) + (pos_y - d_y) * ( pos_y - d_y),
curr_pos._base_cost + 1);
if(!contains(open, temp_pos) && !contains(closed, temp_pos)) //该点不在列表中
{
open.push_back(temp_pos);
cost[temp_pos._x][temp_pos._y] = temp_pos._base_cost;
}
}
}

if(curr_pos._y + 1 < _max) //下侧
{
pos_x = curr_pos._x ;
pos_y = curr_pos._y + 1;

//到达目标
if(pos_x == d_x && pos_y == d_y)
{
AStarPos temp_pos(pos_x,pos_y, curr_pos._x, curr_pos._y,
curr_pos._base_cost + 1,
curr_pos._base_cost + 1);
closed.push_back(temp_pos);
break;
}

if(map[pos_x][pos_y] != 1) //不是障碍物
{
AStarPos temp_pos(pos_x,pos_y, curr_pos._x, curr_pos._y,
curr_pos._base_cost + 1 + (pos_x - d_x) * (pos_x - d_x) + (pos_y - d_y) * ( pos_y - d_y),
curr_pos._base_cost + 1);
if(!contains(open, temp_pos) && !contains(closed, temp_pos)) //该点不在列表中
{
open.push_back(temp_pos);
cost[temp_pos._x][temp_pos._y] = temp_pos._base_cost;
}
}
}

//TODO 增加斜边的四个顶点的测试，来实现斜形效果。

}

if(pos_x == d_x && pos_y == d_y)
{
while((pos_x != o_x) || (pos_y != o_y))
{

cout<<"("<<pos_x<<", "<<pos_y<<")<-";
map[pos_x][pos_y] = 100;
AStarPos temp = find_pos(closed, pos_x, pos_y);
pos_x = temp._px;
pos_y = temp._py;

}
cout<<"("<<pos_x<<", "<<pos_y<<")"<<endl;

}
else
{
cout<<"寻路失败!"<<endl;
}

stringstream ss_o, ss_cost;
for(int i = 0; i < _max; i++)
{
for(int j = 0; j < _max; j++)
{
if(((o_x == i) && (o_y == j)) ||((d_x == i) && (d_y == j)))
{
ss_o<<"+"<<",";
}
else if((map[i][j] == 100))
{
ss_o<<"="<<",";
}
else
{
ss_o<<map[i][j]<<",";
}
ss_cost<<cost[i][j]<<",";
}
ss_o<<endl;
ss_cost<<endl;
}
ss_o<<endl;
ss_cost<<endl;
cout<<"寻路结果:"<<endl;
cout<<ss_o.str()<<endl;

cout<<"成本评估:"<<endl;
cout<<ss_cost.str()<<endl;
system("pause");

}

bool contains(list<AStarPos> a, AStarPos& b)
{
list<AStarPos>::iterator ind;
for(ind = a.begin(); ind !=a.end(); ind++)
{
if(((*ind)._x == b._x) && ((*ind)._y ==b._y))
{
return true;
}
}

return false;
}

AStarPos& min_cost(list<AStarPos> a)
{
int cost = 1000000;
AStarPos res;
list<AStarPos>::iterator ind;
for(ind = a.begin(); ind !=a.end(); ind++)
{
if((*ind)._cost < cost)
{
cost = (*ind)._cost;
res = *ind;
}
}

return AStarPos(res._x, res._y, res._px, res._py, res._cost, res._base_cost);
}

AStarPos& find_pos(list<AStarPos> a, int x, int y)
{
AStarPos res;
list<AStarPos>::iterator ind;
for(ind = a.begin(); ind !=a.end(); ind++)
{
if((*ind)._x == x && (*ind)._y == y)
{
return AStarPos((*ind)._x, (*ind)._y, (*ind)._px, (*ind)._py, (*ind)._cost, (*ind)._base_cost);
}
}

return AStarPos(res._x, res._y, res._px, res._py, res._cost, res._base_cost);