@ -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 = distance Squared ( 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 : b ased on buildPath2 ( ) , please check git history if interested
* NOTE : B ased 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 re verse 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 3 rd 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 ( m GScore[ * it ] > mG Score[ dest ] )
if ( m FScore[ * it ] > mF Score[ 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 .
*
* NOTE : startPoint & endPoint are in world co - ordinates
*
* Updates mPath using aStarSearch ( ) .
* mPathConstructed is set true if successful , false if not
* 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 .
*
* May update mGraph by calling buildPathgridGraph ( ) if it isn ' t constructed yet .
* 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 ( mCell ! = cell )
{
mIsGraphConstructed = false ; // must be in a new cell, need a new mGraph and mSCComp
mCell = cell ;
}
if ( ! allowShortcuts )
const ESM : : Pathgrid * pathGrid =
MWBase : : Environment : : get ( ) . getWorld ( ) - > getStore ( ) . get < ESM : : Pathgrid > ( ) . search ( * mCell - > getCell ( ) ) ;
float xCell = 0 ;
float yCell = 0 ;
if ( mCell - > isExterior ( ) )
{
const ESM : : Pathgrid * pathGrid =
MWBase : : Environment : : get ( ) . getWorld ( ) - > getStore ( ) . get < ESM : : Pathgrid > ( ) . search ( * mCell - > getCell ( ) ) ;
float xCell = 0 ;
float yCell = 0 ;
xCell = mCell - > getCell ( ) - > mData . mX * ESM : : Land : : REAL_SIZE ;
yCell = mCell - > getCell ( ) - > mData . mY * ESM : : Land : : REAL_SIZE ;
}
if ( mCell - > isExterior ( ) )
// 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 )
cc9cii
11 years ago