mirror/openmw
1
0
Fork 0
mirror of https://github.com/OpenMW/openmw.git synced 2025-02-01 17:15:34 +00:00
Code Issues 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.
//
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));
}
// Choose a heuristics - these may not be the best for directed graphs with
// non uniform edge costs.
// 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)
@ -69,7 +69,7 @@ namespace
// 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)
float costAStar(const ESM::Pathgrid::Point a, const ESM::Pathgrid::Point b)
{
//return distance(a, b);
return manhattan(a, b);
@ -113,12 +113,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<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
int startGroup = sCComp[start];
if(!grid || grid->mPoints.empty())
return std::pair<int, bool> (-1, false);
float distanceBetween = distanceSquared(grid->mPoints[0], pos);
int closestIndex = 0;
@ -133,7 +134,7 @@ namespace
// found a closer one
distanceBetween = potentialDistBetween;
closestIndex = counter;
if (sCComp[counter] == startGroup)
if (cell->isPointConnected(start, counter))
{
closestReachableIndex = counter;
}
@ -152,7 +153,7 @@ namespace MWMechanics
{
PathFinder::PathFinder()
: mIsPathConstructed(false),
mIsGraphConstructed(false),
mPathgrid(NULL),
mCell(NULL)
{
}
@ -164,293 +165,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<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
* 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 +184,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 +205,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,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;
// 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 =
MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().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())
{
//#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 yCell = 0;
if (mCell->isExterior())
{
xCell = mCell->getCell()->mData.mX * ESM::Land::REAL_SIZE;
yCell = mCell->getCell()->mData.mY * ESM::Land::REAL_SIZE;
}
// NOTE: It is possible that getClosestPoint returns a pathgrind point index
// that is unreachable in some situations. e.g. actor is standing
// 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<int, bool> endNode = getClosestReachablePoint(pathGrid,
std::pair<int, bool> endNode = getClosestReachablePoint(mPathgrid, cell,
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)
{
mPath = aStarSearch(pathGrid, startNode, endNode.first, xCell, yCell);
mPath = mCell->aStarSearch(startNode, endNode.first, mCell->isExterior());
if(!mPath.empty())
{
@ -561,13 +309,34 @@ namespace MWMechanics
mPath.push_back(endPoint);
}
else
{
mIsPathConstructed = false;
std::cout << "empty path error " << std::endl;
}
//mIsPathConstructed = false;
}
else
{
mIsPathConstructed = false;
std::cout << "second point error " << std::endl;
}
//mIsPathConstructed = false;
}
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
@ -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
#include <components/esm/loadpgrd.hpp>
#include <components/esm/loadcell.hpp>
#include <list>
#include <OgreMath.h>
@ -34,8 +35,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 +74,73 @@ 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<int> getSCComp() const
{
return mSCComp;
}
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;
std::list<ESM::Pathgrid::Point> 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)
const ESM::Pathgrid *mPathgrid;
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:
//
// 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
//
std::vector<int> mSCComp;
// variables used to calculate mSCComp
std::vector<Node> mGraph;
bool mIsGraphConstructed;
// variables used to calculate connected components
int mSCCId;
int mSCCIndex;
std::list<int> mSCCStack;
std::vector<int> mSCCStack;
typedef std::pair<int, int> VPair; // first is index, second is lowlink
std::vector<VPair> mSCCPoint;
// methods used to calculate mSCComp
// methods used to calculate connected components
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);
}
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 "cellreflist.hpp"
#include "../mwmechanics/pathfinding.hpp"
namespace ESM
{
struct CellState;
@ -141,6 +143,11 @@ 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<ESM::Pathgrid::Point> aStarSearch(const int start, const int end,
const bool isOutside) const;
private:
template<class Functor, class List>
@ -166,6 +173,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<>