bool M_PathFinding::StartPathFinding() { bool ret = false; pathFound = false; currentSteps = 0; if (mapChanged) { LoadMapData(); mapChanged = false; } if (IfPathPossible()) { pathStarted = false; pathFinished = false; newLowest = false; lowestF = App->map->data.height * App->map->data.width; if (openList.count() != 0) openList.clear(); if (closedList.count() != 0) closedList.clear(); if (path.Count() != 0) path.Clear(); ret = CreateFirstNode(); if (ret) { pathStarted = true; LOG("-- Pathfinding: Correct pathfinding start --"); } } else LOG("-- Pathfinding: Could not start the process, not possible path to be found"); return ret; }
bool CPathFind::PathFind(int startx, int starty, int endx, int endy) { InitNode(startx,starty,endx,endy); //首先做几个预先判断 if ((startx == endx) && (starty == endy)) { cout << "起点就是终点"; } m_startx = startx; m_starty = starty; m_endx = endx; m_endy = endy; pNode node = CreateFirstNode(startx, starty); //将这个节点放入openlist m_openlist.push_back(node); //对这个地方进行排序 push_heap(m_openlist.begin(),m_openlist.end(),NodeSort); pNode tmpnode = NULL; for(;;) { if(m_openlist.empty()) { cout<<"错误:不能找到目标节点"<<endl; return false; } tmpnode = m_openlist.front(); ++m_step; pop_heap(m_openlist.begin(),m_openlist.end(),NodeSort); m_openlist.pop_back(); if (!CheckIsDestination(tmpnode)) { //这里不是是目标地点 for(int i = 0 ;i<8 ;i++) { int nextx ; int nexty ; GetShiftByDirectory(i,&nextx,&nexty); nextx = tmpnode->x + nextx; nexty = tmpnode->y + nexty; //判断这个点是可以通过的 cout<<"next is"<<nextx<<":"<<nexty<<endl; if(isIllegle(nextx,nexty)) { //这里可以通过 //计算这个点的G值 int newGvalue; if(i % 2 ==0) newGvalue = tmpnode->G+10; else newGvalue = tmpnode->G+14; vector<pNode>::iterator OpenIt; //说明该节点在OPEN表中 for(OpenIt=m_openlist.begin();OpenIt<m_openlist.end();OpenIt++) { if (((*OpenIt)->x == nextx)&&((*OpenIt)->y ==nexty)) { break; } } if(OpenIt != m_openlist.end()) { if ((*OpenIt)->G <= newGvalue) continue; } //说明该节点在close表中 vector<pNode>::iterator CloseIt; for(CloseIt=m_closelist.begin();CloseIt<m_closelist.end();CloseIt++) { if (((*CloseIt)->x == nextx)&&((*CloseIt)->y ==nexty)) { break; } } if(CloseIt != m_closelist.end()) { if ((*CloseIt)->G <= newGvalue) continue; } //如果都不满足上边的条件那么说明这个节点是最优节点 Node *bestNode = new Node; bestNode->x = nextx; bestNode->y = nexty; bestNode->father = tmpnode; bestNode->G = newGvalue; bestNode->H = CalcG(nextx,nexty); bestNode->F = bestNode->G + bestNode->H; if (CloseIt != m_closelist.end()) { delete(*CloseIt); m_closelist.erase(CloseIt); } if (OpenIt != m_openlist.end()) { delete(*OpenIt); m_openlist.erase(OpenIt); make_heap(m_openlist.begin(),m_openlist.end(),NodeSort); } m_openlist.push_back(bestNode); push_heap(m_openlist.begin(),m_openlist.end(),NodeSort); for(vector<pNode>::iterator k = m_openlist.begin() ;k<m_openlist.end();k++) { cout<<"x:"<<(*k)->x<<",y:"<<(*k)->y<<endl; } cout<<" isIlleglea is true"<<endl; } else { cout<<" isIlleglea is false"<<endl; //不能通过 } } m_closelist.push_back(tmpnode); } else { generatePath(); return true; } } }