forked from teamnwah/openmw-tes3coop
My previous analysis of the pathfinding issue was incorrect. It was in fact caused due to some of the pathgrid points being unreachable. Instead of returning an empty path in such a scenario, incorrect path + requested destination were being returned. There was also a defect where past cost was being used for selecting open points.
There is still an unresolved issue where mGraph and mSCComp are being rebuilt unnecessarily. The check mCell != cell in buildPath() is being triggered frequently. Not sure why.
This commit is contained in:
parent
267855c44e
commit
07fd801d94
4 changed files with 378 additions and 77 deletions
|
@ -127,13 +127,17 @@ namespace MWMechanics
|
|||
mYCell = mCellY * ESM::Land::REAL_SIZE;
|
||||
}
|
||||
|
||||
// convert npcPos to local (i.e. cell) co-ordinates
|
||||
Ogre::Vector3 npcPos(actor.getRefData().getPosition().pos);
|
||||
npcPos[0] = npcPos[0] - mXCell;
|
||||
npcPos[1] = npcPos[1] - mYCell;
|
||||
|
||||
// populate mAllowedNodes for this actor with pathgrid point indexes based on mDistance
|
||||
// NOTE: mPoints and mAllowedNodes contain points in local co-ordinates
|
||||
for(unsigned int counter = 0; counter < mPathgrid->mPoints.size(); counter++)
|
||||
{
|
||||
Ogre::Vector3 nodePos(mPathgrid->mPoints[counter].mX, mPathgrid->mPoints[counter].mY,
|
||||
Ogre::Vector3 nodePos(mPathgrid->mPoints[counter].mX,
|
||||
mPathgrid->mPoints[counter].mY,
|
||||
mPathgrid->mPoints[counter].mZ);
|
||||
if(npcPos.squaredDistance(nodePos) <= mDistance * mDistance)
|
||||
mAllowedNodes.push_back(mPathgrid->mPoints[counter]);
|
||||
|
@ -145,7 +149,8 @@ namespace MWMechanics
|
|||
unsigned int index = 0;
|
||||
for(unsigned int counterThree = 1; counterThree < mAllowedNodes.size(); counterThree++)
|
||||
{
|
||||
Ogre::Vector3 nodePos(mAllowedNodes[counterThree].mX, mAllowedNodes[counterThree].mY,
|
||||
Ogre::Vector3 nodePos(mAllowedNodes[counterThree].mX,
|
||||
mAllowedNodes[counterThree].mY,
|
||||
mAllowedNodes[counterThree].mZ);
|
||||
float tempDist = npcPos.squaredDistance(nodePos);
|
||||
if(tempDist < closestNode)
|
||||
|
@ -277,16 +282,24 @@ namespace MWMechanics
|
|||
dest.mY = destNodePos[1] + mYCell;
|
||||
dest.mZ = destNodePos[2];
|
||||
|
||||
// actor position is already in world co-ordinates
|
||||
ESM::Pathgrid::Point start;
|
||||
start.mX = pos.pos[0];
|
||||
start.mY = pos.pos[1];
|
||||
start.mZ = pos.pos[2];
|
||||
|
||||
// don't take shortcuts for wandering
|
||||
mPathFinder.buildPath(start, dest, actor.getCell(), false);
|
||||
|
||||
if(mPathFinder.isPathConstructed())
|
||||
{
|
||||
// Remove this node as an option and add back the previously used node (stops NPC from picking the same node):
|
||||
// buildPath inserts dest in case it is not a pathgraph point index
|
||||
// which is a duplicate for AiWander
|
||||
//if(mPathFinder.getPathSize() > 1)
|
||||
//mPathFinder.getPath().pop_back();
|
||||
|
||||
// Remove this node as an option and add back the previously used node
|
||||
// (stops NPC from picking the same node):
|
||||
ESM::Pathgrid::Point temp = mAllowedNodes[randNode];
|
||||
mAllowedNodes.erase(mAllowedNodes.begin() + randNode);
|
||||
mAllowedNodes.push_back(mCurrentNode);
|
||||
|
@ -377,7 +390,10 @@ namespace MWMechanics
|
|||
}
|
||||
else
|
||||
{
|
||||
// normal walk forward
|
||||
actor.getClass().getMovementSettings(actor).mPosition[1] = 1;
|
||||
// turn towards the next point in mPath
|
||||
// TODO: possibly no need to check every frame, maybe every 30 should be ok?
|
||||
zTurn(actor, Ogre::Degree(mPathFinder.getZAngleToNext(pos.pos[0], pos.pos[1])));
|
||||
}
|
||||
|
||||
|
|
|
@ -38,8 +38,10 @@ namespace MWMechanics
|
|||
float mY;
|
||||
float mZ;
|
||||
|
||||
// Cell location
|
||||
int mCellX;
|
||||
int mCellY;
|
||||
// Cell location multiply by ESM::Land::REAL_SIZE to get the right scale
|
||||
float mXCell;
|
||||
float mYCell;
|
||||
|
||||
|
|
|
@ -37,19 +37,75 @@ namespace
|
|||
return sqrt(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
int getClosestPoint(const ESM::Pathgrid* grid, float x, float y, float z)
|
||||
// See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
|
||||
//
|
||||
// One of the smallest cost in Seyda Neen is between points 77 & 78:
|
||||
// pt x y
|
||||
// 77 = 8026, 4480
|
||||
// 78 = 7986, 4218
|
||||
//
|
||||
// Euclidean distance is about 262 (ignoring z) and Manhattan distance is 300
|
||||
// (again ignoring z). Using a value of about 300 for D seems like a reasonable
|
||||
// starting point for experiments. If in doubt, just use value 1.
|
||||
//
|
||||
// The distance between 3 & 4 are pretty small, too.
|
||||
// 3 = 5435, 223
|
||||
// 4 = 5948, 193
|
||||
//
|
||||
// Approx. 514 Euclidean distance and 533 Manhattan distance.
|
||||
//
|
||||
float manhattan(ESM::Pathgrid::Point a, ESM::Pathgrid::Point b)
|
||||
{
|
||||
return 300 * (abs(a.mX - b.mX) + abs(a.mY - b.mY) + abs(a.mZ - b.mZ));
|
||||
}
|
||||
|
||||
// Choose a heuristics - these may not be the best for directed graphs with
|
||||
// non uniform edge costs.
|
||||
//
|
||||
// distance:
|
||||
// - sqrt((curr.x - goal.x)^2 + (curr.y - goal.y)^2 + (curr.z - goal.z)^2)
|
||||
// - slower but more accurate
|
||||
//
|
||||
// Manhattan:
|
||||
// - |curr.x - goal.x| + |curr.y - goal.y| + |curr.z - goal.z|
|
||||
// - faster but not the shortest path
|
||||
float costAStar(ESM::Pathgrid::Point a, ESM::Pathgrid::Point b)
|
||||
{
|
||||
//return distance(a, b);
|
||||
return manhattan(a, b);
|
||||
}
|
||||
|
||||
// Slightly cheaper version for comparisons.
|
||||
// Caller needs to be careful for very short distances (i.e. less than 1)
|
||||
// or when accumuating the results i.e. (a + b)^2 != a^2 + b^2
|
||||
//
|
||||
float distanceSquared(ESM::Pathgrid::Point point, Ogre::Vector3 pos)
|
||||
{
|
||||
return Ogre::Vector3(point.mX, point.mY, point.mZ).squaredDistance(pos);
|
||||
}
|
||||
|
||||
// Return the closest pathgrid point index from the specified position co
|
||||
// -ordinates. NOTE: Does not check if there is a sensible way to get there
|
||||
// (e.g. a cliff in front).
|
||||
//
|
||||
// NOTE: pos is expected to be in local co-ordinates, as is grid->mPoints
|
||||
//
|
||||
int getClosestPoint(const ESM::Pathgrid* grid, Ogre::Vector3 pos)
|
||||
{
|
||||
if(!grid || grid->mPoints.empty())
|
||||
return -1;
|
||||
|
||||
float distanceBetween = distance(grid->mPoints[0], x, y, z);
|
||||
float distanceBetween = distanceSquared(grid->mPoints[0], pos);
|
||||
int closestIndex = 0;
|
||||
|
||||
// TODO: if this full scan causes performance problems mapping pathgrid
|
||||
// points to a quadtree may help
|
||||
for(unsigned int counter = 1; counter < grid->mPoints.size(); counter++)
|
||||
{
|
||||
if(distance(grid->mPoints[counter], x, y, z) < distanceBetween)
|
||||
float potentialDistBetween = distanceSquared(grid->mPoints[counter], pos);
|
||||
if(potentialDistBetween < distanceBetween)
|
||||
{
|
||||
distanceBetween = distance(grid->mPoints[counter], x, y, z);
|
||||
distanceBetween = potentialDistBetween;
|
||||
closestIndex = counter;
|
||||
}
|
||||
}
|
||||
|
@ -57,6 +113,39 @@ namespace
|
|||
return closestIndex;
|
||||
}
|
||||
|
||||
// Uses mSCComp to choose 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)
|
||||
{
|
||||
// assume grid is fine
|
||||
int startGroup = sCComp[start];
|
||||
|
||||
float distanceBetween = distanceSquared(grid->mPoints[0], pos);
|
||||
int closestIndex = 0;
|
||||
int closestReachableIndex = 0;
|
||||
// TODO: if this full scan causes performance problems mapping pathgrid
|
||||
// points to a quadtree may help
|
||||
for(unsigned int counter = 1; counter < grid->mPoints.size(); counter++)
|
||||
{
|
||||
float potentialDistBetween = distanceSquared(grid->mPoints[counter], pos);
|
||||
if(potentialDistBetween < distanceBetween)
|
||||
{
|
||||
// found a closer one
|
||||
distanceBetween = potentialDistBetween;
|
||||
closestIndex = counter;
|
||||
if (sCComp[counter] == startGroup)
|
||||
{
|
||||
closestReachableIndex = counter;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(start == closestReachableIndex)
|
||||
closestReachableIndex = -1; // couldn't find anyting other than start
|
||||
|
||||
return std::pair<int, bool>
|
||||
(closestReachableIndex, closestReachableIndex == closestIndex);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
namespace MWMechanics
|
||||
|
@ -76,13 +165,13 @@ namespace MWMechanics
|
|||
}
|
||||
|
||||
/*
|
||||
* NOTE: based on buildPath2(), please check git history if interested
|
||||
* NOTE: Based on buildPath2(), please check git history if interested
|
||||
*
|
||||
* Populate mGraph with the cost of each allowed edge (measured in distance ^2)
|
||||
* 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().
|
||||
* mGSore and mFScore are also resized.
|
||||
* 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
|
||||
*
|
||||
|
@ -91,7 +180,7 @@ namespace MWMechanics
|
|||
* n = index of edges from point f
|
||||
*
|
||||
*
|
||||
* Example: (note from p(0) to p(2) not allowed)
|
||||
* Example: (note from p(0) to p(2) not allowed in this example)
|
||||
*
|
||||
* mGraph[0].edges[0].destination = 1
|
||||
* .edges[1].destination = 3
|
||||
|
@ -130,25 +219,110 @@ namespace MWMechanics
|
|||
Node node;
|
||||
node.label = i;
|
||||
node.parent = -1;
|
||||
mGraph[i] = node; // TODO: old code used push_back(node), check if any difference
|
||||
mGraph[i] = node;
|
||||
}
|
||||
// store the costs (measured in distance ^2) of each edge, in both directions
|
||||
// store the costs of each edge
|
||||
for(unsigned int i = 0; i < pathGrid->mEdges.size(); i++)
|
||||
{
|
||||
Edge edge;
|
||||
edge.cost = distance(pathGrid->mPoints[pathGrid->mEdges[i].mV0],
|
||||
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, ESM already contains the required reverse paths
|
||||
// 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 < 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++)
|
||||
|
@ -160,7 +334,8 @@ namespace MWMechanics
|
|||
}
|
||||
|
||||
/*
|
||||
* NOTE: based on buildPath2(), please check git history if interested
|
||||
* 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
|
||||
|
@ -169,20 +344,27 @@ namespace MWMechanics
|
|||
*
|
||||
* 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()
|
||||
*
|
||||
* The heuristics used is distance^2 from current position to the final goal.
|
||||
*/
|
||||
std::list<ESM::Pathgrid::Point> PathFinder::aStarSearch(const ESM::Pathgrid* pathGrid,int start,int goal,float xCell, float yCell)
|
||||
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] = distance(pathGrid->mPoints[start],pathGrid->mPoints[goal]);
|
||||
mFScore[start] = costAStar(pathGrid->mPoints[start], pathGrid->mPoints[goal]);
|
||||
|
||||
std::list<int> openset;
|
||||
std::list<int> closedset;
|
||||
|
@ -195,33 +377,36 @@ namespace MWMechanics
|
|||
current = openset.front(); // front has the lowest cost
|
||||
openset.pop_front();
|
||||
|
||||
if(current == goal) break;
|
||||
if(current == goal)
|
||||
break;
|
||||
|
||||
closedset.push_back(current);
|
||||
closedset.push_back(current); // remember we've been here
|
||||
|
||||
// check all edges for the "current" point index
|
||||
// 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())
|
||||
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();
|
||||
bool isInOpenSet = std::find(openset.begin(), openset.end(), dest) != openset.end();
|
||||
if(!isInOpenSet
|
||||
|| tentative_g < mGScore[dest] )
|
||||
|| tentative_g < mGScore[dest])
|
||||
{
|
||||
mGraph[dest].parent = current;
|
||||
mGScore[dest] = tentative_g;
|
||||
mFScore[dest] = tentative_g + distance(pathGrid->mPoints[dest],pathGrid->mPoints[goal]);
|
||||
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 (apparently)
|
||||
// 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++)
|
||||
for(it = openset.begin(); it!= openset.end(); it++)
|
||||
{
|
||||
if(mGScore[*it] > mGScore[dest])
|
||||
if(mFScore[*it] > mFScore[dest])
|
||||
break;
|
||||
}
|
||||
openset.insert(it, dest);
|
||||
|
@ -231,11 +416,14 @@ namespace MWMechanics
|
|||
}
|
||||
}
|
||||
|
||||
// reconstruct path to return
|
||||
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)
|
||||
{
|
||||
//std::cout << "not empty" << xCell;
|
||||
ESM::Pathgrid::Point pt = pathGrid->mPoints[current];
|
||||
pt.mX += xCell;
|
||||
pt.mY += yCell;
|
||||
|
@ -243,7 +431,8 @@ namespace MWMechanics
|
|||
current = mGraph[current].parent;
|
||||
}
|
||||
|
||||
// TODO: Is this a bug? If path is empty the destination is inserted.
|
||||
// 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())
|
||||
|
@ -258,30 +447,67 @@ namespace MWMechanics
|
|||
}
|
||||
|
||||
/*
|
||||
* 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: 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: startPoint & endPoint are in world co-ordinates
|
||||
*
|
||||
* Updates mPath using aStarSearch() or ray test (if shortcut allowed).
|
||||
* mPath consists of pathgrid points, except the last element which is
|
||||
* endPoint. This may be useful where the endPoint is not on a pathgrid
|
||||
* point (e.g. combat). However, if the caller has already chosen a
|
||||
* pathgrid point (e.g. wander) then it may be worth while to call
|
||||
* pop_back() to remove the redundant entry.
|
||||
*
|
||||
* Updates mPath using aStarSearch().
|
||||
* mPathConstructed is set true if successful, false if not
|
||||
*
|
||||
* May update mGraph by calling buildPathgridGraph() if it isn't constructed yet.
|
||||
* 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()
|
||||
*
|
||||
* |
|
||||
* | cell
|
||||
* | +-----------+
|
||||
* | | |
|
||||
* | | |
|
||||
* | | @ |
|
||||
* | i | j |
|
||||
* |<--->|<---->| |
|
||||
* | +-----------+
|
||||
* | k
|
||||
* |<---------->| world
|
||||
* +-----------------------------
|
||||
*
|
||||
* i = x value of cell itself (multiply by ESM::Land::REAL_SIZE to convert)
|
||||
* j = @.x in local co-ordinates (i.e. within the cell)
|
||||
* k = @.x in world co-ordinates
|
||||
*/
|
||||
void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint,
|
||||
void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint,
|
||||
const ESM::Pathgrid::Point &endPoint,
|
||||
const MWWorld::CellStore* cell, bool allowShortcuts)
|
||||
{
|
||||
mPath.clear();
|
||||
if(mCell != cell) mIsGraphConstructed = false;
|
||||
mCell = cell;
|
||||
|
||||
if(allowShortcuts)
|
||||
{
|
||||
if(MWBase::Environment::get().getWorld()->castRay(startPoint.mX, startPoint.mY, startPoint.mZ,
|
||||
// if there's a ray cast hit, can't take a direct path
|
||||
if(!MWBase::Environment::get().getWorld()->castRay(startPoint.mX, startPoint.mY, startPoint.mZ,
|
||||
endPoint.mX, endPoint.mY, endPoint.mZ))
|
||||
allowShortcuts = false;
|
||||
{
|
||||
mPath.push_back(endPoint);
|
||||
mIsPathConstructed = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if(!allowShortcuts)
|
||||
if(mCell != cell)
|
||||
{
|
||||
mIsGraphConstructed = false; // must be in a new cell, need a new mGraph and mSCComp
|
||||
mCell = cell;
|
||||
}
|
||||
|
||||
const ESM::Pathgrid *pathGrid =
|
||||
MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*mCell->getCell());
|
||||
float xCell = 0;
|
||||
|
@ -292,30 +518,57 @@ namespace MWMechanics
|
|||
xCell = mCell->getCell()->mData.mX * ESM::Land::REAL_SIZE;
|
||||
yCell = mCell->getCell()->mData.mY * ESM::Land::REAL_SIZE;
|
||||
}
|
||||
int startNode = getClosestPoint(pathGrid, startPoint.mX - xCell, startPoint.mY - yCell,startPoint.mZ);
|
||||
int endNode = getClosestPoint(pathGrid, endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ);
|
||||
|
||||
if(startNode != -1 && endNode != -1)
|
||||
// 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
|
||||
{
|
||||
if(!mIsGraphConstructed) buildPathgridGraph(pathGrid);
|
||||
if(!mIsGraphConstructed)
|
||||
{
|
||||
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),
|
||||
startNode, mSCComp);
|
||||
|
||||
mPath = aStarSearch(pathGrid,startNode,endNode,xCell,yCell);
|
||||
if(endNode.first != -1)
|
||||
{
|
||||
mPath = aStarSearch(pathGrid, startNode, endNode.first, xCell, yCell);
|
||||
|
||||
if(!mPath.empty())
|
||||
{
|
||||
mIsPathConstructed = true;
|
||||
}
|
||||
}
|
||||
// Add the destination (which may be different to the closest
|
||||
// pathgrid point). However only add if endNode was the closest
|
||||
// point to endPoint.
|
||||
//
|
||||
// This logic can fail in the opposite situate, e.g. endPoint may
|
||||
// have been reachable but happened to be very close to an
|
||||
// unreachable pathgrid point.
|
||||
//
|
||||
// The AI routines will have to deal with such situations.
|
||||
if(endNode.second)
|
||||
mPath.push_back(endPoint);
|
||||
}
|
||||
else
|
||||
{
|
||||
mPath.push_back(endPoint);
|
||||
mIsPathConstructed = true;
|
||||
}
|
||||
|
||||
if(mPath.empty())
|
||||
mIsPathConstructed = false;
|
||||
}
|
||||
else
|
||||
mIsPathConstructed = false;
|
||||
}
|
||||
else
|
||||
mIsPathConstructed = false; // this shouldn't really happen, but just in case
|
||||
}
|
||||
|
||||
float PathFinder::getZAngleToNext(float x, float y) const
|
||||
{
|
||||
|
@ -332,6 +585,7 @@ namespace MWMechanics
|
|||
return Ogre::Radian(Ogre::Math::ACos(directionY / directionResult) * sgn(Ogre::Math::ASin(directionX / directionResult))).valueDegrees();
|
||||
}
|
||||
|
||||
// Used by AiCombat, use Euclidean distance
|
||||
float PathFinder::getDistToNext(float x, float y, float z)
|
||||
{
|
||||
ESM::Pathgrid::Point nextPoint = *mPath.begin();
|
||||
|
@ -372,6 +626,7 @@ namespace MWMechanics
|
|||
return false;
|
||||
}
|
||||
|
||||
// used by AiCombat, see header for the rationale
|
||||
void PathFinder::syncStart(const std::list<ESM::Pathgrid::Point> &path)
|
||||
{
|
||||
if (mPath.size() < 2)
|
||||
|
|
|
@ -64,9 +64,10 @@ namespace MWMechanics
|
|||
return mPath;
|
||||
}
|
||||
|
||||
//When first point of newly created path is the nearest to actor point, then
|
||||
//the cituation can occure when this point is undesirable (if the 2nd point of new path == the 1st point of old path)
|
||||
//This functions deletes that point.
|
||||
// When first point of newly created path is the nearest to actor point,
|
||||
// then a situation can occure when this point is undesirable
|
||||
// (if the 2nd point of new path == the 1st point of old path)
|
||||
// This functions deletes that point.
|
||||
void syncStart(const std::list<ESM::Pathgrid::Point> &path);
|
||||
|
||||
void addPointToPath(ESM::Pathgrid::Point &point)
|
||||
|
@ -74,6 +75,13 @@ 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
|
||||
|
@ -101,6 +109,26 @@ namespace MWMechanics
|
|||
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)
|
||||
//
|
||||
// In Seyda Neen there are 3:
|
||||
//
|
||||
// 52, 53 and 54 are one set (enclosed yard)
|
||||
// 48, 49, 50, 51, 84, 85, 86, 87, 88, 89, 90 are another (ship & office)
|
||||
// all other pathgrid points are the third set
|
||||
//
|
||||
std::vector<int> mSCComp;
|
||||
// variables used to calculate mSCComp
|
||||
int mSCCId;
|
||||
int mSCCIndex;
|
||||
std::list<int> mSCCStack;
|
||||
typedef std::pair<int, int> VPair; // first is index, second is lowlink
|
||||
std::vector<VPair> mSCCPoint;
|
||||
// methods used to calculate mSCComp
|
||||
void recursiveStrongConnect(int v);
|
||||
void buildConnectedPoints(const ESM::Pathgrid* pathGrid);
|
||||
};
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in a new issue