代码小记
#include <iostream>
#include <list> struct POINT {
int X;
int Y;
}; // G: 起点到当前点的成本
// H: 当前点到终点的估算成本
// F: G,H之和
struct MapNode {
int G;
int H;
int F;
bool can_pass;
POINT pt;
MapNode* pParent; MapNode() : G(), H(), F(), can_pass(false), pParent(NULL) {} bool operator==(const MapNode& other) {
return (other.pt.X == pt.X && other.pt.Y == pt.Y);
} bool operator!=(const MapNode& other) {
return !(*this == other);
}
}; bool nodeCompare(MapNode* p1, MapNode* p2) {
return (p1->F < p2->F);
} int maps[][] = {
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
}; MapNode* findNode(std::list<MapNode*>& nodes, const POINT& pt) {
for(std::list<MapNode*>::iterator it = nodes.begin();
it != nodes.end();
++it) {
if((*it)->pt.X == pt.X && (*it)->pt.Y == pt.Y) {
return *it;
}
}
return NULL;
} void pathFinding() {
std::list<MapNode*> all;
std::list<MapNode*> open;
std::list<MapNode*> close; for(int row=; row<; ++row) {
for(int col=; col<; ++col) {
MapNode* pNode = new MapNode;
pNode->pt.X = row;
pNode->pt.Y = col;
pNode->can_pass = (maps[row][col] == ? true : false);
all.push_back(pNode);
}
} POINT pt_start = {, };
POINT pt_target = {, };
MapNode* pStart = findNode(all, pt_start);
MapNode* pTarget = findNode(all, pt_target); open.push_back(pStart);
while(!open.empty()) {
open.sort(nodeCompare);
MapNode *pNode = open.front();
if (*pNode == *pTarget) {
break;
}
open.pop_front();
close.push_back(pNode);
// 查找周围节点并将合适的加入open
int ptMask[][] = { // x坐标相对位置，y坐标相对位置，移动耗费
{-, -, }, // 左上
{ , -, }, // 正上
{ , -, }, // 右上
{-, , }, // 正左
{ , , }, // 正右
{-, , }, // 左下
{ , , }, // 正下
{ , , } // 右下
};
for(int i=; i<; ++i) {
POINT pt;
pt.X = pNode->pt.X + ptMask[i][];
pt.Y = pNode->pt.Y + ptMask[i][]; if(pt.X > && pt.X < && pt.Y > && pt.Y < ) {
MapNode* pChildNode = findNode(all, pt);
if(!pChildNode->can_pass || NULL != findNode(close, pt)) {
// 如果pNodeLT不可抵达的或者它在 close list 中，忽略它
} else {
if(NULL == findNode(open, pt)) {
// 如果pNodeLT不在 open list 中，把它加入 open list，
// 并且把当前方格设置为它的父亲，记录该方格的F,G和H值
pChildNode->pParent = pNode;
pChildNode->G = pChildNode->pParent->G + ptMask[i][];
pChildNode->H = * (abs(pTarget->pt.X - pt.X) + abs(pTarget->pt.Y - pt.Y));
pChildNode->F = pChildNode->G + pChildNode->H;
open.push_back(pChildNode);
} else {
// 如果pNodeLT已经在 open list 中，检查这条路径 ( 即经由
// 当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G
// 值表示这是更好的路径
int tG = pNode->G + ptMask[i][];
if(tG < pChildNode->G) {
pChildNode->pParent = pNode;
pChildNode->G = tG;
pChildNode->H = * (abs(pTarget->pt.X - pt.X) + abs(pTarget->pt.Y - pt.Y));
pChildNode->F = pChildNode->G + pChildNode->H;
}
}
}
}
}
}
if(NULL != pTarget->pParent) {
MapNode* pNode = pTarget;
std::cout << "path is : " << std::endl;
do {
std::cout << "pt[" << pNode->pt.X << ", " << pNode->pt.Y << "] ->" << std::endl;
pNode = pNode->pParent;
} while(*pNode != *pStart);
std::cout << "pt[" << pNode->pt.X << ", " << pNode->pt.Y << "]" << std::endl;
}
} int main() { pathFinding(); system("pause"); return ;
}