forked from 0ad/0ad
Matei
bde2a0b0c7
The pathfinder was pushing locations with negative coordinates (or coordinates larger than the map size) onto the queue, which led to memory corruption in CAStarEngine's node array and later a crash in its destructor. This was SVN commit r3923.
472 lines
9.3 KiB
C++
472 lines
9.3 KiB
C++
#include "precompiled.h"
|
|
|
|
#include "AStarEngine.h"
|
|
|
|
/* For AStarGoalLowLevel isPassable/cost */
|
|
#include "Collision.h"
|
|
#include "ps/Game.h"
|
|
#include "ps/World.h"
|
|
|
|
#include "Profile.h"
|
|
|
|
#define DEFAULT_SEARCH_LIMIT 1000
|
|
#define DEFAULT_INIT_NODES 1000
|
|
// TODO: do this for real
|
|
#define MAXSLOPE 3500
|
|
|
|
|
|
// Node status flags
|
|
enum
|
|
{
|
|
kClear = 0x00, // empty, unexamined
|
|
kPassable = 0x01, // examined, not blocked
|
|
kBlocked = 0x02, // examined, blocked
|
|
kOpen = 0x04, // on the open list
|
|
kClosed = 0x08 // on the closed list
|
|
};
|
|
|
|
|
|
CAStarEngine::CAStarEngine()
|
|
{
|
|
mSearchLimit = DEFAULT_SEARCH_LIMIT;
|
|
for(int i=0; i<DEFAULT_INIT_NODES; i++)
|
|
{
|
|
freeNodes.push_back(new AStarNode);
|
|
}
|
|
mFlagArraySize = 300;
|
|
mFlags = new AStarNodeFlag[mFlagArraySize*mFlagArraySize];
|
|
memset(mFlags, 0, mFlagArraySize*mFlagArraySize*sizeof(AStarNodeFlag));
|
|
}
|
|
|
|
CAStarEngine::CAStarEngine(AStarGoalBase *goal)
|
|
{
|
|
CAStarEngine();
|
|
mGoal = goal;
|
|
}
|
|
|
|
|
|
CAStarEngine::~CAStarEngine()
|
|
{
|
|
delete[] mFlags;
|
|
std::vector<AStarNode*>::iterator it;
|
|
for( it = usedNodes.begin(); it != usedNodes.end(); it++)
|
|
{
|
|
delete (*it);
|
|
}
|
|
for( it = freeNodes.begin(); it != freeNodes.end(); it++)
|
|
{
|
|
delete (*it);
|
|
}
|
|
}
|
|
|
|
|
|
bool CAStarEngine::findPath(
|
|
const CVector2D &src, const CVector2D &dest, CPlayer* player )
|
|
{
|
|
mSolved = false;
|
|
int iterations = 0;
|
|
|
|
mGoal->setDestination(dest);
|
|
|
|
AStarNode *start = getFreeASNode();
|
|
start->coord = mGoal->getTile(src);
|
|
start->parent = NULL;
|
|
start->g = 0;
|
|
start->f = start->h = mGoal->distanceToGoal(start->coord);
|
|
|
|
clearOpen();
|
|
clearClosed();
|
|
PROFILE_START("memset cache");
|
|
memset(mFlags, 0, mFlagArraySize*mFlagArraySize*sizeof(AStarNodeFlag));
|
|
PROFILE_END("memset cache");
|
|
|
|
addToOpen(start);
|
|
|
|
AStarNode *best = NULL;
|
|
|
|
while( iterations<mSearchLimit && (best = removeBestOpenNode()) != NULL )
|
|
{
|
|
iterations++;
|
|
PROFILE_START("addToClosed");
|
|
addToClosed(best);
|
|
PROFILE_END("addToClosed");
|
|
if ( mGoal->atGoal(best->coord) )
|
|
{
|
|
/* Path solved */
|
|
mSolved = true;
|
|
break;
|
|
}
|
|
|
|
/* Get neighbors of the best node */
|
|
std::vector<CVector2D> neighbors;
|
|
PROFILE_START("get neighbors");
|
|
neighbors = mGoal->getNeighbors(best->coord, player);
|
|
PROFILE_END("get neighbors");
|
|
|
|
/* Update the neighbors of the best node */
|
|
std::vector<CVector2D>::iterator it;
|
|
for( it = neighbors.begin(); it != neighbors.end(); it++ )
|
|
{
|
|
AStarNode* N = getFreeASNode();
|
|
PROFILE_START("initialize neighbor");
|
|
N->coord = *it;
|
|
// Assign f,g,h to neighbor
|
|
N->g = best->g + mGoal->getTileCost(best->coord, N->coord);
|
|
N->h = mGoal->distanceToGoal(*it);
|
|
N->f = N->g + N->h;
|
|
N->parent = best;
|
|
PROFILE_END("initialize neighbor");
|
|
|
|
AStarNode* C;
|
|
PROFILE_START("search closed");
|
|
C = getFromClosed(N->coord);
|
|
PROFILE_END("search closed");
|
|
bool update = false;
|
|
if( C!=NULL && (N->f < C->f) )
|
|
{
|
|
PROFILE_START("remove from closed");
|
|
/* N is on Closed and N->f is better */
|
|
removeFromClosed(C);
|
|
update = true;
|
|
PROFILE_END("remove from closed");
|
|
}
|
|
if (C==NULL || update)
|
|
{
|
|
PROFILE_START("add to open");
|
|
/* N is not on Closed */
|
|
addToOpen(N);
|
|
PROFILE_END("add to open");
|
|
}
|
|
}
|
|
}
|
|
|
|
if (mSolved && best!=NULL)
|
|
{
|
|
debug_printf("Number of nodes searched: %d\n", iterations);
|
|
constructPath(best);
|
|
}
|
|
|
|
cleanup();
|
|
|
|
return mSolved;
|
|
}
|
|
|
|
|
|
void CAStarEngine::constructPath( AStarNode* end )
|
|
{
|
|
std::deque<CVector2D> path;
|
|
mPath.clear();
|
|
while( end!=NULL && (end->parent)!=NULL )
|
|
{
|
|
path.push_front(mGoal->getCoord(end->coord));
|
|
end = end->parent;
|
|
}
|
|
mPath.insert(mPath.begin(), path.begin(), path.end());
|
|
}
|
|
|
|
|
|
std::vector<CVector2D> CAStarEngine::getLastPath()
|
|
{
|
|
return mPath;
|
|
}
|
|
|
|
|
|
void CAStarEngine::setSearchLimit( int limit )
|
|
{
|
|
mSearchLimit = limit;
|
|
}
|
|
|
|
|
|
void CAStarEngine::addToOpen( AStarNode* node )
|
|
{
|
|
/* If not in open, should add, otherwise should promote */
|
|
AStarNodeFlag *flag = GetFlag(node->coord);
|
|
if (!GetIsOpen(flag))
|
|
{
|
|
mOpen.push(node);
|
|
}
|
|
else
|
|
{
|
|
mOpen.promote(node);
|
|
}
|
|
SetOpenFlag(flag);
|
|
}
|
|
|
|
|
|
AStarNode* CAStarEngine::removeBestOpenNode()
|
|
{
|
|
if (mOpen.empty())
|
|
return NULL;
|
|
AStarNode* top;
|
|
PROFILE_START("remove from open");
|
|
top = mOpen.top();
|
|
mOpen.pop();
|
|
ClearOpenFlag(GetFlag(top->coord));
|
|
PROFILE_END("remove from open");
|
|
return top;
|
|
}
|
|
|
|
|
|
void CAStarEngine::addToClosed( AStarNode* node )
|
|
{
|
|
mClosed[node->coord] = node;
|
|
SetClosedFlag(GetFlag(node->coord));
|
|
}
|
|
|
|
|
|
void CAStarEngine::removeFromClosed( AStarNode* node )
|
|
{
|
|
mClosed.erase(node->coord);
|
|
ClearClosedFlag(GetFlag(node->coord));
|
|
}
|
|
|
|
|
|
AStarNode* CAStarEngine::getFromClosed( const CVector2D& loc )
|
|
{
|
|
if (!GetIsClosed(GetFlag(loc)))
|
|
{
|
|
return NULL;
|
|
}
|
|
ASNodeHashMap::iterator it = mClosed.find(loc);
|
|
return ( it != mClosed.end() ) ? (it->second) : (NULL);
|
|
}
|
|
|
|
|
|
void CAStarEngine::clearOpen()
|
|
{
|
|
mOpen.clear();
|
|
}
|
|
|
|
|
|
void CAStarEngine::clearClosed()
|
|
{
|
|
mClosed.clear();
|
|
}
|
|
|
|
|
|
AStarNode* CAStarEngine::getFreeASNode()
|
|
{
|
|
AStarNode* ret;
|
|
PROFILE_START("allocator");
|
|
if (!freeNodes.empty())
|
|
{
|
|
ret = freeNodes.back();
|
|
freeNodes.pop_back();
|
|
}
|
|
else
|
|
{
|
|
ret = new AStarNode;
|
|
}
|
|
usedNodes.push_back(ret);
|
|
PROFILE_END("allocator");
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
void CAStarEngine::cleanup()
|
|
{
|
|
std::vector<AStarNode*>::iterator it;
|
|
for( it = usedNodes.begin(); it != usedNodes.end(); it++)
|
|
{
|
|
freeNodes.push_back(*it);
|
|
}
|
|
|
|
usedNodes.clear();
|
|
}
|
|
|
|
void PriQueue::promote( AStarNode *node )
|
|
{
|
|
if (node == NULL)
|
|
{
|
|
return;
|
|
}
|
|
|
|
std::vector<AStarNode*>::iterator ind, first;
|
|
for( ind = c.begin(); ind!=c.end() && !((*ind)->equals(*node)); ind++ );
|
|
if (ind == c.end())
|
|
{
|
|
push(node);
|
|
return;
|
|
}
|
|
if( (*ind)->f <= node->f ) return;
|
|
|
|
first = c.begin();
|
|
int index = ind-first;
|
|
int parent = (index - 1)/2;
|
|
|
|
while ( index>0 && (*(first+parent))->f > node->f )
|
|
{
|
|
*(first+index) = *(first+parent);
|
|
index = parent;
|
|
parent = (parent - 1)/2;
|
|
}
|
|
*(first+index) = node;
|
|
}
|
|
|
|
void PriQueue::clear()
|
|
{
|
|
c.clear();
|
|
}
|
|
|
|
CVector2D TilespaceToWorldspace( const CVector2D &ts )
|
|
{
|
|
return CVector2D(ts.x*CELL_SIZE+CELL_SIZE/2, ts.y*CELL_SIZE+CELL_SIZE/2);
|
|
}
|
|
|
|
CVector2D WorldspaceToTilespace( const CVector2D &ws )
|
|
{
|
|
return CVector2D(floor(ws.x/CELL_SIZE), floor(ws.y/CELL_SIZE));
|
|
}
|
|
|
|
|
|
void AStarGoalLowLevel::setDestination( const CVector2D &dest )
|
|
{
|
|
coord = WorldspaceToTilespace(dest);
|
|
}
|
|
|
|
float AStarGoalLowLevel::distanceToGoal( const CVector2D &loc )
|
|
{
|
|
return 3*((coord-loc).length());
|
|
}
|
|
|
|
bool AStarGoalLowLevel::atGoal( const CVector2D &loc )
|
|
{
|
|
return (coord.x == loc.x) && (coord.y == loc.y);
|
|
}
|
|
|
|
float AStarGoalLowLevel::getTileCost( const CVector2D& loc1, const CVector2D& loc2 )
|
|
{
|
|
return (loc2-loc1).length();
|
|
}
|
|
|
|
bool AStarGoalLowLevel::isPassable( const CVector2D &loc, CPlayer* player )
|
|
{
|
|
CTerrain* pTerrain = g_Game->GetWorld()->GetTerrain();
|
|
int size = pTerrain->GetTilesPerSide();
|
|
|
|
if( loc.x<0 || loc.y<0 || loc.x>=size || loc.y>=size )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
CVector2D wloc = TilespaceToWorldspace(loc);
|
|
float slope = pTerrain->getSlope(wloc.x, wloc.y);
|
|
if ( slope < MAXSLOPE )
|
|
{
|
|
// If no entity blocking, return true
|
|
CBoundingBox bounds(wloc.x, wloc.y, 0, CELL_SIZE, CELL_SIZE, 3);
|
|
if ( getCollisionObject(&bounds, player) == NULL )
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
CVector2D AStarGoalLowLevel::getCoord( const CVector2D &loc )
|
|
{
|
|
return TilespaceToWorldspace(loc);
|
|
}
|
|
|
|
CVector2D AStarGoalLowLevel::getTile( const CVector2D &loc )
|
|
{
|
|
return WorldspaceToTilespace(loc);
|
|
}
|
|
|
|
std::vector<CVector2D> AStarGoalLowLevel::getNeighbors( const CVector2D &loc, CPlayer* player )
|
|
{
|
|
std::vector<CVector2D> vec;
|
|
|
|
for( int xdiff = -1; xdiff <= 1; xdiff++ )
|
|
{
|
|
for( int ydiff = -1; ydiff <= 1; ydiff++ )
|
|
{
|
|
if ( xdiff!=0 || ydiff!=0 )
|
|
{
|
|
CVector2D c = loc;
|
|
c.x += xdiff; c.y += ydiff;
|
|
if ( isPassable(c, player) )
|
|
{
|
|
vec.push_back(c);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return vec;
|
|
}
|
|
|
|
|
|
|
|
inline AStarNodeFlag* CAStarEngine::GetFlag(const CVector2D &loc)
|
|
{
|
|
debug_assert(loc.x>=0 && loc.y>=0 && loc.x<mFlagArraySize && loc.y<mFlagArraySize);
|
|
return mFlags + (mFlagArraySize * (long)loc.x + (long)loc.y);
|
|
}
|
|
|
|
|
|
inline bool CAStarEngine::GetIsClear(AStarNodeFlag* flag)
|
|
{
|
|
return (*flag)==kClear;
|
|
}
|
|
|
|
|
|
inline bool CAStarEngine::GetIsClosed(AStarNodeFlag* flag)
|
|
{
|
|
return ((*flag) & kClosed) != kClear;
|
|
}
|
|
|
|
|
|
inline bool CAStarEngine::GetIsOpen(AStarNodeFlag* flag)
|
|
{
|
|
return ((*flag) & kOpen) != kClear;
|
|
}
|
|
|
|
|
|
inline void CAStarEngine::SetClosedFlag(AStarNodeFlag* flag)
|
|
{
|
|
*flag |= kClosed;
|
|
}
|
|
|
|
|
|
inline void CAStarEngine::SetOpenFlag(AStarNodeFlag* flag)
|
|
{
|
|
*flag |= kOpen;
|
|
}
|
|
|
|
|
|
inline void CAStarEngine::ClearClosedFlag(AStarNodeFlag* flag)
|
|
{
|
|
*flag &= -kClosed;
|
|
}
|
|
|
|
|
|
inline void CAStarEngine::ClearOpenFlag(AStarNodeFlag* flag)
|
|
{
|
|
*flag &= -kOpen;
|
|
}
|
|
|
|
|
|
inline bool CAStarEngine::GetIsPassable(AStarNodeFlag* flag)
|
|
{
|
|
return ((*flag) & kPassable) != kClear;
|
|
}
|
|
|
|
|
|
inline bool CAStarEngine::GetIsBlocked(AStarNodeFlag* flag)
|
|
{
|
|
return ((*flag) & kBlocked) != kClear;
|
|
}
|
|
|
|
|
|
inline void CAStarEngine::SetPassableFlag(AStarNodeFlag* flag)
|
|
{
|
|
*flag |= kPassable;
|
|
}
|
|
|
|
|
|
inline void CAStarEngine::SetBlockedFlag(AStarNodeFlag* flag)
|
|
{
|
|
*flag |= kBlocked;
|
|
} |