mirror of
https://gitlab.com/OpenMW/openmw.git
synced 2025-02-10 03:39:55 +00:00
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
@ -127,14 +127,18 @@ 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,
|
||||
mPathgrid->mPoints[counter].mZ);
|
||||
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,8 +149,9 @@ namespace MWMechanics
|
||||
unsigned int index = 0;
|
||||
for(unsigned int counterThree = 1; counterThree < mAllowedNodes.size(); counterThree++)
|
||||
{
|
||||
Ogre::Vector3 nodePos(mAllowedNodes[counterThree].mX, mAllowedNodes[counterThree].mY,
|
||||
mAllowedNodes[counterThree].mZ);
|
||||
Ogre::Vector3 nodePos(mAllowedNodes[counterThree].mX,
|
||||
mAllowedNodes[counterThree].mY,
|
||||
mAllowedNodes[counterThree].mZ);
|
||||
float tempDist = npcPos.squaredDistance(nodePos);
|
||||
if(tempDist < closestNode)
|
||||
index = counterThree;
|
||||
@ -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],
|
||||
pathGrid->mPoints[pathGrid->mEdges[i].mV1]);
|
||||
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.
|
||||
*
|
||||
* openset - point indexes to be traversed, lowest cost at the front
|
||||
* closedset - point indexes already traversed
|
||||
* Input params:
|
||||
* start, goal - pathgrid point indexes (for this cell)
|
||||
* xCell, yCell - values to add to convert path back to world scale
|
||||
*
|
||||
* mGScore - past accumulated costs vector indexed by point index
|
||||
* mFScore - future estimated costs vector indexed by point index
|
||||
* these are resized by buildPathgridGraph()
|
||||
* Variables:
|
||||
* openset - point indexes to be traversed, lowest cost at the front
|
||||
* closedset - point indexes already traversed
|
||||
*
|
||||
* The heuristics used is distance^2 from current position to the final goal.
|
||||
* 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)
|
||||
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,63 +447,127 @@ 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.
|
||||
*
|
||||
* Updates mPath using aStarSearch().
|
||||
* mPathConstructed is set true if successful, false if not
|
||||
* NOTE: startPoint & endPoint are in world co-ordinates
|
||||
*
|
||||
* May update mGraph by calling buildPathgridGraph() if it isn't constructed yet.
|
||||
* 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.
|
||||
*
|
||||
* 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()
|
||||
*
|
||||
* |
|
||||
* | 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,
|
||||
endPoint.mX, endPoint.mY, endPoint.mZ))
|
||||
allowShortcuts = false;
|
||||
// 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))
|
||||
{
|
||||
mPath.push_back(endPoint);
|
||||
mIsPathConstructed = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if(!allowShortcuts)
|
||||
if(mCell != cell)
|
||||
{
|
||||
const ESM::Pathgrid *pathGrid =
|
||||
MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*mCell->getCell());
|
||||
float xCell = 0;
|
||||
float yCell = 0;
|
||||
mIsGraphConstructed = false; // must be in a new cell, need a new mGraph and mSCComp
|
||||
mCell = cell;
|
||||
}
|
||||
|
||||
if (mCell->isExterior())
|
||||
const ESM::Pathgrid *pathGrid =
|
||||
MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*mCell->getCell());
|
||||
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
|
||||
{
|
||||
if(!mIsGraphConstructed)
|
||||
{
|
||||
xCell = mCell->getCell()->mData.mX * ESM::Land::REAL_SIZE;
|
||||
yCell = mCell->getCell()->mData.mY * ESM::Land::REAL_SIZE;
|
||||
buildPathgridGraph(pathGrid); // pre-compute costs for use with aStarSearch
|
||||
buildConnectedPoints(pathGrid); // must before calling getClosestReachablePoint
|
||||
}
|
||||
int startNode = getClosestPoint(pathGrid, startPoint.mX - xCell, startPoint.mY - yCell,startPoint.mZ);
|
||||
int endNode = getClosestPoint(pathGrid, endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ);
|
||||
std::pair<int, bool> endNode = getClosestReachablePoint(pathGrid,
|
||||
Ogre::Vector3(endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ),
|
||||
startNode, mSCComp);
|
||||
|
||||
if(startNode != -1 && endNode != -1)
|
||||
if(endNode.first != -1)
|
||||
{
|
||||
if(!mIsGraphConstructed) buildPathgridGraph(pathGrid);
|
||||
|
||||
mPath = aStarSearch(pathGrid,startNode,endNode,xCell,yCell);
|
||||
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
|
||||
mIsPathConstructed = false;
|
||||
}
|
||||
else
|
||||
mIsPathConstructed = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
mPath.push_back(endPoint);
|
||||
mIsPathConstructed = true;
|
||||
}
|
||||
|
||||
if(mPath.empty())
|
||||
mIsPathConstructed = false;
|
||||
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…
x
Reference in New Issue
Block a user