@ -54,13 +54,13 @@ namespace
//
// Approx. 514 Euclidean distance and 533 Manhattan distance.
//
float manhattan ( ESM : : Pathgrid : : Point a , ESM : : Pathgrid : : Point b )
float manhattan ( const ESM : : Pathgrid : : Point a , const ESM : : Pathgrid : : Point b )
{
return 300 * ( abs ( a . mX - b . mX ) + abs ( a . mY - b . mY ) + abs ( a . mZ - b . mZ ) ) ;
}
// Choose a heuristics - these may not be the best for directed graphs with
// non uniform edge costs.
// Choose a heuristics - Note that these may not be the best for directed
// graphs with non- uniform edge costs.
//
// distance:
// - sqrt((curr.x - goal.x)^2 + (curr.y - goal.y)^2 + (curr.z - goal.z)^2)
@ -69,7 +69,7 @@ namespace
// Manhattan:
// - |curr.x - goal.x| + |curr.y - goal.y| + |curr.z - goal.z|
// - faster but not the shortest path
float costAStar ( ESM : : Pathgrid : : Point a , ESM : : Pathgrid : : Point b )
float costAStar ( const ESM : : Pathgrid : : Point a , const ESM : : Pathgrid : : Point b )
{
//return distance(a, b);
return manhattan ( a , b ) ;
@ -113,12 +113,13 @@ namespace
return closestIndex ;
}
// Uses mS CComp to c hoose a reachable end pathgrid point. start is assumed reachable.
// Chooses a reachable end pathgrid point. start is assumed reachable.
std : : pair < int , bool > getClosestReachablePoint ( const ESM : : Pathgrid * grid ,
Ogre : : Vector3 pos , int start , std : : vector < int > & sCComp )
const MWWorld : : CellStore * cell ,
Ogre : : Vector3 pos , int start )
{
// assume grid is fine
int startGroup = sCComp [ start ] ;
if ( ! grid | | grid - > mPoints . empty ( ) )
return std : : pair < int , bool > ( - 1 , false ) ;
float distanceBetween = distanceSquared ( grid - > mPoints [ 0 ] , pos ) ;
int closestIndex = 0 ;
@ -133,7 +134,7 @@ namespace
// found a closer one
distanceBetween = potentialDistBetween ;
closestIndex = counter ;
if ( sCComp[ counter ] = = startGroup )
if ( cell- > isPointConnected ( start , counter ) )
{
closestReachableIndex = counter ;
}
@ -152,7 +153,7 @@ namespace MWMechanics
{
PathFinder : : PathFinder ( )
: mIsPathConstructed ( false ) ,
m IsGraphConstructed( false ) ,
m Pathgrid( NULL ) ,
mCell ( NULL )
{
}
@ -164,293 +165,14 @@ namespace MWMechanics
mIsPathConstructed = false ;
}
/*
* NOTE : Based on buildPath2 ( ) , please check git history if interested
*
* Populate mGraph with the cost of each allowed edge .
*
* Any existing data in mGraph is wiped clean first . The node ' s parent
* is set with initial value of - 1. The parent values are populated by
* aStarSearch ( ) in order to reconstruct a path .
*
* mGraph [ f ] . edges [ n ] . destination = t
*
* f = point index of location " from "
* t = point index of location " to "
* n = index of edges from point f
*
*
* Example : ( note from p ( 0 ) to p ( 2 ) not allowed in this example )
*
* mGraph [ 0 ] . edges [ 0 ] . destination = 1
* . edges [ 1 ] . destination = 3
*
* mGraph [ 1 ] . edges [ 0 ] . destination = 0
* . edges [ 1 ] . destination = 2
* . edges [ 2 ] . destination = 3
*
* mGraph [ 2 ] . edges [ 0 ] . destination = 1
*
* ( etc , etc )
*
*
* low
* cost
* p ( 0 ) < - - - > p ( 1 ) < - - - - - - - - - - - - > p ( 2 )
* ^ ^
* | |
* | + - - - - - > p ( 3 )
* + - - - - - - - - - - - - - - - - >
* high cost
*/
void PathFinder : : buildPathgridGraph ( const ESM : : Pathgrid * pathGrid )
{
mGraph . clear ( ) ;
// resize lists
mGScore . resize ( pathGrid - > mPoints . size ( ) , - 1 ) ;
mFScore . resize ( pathGrid - > mPoints . size ( ) , - 1 ) ;
Node defaultNode ;
defaultNode . label = - 1 ;
defaultNode . parent = - 1 ;
mGraph . resize ( pathGrid - > mPoints . size ( ) , defaultNode ) ;
// initialise mGraph
for ( unsigned int i = 0 ; i < pathGrid - > mPoints . size ( ) ; i + + )
{
Node node ;
node . label = i ;
node . parent = - 1 ;
mGraph [ i ] = node ;
}
// store the costs of each edge
for ( unsigned int i = 0 ; i < pathGrid - > mEdges . size ( ) ; i + + )
{
Edge edge ;
edge . cost = costAStar ( pathGrid - > mPoints [ pathGrid - > mEdges [ i ] . mV0 ] ,
pathGrid - > mPoints [ pathGrid - > mEdges [ i ] . mV1 ] ) ;
// forward path of the edge
edge . destination = pathGrid - > mEdges [ i ] . mV1 ;
mGraph [ pathGrid - > mEdges [ i ] . mV0 ] . edges . push_back ( edge ) ;
// reverse path of the edge
// NOTE: These are redundant, the ESM already contains the reverse paths.
//edge.destination = pathGrid->mEdges[i].mV0;
//mGraph[pathGrid->mEdges[i].mV1].edges.push_back(edge);
}
mIsGraphConstructed = true ;
}
// v is the pathgrid point index (some call them vertices)
void PathFinder : : recursiveStrongConnect ( int v )
{
mSCCPoint [ v ] . first = mSCCIndex ; // index
mSCCPoint [ v ] . second = mSCCIndex ; // lowlink
mSCCIndex + + ;
mSCCStack . push_back ( v ) ;
int w ;
for ( int i = 0 ; i < static_cast < int > ( mGraph [ v ] . edges . size ( ) ) ; i + + )
{
w = mGraph [ v ] . edges [ i ] . destination ;
if ( mSCCPoint [ w ] . first = = - 1 ) // not visited
{
recursiveStrongConnect ( w ) ; // recurse
mSCCPoint [ v ] . second = std : : min ( mSCCPoint [ v ] . second ,
mSCCPoint [ w ] . second ) ;
}
else
{
if ( find ( mSCCStack . begin ( ) , mSCCStack . end ( ) , w ) ! = mSCCStack . end ( ) )
mSCCPoint [ v ] . second = std : : min ( mSCCPoint [ v ] . second ,
mSCCPoint [ w ] . first ) ;
}
}
if ( mSCCPoint [ v ] . second = = mSCCPoint [ v ] . first )
{
// new component
do
{
w = mSCCStack . back ( ) ;
mSCCStack . pop_back ( ) ;
mSCComp [ w ] = mSCCId ;
}
while ( w ! = v ) ;
mSCCId + + ;
}
return ;
}
/*
* mSCComp contains the strongly connected component group id ' s .
*
* A cell can have disjointed pathgrid , e . g . Seyda Neen which has 3
*
* mSCComp for Seyda Neen will have 3 different values . When selecting a
* random pathgrid point for AiWander , mSCComp can be checked for quickly
* finding whether the destination is reachable .
*
* Otherwise , buildPath will automatically select a closest reachable end
* pathgrid point ( reachable from the closest start point ) .
*
* Using Tarjan ' s algorithm
*
* mGraph | graph G |
* mSCCPoint | V | derived from pathGrid - > mPoints
* mGraph [ v ] . edges | E ( for v ) |
* mSCCIndex | index | keep track of smallest unused index
* mSCCStack | S |
* pathGrid
* - > mEdges [ v ] . mV1 | w | = mGraph [ v ] . edges [ i ] . destination
*
* FIXME : Some of these can be cleaned up by including them to struct
* Node used by mGraph
*/
void PathFinder : : buildConnectedPoints ( const ESM : : Pathgrid * pathGrid )
{
mSCComp . clear ( ) ;
mSCComp . resize ( pathGrid - > mPoints . size ( ) , 0 ) ;
mSCCId = 0 ;
mSCCIndex = 0 ;
mSCCStack . clear ( ) ;
mSCCPoint . clear ( ) ;
mSCCPoint . resize ( pathGrid - > mPoints . size ( ) , std : : pair < int , int > ( - 1 , - 1 ) ) ;
for ( unsigned int v = 0 ; v < pathGrid - > mPoints . size ( ) ; v + + )
{
if ( mSCCPoint [ v ] . first = = - 1 ) // undefined (haven't visited)
recursiveStrongConnect ( v ) ;
}
}
void PathFinder : : cleanUpAStar ( )
{
for ( int i = 0 ; i < static_cast < int > ( mGraph . size ( ) ) ; i + + )
{
mGraph [ i ] . parent = - 1 ;
mGScore [ i ] = - 1 ;
mFScore [ i ] = - 1 ;
}
}
/*
* NOTE : Based on buildPath2 ( ) , please check git history if interested
* Should consider a using 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
* that mGraph is already constructed . The caller , i . e . buildPath ( ) , needs
* to ensure this .
*
* Returns path ( a list of pathgrid point indexes ) which may be empty .
*
* Input params :
* start , goal - pathgrid point indexes ( for this cell )
* xCell , yCell - values to add to convert path back to world scale
*
* Variables :
* openset - point indexes to be traversed , lowest cost at the front
* closedset - point indexes already traversed
*
* Class variables :
* mGScore - past accumulated costs vector indexed by point index
* mFScore - future estimated costs vector indexed by point index
* these are resized by buildPathgridGraph ( )
*/
std : : list < ESM : : Pathgrid : : Point > PathFinder : : aStarSearch ( const ESM : : Pathgrid * pathGrid ,
int start , int goal ,
float xCell , float yCell )
{
cleanUpAStar ( ) ;
// mGScore & mFScore keep costs for each pathgrid point in pathGrid->mPoints
mGScore [ start ] = 0 ;
mFScore [ start ] = costAStar ( pathGrid - > mPoints [ start ] , pathGrid - > mPoints [ goal ] ) ;
std : : list < int > openset ;
std : : list < int > closedset ;
openset . push_back ( start ) ;
int current = - 1 ;
while ( ! openset . empty ( ) )
{
current = openset . front ( ) ; // front has the lowest cost
openset . pop_front ( ) ;
if ( current = = goal )
break ;
closedset . push_back ( current ) ; // remember we've been here
// check all edges for the current point index
for ( int j = 0 ; j < static_cast < int > ( mGraph [ current ] . edges . size ( ) ) ; j + + )
{
if ( std : : find ( closedset . begin ( ) , closedset . end ( ) , mGraph [ current ] . edges [ j ] . destination ) = =
closedset . end ( ) )
{
// not in closedset - i.e. have not traversed this edge destination
int dest = mGraph [ current ] . edges [ j ] . destination ;
float tentative_g = mGScore [ current ] + mGraph [ current ] . edges [ j ] . cost ;
bool isInOpenSet = std : : find ( openset . begin ( ) , openset . end ( ) , dest ) ! = openset . end ( ) ;
if ( ! isInOpenSet
| | tentative_g < mGScore [ dest ] )
{
mGraph [ dest ] . parent = current ;
mGScore [ dest ] = tentative_g ;
mFScore [ dest ] = tentative_g +
costAStar ( pathGrid - > mPoints [ dest ] , pathGrid - > mPoints [ goal ] ) ;
if ( ! isInOpenSet )
{
// add this edge to openset, lowest cost goes to the front
// TODO: if this causes performance problems a hash table may help
std : : list < int > : : iterator it = openset . begin ( ) ;
for ( it = openset . begin ( ) ; it ! = openset . end ( ) ; it + + )
{
if ( mFScore [ * it ] > mFScore [ dest ] )
break ;
}
openset . insert ( it , dest ) ;
}
}
} // if in closedset, i.e. traversed this edge already, try the next edge
}
}
std : : list < ESM : : Pathgrid : : Point > path ;
if ( current ! = goal )
return path ; // for some reason couldn't build a path
// e.g. start was not reachable (we assume it is)
// reconstruct path to return, using world co-ordinates
while ( mGraph [ current ] . parent ! = - 1 )
{
ESM : : Pathgrid : : Point pt = pathGrid - > mPoints [ current ] ;
pt . mX + = xCell ;
pt . mY + = yCell ;
path . push_front ( pt ) ;
current = mGraph [ current ] . parent ;
}
// TODO: Is this a bug? If path is empty the algorithm couldn't find a path.
// Simply using the destination as the path in this scenario seems strange.
// Commented out pending further testing.
#if 0
if ( path . empty ( ) )
{
ESM : : Pathgrid : : Point pt = pathGrid - > mPoints [ goal ] ;
pt . mX + = xCell ;
pt . mY + = yCell ;
path . push_front ( pt ) ;
}
# endif
return path ;
}
/*
* NOTE : This method may fail to find a path . The caller must check the
* result before using it . If there is no path the AI routies need to
* implement some other heuristics to reach the target .
*
* NOTE : It may be desirable to simply go directly to the endPoint if for
* example there are no pathgrids in this cell .
*
* NOTE : startPoint & endPoint are in world co - ordinates
*
* Updates mPath using aStarSearch ( ) or ray test ( if shortcut allowed ) .
@ -462,9 +184,6 @@ namespace MWMechanics
*
* mPathConstructed is set true if successful , false if not
*
* May update mGraph by calling buildPathgridGraph ( ) if it isn ' t
* constructed yet . At the same time mConnectedPoints is also updated .
*
* NOTE : co - ordinates must be converted prior to calling getClosestPoint ( )
*
* |
@ -486,7 +205,8 @@ namespace MWMechanics
*/
void PathFinder : : buildPath ( const ESM : : Pathgrid : : Point & startPoint ,
const ESM : : Pathgrid : : Point & endPoint ,
const MWWorld : : CellStore * cell , bool allowShortcuts )
const MWWorld : : CellStore * cell ,
bool allowShortcuts )
{
mPath . clear ( ) ;
@ -502,48 +222,76 @@ namespace MWMechanics
}
}
if ( mCell ! = cell )
if ( mCell ! = cell | | ! mPathgrid )
{
mIsGraphConstructed = false ; // must be in a new cell, need a new mGraph and mSCComp
mCell = cell ;
// Cache pathgrid as mPathgrid and update on cell changes. There
// might be a small gain in avoiding to search for it.
mPathgrid = MWBase : : Environment : : get ( ) . getWorld ( ) - > getStore ( ) . get < ESM : : Pathgrid > ( ) . search ( * mCell - > getCell ( ) ) ;
}
// Refer to AiWander reseach topic on openmw forums for some background.
// Maybe there is no pathgrid for this cell. Just go to destination and let
// physics take care of any blockages.
if ( ! mPathgrid | | mPathgrid - > mPoints . empty ( ) )
{
//#if 0
std : : cout < < " no pathgrid " < <
+ " \" " + mCell - > getCell ( ) - > mName + " \" "
+ " , " + std : : to_string ( mCell - > getCell ( ) - > mData . mX )
+ " , " + std : : to_string ( mCell - > getCell ( ) - > mData . mY )
< < std : : endl ;
//#endif
mPath . push_back ( endPoint ) ;
mIsPathConstructed = true ;
return ;
}
const ESM : : Pathgrid * pathGrid =
MWBase : : Environment : : get ( ) . getWorld ( ) - > getStore ( ) . get < ESM : : Pathgrid > ( ) . search ( * mCell - > getCell ( ) ) ;
// NOTE: getClosestPoint expects local co-ordinates
float xCell = 0 ;
float yCell = 0 ;
if ( mCell - > isExterior ( ) )
{
xCell = mCell - > getCell ( ) - > mData . mX * ESM : : Land : : REAL_SIZE ;
yCell = mCell - > getCell ( ) - > mData . mY * ESM : : Land : : REAL_SIZE ;
}
// NOTE: It is possible that getClosestPoint returns a pathgrind point index
// that is unreachable in some situations. e.g. actor is standing
// outside an area enclosed by walls, but there is a pathgrid
// point right behind the wall that is closer than any pathgrid
// point outside the wall
//
// NOTE: getClosestPoint expects local co-ordinates
//
int startNode = getClosestPoint ( pathGrid ,
Ogre : : Vector3 ( startPoint . mX - xCell , startPoint . mY - yCell , startPoint . mZ ) ) ;
if ( startNode ! = - 1 ) // only check once, assume pathGrid won't change
int startNode = getClosestPoint ( mPathgrid ,
Ogre : : Vector3 ( startPoint . mX - xCell , startPoint . mY - yCell , startPoint . mZ ) ) ;
// Some cells don't have any pathgrids at all
if ( startNode ! = - 1 )
{
if ( ! mIsGraphConstructed )
{
buildPathgridGraph ( pathGrid ) ; // pre-compute costs for use with aStarSearch
buildConnectedPoints ( pathGrid ) ; // must before calling getClosestReachablePoint
}
std : : pair < int , bool > endNode = getClosestReachablePoint ( pathGrid ,
std : : pair < int , bool > endNode = getClosestReachablePoint ( mPathgrid , cell ,
Ogre : : Vector3 ( endPoint . mX - xCell , endPoint . mY - yCell , endPoint . mZ ) ,
startNode , mSCComp ) ;
startNode ) ;
//#if 0
if ( ! mPathgrid )
std : : cout < < " no pathgrid " < <
+ " \" " + mCell - > getCell ( ) - > mName + " \" "
+ " , " + std : : to_string ( mCell - > getCell ( ) - > mData . mX )
+ " , " + std : : to_string ( mCell - > getCell ( ) - > mData . mY )
< < std : : endl ;
//#endif
// this shouldn't really happen, but just in case
if ( endNode . first ! = - 1 )
{
mPath = aStarSearch ( pathGrid , startNode , endNode . first , xCell , yCell ) ;
mPath = mCell- > aStarSearch( startNode , endNode . first , mCell- > isExterior ( ) ) ;
if ( ! mPath . empty ( ) )
{
@ -561,13 +309,34 @@ namespace MWMechanics
mPath . push_back ( endPoint ) ;
}
else
{
mIsPathConstructed = false ;
std : : cout < < " empty path error " < < std : : endl ;
}
//mIsPathConstructed = false;
}
else
{
mIsPathConstructed = false ;
std : : cout < < " second point error " < < std : : endl ;
}
//mIsPathConstructed = false;
}
else
mIsPathConstructed = false ; // this shouldn't really happen, but just in case
{
// FIXME: shouldn't return endpoint if first point error?
mIsPathConstructed = false ;
std : : cout < < " first point error " < < std : : endl ;
}
#if 0
if ( ! mIsPathConstructed )
{
mPath . push_back ( endPoint ) ;
mIsPathConstructed = true ;
}
# endif
return ;
}
float PathFinder : : getZAngleToNext ( float x , float y ) const
@ -645,5 +414,326 @@ namespace MWMechanics
}
// TODO: Any multi threading concerns?
PathgridGraph : : PathgridGraph ( )
: mCell ( NULL )
, mIsGraphConstructed ( false )
, mPathgrid ( NULL )
, mGraph ( 0 )
, mSCCId ( 0 )
, mSCCIndex ( 0 )
{
}
/*
* mGraph is populated with the cost of each allowed edge .
*
* The data structure is based on the code in buildPath2 ( ) but modified .
* Please check git history if interested .
*
* mGraph [ v ] . edges [ i ] . index = w
*
* v = point index of location " from "
* i = index of edges from point v
* w = point index of location " to "
*
*
* Example : ( notice from p ( 0 ) to p ( 2 ) is not allowed in this example )
*
* mGraph [ 0 ] . edges [ 0 ] . index = 1
* . edges [ 1 ] . index = 3
*
* mGraph [ 1 ] . edges [ 0 ] . index = 0
* . edges [ 1 ] . index = 2
* . edges [ 2 ] . index = 3
*
* mGraph [ 2 ] . edges [ 0 ] . index = 1
*
* ( etc , etc )
*
*
* low
* cost
* p ( 0 ) < - - - > p ( 1 ) < - - - - - - - - - - - - > p ( 2 )
* ^ ^
* | |
* | + - - - - - > p ( 3 )
* + - - - - - - - - - - - - - - - - >
* high cost
*/
bool PathgridGraph : : initPathgridGraph ( const ESM : : Cell * cell )
{
if ( ! cell )
{
std : : cout < < " init error " < < std : : endl ;
return false ;
}
mCell = cell ;
mPathgrid = MWBase : : Environment : : get ( ) . getWorld ( ) - > getStore ( ) . get < ESM : : Pathgrid > ( ) . search ( * cell ) ;
if ( ! mPathgrid )
{
std : : cout < < " init error " < < std : : endl ;
return false ;
}
mGraph . resize ( mPathgrid - > mPoints . size ( ) ) ;
for ( int i = 0 ; i < static_cast < int > ( mPathgrid - > mEdges . size ( ) ) ; i + + )
{
ConnectedPoint neighbour ;
neighbour . cost = costAStar ( mPathgrid - > mPoints [ mPathgrid - > mEdges [ i ] . mV0 ] ,
mPathgrid - > mPoints [ mPathgrid - > mEdges [ i ] . mV1 ] ) ;
// forward path of the edge
neighbour . index = mPathgrid - > mEdges [ i ] . mV1 ;
mGraph [ mPathgrid - > mEdges [ i ] . mV0 ] . edges . push_back ( neighbour ) ;
// reverse path of the edge
// NOTE: These are redundant, ESM already contains the required reverse paths
//neighbour.index = mPathgrid->mEdges[i].mV0;
//mGraph[mPathgrid->mEdges[i].mV1].edges.push_back(neighbour);
}
buildConnectedPoints ( ) ;
mIsGraphConstructed = true ;
//#if 0
std : : cout < < " loading pathgrid " < <
+ " \" " + mPathgrid - > mCell + " \" "
+ " , " + std : : to_string ( mPathgrid - > mData . mX )
+ " , " + std : : to_string ( mPathgrid - > mData . mY )
< < std : : endl ;
//#endif
return true ;
}
// v is the pathgrid point index (some call them vertices)
void PathgridGraph : : recursiveStrongConnect ( int v )
{
mSCCPoint [ v ] . first = mSCCIndex ; // index
mSCCPoint [ v ] . second = mSCCIndex ; // lowlink
mSCCIndex + + ;
mSCCStack . push_back ( v ) ;
int w ;
for ( int i = 0 ; i < static_cast < int > ( mGraph [ v ] . edges . size ( ) ) ; i + + )
{
w = mGraph [ v ] . edges [ i ] . index ;
if ( mSCCPoint [ w ] . first = = - 1 ) // not visited
{
recursiveStrongConnect ( w ) ; // recurse
mSCCPoint [ v ] . second = std : : min ( mSCCPoint [ v ] . second ,
mSCCPoint [ w ] . second ) ;
}
else
{
if ( find ( mSCCStack . begin ( ) , mSCCStack . end ( ) , w ) ! = mSCCStack . end ( ) )
mSCCPoint [ v ] . second = std : : min ( mSCCPoint [ v ] . second ,
mSCCPoint [ w ] . first ) ;
}
}
if ( mSCCPoint [ v ] . second = = mSCCPoint [ v ] . first )
{ // new component
do
{
w = mSCCStack . back ( ) ;
mSCCStack . pop_back ( ) ;
mGraph [ w ] . componentId = mSCCId ;
}
while ( w ! = v ) ;
mSCCId + + ;
}
return ;
}
/*
* mGraph contains the strongly connected component group id ' s along
* with pre - calculated edge costs .
*
* A cell can have disjointed pathgrids , e . g . Seyda Neen has 3
*
* mGraph for Seyda Neen will therefore have 3 different values . When
* selecting a random pathgrid point for AiWander , mGraph can be checked
* for quickly finding whether the destination is reachable .
*
* Otherwise , buildPath can automatically select a closest reachable end
* pathgrid point ( reachable from the closest start point ) .
*
* Using Tarjan ' s algorithm :
*
* mGraph | graph G |
* mSCCPoint | V | derived from mPoints
* mGraph [ v ] . edges | E ( for v ) |
* mSCCIndex | index | tracking smallest unused index
* mSCCStack | S |
* mGraph [ v ] . edges [ i ] . index | w |
*
*/
void PathgridGraph : : buildConnectedPoints ( )
{
// both of these are set to zero in the constructor
//mSCCId = 0; // how many strongly connected components in this cell
//mSCCIndex = 0;
int pointsSize = mPathgrid - > mPoints . size ( ) ;
mSCCPoint . resize ( pointsSize , std : : pair < int , int > ( - 1 , - 1 ) ) ;
mSCCStack . reserve ( pointsSize ) ;
for ( int v = 0 ; v < static_cast < int > ( pointsSize ) ; v + + )
{
if ( mSCCPoint [ v ] . first = = - 1 ) // undefined (haven't visited)
recursiveStrongConnect ( v ) ;
}
//#if 0
std : : cout < < " components: " < < std : : to_string ( mSCCId )
+ " , " + mPathgrid - > mCell
< < std : : endl ;
//#endif
}
bool PathgridGraph : : isPointConnected ( const int start , const int end ) const
{
return ( mGraph [ start ] . componentId = = mGraph [ end ] . componentId ) ;
}
/*
* NOTE : Based on buildPath2 ( ) , please check git history if interested
* Should consider using a 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
* that mGraph is already constructed .
*
* Should be possible to make this MT safe .
*
* Returns path ( a list of pathgrid point indexes ) which may be empty .
*
* Input params :
* start , goal - pathgrid point indexes ( for this cell )
* isExterior - used to determine whether to convert to world co - ordinates
*
* Variables :
* openset - point indexes to be traversed , lowest cost at the front
* closedset - point indexes already traversed
* gScore - past accumulated costs vector indexed by point index
* fScore - future estimated costs vector indexed by point index
*
* TODO : An intersting exercise might be to cache the paths created for a
* start / goal pair . To cache the results the paths need to be in
* pathgrid points form ( currently they are converted to world
* co - ordinates ) . Essentially trading speed w / memory .
*/
std : : list < ESM : : Pathgrid : : Point > PathgridGraph : : aStarSearch ( const int start ,
const int goal ,
bool isExterior ) const
{
std : : list < ESM : : Pathgrid : : Point > path ;
if ( ! isPointConnected ( start , goal ) )
{
return path ; // there is no path, return an empty path
}
int graphSize = mGraph . size ( ) ;
std : : vector < float > gScore ;
gScore . resize ( graphSize , - 1 ) ;
std : : vector < float > fScore ;
fScore . resize ( graphSize , - 1 ) ;
std : : vector < int > graphParent ;
graphParent . resize ( graphSize , - 1 ) ;
// gScore & fScore keep costs for each pathgrid point in mPoints
gScore [ start ] = 0 ;
fScore [ start ] = costAStar ( mPathgrid - > mPoints [ start ] , mPathgrid - > mPoints [ goal ] ) ;
std : : list < int > openset ;
std : : list < int > closedset ;
openset . push_back ( start ) ;
int current = - 1 ;
while ( ! openset . empty ( ) )
{
current = openset . front ( ) ; // front has the lowest cost
openset . pop_front ( ) ;
if ( current = = goal )
break ;
closedset . push_back ( current ) ; // remember we've been here
// check all edges for the current point index
for ( int j = 0 ; j < static_cast < int > ( mGraph [ current ] . edges . size ( ) ) ; j + + )
{
if ( std : : find ( closedset . begin ( ) , closedset . end ( ) , mGraph [ current ] . edges [ j ] . index ) = =
closedset . end ( ) )
{
// not in closedset - i.e. have not traversed this edge destination
int dest = mGraph [ current ] . edges [ j ] . index ;
float tentative_g = gScore [ current ] + mGraph [ current ] . edges [ j ] . cost ;
bool isInOpenSet = std : : find ( openset . begin ( ) , openset . end ( ) , dest ) ! = openset . end ( ) ;
if ( ! isInOpenSet
| | tentative_g < gScore [ dest ] )
{
graphParent [ dest ] = current ;
gScore [ dest ] = tentative_g ;
fScore [ dest ] = tentative_g + costAStar ( mPathgrid - > mPoints [ dest ] ,
mPathgrid - > mPoints [ goal ] ) ;
if ( ! isInOpenSet )
{
// add this edge to openset, lowest cost goes to the front
// TODO: if this causes performance problems a hash table may help
std : : list < int > : : iterator it = openset . begin ( ) ;
for ( it = openset . begin ( ) ; it ! = openset . end ( ) ; it + + )
{
if ( fScore [ * it ] > fScore [ dest ] )
break ;
}
openset . insert ( it , dest ) ;
}
}
} // if in closedset, i.e. traversed this edge already, try the next edge
}
}
if ( current ! = goal )
return path ; // for some reason couldn't build a path
// reconstruct path to return, using world co-ordinates
float xCell = 0 ;
float yCell = 0 ;
if ( isExterior )
{
xCell = mPathgrid - > mData . mX * ESM : : Land : : REAL_SIZE ;
yCell = mPathgrid - > mData . mY * ESM : : Land : : REAL_SIZE ;
}
//#if 0
// for debugging only
int tmp = current ;
if ( tmp ! = goal )
{
std : : cout < < " aStarSearch: goal and result differ " < < std : : endl ;
std : : cout < < " goal: " < < std : : to_string ( goal )
+ " , result: " + std : : to_string ( tmp )
< < std : : endl ;
}
std : : cout < < " start: " < < std : : to_string ( start )
+ " , goal: " + std : : to_string ( goal )
+ " , result: " + std : : to_string ( tmp )
< < std : : endl ;
//#endif
while ( graphParent [ current ] ! = - 1 )
{
ESM : : Pathgrid : : Point pt = mPathgrid - > mPoints [ current ] ;
//#if 0
// for debugging only
std : : cout < < " point: " + std : : to_string ( current )
+ " , X: " + std : : to_string ( pt . mX )
+ " , Y: " + std : : to_string ( pt . mY )
< < std : : endl ;
//#endif
pt . mX + = xCell ;
pt . mY + = yCell ;
path . push_front ( pt ) ;
current = graphParent [ current ] ;
}
return path ;
}
}
cc9cii
11 years ago