teamnwah/openmw-tes3coop
2
0
Fork 1
forked from mirror/openmw-tes3mp
Code Issues Pull requests 1 Releases Wiki Activity

Per-cell pathgrid data and calculation moved off PathFinder. Now the edge cost calculations and strongly connected component searches are done only once per cell. Per-actor data and methods still remain with PathFinder.

Browse source 
This version still has debugging statements and needs cleaning up.
This commit is contained in:
cc9cii 2014-04-03 21:41:34 +11:00
parent f597d3e88b
commit 98f77714ce
4 changed files with 498 additions and 357 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

726
apps/openmw/mwmechanics/pathfinding.cpp
View file

@ -54,13 +54,13 @@ namespace
// //
// Approx. 514 Euclidean distance and 533 Manhattan distance. // Approx. 514 Euclidean distance and 533 Manhattan distance.
// //
float manhattan(ESM::Pathgrid::Point a, ESM::Pathgrid::Point b) 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)); 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 // Choose a heuristics - Note that these may not be the best for directed
// non uniform edge costs. // graphs with non-uniform edge costs.
// //
// distance: // distance:
// - sqrt((curr.x - goal.x)^2 + (curr.y - goal.y)^2 + (curr.z - goal.z)^2) // - sqrt((curr.x - goal.x)^2 + (curr.y - goal.y)^2 + (curr.z - goal.z)^2)
@ -69,7 +69,7 @@ namespace
// Manhattan: // Manhattan:
// - |curr.x - goal.x| + |curr.y - goal.y| + |curr.z - goal.z| // - |curr.x - goal.x| + |curr.y - goal.y| + |curr.z - goal.z|
// - faster but not the shortest path // - faster but not the shortest path
float costAStar(ESM::Pathgrid::Point a, ESM::Pathgrid::Point b) float costAStar(const ESM::Pathgrid::Point a, const ESM::Pathgrid::Point b)
{ {
//return distance(a, b); //return distance(a, b);
return manhattan(a, b); return manhattan(a, b);
@ -113,12 +113,13 @@ namespace
return closestIndex; 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<int, bool> getClosestReachablePoint(const ESM::Pathgrid* grid, std::pair<int, bool> getClosestReachablePoint(const ESM::Pathgrid* grid,
Ogre::Vector3 pos, int start, std::vector<int> &sCComp) const MWWorld::CellStore *cell,
Ogre::Vector3 pos, int start)
{ {
// assume grid is fine if(!grid || grid->mPoints.empty())
int startGroup = sCComp[start]; return std::pair<int, bool> (-1, false);
float distanceBetween = distanceSquared(grid->mPoints[0], pos); float distanceBetween = distanceSquared(grid->mPoints[0], pos);
int closestIndex = 0; int closestIndex = 0;
@ -133,7 +134,7 @@ namespace
// found a closer one // found a closer one
distanceBetween = potentialDistBetween; distanceBetween = potentialDistBetween;
closestIndex = counter; closestIndex = counter;
if (sCComp[counter] == startGroup) if (cell->isPointConnected(start, counter))
{ {
closestReachableIndex = counter; closestReachableIndex = counter;
} }
@ -152,7 +153,7 @@ namespace MWMechanics
{ {
PathFinder::PathFinder() PathFinder::PathFinder()
: mIsPathConstructed(false), : mIsPathConstructed(false),
mIsGraphConstructed(false), mPathgrid(NULL),
mCell(NULL) mCell(NULL)
{ {
} }
@ -164,293 +165,14 @@ namespace MWMechanics
mIsPathConstructed = false; 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<int> (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<int, int> (-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<int> (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<ESM::Pathgrid::Point> 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<int> openset;
std::list<int> 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<int> (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<int>::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<ESM::Pathgrid::Point> 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 * 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 * result before using it. If there is no path the AI routies need to
* implement some other heuristics to reach the target. * 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 * NOTE: startPoint & endPoint are in world co-ordinates
* *
* Updates mPath using aStarSearch() or ray test (if shortcut allowed). * Updates mPath using aStarSearch() or ray test (if shortcut allowed).
@ -462,9 +184,6 @@ namespace MWMechanics
* *
* mPathConstructed is set true if successful, false if not * 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() * NOTE: co-ordinates must be converted prior to calling getClosestPoint()
* *
* | * |
@ -486,7 +205,8 @@ namespace MWMechanics
*/ */
void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint,
const ESM::Pathgrid::Point &endPoint, const ESM::Pathgrid::Point &endPoint,
const MWWorld::CellStore* cell, bool allowShortcuts) const MWWorld::CellStore* cell,
bool allowShortcuts)
{ {
mPath.clear(); mPath.clear();
@ -502,48 +222,76 @@ 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; mCell = cell;
// Cache pathgrid as mPathgrid and update on cell changes. There
// might be a small gain in avoiding to search for it.
mPathgrid = MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*mCell->getCell());
} }
const ESM::Pathgrid *pathGrid = // Refer to AiWander reseach topic on openmw forums for some background.
MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*mCell->getCell()); // 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())
{
//#if 0
std::cout << "no pathgrid " <<
+"\"" +mCell->getCell()->mName+ "\""
+", " +std::to_string(mCell->getCell()->mData.mX)
+", " +std::to_string(mCell->getCell()->mData.mY)
<< std::endl;
//#endif
mPath.push_back(endPoint);
mIsPathConstructed = true;
return;
}
// NOTE: getClosestPoint expects local co-ordinates
float xCell = 0; float xCell = 0;
float yCell = 0; float yCell = 0;
if (mCell->isExterior()) if (mCell->isExterior())
{ {
xCell = mCell->getCell()->mData.mX * ESM::Land::REAL_SIZE; xCell = mCell->getCell()->mData.mX * ESM::Land::REAL_SIZE;
yCell = mCell->getCell()->mData.mY * ESM::Land::REAL_SIZE; yCell = mCell->getCell()->mData.mY * ESM::Land::REAL_SIZE;
} }
// NOTE: It is possible that getClosestPoint returns a pathgrind point index // NOTE: It is possible that getClosestPoint returns a pathgrind point index
// that is unreachable in some situations. e.g. actor is standing // that is unreachable in some situations. e.g. actor is standing
// outside an area enclosed by walls, but there is a pathgrid // outside an area enclosed by walls, but there is a pathgrid
// point right behind the wall that is closer than any pathgrid // point right behind the wall that is closer than any pathgrid
// point outside the wall // point outside the wall
// int startNode = getClosestPoint(mPathgrid,
// NOTE: getClosestPoint expects local co-ordinates Ogre::Vector3(startPoint.mX - xCell, startPoint.mY - yCell, startPoint.mZ));
// // Some cells don't have any pathgrids at all
int startNode = getClosestPoint(pathGrid, if(startNode != -1)
Ogre::Vector3(startPoint.mX - xCell, startPoint.mY - yCell, startPoint.mZ));
if(startNode != -1) // only check once, assume pathGrid won't change
{ {
if(!mIsGraphConstructed) std::pair<int, bool> endNode = getClosestReachablePoint(mPathgrid, cell,
{
buildPathgridGraph(pathGrid); // pre-compute costs for use with aStarSearch
buildConnectedPoints(pathGrid); // must before calling getClosestReachablePoint
}
std::pair<int, bool> endNode = getClosestReachablePoint(pathGrid,
Ogre::Vector3(endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ), Ogre::Vector3(endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ),
startNode, mSCComp); startNode);
//#if 0
if(!mPathgrid)
std::cout << "no pathgrid " <<
+"\"" +mCell->getCell()->mName+ "\""
+", " +std::to_string(mCell->getCell()->mData.mX)
+", " +std::to_string(mCell->getCell()->mData.mY)
<< std::endl;
//#endif
// this shouldn't really happen, but just in case
if(endNode.first != -1) if(endNode.first != -1)
{ {
mPath = aStarSearch(pathGrid, startNode, endNode.first, xCell, yCell); mPath = mCell->aStarSearch(startNode, endNode.first, mCell->isExterior());
if(!mPath.empty()) if(!mPath.empty())
{ {
@ -561,13 +309,34 @@ namespace MWMechanics
mPath.push_back(endPoint); mPath.push_back(endPoint);
} }
else else
{
mIsPathConstructed = false; mIsPathConstructed = false;
std::cout << "empty path error " << std::endl;
}
//mIsPathConstructed = false;
} }
else else
{
mIsPathConstructed = false; mIsPathConstructed = false;
std::cout << "second point error " << std::endl;
}
//mIsPathConstructed = false;
} }
else else
mIsPathConstructed = false; // this shouldn't really happen, but just in case {
// FIXME: shouldn't return endpoint if first point error?
mIsPathConstructed = false;
std::cout << "first point error " << std::endl;
}
#if 0
if(!mIsPathConstructed)
{
mPath.push_back(endPoint);
mIsPathConstructed = true;
}
#endif
return;
} }
float PathFinder::getZAngleToNext(float x, float y) const float PathFinder::getZAngleToNext(float x, float y) const
@ -645,5 +414,326 @@ namespace MWMechanics
} }
// TODO: Any multi threading concerns?
PathgridGraph::PathgridGraph()
: mCell(NULL)
, mIsGraphConstructed(false)
, mPathgrid(NULL)
, mGraph(0)
, mSCCId(0)
, mSCCIndex(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::initPathgridGraph(const ESM::Cell* cell)
{
if(!cell)
{
std::cout << "init error " << std::endl;
return false;
}
mCell = cell;
mPathgrid = MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*cell);
if(!mPathgrid)
{
std::cout << "init error " << std::endl;
return false;
}
mGraph.resize(mPathgrid->mPoints.size());
for(int i = 0; i < static_cast<int> (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;
//#if 0
std::cout << "loading pathgrid " <<
+"\""+ mPathgrid->mCell +"\""
+", "+ std::to_string(mPathgrid->mData.mX)
+", "+ std::to_string(mPathgrid->mData.mY)
<< std::endl;
//#endif
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<int> (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<int, int> (-1, -1));
mSCCStack.reserve(pointsSize);
for(int v = 0; v < static_cast<int> (pointsSize); v++)
{
if(mSCCPoint[v].first == -1) // undefined (haven't visited)
recursiveStrongConnect(v);
}
//#if 0
std::cout << "components: " << std::to_string(mSCCId)
+", "+ mPathgrid->mCell
<< std::endl;
//#endif
}
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 (a list of pathgrid point indexes) which may be empty.
*
* Input params:
* start, goal - pathgrid point indexes (for this cell)
* isExterior - used to determine whether to convert to world co-ordinates
*
* 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<ESM::Pathgrid::Point> PathgridGraph::aStarSearch(const int start,
const int goal,
bool isExterior) const
{
std::list<ESM::Pathgrid::Point> path;
if(!isPointConnected(start, goal))
{
return path; // there is no path, return an empty path
}
int graphSize = mGraph.size();
std::vector<float> gScore;
gScore.resize(graphSize, -1);
std::vector<float> fScore;
fScore.resize(graphSize, -1);
std::vector<int> 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<int> openset;
std::list<int> 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<int> (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<int>::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 (isExterior)
{
xCell = mPathgrid->mData.mX * ESM::Land::REAL_SIZE;
yCell = mPathgrid->mData.mY * ESM::Land::REAL_SIZE;
}
//#if 0
// for debugging only
int tmp = current;
if(tmp != goal)
{
std::cout << "aStarSearch: goal and result differ" << std::endl;
std::cout << "goal: " << std::to_string(goal)
+", result: "+ std::to_string(tmp)
<< std::endl;
}
std::cout << "start: " << std::to_string(start)
+", goal: "+ std::to_string(goal)
+", result: "+ std::to_string(tmp)
<< std::endl;
//#endif
while(graphParent[current] != -1)
{
ESM::Pathgrid::Point pt = mPathgrid->mPoints[current];
//#if 0
// for debugging only
std::cout << " point: "+ std::to_string(current)
+", X: "+ std::to_string(pt.mX)
+", Y: "+ std::to_string(pt.mY)
<< std::endl;
//#endif
pt.mX += xCell;
pt.mY += yCell;
path.push_front(pt);
current = graphParent[current];
}
return path;
}
} }

90
apps/openmw/mwmechanics/pathfinding.hpp
View file

@ -2,6 +2,7 @@
#define GAME_MWMECHANICS_PATHFINDING_H #define GAME_MWMECHANICS_PATHFINDING_H
#include <components/esm/loadpgrd.hpp> #include <components/esm/loadpgrd.hpp>
#include <components/esm/loadcell.hpp>
#include <list> #include <list>
#include <OgreMath.h> #include <OgreMath.h>
@ -34,8 +35,6 @@ namespace MWMechanics
void clearPath(); void clearPath();
void buildPathgridGraph(const ESM::Pathgrid* pathGrid);
void buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint, void buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint,
const MWWorld::CellStore* cell, bool allowShortcuts = true); const MWWorld::CellStore* cell, bool allowShortcuts = true);
@ -75,60 +74,73 @@ namespace MWMechanics
mPath.push_back(point); mPath.push_back(point);
} }
// While a public method is defined here, it is anticipated that
// mSCComp will only be used internally.
std::vector<int> getSCComp() const
{
return mSCComp;
}
private: private:
struct Edge
{
int destination;
float cost;
};
struct Node
{
int label;
std::vector<Edge> edges;
int parent;//used in pathfinding
};
std::vector<float> mGScore;
std::vector<float> mFScore;
std::list<ESM::Pathgrid::Point> aStarSearch(const ESM::Pathgrid* pathGrid,int start,int goal,float xCell = 0, float yCell = 0);
void cleanUpAStar();
std::vector<Node> mGraph;
bool mIsPathConstructed; bool mIsPathConstructed;
std::list<ESM::Pathgrid::Point> mPath; std::list<ESM::Pathgrid::Point> mPath;
bool mIsGraphConstructed;
const MWWorld::CellStore* mCell;
// contains an integer indicating the groups of connected pathgrid points const ESM::Pathgrid *mPathgrid;
// (all connected points will have the same value) const MWWorld::CellStore* mCell;
};
class PathgridGraph
{
public:
PathgridGraph();
bool isGraphConstructed() const
{
return mIsGraphConstructed;
};
bool initPathgridGraph(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;
// isOutside is used whether to convert path to world co-ordinates
std::list<ESM::Pathgrid::Point> aStarSearch(const int start, const int end,
const bool isOutside) const;
private:
const ESM::Cell *mCell;
const ESM::Pathgrid *mPathgrid;
struct ConnectedPoint // edge
{
int index; // pathgrid point index of neighbour
float cost;
};
struct Node // point
{
int componentId;
std::vector<ConnectedPoint> 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: // In Seyda Neen there are 3:
// //
// 52, 53 and 54 are one set (enclosed yard) // 52, 53 and 54 are one set (enclosed yard)
// 48, 49, 50, 51, 84, 85, 86, 87, 88, 89, 90 are another (ship & office) // 48, 49, 50, 51, 84, 85, 86, 87, 88, 89, 90 (ship & office)
// all other pathgrid points are the third set // all other pathgrid points are the third set
// //
std::vector<int> mSCComp; std::vector<Node> mGraph;
// variables used to calculate mSCComp bool mIsGraphConstructed;
// variables used to calculate connected components
int mSCCId; int mSCCId;
int mSCCIndex; int mSCCIndex;
std::list<int> mSCCStack; std::vector<int> mSCCStack;
typedef std::pair<int, int> VPair; // first is index, second is lowlink typedef std::pair<int, int> VPair; // first is index, second is lowlink
std::vector<VPair> mSCCPoint; std::vector<VPair> mSCCPoint;
// methods used to calculate mSCComp // methods used to calculate connected components
void recursiveStrongConnect(int v); void recursiveStrongConnect(int v);
void buildConnectedPoints(const ESM::Pathgrid* pathGrid); void buildConnectedPoints();
}; };
} }

30
apps/openmw/mwworld/cellstore.cpp
View file

@ -680,4 +680,34 @@ namespace MWWorld
{ {
return !(left==right); return !(left==right);
} }
bool CellStore::isPointConnected(const int start, const int end) const
{
if(!mPathgridGraph.isGraphConstructed())
{
// Ugh... there must be a better way...
MWMechanics::PathgridGraph *p = const_cast<MWMechanics::PathgridGraph *> (&mPathgridGraph);
if(!p->initPathgridGraph(mCell))
return false;
}
return mPathgridGraph.isPointConnected(start, end);
}
std::list<ESM::Pathgrid::Point> CellStore::aStarSearch(const int start, const int end,
const bool isOutside) const
{
if(!mPathgridGraph.isGraphConstructed())
{
MWMechanics::PathgridGraph *p = const_cast<MWMechanics::PathgridGraph *> (&mPathgridGraph);
if(!p->initPathgridGraph(mCell))
{
std::list<ESM::Pathgrid::Point> path; // empty
return path;
}
}
return mPathgridGraph.aStarSearch(start, end, isOutside);
}
} }

9
apps/openmw/mwworld/cellstore.hpp
View file

@ -8,6 +8,8 @@
#include "esmstore.hpp" #include "esmstore.hpp"
#include "cellreflist.hpp" #include "cellreflist.hpp"
#include "../mwmechanics/pathfinding.hpp"
namespace ESM namespace ESM
{ {
struct CellState; struct CellState;
@ -141,6 +143,11 @@ namespace MWWorld
throw std::runtime_error ("Storage for this type not exist in cells"); throw std::runtime_error ("Storage for this type not exist in cells");
} }
bool isPointConnected(const int start, const int end) const;
std::list<ESM::Pathgrid::Point> aStarSearch(const int start, const int end,
const bool isOutside) const;
private: private:
template<class Functor, class List> template<class Functor, class List>
@ -166,6 +173,8 @@ namespace MWWorld
///< Make case-adjustments to \a ref and insert it into the respective container. ///< Make case-adjustments to \a ref and insert it into the respective container.
/// ///
/// Invalid \a ref objects are silently dropped. /// Invalid \a ref objects are silently dropped.
MWMechanics::PathgridGraph mPathgridGraph;
}; };
template<> template<>