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);