A* 寻路算法学习
2015-09-12 10:45
218 查看
代码小记
#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(0), H(0), F(0), 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[20][30] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1, 1,0,1,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,0,1, 1,0,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,1, 1,0,0,0,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,1, 1,0,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,1, 1,0,1,0,1,0,1,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,0,1, 1,0,1,0,1,0,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1, 1,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,0,1,0,1, 1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1, 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,0,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1, 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,0,1,0,1, 1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1, 1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,0,1, 1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,1,0,1, 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,0,1,1,1,1,1,1,0,1, 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1 }; 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=0; row<20; ++row) { for(int col=0; col<30; ++col) { MapNode* pNode = new MapNode; pNode->pt.X = row; pNode->pt.Y = col; pNode->can_pass = (maps[row][col] == 0 ? true : false); all.push_back(pNode); } } POINT pt_start = {1, 1}; POINT pt_target = {10, 16}; 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[8][3] = { // x坐标相对位置,y坐标相对位置,移动耗费 {-1, -1, 14}, // 左上 {0 , -1, 10}, // 正上 {1 , -1, 14}, // 右上 {-1, 0 , 10}, // 正左 {1 , 0 , 10}, // 正右 {-1, 1 , 14}, // 左下 {0 , 1 , 10}, // 正下 {1 , 1 , 14} // 右下 }; for(int i=0; i<8; ++i) { POINT pt; pt.X = pNode->pt.X + ptMask[i][0]; pt.Y = pNode->pt.Y + ptMask[i][1]; if(pt.X > 0 && pt.X < 30 && pt.Y >0 && pt.Y < 20) { 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][2]; pChildNode->H = 10 * (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][2]; if(tG < pChildNode->G) { pChildNode->pParent = pNode; pChildNode->G = tG; pChildNode->H = 10 * (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 0; }
相关文章推荐
- java jsp开发的电子商务系统代码下载
- The Log:每个程序员都应该知道有关实时数据的统一抽象(1)概念
- 我的日记——链表的创建
- linux下卸载mysql rpm安装方式和源码安装方式的两种方法
- hdu-1013 Digital Roots
- php header()的用法总结
- socket,http,tcp区别
- hdu acm 2159 FATE
- Lynis基线检测使用解析
- 倒排序原理和实例
- C++大数据处理
- Mocking
- 人肉工程在机器学习实践中的作用
- C++基础之const系列
- HDOJ 1009
- 7.性能测试工具Locust的初级使用
- OC第四课
- ChartType属性
- iOS开发系列--Objective-C之类和对象
- html css 图片居中