forked from mirror/openmw-tes3mp
Move PathgridGraph into separate files.
This commit is contained in:
cc9cii
parent
a8b2eb1fe9
commit
040d4f8fc4
7 changed files with 426 additions and 415 deletions
|
|
@ -68,7 +68,7 @@ add_openmw_dir (mwclass
|
||||||
add_openmw_dir (mwmechanics
|
add_openmw_dir (mwmechanics
|
||||||
mechanicsmanagerimp stat character creaturestats magiceffects movement actors objects
|
mechanicsmanagerimp stat character creaturestats magiceffects movement actors objects
|
||||||
drawstate spells activespells npcstats aipackage aisequence alchemy aiwander aitravel aifollow
|
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
|
disease pickpocket levelledlist combat steering
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,7 +1,5 @@
|
||||||
#include "pathfinding.hpp"
|
#include "pathfinding.hpp"
|
||||||
|
|
||||||
#include <map>
|
|
||||||
|
|
||||||
#include "OgreMath.h"
|
#include "OgreMath.h"
|
||||||
#include "OgreVector3.h"
|
#include "OgreVector3.h"
|
||||||
|
|
||||||
|
|
@ -37,44 +35,6 @@ namespace
|
||||||
return sqrt(x * x + y * y + z * z);
|
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(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);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Slightly cheaper version for comparisons.
|
// Slightly cheaper version for comparisons.
|
||||||
// Caller needs to be careful for very short distances (i.e. less than 1)
|
// 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
|
// or when accumuating the results i.e. (a + b)^2 != a^2 + b^2
|
||||||
|
|
@ -368,299 +328,4 @@ 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)
|
|
||||||
return false;
|
|
||||||
|
|
||||||
mCell = cell;
|
|
||||||
mPathgrid = MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*cell);
|
|
||||||
|
|
||||||
if(!mPathgrid)
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -2,7 +2,6 @@
|
||||||
#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>
|
||||||
|
|
@ -83,65 +82,6 @@ namespace MWMechanics
|
||||||
const ESM::Pathgrid *mPathgrid;
|
const ESM::Pathgrid *mPathgrid;
|
||||||
const MWWorld::CellStore* mCell;
|
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 (ship & office)
|
|
||||||
// all other pathgrid points are the third set
|
|
||||||
//
|
|
||||||
std::vector<Node> mGraph;
|
|
||||||
bool mIsGraphConstructed;
|
|
||||||
|
|
||||||
// variables used to calculate connected components
|
|
||||||
int mSCCId;
|
|
||||||
int mSCCIndex;
|
|
||||||
std::vector<int> mSCCStack;
|
|
||||||
typedef std::pair<int, int> VPair; // first is index, second is lowlink
|
|
||||||
std::vector<VPair> mSCCPoint;
|
|
||||||
// methods used to calculate connected components
|
|
||||||
void recursiveStrongConnect(int v);
|
|
||||||
void buildConnectedPoints();
|
|
||||||
};
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
348
apps/openmw/mwmechanics/pathgrid.cpp
Normal file
348
apps/openmw/mwmechanics/pathgrid.cpp
Normal file
|
|
@ -0,0 +1,348 @@
|
||||||
|
#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)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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;
|
||||||
|
mPathgrid = MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*cell);
|
||||||
|
|
||||||
|
if(!mPathgrid)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
if(mIsGraphConstructed)
|
||||||
|
return true;
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
75
apps/openmw/mwmechanics/pathgrid.hpp
Normal file
75
apps/openmw/mwmechanics/pathgrid.hpp
Normal file
|
|
@ -0,0 +1,75 @@
|
||||||
|
#ifndef GAME_MWMECHANICS_PATHGRID_H
|
||||||
|
#define GAME_MWMECHANICS_PATHGRID_H
|
||||||
|
|
||||||
|
#include <components/esm/loadpgrd.hpp>
|
||||||
|
#include <list>
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
// 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 (ship & office)
|
||||||
|
// all other pathgrid points are the third set
|
||||||
|
//
|
||||||
|
std::vector<Node> mGraph;
|
||||||
|
bool mIsGraphConstructed;
|
||||||
|
|
||||||
|
// variables used to calculate connected components
|
||||||
|
int mSCCId;
|
||||||
|
int mSCCIndex;
|
||||||
|
std::vector<int> mSCCStack;
|
||||||
|
typedef std::pair<int, int> VPair; // first is index, second is lowlink
|
||||||
|
std::vector<VPair> mSCCPoint;
|
||||||
|
// methods used to calculate connected components
|
||||||
|
void recursiveStrongConnect(int v);
|
||||||
|
void buildConnectedPoints();
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -363,6 +363,7 @@ namespace MWWorld
|
||||||
loadRefs (store, esm);
|
loadRefs (store, esm);
|
||||||
|
|
||||||
mState = State_Loaded;
|
mState = State_Loaded;
|
||||||
|
mPathgridGraph.load(mCell);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -683,14 +684,6 @@ namespace MWWorld
|
||||||
|
|
||||||
bool CellStore::isPointConnected(const int start, const int end) const
|
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);
|
return mPathgridGraph.isPointConnected(start, end);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
@ -698,16 +691,6 @@ namespace MWWorld
|
||||||
std::list<ESM::Pathgrid::Point> CellStore::aStarSearch(const int start, const int end,
|
std::list<ESM::Pathgrid::Point> CellStore::aStarSearch(const int start, const int end,
|
||||||
const bool isOutside) const
|
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);
|
return mPathgridGraph.aStarSearch(start, end, isOutside);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -8,7 +8,7 @@
|
||||||
#include "esmstore.hpp"
|
#include "esmstore.hpp"
|
||||||
#include "cellreflist.hpp"
|
#include "cellreflist.hpp"
|
||||||
|
|
||||||
#include "../mwmechanics/pathfinding.hpp"
|
#include "../mwmechanics/pathgrid.hpp"
|
||||||
|
|
||||||
namespace ESM
|
namespace ESM
|
||||||
{
|
{
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue