diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index 1b42b11a4a..dcf47ca337 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -5,30 +5,10 @@ #include "OgreMath.h" -#include #include namespace { - struct found_path {}; - - typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, - boost::property, boost::property > - PathGridGraph; - typedef boost::property_map::type WeightMap; - typedef PathGridGraph::vertex_descriptor PointID; - typedef PathGridGraph::edge_descriptor PointConnectionID; - - class goalVisited : public boost::default_dijkstra_visitor - { - public: - goalVisited(PointID goal) {mGoal = goal;}; - void examine_vertex(PointID u, const PathGridGraph g) {if(u == mGoal) throw found_path();}; - - private: - PointID mGoal; - }; - float distanceZCorrected(ESM::Pathgrid::Point point, float x, float y, float z) { x -= point.mX; @@ -80,74 +60,7 @@ namespace return closestIndex; } - PathGridGraph buildGraph(const ESM::Pathgrid* pathgrid, float xCell = 0, float yCell = 0) - { - PathGridGraph graph; - - for(unsigned int counter = 0; counter < pathgrid->mPoints.size(); counter++) - { - PointID pID = boost::add_vertex(graph); - graph[pID].mX = pathgrid->mPoints[counter].mX + xCell; - graph[pID].mY = pathgrid->mPoints[counter].mY + yCell; - graph[pID].mZ = pathgrid->mPoints[counter].mZ; - } - - for(unsigned int counterTwo = 0; counterTwo < pathgrid->mEdges.size(); counterTwo++) - { - PointID u = pathgrid->mEdges[counterTwo].mV0; - PointID v = pathgrid->mEdges[counterTwo].mV1; - - PointConnectionID edge; - bool done; - boost::tie(edge, done) = boost::add_edge(u, v, graph); - WeightMap weightmap = boost::get(boost::edge_weight, graph); - weightmap[edge] = distance(graph[u], graph[v]); - } - - return graph; - } - - std::list findPath(PointID start, PointID end,const PathGridGraph& graph) - { - std::vector p(boost::num_vertices(graph)); - std::vector d(boost::num_vertices(graph)); - std::list shortest_path; - - try - { - boost::dijkstra_shortest_paths(graph, start, - boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(goalVisited(end))); - } - - catch(found_path& fg) - { - for(PointID v = end; ; v = p[v]) - { - shortest_path.push_front(graph[v]); - if(p[v] == v) - break; - } - } - - return shortest_path; - } -} - -namespace -{ - struct Edge - { - int destination; - float cost; - }; - struct Node - { - int label; - std::vector edges; - int parent;//used in pathfinding - }; - - std::list reconstructPath(const std::vector& graph,const ESM::Pathgrid* pathgrid, int lastNode,float xCell, float yCell) + /*std::list reconstructPath(const std::vector& graph,const ESM::Pathgrid* pathgrid, int lastNode,float xCell, float yCell) { std::list path; while(graph[lastNode].parent != -1) @@ -160,9 +73,11 @@ namespace lastNode = graph[lastNode].parent; } return path; - } + }*/ - std::list buildPath2(const ESM::Pathgrid* pathgrid,int start,int goal,float xCell = 0, float yCell = 0) + + + /*std::list buildPath2(const ESM::Pathgrid* pathgrid,int start,int goal,float xCell = 0, float yCell = 0) { std::vector graph; for(unsigned int i = 0; i < pathgrid->mPoints.size(); i++) @@ -234,7 +149,7 @@ namespace } return reconstructPath(graph,pathgrid,current,xCell,yCell); - } + }*/ } @@ -252,12 +167,108 @@ namespace MWMechanics mIsPathConstructed = false; } - void PathFinder::buildPathgridGraph(const ESM::Pathgrid* pathGrid,float xCell, float yCell) + void PathFinder::buildPathgridGraph(const ESM::Pathgrid* pathGrid) { - mGraph = buildGraph(pathGrid, xCell, yCell); + mGraph.clear(); + mGScore.resize(pathGrid->mPoints.size(),-1); + mFScore.resize(pathGrid->mPoints.size(),-1); + Node defaultNode; + defaultNode.label = -1; + defaultNode.parent = -1; + mGraph.resize(pathGrid->mPoints.size(),defaultNode); + for(unsigned int i = 0; i < pathGrid->mPoints.size(); i++) + { + Node node; + node.label = i; + node.parent = -1; + mGraph[i] = node; + } + for(unsigned int i = 0; i < pathGrid->mEdges.size(); i++) + { + Edge edge; + edge.destination = pathGrid->mEdges[i].mV1; + edge.cost = distance(pathGrid->mPoints[pathGrid->mEdges[i].mV0],pathGrid->mPoints[pathGrid->mEdges[i].mV1]); + mGraph[pathGrid->mEdges[i].mV0].edges.push_back(edge); + edge.destination = pathGrid->mEdges[i].mV0; + mGraph[pathGrid->mEdges[i].mV1].edges.push_back(edge); + } mIsGraphConstructed = true; } + void PathFinder::cleanUpAStar() + { + for(int i=0;i PathFinder::aStarSearch(const ESM::Pathgrid* pathGrid,int start,int goal,float xCell, float yCell) + { + cleanUpAStar(); + mGScore[start] = 0; + mFScore[start] = distance(pathGrid->mPoints[start],pathGrid->mPoints[goal]); + + std::list openset; + std::list closedset; + openset.push_back(start); + + int current = -1; + + while(!openset.empty()) + { + current = openset.front(); + openset.pop_front(); + + if(current == goal) break; + + closedset.push_back(current); + + for(int j = 0;jmPoints[dest],pathGrid->mPoints[goal]); + if(!isInOpenSet) + { + std::list::iterator it = openset.begin(); + for(it = openset.begin();it!= openset.end();it++) + { + if(mGScore[*it]>mGScore[dest]) + break; + } + openset.insert(it,dest); + } + } + } + } + + } + + std::list path; + while(mGraph[current].parent != -1) + { + //std::cout << "not empty" << xCell; + ESM::Pathgrid::Point pt = pathGrid->mPoints[current]; + pt.mX += xCell; + pt.mY += yCell; + path.push_front(pt); + current = mGraph[current].parent; + } + return path; + } + void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint, const MWWorld::CellStore* cell, bool allowShortcuts) { @@ -289,9 +300,9 @@ namespace MWMechanics if(startNode != -1 && endNode != -1) { - //if(!mIsGraphConstructed) buildPathgridGraph(pathGrid, xCell, yCell); + if(!mIsGraphConstructed) buildPathgridGraph(pathGrid); - mPath = buildPath2(pathGrid,startNode,endNode,xCell,yCell);//findPath(startNode, endNode, mGraph); + mPath = aStarSearch(pathGrid,startNode,endNode,xCell,yCell);//findPath(startNode, endNode, mGraph); if(!mPath.empty()) { diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index 6870314910..5bc9d7493f 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -19,7 +19,7 @@ namespace MWMechanics void clearPath(); - void buildPathgridGraph(const ESM::Pathgrid* pathGrid,float xCell = 0, float yCell = 0); + void buildPathgridGraph(const ESM::Pathgrid* pathGrid); void buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint, const MWWorld::CellStore* cell, bool allowShortcuts = true); @@ -51,13 +51,30 @@ namespace MWMechanics } private: - std::list mPath; + + struct Edge + { + int destination; + float cost; + }; + struct Node + { + int label; + std::vector edges; + int parent;//used in pathfinding + }; + + std::vector mGScore; + std::vector mFScore; + + std::list aStarSearch(const ESM::Pathgrid* pathGrid,int start,int goal,float xCell = 0, float yCell = 0); + void cleanUpAStar(); + + std::vector mGraph; bool mIsPathConstructed; - typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, - boost::property, boost::property > - PathGridGraph; - PathGridGraph mGraph; + + std::list mPath; bool mIsGraphConstructed; const MWWorld::CellStore* mCell; };