diff --git a/apps/openmw/CMakeLists.txt b/apps/openmw/CMakeLists.txt index d1f7c45f3..ea4756ac4 100644 --- a/apps/openmw/CMakeLists.txt +++ b/apps/openmw/CMakeLists.txt @@ -68,7 +68,7 @@ add_openmw_dir (mwclass add_openmw_dir (mwmechanics mechanicsmanagerimp stat character creaturestats magiceffects movement actors objects drawstate spells activespells npcstats aipackage aisequence alchemy aiwander aitravel aifollow - aiescort aiactivate aicombat repair enchanting pathfinding security spellsuccess spellcasting + aiescort aiactivate aicombat repair enchanting pathfinding pathgrid security spellsuccess spellcasting disease pickpocket levelledlist combat steering ) diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index 86f9f9af2..730b8cb92 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -1,7 +1,5 @@ #include "pathfinding.hpp" -#include - #include "OgreMath.h" #include "OgreVector3.h" @@ -37,44 +35,6 @@ namespace return sqrt(x * x + y * y + z * z); } - // See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html - // - // One of the smallest cost in Seyda Neen is between points 77 & 78: - // pt x y - // 77 = 8026, 4480 - // 78 = 7986, 4218 - // - // Euclidean distance is about 262 (ignoring z) and Manhattan distance is 300 - // (again ignoring z). Using a value of about 300 for D seems like a reasonable - // starting point for experiments. If in doubt, just use value 1. - // - // The distance between 3 & 4 are pretty small, too. - // 3 = 5435, 223 - // 4 = 5948, 193 - // - // Approx. 514 Euclidean distance and 533 Manhattan distance. - // - float manhattan(ESM::Pathgrid::Point a, ESM::Pathgrid::Point b) - { - return 300 * (abs(a.mX - b.mX) + abs(a.mY - b.mY) + abs(a.mZ - b.mZ)); - } - - // Choose a heuristics - these may not be the best for directed graphs with - // non uniform edge costs. - // - // distance: - // - sqrt((curr.x - goal.x)^2 + (curr.y - goal.y)^2 + (curr.z - goal.z)^2) - // - slower but more accurate - // - // Manhattan: - // - |curr.x - goal.x| + |curr.y - goal.y| + |curr.z - goal.z| - // - faster but not the shortest path - float costAStar(ESM::Pathgrid::Point a, ESM::Pathgrid::Point b) - { - //return distance(a, b); - return manhattan(a, b); - } - // Slightly cheaper version for comparisons. // Caller needs to be careful for very short distances (i.e. less than 1) // or when accumuating the results i.e. (a + b)^2 != a^2 + b^2 @@ -113,12 +73,13 @@ namespace return closestIndex; } - // Uses mSCComp to choose a reachable end pathgrid point. start is assumed reachable. + // Chooses a reachable end pathgrid point. start is assumed reachable. std::pair getClosestReachablePoint(const ESM::Pathgrid* grid, - Ogre::Vector3 pos, int start, std::vector &sCComp) + const MWWorld::CellStore *cell, + Ogre::Vector3 pos, int start) { - // assume grid is fine - int startGroup = sCComp[start]; + if(!grid || grid->mPoints.empty()) + return std::pair (-1, false); float distanceBetween = distanceSquared(grid->mPoints[0], pos); int closestIndex = 0; @@ -133,7 +94,7 @@ namespace // found a closer one distanceBetween = potentialDistBetween; closestIndex = counter; - if (sCComp[counter] == startGroup) + if (cell->isPointConnected(start, counter)) { closestReachableIndex = counter; } @@ -152,7 +113,7 @@ namespace MWMechanics { PathFinder::PathFinder() : mIsPathConstructed(false), - mIsGraphConstructed(false), + mPathgrid(NULL), mCell(NULL) { } @@ -164,293 +125,14 @@ namespace MWMechanics mIsPathConstructed = false; } - /* - * NOTE: Based on buildPath2(), please check git history if interested - * - * Populate mGraph with the cost of each allowed edge. - * - * Any existing data in mGraph is wiped clean first. The node's parent - * is set with initial value of -1. The parent values are populated by - * aStarSearch() in order to reconstruct a path. - * - * mGraph[f].edges[n].destination = t - * - * f = point index of location "from" - * t = point index of location "to" - * n = index of edges from point f - * - * - * Example: (note from p(0) to p(2) not allowed in this example) - * - * mGraph[0].edges[0].destination = 1 - * .edges[1].destination = 3 - * - * mGraph[1].edges[0].destination = 0 - * .edges[1].destination = 2 - * .edges[2].destination = 3 - * - * mGraph[2].edges[0].destination = 1 - * - * (etc, etc) - * - * - * low - * cost - * p(0) <---> p(1) <------------> p(2) - * ^ ^ - * | | - * | +-----> p(3) - * +----------------> - * high cost - */ - void PathFinder::buildPathgridGraph(const ESM::Pathgrid* pathGrid) - { - mGraph.clear(); - // resize lists - 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); - // initialise mGraph - for(unsigned int i = 0; i < pathGrid->mPoints.size(); i++) - { - Node node; - node.label = i; - node.parent = -1; - mGraph[i] = node; - } - // store the costs of each edge - for(unsigned int i = 0; i < pathGrid->mEdges.size(); i++) - { - Edge edge; - edge.cost = costAStar(pathGrid->mPoints[pathGrid->mEdges[i].mV0], - pathGrid->mPoints[pathGrid->mEdges[i].mV1]); - // forward path of the edge - edge.destination = pathGrid->mEdges[i].mV1; - mGraph[pathGrid->mEdges[i].mV0].edges.push_back(edge); - // reverse path of the edge - // NOTE: These are redundant, the ESM already contains the reverse paths. - //edge.destination = pathGrid->mEdges[i].mV0; - //mGraph[pathGrid->mEdges[i].mV1].edges.push_back(edge); - } - mIsGraphConstructed = true; - } - - // v is the pathgrid point index (some call them vertices) - void PathFinder::recursiveStrongConnect(int v) - { - mSCCPoint[v].first = mSCCIndex; // index - mSCCPoint[v].second = mSCCIndex; // lowlink - mSCCIndex++; - mSCCStack.push_back(v); - int w; - - for(int i = 0; i < static_cast (mGraph[v].edges.size()); i++) - { - w = mGraph[v].edges[i].destination; - if(mSCCPoint[w].first == -1) // not visited - { - recursiveStrongConnect(w); // recurse - mSCCPoint[v].second = std::min(mSCCPoint[v].second, - mSCCPoint[w].second); - } - else - { - if(find(mSCCStack.begin(), mSCCStack.end(), w) != mSCCStack.end()) - mSCCPoint[v].second = std::min(mSCCPoint[v].second, - mSCCPoint[w].first); - } - } - - if(mSCCPoint[v].second == mSCCPoint[v].first) - { - // new component - do - { - w = mSCCStack.back(); - mSCCStack.pop_back(); - mSCComp[w] = mSCCId; - } - while(w != v); - - mSCCId++; - } - return; - } - - /* - * mSCComp contains the strongly connected component group id's. - * - * A cell can have disjointed pathgrid, e.g. Seyda Neen which has 3 - * - * mSCComp for Seyda Neen will have 3 different values. When selecting a - * random pathgrid point for AiWander, mSCComp can be checked for quickly - * finding whether the destination is reachable. - * - * Otherwise, buildPath will automatically select a closest reachable end - * pathgrid point (reachable from the closest start point). - * - * Using Tarjan's algorithm - * - * mGraph | graph G | - * mSCCPoint | V | derived from pathGrid->mPoints - * mGraph[v].edges | E (for v) | - * mSCCIndex | index | keep track of smallest unused index - * mSCCStack | S | - * pathGrid - * ->mEdges[v].mV1 | w | = mGraph[v].edges[i].destination - * - * FIXME: Some of these can be cleaned up by including them to struct - * Node used by mGraph - */ - void PathFinder::buildConnectedPoints(const ESM::Pathgrid* pathGrid) - { - mSCComp.clear(); - mSCComp.resize(pathGrid->mPoints.size(), 0); - mSCCId = 0; - - mSCCIndex = 0; - mSCCStack.clear(); - mSCCPoint.clear(); - mSCCPoint.resize(pathGrid->mPoints.size(), std::pair (-1, -1)); - - for(unsigned int v = 0; v < pathGrid->mPoints.size(); v++) - { - if(mSCCPoint[v].first == -1) // undefined (haven't visited) - recursiveStrongConnect(v); - } - } - - void PathFinder::cleanUpAStar() - { - for(int i = 0; i < static_cast (mGraph.size()); i++) - { - mGraph[i].parent = -1; - mGScore[i] = -1; - mFScore[i] = -1; - } - } - - /* - * NOTE: Based on buildPath2(), please check git history if interested - * Should consider a using 3rd party library version (e.g. boost) - * - * Find the shortest path to the target goal using a well known algorithm. - * Uses mGraph which has pre-computed costs for allowed edges. It is assumed - * that mGraph is already constructed. The caller, i.e. buildPath(), needs - * to ensure this. - * - * Returns path (a list of pathgrid point indexes) which may be empty. - * - * Input params: - * start, goal - pathgrid point indexes (for this cell) - * xCell, yCell - values to add to convert path back to world scale - * - * Variables: - * openset - point indexes to be traversed, lowest cost at the front - * closedset - point indexes already traversed - * - * Class variables: - * mGScore - past accumulated costs vector indexed by point index - * mFScore - future estimated costs vector indexed by point index - * these are resized by buildPathgridGraph() - */ - std::list PathFinder::aStarSearch(const ESM::Pathgrid* pathGrid, - int start, int goal, - float xCell, float yCell) - { - cleanUpAStar(); - // mGScore & mFScore keep costs for each pathgrid point in pathGrid->mPoints - mGScore[start] = 0; - mFScore[start] = costAStar(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(); // front has the lowest cost - openset.pop_front(); - - if(current == goal) - break; - - closedset.push_back(current); // remember we've been here - - // check all edges for the current point index - for(int j = 0; j < static_cast (mGraph[current].edges.size()); j++) - { - if(std::find(closedset.begin(), closedset.end(), mGraph[current].edges[j].destination) == - closedset.end()) - { - // not in closedset - i.e. have not traversed this edge destination - int dest = mGraph[current].edges[j].destination; - float tentative_g = mGScore[current] + mGraph[current].edges[j].cost; - bool isInOpenSet = std::find(openset.begin(), openset.end(), dest) != openset.end(); - if(!isInOpenSet - || tentative_g < mGScore[dest]) - { - mGraph[dest].parent = current; - mGScore[dest] = tentative_g; - mFScore[dest] = tentative_g + - costAStar(pathGrid->mPoints[dest], pathGrid->mPoints[goal]); - if(!isInOpenSet) - { - // add this edge to openset, lowest cost goes to the front - // TODO: if this causes performance problems a hash table may help - std::list::iterator it = openset.begin(); - for(it = openset.begin(); it!= openset.end(); it++) - { - if(mFScore[*it] > mFScore[dest]) - break; - } - openset.insert(it, dest); - } - } - } // if in closedset, i.e. traversed this edge already, try the next edge - } - } - - std::list path; - if(current != goal) - return path; // for some reason couldn't build a path - // e.g. start was not reachable (we assume it is) - - // reconstruct path to return, using world co-ordinates - while(mGraph[current].parent != -1) - { - ESM::Pathgrid::Point pt = pathGrid->mPoints[current]; - pt.mX += xCell; - pt.mY += yCell; - path.push_front(pt); - current = mGraph[current].parent; - } - - // TODO: Is this a bug? If path is empty the algorithm couldn't find a path. - // Simply using the destination as the path in this scenario seems strange. - // Commented out pending further testing. -#if 0 - if(path.empty()) - { - ESM::Pathgrid::Point pt = pathGrid->mPoints[goal]; - pt.mX += xCell; - pt.mY += yCell; - path.push_front(pt); - } -#endif - return path; - } - /* * NOTE: This method may fail to find a path. The caller must check the * result before using it. If there is no path the AI routies need to * implement some other heuristics to reach the target. * + * NOTE: It may be desirable to simply go directly to the endPoint if for + * example there are no pathgrids in this cell. + * * NOTE: startPoint & endPoint are in world co-ordinates * * Updates mPath using aStarSearch() or ray test (if shortcut allowed). @@ -462,9 +144,6 @@ namespace MWMechanics * * mPathConstructed is set true if successful, false if not * - * May update mGraph by calling buildPathgridGraph() if it isn't - * constructed yet. At the same time mConnectedPoints is also updated. - * * NOTE: co-ordinates must be converted prior to calling getClosestPoint() * * | @@ -486,7 +165,8 @@ namespace MWMechanics */ void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, bool allowShortcuts) + const MWWorld::CellStore* cell, + bool allowShortcuts) { mPath.clear(); @@ -502,17 +182,25 @@ namespace MWMechanics } } - if(mCell != cell) + if(mCell != cell || !mPathgrid) { - mIsGraphConstructed = false; // must be in a new cell, need a new mGraph and mSCComp mCell = cell; + mPathgrid = MWBase::Environment::get().getWorld()->getStore().get().search(*mCell->getCell()); } - const ESM::Pathgrid *pathGrid = - MWBase::Environment::get().getWorld()->getStore().get().search(*mCell->getCell()); + // Refer to AiWander reseach topic on openmw forums for some background. + // Maybe there is no pathgrid for this cell. Just go to destination and let + // physics take care of any blockages. + if(!mPathgrid || mPathgrid->mPoints.empty()) + { + mPath.push_back(endPoint); + mIsPathConstructed = true; + return; + } + + // NOTE: getClosestPoint expects local co-ordinates float xCell = 0; float yCell = 0; - if (mCell->isExterior()) { xCell = mCell->getCell()->mData.mX * ESM::Land::REAL_SIZE; @@ -524,26 +212,19 @@ namespace MWMechanics // outside an area enclosed by walls, but there is a pathgrid // point right behind the wall that is closer than any pathgrid // point outside the wall - // - // NOTE: getClosestPoint expects local co-ordinates - // - int startNode = getClosestPoint(pathGrid, - Ogre::Vector3(startPoint.mX - xCell, startPoint.mY - yCell, startPoint.mZ)); - - if(startNode != -1) // only check once, assume pathGrid won't change + int startNode = getClosestPoint(mPathgrid, + Ogre::Vector3(startPoint.mX - xCell, startPoint.mY - yCell, startPoint.mZ)); + // Some cells don't have any pathgrids at all + if(startNode != -1) { - if(!mIsGraphConstructed) - { - buildPathgridGraph(pathGrid); // pre-compute costs for use with aStarSearch - buildConnectedPoints(pathGrid); // must before calling getClosestReachablePoint - } - std::pair endNode = getClosestReachablePoint(pathGrid, + std::pair endNode = getClosestReachablePoint(mPathgrid, cell, Ogre::Vector3(endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ), - startNode, mSCComp); + startNode); + // this shouldn't really happen, but just in case if(endNode.first != -1) { - mPath = aStarSearch(pathGrid, startNode, endNode.first, xCell, yCell); + mPath = mCell->aStarSearch(startNode, endNode.first); if(!mPath.empty()) { @@ -567,7 +248,9 @@ namespace MWMechanics mIsPathConstructed = false; } else - mIsPathConstructed = false; // this shouldn't really happen, but just in case + mIsPathConstructed = false; + + return; } float PathFinder::getZAngleToNext(float x, float y) const @@ -646,4 +329,3 @@ namespace MWMechanics } } - diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index ae849bff2..eb093ad69 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -34,8 +34,6 @@ namespace MWMechanics void clearPath(); - 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); @@ -75,60 +73,14 @@ namespace MWMechanics mPath.push_back(point); } - // While a public method is defined here, it is anticipated that - // mSCComp will only be used internally. - std::vector getSCComp() const - { - return mSCComp; - } - private: - 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; - std::list mPath; - bool mIsGraphConstructed; - const MWWorld::CellStore* mCell; - // contains an integer indicating the groups of connected pathgrid points - // (all connected points will have the same value) - // - // In Seyda Neen there are 3: - // - // 52, 53 and 54 are one set (enclosed yard) - // 48, 49, 50, 51, 84, 85, 86, 87, 88, 89, 90 are another (ship & office) - // all other pathgrid points are the third set - // - std::vector mSCComp; - // variables used to calculate mSCComp - int mSCCId; - int mSCCIndex; - std::list mSCCStack; - typedef std::pair VPair; // first is index, second is lowlink - std::vector mSCCPoint; - // methods used to calculate mSCComp - void recursiveStrongConnect(int v); - void buildConnectedPoints(const ESM::Pathgrid* pathGrid); + const ESM::Pathgrid *mPathgrid; + const MWWorld::CellStore* mCell; }; } diff --git a/apps/openmw/mwmechanics/pathgrid.cpp b/apps/openmw/mwmechanics/pathgrid.cpp new file mode 100644 index 000000000..83ee86db5 --- /dev/null +++ b/apps/openmw/mwmechanics/pathgrid.cpp @@ -0,0 +1,337 @@ +#include "pathgrid.hpp" + +#include "../mwbase/world.hpp" +#include "../mwbase/environment.hpp" + +#include "../mwworld/cellstore.hpp" + +namespace +{ + // See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html + // + // One of the smallest cost in Seyda Neen is between points 77 & 78: + // pt x y + // 77 = 8026, 4480 + // 78 = 7986, 4218 + // + // Euclidean distance is about 262 (ignoring z) and Manhattan distance is 300 + // (again ignoring z). Using a value of about 300 for D seems like a reasonable + // starting point for experiments. If in doubt, just use value 1. + // + // The distance between 3 & 4 are pretty small, too. + // 3 = 5435, 223 + // 4 = 5948, 193 + // + // Approx. 514 Euclidean distance and 533 Manhattan distance. + // + float manhattan(const ESM::Pathgrid::Point& a, const ESM::Pathgrid::Point& b) + { + return 300 * (abs(a.mX - b.mX) + abs(a.mY - b.mY) + abs(a.mZ - b.mZ)); + } + + // Choose a heuristics - Note that these may not be the best for directed + // graphs with non-uniform edge costs. + // + // distance: + // - sqrt((curr.x - goal.x)^2 + (curr.y - goal.y)^2 + (curr.z - goal.z)^2) + // - slower but more accurate + // + // Manhattan: + // - |curr.x - goal.x| + |curr.y - goal.y| + |curr.z - goal.z| + // - faster but not the shortest path + float costAStar(const ESM::Pathgrid::Point& a, const ESM::Pathgrid::Point& b) + { + //return distance(a, b); + return manhattan(a, b); + } +} + +namespace MWMechanics +{ + PathgridGraph::PathgridGraph() + : mCell(NULL) + , mIsGraphConstructed(false) + , mPathgrid(NULL) + , mGraph(0) + , mSCCId(0) + , mSCCIndex(0) + , mIsExterior(0) + { + } + + /* + * mGraph is populated with the cost of each allowed edge. + * + * The data structure is based on the code in buildPath2() but modified. + * Please check git history if interested. + * + * mGraph[v].edges[i].index = w + * + * v = point index of location "from" + * i = index of edges from point v + * w = point index of location "to" + * + * + * Example: (notice from p(0) to p(2) is not allowed in this example) + * + * mGraph[0].edges[0].index = 1 + * .edges[1].index = 3 + * + * mGraph[1].edges[0].index = 0 + * .edges[1].index = 2 + * .edges[2].index = 3 + * + * mGraph[2].edges[0].index = 1 + * + * (etc, etc) + * + * + * low + * cost + * p(0) <---> p(1) <------------> p(2) + * ^ ^ + * | | + * | +-----> p(3) + * +----------------> + * high cost + */ + bool PathgridGraph::load(const ESM::Cell* cell) + { + if(!cell) + return false; + + mCell = cell; + mIsExterior = cell->isExterior(); + mPathgrid = MWBase::Environment::get().getWorld()->getStore().get().search(*cell); + + if(!mPathgrid) + return false; + + if(mIsGraphConstructed) + return true; + + mGraph.resize(mPathgrid->mPoints.size()); + for(int i = 0; i < static_cast (mPathgrid->mEdges.size()); i++) + { + ConnectedPoint neighbour; + neighbour.cost = costAStar(mPathgrid->mPoints[mPathgrid->mEdges[i].mV0], + mPathgrid->mPoints[mPathgrid->mEdges[i].mV1]); + // forward path of the edge + neighbour.index = mPathgrid->mEdges[i].mV1; + mGraph[mPathgrid->mEdges[i].mV0].edges.push_back(neighbour); + // reverse path of the edge + // NOTE: These are redundant, ESM already contains the required reverse paths + //neighbour.index = mPathgrid->mEdges[i].mV0; + //mGraph[mPathgrid->mEdges[i].mV1].edges.push_back(neighbour); + } + buildConnectedPoints(); + mIsGraphConstructed = true; + return true; + } + + // v is the pathgrid point index (some call them vertices) + void PathgridGraph::recursiveStrongConnect(int v) + { + mSCCPoint[v].first = mSCCIndex; // index + mSCCPoint[v].second = mSCCIndex; // lowlink + mSCCIndex++; + mSCCStack.push_back(v); + int w; + + for(int i = 0; i < static_cast (mGraph[v].edges.size()); i++) + { + w = mGraph[v].edges[i].index; + if(mSCCPoint[w].first == -1) // not visited + { + recursiveStrongConnect(w); // recurse + mSCCPoint[v].second = std::min(mSCCPoint[v].second, + mSCCPoint[w].second); + } + else + { + if(find(mSCCStack.begin(), mSCCStack.end(), w) != mSCCStack.end()) + mSCCPoint[v].second = std::min(mSCCPoint[v].second, + mSCCPoint[w].first); + } + } + + if(mSCCPoint[v].second == mSCCPoint[v].first) + { // new component + do + { + w = mSCCStack.back(); + mSCCStack.pop_back(); + mGraph[w].componentId = mSCCId; + } + while(w != v); + mSCCId++; + } + return; + } + + /* + * mGraph contains the strongly connected component group id's along + * with pre-calculated edge costs. + * + * A cell can have disjointed pathgrids, e.g. Seyda Neen has 3 + * + * mGraph for Seyda Neen will therefore have 3 different values. When + * selecting a random pathgrid point for AiWander, mGraph can be checked + * for quickly finding whether the destination is reachable. + * + * Otherwise, buildPath can automatically select a closest reachable end + * pathgrid point (reachable from the closest start point). + * + * Using Tarjan's algorithm: + * + * mGraph | graph G | + * mSCCPoint | V | derived from mPoints + * mGraph[v].edges | E (for v) | + * mSCCIndex | index | tracking smallest unused index + * mSCCStack | S | + * mGraph[v].edges[i].index | w | + * + */ + void PathgridGraph::buildConnectedPoints() + { + // both of these are set to zero in the constructor + //mSCCId = 0; // how many strongly connected components in this cell + //mSCCIndex = 0; + int pointsSize = mPathgrid->mPoints.size(); + mSCCPoint.resize(pointsSize, std::pair (-1, -1)); + mSCCStack.reserve(pointsSize); + + for(int v = 0; v < static_cast (pointsSize); v++) + { + if(mSCCPoint[v].first == -1) // undefined (haven't visited) + recursiveStrongConnect(v); + } + } + + bool PathgridGraph::isPointConnected(const int start, const int end) const + { + return (mGraph[start].componentId == mGraph[end].componentId); + } + + /* + * NOTE: Based on buildPath2(), please check git history if interested + * Should consider using a 3rd party library version (e.g. boost) + * + * Find the shortest path to the target goal using a well known algorithm. + * Uses mGraph which has pre-computed costs for allowed edges. It is assumed + * that mGraph is already constructed. + * + * Should be possible to make this MT safe. + * + * Returns path which may be empty. path contains pathgrid points in local + * cell co-ordinates (indoors) or world co-ordinates (external). + * + * Input params: + * start, goal - pathgrid point indexes (for this cell) + * + * Variables: + * openset - point indexes to be traversed, lowest cost at the front + * closedset - point indexes already traversed + * gScore - past accumulated costs vector indexed by point index + * fScore - future estimated costs vector indexed by point index + * + * TODO: An intersting exercise might be to cache the paths created for a + * start/goal pair. To cache the results the paths need to be in + * pathgrid points form (currently they are converted to world + * co-ordinates). Essentially trading speed w/ memory. + */ + std::list PathgridGraph::aStarSearch(const int start, + const int goal) const + { + std::list path; + if(!isPointConnected(start, goal)) + { + return path; // there is no path, return an empty path + } + + int graphSize = mGraph.size(); + std::vector gScore; + gScore.resize(graphSize, -1); + std::vector fScore; + fScore.resize(graphSize, -1); + std::vector graphParent; + graphParent.resize(graphSize, -1); + + // gScore & fScore keep costs for each pathgrid point in mPoints + gScore[start] = 0; + fScore[start] = costAStar(mPathgrid->mPoints[start], mPathgrid->mPoints[goal]); + + std::list openset; + std::list closedset; + openset.push_back(start); + + int current = -1; + + while(!openset.empty()) + { + current = openset.front(); // front has the lowest cost + openset.pop_front(); + + if(current == goal) + break; + + closedset.push_back(current); // remember we've been here + + // check all edges for the current point index + for(int j = 0; j < static_cast (mGraph[current].edges.size()); j++) + { + if(std::find(closedset.begin(), closedset.end(), mGraph[current].edges[j].index) == + closedset.end()) + { + // not in closedset - i.e. have not traversed this edge destination + int dest = mGraph[current].edges[j].index; + float tentative_g = gScore[current] + mGraph[current].edges[j].cost; + bool isInOpenSet = std::find(openset.begin(), openset.end(), dest) != openset.end(); + if(!isInOpenSet + || tentative_g < gScore[dest]) + { + graphParent[dest] = current; + gScore[dest] = tentative_g; + fScore[dest] = tentative_g + costAStar(mPathgrid->mPoints[dest], + mPathgrid->mPoints[goal]); + if(!isInOpenSet) + { + // add this edge to openset, lowest cost goes to the front + // TODO: if this causes performance problems a hash table may help + std::list::iterator it = openset.begin(); + for(it = openset.begin(); it!= openset.end(); it++) + { + if(fScore[*it] > fScore[dest]) + break; + } + openset.insert(it, dest); + } + } + } // if in closedset, i.e. traversed this edge already, try the next edge + } + } + + if(current != goal) + return path; // for some reason couldn't build a path + + // reconstruct path to return, using world co-ordinates + float xCell = 0; + float yCell = 0; + if (mIsExterior) + { + xCell = mPathgrid->mData.mX * ESM::Land::REAL_SIZE; + yCell = mPathgrid->mData.mY * ESM::Land::REAL_SIZE; + } + + while(graphParent[current] != -1) + { + ESM::Pathgrid::Point pt = mPathgrid->mPoints[current]; + pt.mX += xCell; + pt.mY += yCell; + path.push_front(pt); + current = graphParent[current]; + } + return path; + } +} + diff --git a/apps/openmw/mwmechanics/pathgrid.hpp b/apps/openmw/mwmechanics/pathgrid.hpp new file mode 100644 index 000000000..ac545efbc --- /dev/null +++ b/apps/openmw/mwmechanics/pathgrid.hpp @@ -0,0 +1,77 @@ +#ifndef GAME_MWMECHANICS_PATHGRID_H +#define GAME_MWMECHANICS_PATHGRID_H + +#include +#include + +namespace ESM +{ + class Cell; +} + +namespace MWWorld +{ + class CellStore; +} + +namespace MWMechanics +{ + class PathgridGraph + { + public: + PathgridGraph(); + + bool load(const ESM::Cell *cell); + + // returns true if end point is strongly connected (i.e. reachable + // from start point) both start and end are pathgrid point indexes + bool isPointConnected(const int start, const int end) const; + + // the input parameters are pathgrid point indexes + // the output list is in local (internal cells) or world (external + // cells) co-ordinates + std::list aStarSearch(const int start, + const int end) const; + private: + + const ESM::Cell *mCell; + const ESM::Pathgrid *mPathgrid; + bool mIsExterior; + + struct ConnectedPoint // edge + { + int index; // pathgrid point index of neighbour + float cost; + }; + + struct Node // point + { + int componentId; + std::vector edges; // neighbours + }; + + // componentId is an integer indicating the groups of connected + // pathgrid points (all connected points will have the same value) + // + // In Seyda Neen there are 3: + // + // 52, 53 and 54 are one set (enclosed yard) + // 48, 49, 50, 51, 84, 85, 86, 87, 88, 89, 90 (ship & office) + // all other pathgrid points are the third set + // + std::vector mGraph; + bool mIsGraphConstructed; + + // variables used to calculate connected components + int mSCCId; + int mSCCIndex; + std::vector mSCCStack; + typedef std::pair VPair; // first is index, second is lowlink + std::vector mSCCPoint; + // methods used to calculate connected components + void recursiveStrongConnect(int v); + void buildConnectedPoints(); + }; +} + +#endif diff --git a/apps/openmw/mwworld/cellstore.cpp b/apps/openmw/mwworld/cellstore.cpp index 3f1ef8ab2..6bc7657e4 100644 --- a/apps/openmw/mwworld/cellstore.cpp +++ b/apps/openmw/mwworld/cellstore.cpp @@ -363,6 +363,7 @@ namespace MWWorld loadRefs (store, esm); mState = State_Loaded; + mPathgridGraph.load(mCell); } } @@ -680,4 +681,14 @@ namespace MWWorld { return !(left==right); } + + bool CellStore::isPointConnected(const int start, const int end) const + { + return mPathgridGraph.isPointConnected(start, end); + } + + std::list CellStore::aStarSearch(const int start, const int end) const + { + return mPathgridGraph.aStarSearch(start, end); + } } diff --git a/apps/openmw/mwworld/cellstore.hpp b/apps/openmw/mwworld/cellstore.hpp index 4b7c0011b..b970afe1b 100644 --- a/apps/openmw/mwworld/cellstore.hpp +++ b/apps/openmw/mwworld/cellstore.hpp @@ -8,6 +8,8 @@ #include "esmstore.hpp" #include "cellreflist.hpp" +#include "../mwmechanics/pathgrid.hpp" + namespace ESM { struct CellState; @@ -141,6 +143,10 @@ namespace MWWorld throw std::runtime_error ("Storage for this type not exist in cells"); } + bool isPointConnected(const int start, const int end) const; + + std::list aStarSearch(const int start, const int end) const; + private: template @@ -166,6 +172,8 @@ namespace MWWorld ///< Make case-adjustments to \a ref and insert it into the respective container. /// /// Invalid \a ref objects are silently dropped. + + MWMechanics::PathgridGraph mPathgridGraph; }; template<>