2006-05-13 20:50:58 +02:00
|
|
|
#include "precompiled.h"
|
|
|
|
|
|
|
|
#include "AStarEngine.h"
|
|
|
|
|
|
|
|
/* For AStarGoalLowLevel isPassable/cost */
|
|
|
|
#include "Collision.h"
|
|
|
|
#include "ps/Game.h"
|
|
|
|
#include "ps/World.h"
|
2006-06-09 18:44:16 +02:00
|
|
|
#include "graphics/Patch.h"
|
|
|
|
#include "graphics/Terrain.h"
|
2006-05-13 20:50:58 +02:00
|
|
|
|
2006-06-02 04:10:27 +02:00
|
|
|
#include "ps/Profile.h"
|
2006-05-13 20:50:58 +02:00
|
|
|
|
|
|
|
#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()
|
|
|
|
{
|
2006-06-01 08:04:32 +02:00
|
|
|
delete[] mFlags;
|
2006-05-13 20:50:58 +02:00
|
|
|
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");
|
2006-06-01 05:26:51 +02:00
|
|
|
memset(mFlags, 0, mFlagArraySize*mFlagArraySize*sizeof(AStarNodeFlag));
|
2006-05-13 20:50:58 +02:00
|
|
|
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();
|
2006-06-01 08:04:32 +02:00
|
|
|
int size = pTerrain->GetTilesPerSide();
|
|
|
|
|
|
|
|
if( loc.x<0 || loc.y<0 || loc.x>=size || loc.y>=size )
|
|
|
|
{
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
CVector2D wloc = TilespaceToWorldspace(loc);
|
2006-05-13 20:50:58 +02:00
|
|
|
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)
|
|
|
|
{
|
2006-06-01 08:04:32 +02:00
|
|
|
debug_assert(loc.x>=0 && loc.y>=0 && loc.x<mFlagArraySize && loc.y<mFlagArraySize);
|
|
|
|
return mFlags + (mFlagArraySize * (long)loc.x + (long)loc.y);
|
2006-05-13 20:50:58 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
}
|