Cocos2d-x实现的A*寻路

#include "AStar.h" #include "ShortestPathStep.h" #include <algorithm> AStar::AStar(void) { m_nBlockSize = 32; m_shortestPaths = NULL; } AStar::~AStar(void) { } void AStar::setBlockSize(int size) { m_nBlockSize = size; } CCPoint AStar::AStarCoordForPosition(CCPoint point) { //起点为原点（0，0）,块大小为32*32 return CCPointMake(((int)point.x) / m_nBlockSize ,((int)point.y) / m_nBlockSize ); } bool AStar::IsValidPos(CCPoint point) { std::vector<AStar_Coord_Info>::iterator it = m_AstarCoordInfo.begin(); for (;it!=m_AstarCoordInfo.end(); it++) { if((*it).point.equals(point)) return ((*it).nType == 0); } return false; } void AStar::InsertInOpenArrays(ShortestPathStep *step) { int stepFScore = step->getFScore(); // Compute only once the step F score's int count =openArray->count(); int i = 0; // It will be the index at which we will insert the step for (; i < count; i++) { if (stepFScore <= ((ShortestPathStep*)openArray->objectAtIndex(i))->getFScore()) { // if the step F score's is lower or equals to the step at index i // Then we found the index at which we have to insert the new step break; } } // Insert the new step at the good index to preserve the F score ordering openArray->insertObject(step,i); } //构建最短路径 void AStar::ConstructShortestPath(ShortestPathStep* step) { m_shortestPaths = CCArray::create(); m_shortestPaths->retain(); do { // Don't add the last step which is the start position (remember we go backward, so the last one is the origin position ;-) if (step->getParent() != NULL) { m_shortestPaths->insertObject(step,0); // Always insert at index 0 to reverse the path } step = step->getParent(); // Go backward } while (step != NULL); // Until there is not more parent } //结束寻路 void AStar::EndAStar() { openArray->removeAllObjects(); openArray->release(); closeArray->removeAllObjects(); closeArray->release(); } //寻找四周可通过的step void AStar::walkableAdjacentTilesCoordForTileCoord(CCPoint point, std::vector<CCPoint>& tmp) { // std::vector<CCPoint> tmp; BOOL t = false; BOOL l = false; BOOL b = false; BOOL r = false; // Top CCPoint p = CCPointMake(point.x, point.y - 1); if (IsValidPos(p)) { tmp.push_back(p); t = true; } // Left p = CCPointMake(point.x - 1, point.y); if (IsValidPos(p)) { tmp.push_back(p); l = true; } // Bottom p = CCPointMake(point.x, point.y + 1); if (IsValidPos(p)) { tmp.push_back(p); b = true; } // Right p = CCPointMake(point.x + 1, point.y); if (IsValidPos(p)) { tmp.push_back(p); r = true; } // Top Left p = CCPointMake(point.x - 1, point.y - 1); if (t && l && IsValidPos(p)) { tmp.push_back(p); } // Bottom Left p = CCPointMake(point.x - 1, point.y + 1); if (b && l && IsValidPos(p)) { tmp.push_back(p); } // Top Right p = CCPointMake(point.x + 1, point.y - 1); if (t && r && IsValidPos(p)) { tmp.push_back(p); } // Bottom Right p = CCPointMake(point.x + 1, point.y + 1); if (b && r && IsValidPos(p)) { tmp.push_back(p); } } void AStar::MoveToward(CCPoint fromPos,CCPoint toPos) { //清空原来的寻路 if(m_shortestPaths) { m_shortestPaths->removeAllObjects(); } //转换起止点坐标为AStar坐标系 CCPoint fromAStarCoor = AStarCoordForPosition(fromPos); CCPoint toAStarCoord = AStarCoordForPosition(toPos); //起点与终点相同 if(fromAStarCoor.equals(toAStarCoord)) { return; } //检查终点坐标是不是有效的位置 if(!IsValidPos(toAStarCoord)) { return; } //初始化open和close列表 openArray = CCArray::create(); openArray->retain(); closeArray = CCArray::create(); closeArray->retain(); //将起点step添加到open列表中 ShortestPathStep *step = new ShortestPathStep(); step->retain(); //step->autorelease(); step->InitWithPosition(fromAStarCoor); InsertInOpenArrays(step); do { // Get the lowest F cost step // Because the list is ordered, the first step is always the one with the lowest F cost ShortestPathStep *curStep =(ShortestPathStep *) openArray->objectAtIndex(0); curStep->retain(); // Add the current step to the closed set closeArray->addObject(curStep); // Remove it from the open list // Note that if we wanted to first removing from the open list, care should be taken to the memory openArray->removeObjectAtIndex(0); //判断是否到达终点 if (curStep->getPos().equals(toAStarCoord)) { //构建最短路径 ConstructShortestPath(curStep); //结束寻路 EndAStar(); break; } //从当前step寻找最合适的下一步 std::vector<CCPoint> pointVec; walkableAdjacentTilesCoordForTileCoord(curStep->getPos(),pointVec); for (int i=0; i<pointVec.size(); i++) { ShortestPathStep *st = new ShortestPathStep(); st->autorelease(); st->InitWithPosition(pointVec[i]); //todo 此函数能否确定？？ if(ContainObject(closeArray,st))// //if(closeArray->containsObject(st)) { st->release(); continue; } // Compute the cost form the current step to that step int moveCost = costToMoveFromStep(curStep ,st); // Check if the step is already in the open list //int index = openArray->indexOfObject(st); int index =indexOfObject(openArray,st); if(index == UINT_MAX) //该点并不在openArray中，则添加 { // Set the current step as the parent st->setParent(curStep); // The G score is equal to the parent G score + the cost to move from the parent to it st->setGScore( curStep->getGScore() + moveCost); // Compute the H score which is the estimated movement cost to move from that step to the desired tile coordinate st->setHScore(computeHScoreFromCoord(st->getPos(),toAStarCoord)); // Adding it with the function which is preserving the list ordered by F score InsertInOpenArrays(st); // Done, now release the step st->release(); } else { // Already in the open list st->release(); // Release the freshly created one st =(ShortestPathStep*) openArray->objectAtIndex(index); // To retrieve the old one (which has its scores already computed ;-) // Check to see if the G score for that step is lower if we use the current step to get there if ((curStep->getGScore() + moveCost) < st->getGScore()) { // The G score is equal to the parent G score + the cost to move from the parent to it st->setGScore(curStep->getGScore() + moveCost); // Because the G Score has changed, the F score may have changed too // So to keep the open list ordered we have to remove the step, and re-insert it with // the insert function which is preserving the list ordered by F score // We have to retain it before removing it from the list st->retain(); // Now we can removing it from the list without be afraid that it can be released openArray->removeObjectAtIndex(index); // Re-insert it with the function which is preserving the list ordered by F score InsertInOpenArrays(st); // Now we can release it because the oredered list retain it st->release(); } } } } while (openArray->count() > 0); //结束寻路 EndAStar(); } //计算移动一步所用的长度，采用10做默认值而非1是为了却除小数，而对角线移动长度由勾股定理可得为14.1,取整为14 int AStar::costToMoveFromStep(ShortestPathStep *fromStep ,ShortestPathStep *toStep) { return ((fromStep->getPos().x != toStep->getPos().x) && (fromStep->getPos().y != toStep->getPos().y)) ? 14 : 10; } // Compute the H score from a position to another (from the current position to the final desired position int AStar::computeHScoreFromCoord(CCPoint fromCoord ,CCPoint toCoord) { // Here we use the Manhattan method, which calculates the total number of step moved horizontally and vertically to reach the // final desired step from the current step, ignoring any obstacles