# Updated pathfinding to improve speed and ignore allied unit collisions
- Pulled out the A* implementation to a separate class to support reuse of code for the high level pathfinder - Cached the the open/closed status of nodes to improve speed - Added a maximum node limit before the search is cut off (DEFAULT_SEARCH_LIMIT in AStarEngine.cpp for now) - Allied unit collisions are now ignored This was SVN commit r3861.
This commit is contained in:
parent
03f21c772f
commit
68644fb42b
464
source/simulation/AStarEngine.cpp
Normal file
464
source/simulation/AStarEngine.cpp
Normal file
@ -0,0 +1,464 @@
|
||||
#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);
|
||||
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 )
|
||||
{
|
||||
CVector2D wloc = TilespaceToWorldspace(loc);
|
||||
CTerrain* pTerrain = g_Game->GetWorld()->GetTerrain();
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
164
source/simulation/AStarEngine.h
Normal file
164
source/simulation/AStarEngine.h
Normal file
@ -0,0 +1,164 @@
|
||||
#ifndef __ASTAR_ENGINE_H__
|
||||
#define __ASTAR_ENGINE_H__
|
||||
|
||||
#include "Vector2D.h"
|
||||
#include "Player.h"
|
||||
#include "lib/types.h"
|
||||
#include <queue>
|
||||
|
||||
class AStarNode
|
||||
{
|
||||
public:
|
||||
float f, g, h;
|
||||
AStarNode* parent;
|
||||
CVector2D coord;
|
||||
bool operator <(const AStarNode& rhs) const { return f<rhs.f; }
|
||||
bool equals(const AStarNode& rhs) const
|
||||
{
|
||||
return ( coord.x==rhs.coord.x ) && ( coord.y==rhs.coord.y );
|
||||
}
|
||||
};
|
||||
class AStarGoalBase;
|
||||
class AStarGoalLowLevel;
|
||||
struct AStarNodeComp
|
||||
{
|
||||
bool operator()(const AStarNode* n1, const AStarNode* n2) const
|
||||
{
|
||||
return (*n2) < (*n1);
|
||||
}
|
||||
};
|
||||
|
||||
class CVector2D_hash_compare
|
||||
{
|
||||
public:
|
||||
static const size_t bucket_size = 4;
|
||||
static const size_t min_buckets = 16;
|
||||
size_t operator() (const CVector2D& Key) const
|
||||
{
|
||||
return Key.x + Key.y*1024;
|
||||
}
|
||||
bool operator() (const CVector2D& _Key1, const CVector2D& _Key2) const
|
||||
{
|
||||
return (_Key1.x < _Key2.x) || (_Key1.x==_Key2.x && _Key1.y < _Key2.y);
|
||||
}
|
||||
};
|
||||
typedef STL_HASH_MAP<CVector2D, AStarNode*, CVector2D_hash_compare> ASNodeHashMap;
|
||||
|
||||
class PriQueue
|
||||
: public std::priority_queue<AStarNode*, std::vector<AStarNode*>, AStarNodeComp>
|
||||
{
|
||||
public:
|
||||
// Promote a node in the PQ, or if it doesn't exist, add it
|
||||
void promote(AStarNode* node);
|
||||
void clear();
|
||||
};
|
||||
|
||||
typedef unsigned char AStarNodeFlag;
|
||||
|
||||
class CAStarEngine
|
||||
{
|
||||
public:
|
||||
CAStarEngine();
|
||||
CAStarEngine(AStarGoalBase* goal);
|
||||
~CAStarEngine();
|
||||
|
||||
void setGoal(AStarGoalBase* goal) { mGoal = goal; }
|
||||
|
||||
bool findPath( const CVector2D& src, const CVector2D& dest, CPlayer* player=0 );
|
||||
std::vector<CVector2D> getLastPath();
|
||||
|
||||
// The maximum number of nodes that will be expanded before failure is declared
|
||||
void setSearchLimit( int limit );
|
||||
|
||||
protected:
|
||||
AStarGoalBase* mGoal;
|
||||
private:
|
||||
int mSearchLimit;
|
||||
bool mSolved;
|
||||
std::vector<CVector2D> mPath;
|
||||
AStarNodeFlag *mFlags;
|
||||
long mFlagArraySize;
|
||||
|
||||
ASNodeHashMap mClosed;
|
||||
PriQueue mOpen;
|
||||
|
||||
std::vector<AStarNode*> freeNodes;
|
||||
std::vector<AStarNode*> usedNodes;
|
||||
|
||||
// addToOpen will promote if the node already is in Open
|
||||
void addToOpen( AStarNode* );
|
||||
AStarNode* removeBestOpenNode();
|
||||
|
||||
void addToClosed( AStarNode* );
|
||||
void removeFromClosed( AStarNode* );
|
||||
AStarNode* getFromClosed( const CVector2D& );
|
||||
|
||||
void clearOpen();
|
||||
void clearClosed();
|
||||
|
||||
void constructPath( AStarNode* );
|
||||
|
||||
AStarNode* getFreeASNode();
|
||||
void cleanup();
|
||||
|
||||
inline AStarNodeFlag* GetFlag(const CVector2D&);
|
||||
inline bool GetIsClear(AStarNodeFlag*);
|
||||
inline bool GetIsClosed(AStarNodeFlag*);
|
||||
inline bool GetIsOpen(AStarNodeFlag*);
|
||||
inline void SetClosedFlag(AStarNodeFlag*);
|
||||
inline void ClearClosedFlag(AStarNodeFlag*);
|
||||
inline void SetOpenFlag(AStarNodeFlag*);
|
||||
inline void ClearOpenFlag(AStarNodeFlag*);
|
||||
inline bool GetIsPassable(AStarNodeFlag*);
|
||||
inline bool GetIsBlocked(AStarNodeFlag*);
|
||||
inline void SetPassableFlag(AStarNodeFlag*);
|
||||
inline void SetBlockedFlag(AStarNodeFlag*);
|
||||
|
||||
};
|
||||
|
||||
|
||||
class AStarGoalBase
|
||||
{
|
||||
public:
|
||||
AStarGoalBase() {}
|
||||
virtual void setDestination( const CVector2D& ) = 0;
|
||||
virtual float distanceToGoal( const CVector2D& ) = 0;
|
||||
virtual bool atGoal( const CVector2D& ) = 0;
|
||||
virtual float getTileCost( const CVector2D&, const CVector2D& ) = 0;
|
||||
virtual bool isPassable( const CVector2D&, CPlayer* player=0 ) = 0;
|
||||
virtual std::vector<CVector2D> getNeighbors( const CVector2D&, CPlayer* player=0 ) = 0;
|
||||
virtual CVector2D getCoord( const CVector2D& ) = 0;
|
||||
virtual CVector2D getTile( const CVector2D& ) = 0;
|
||||
};
|
||||
|
||||
class AStarGoalLowLevel : public AStarGoalBase
|
||||
{
|
||||
public:
|
||||
AStarGoalLowLevel() {}
|
||||
void setDestination( const CVector2D& dest );
|
||||
float distanceToGoal( const CVector2D& loc );
|
||||
bool atGoal( const CVector2D& loc );
|
||||
float getTileCost( const CVector2D& loc1, const CVector2D& loc2 );
|
||||
bool isPassable( const CVector2D& loc, CPlayer* player=0 );
|
||||
std::vector<CVector2D> getNeighbors( const CVector2D& loc, CPlayer* player=0 );
|
||||
CVector2D getCoord( const CVector2D& loc);
|
||||
CVector2D getTile( const CVector2D& loc);
|
||||
private:
|
||||
CVector2D coord;
|
||||
};
|
||||
|
||||
class CAStarEngineLowLevel : public CAStarEngine
|
||||
{
|
||||
public:
|
||||
CAStarEngineLowLevel()
|
||||
{
|
||||
mGoal = new AStarGoalLowLevel;
|
||||
}
|
||||
~CAStarEngineLowLevel()
|
||||
{
|
||||
delete mGoal;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif
|
@ -45,7 +45,7 @@ CEntity* GetCollisionObject( float x, float y )
|
||||
return( NULL );
|
||||
}
|
||||
|
||||
CBoundingObject* getCollisionObject( CBoundingObject* bounds )
|
||||
CBoundingObject* getCollisionObject( CBoundingObject* bounds, CPlayer* player )
|
||||
{
|
||||
std::vector<CEntity*> entities;
|
||||
g_EntityManager.GetInRange( bounds->m_pos.x, bounds->m_pos.y, COLLISION_RANGE, entities );
|
||||
@ -55,6 +55,9 @@ CBoundingObject* getCollisionObject( CBoundingObject* bounds )
|
||||
{
|
||||
if( !(*it)->m_bounds ) continue;
|
||||
if( (*it)->m_bounds == bounds ) continue;
|
||||
/* If the unit is marked to ignore ally collisions, and the player parameter
|
||||
is passed in and the same player as the unit, then ignore the (potential) collision */
|
||||
if( player && (*it)->m_passThroughAllies && (*it)->m_player == player ) continue;
|
||||
if( bounds->intersects( (*it)->m_bounds ) )
|
||||
{
|
||||
CBoundingObject* obj = (*it)->m_bounds;
|
||||
|
@ -34,7 +34,7 @@ typedef std::vector<CEntity*> RayIntersects;
|
||||
|
||||
HEntity getCollisionObject( CEntity* entity );
|
||||
HEntity getCollisionObject( CEntity* entity, float x, float y );
|
||||
CBoundingObject* getCollisionObject( CBoundingObject* bounds );
|
||||
CBoundingObject* getCollisionObject( CBoundingObject* bounds, CPlayer* player=0 );
|
||||
CBoundingObject* getContainingObject( const CVector2D& point );
|
||||
CEntity* GetCollisionObject( float x, float y ); // Point collision
|
||||
bool getRayIntersection( const CVector2D& source, const CVector2D& forward, const CVector2D& right, float length, float maxDistance, CBoundingObject* destinationCollisionObject, rayIntersectionResults* results );
|
||||
|
@ -1,155 +1,14 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
#include <queue>
|
||||
#include "Profile.h"
|
||||
|
||||
#include "EntityOrders.h"
|
||||
#include "Entity.h"
|
||||
#include "PathfindEngine.h"
|
||||
//#include "PathfindSparse.h"
|
||||
#include "ConfigDB.h"
|
||||
#include "Terrain.h"
|
||||
#include "Collision.h"
|
||||
|
||||
#include "ps/Game.h"
|
||||
#include "ps/World.h"
|
||||
|
||||
#define MAXSLOPE 3000
|
||||
#define INITNODES 1000
|
||||
|
||||
void processPath(HEntity, AStarNode*, bool);
|
||||
|
||||
class AStarNode
|
||||
{
|
||||
public:
|
||||
float f, g, h;
|
||||
AStarNode* parent;
|
||||
CVector2D coord;
|
||||
bool operator <(const AStarNode& rhs) const { return f<rhs.f; }
|
||||
bool equals(const AStarNode& rhs) const
|
||||
{
|
||||
return ( coord.x==rhs.coord.x ) && ( coord.y==rhs.coord.y );
|
||||
}
|
||||
};
|
||||
|
||||
struct AStarNodeComp
|
||||
{
|
||||
bool operator()(const AStarNode* n1, const AStarNode* n2) const
|
||||
{
|
||||
return (*n2) < (*n1);
|
||||
}
|
||||
};
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
bool isPassable( const CVector2D &wc )
|
||||
{
|
||||
CTerrain* pTerrain = g_Game->GetWorld()->GetTerrain();
|
||||
float slope = pTerrain->getSlope(wc.x, wc.y);
|
||||
if ( slope < MAXSLOPE )
|
||||
{
|
||||
// If no entity blocking, return true
|
||||
CBoundingBox bounds(wc.x, wc.y, 0, CELL_SIZE, CELL_SIZE, 3);
|
||||
if ( getCollisionObject(&bounds) == NULL )
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
class PriQueue
|
||||
: public std::priority_queue<AStarNode*, std::vector<AStarNode*>, AStarNodeComp>
|
||||
{
|
||||
public:
|
||||
// Promote a node in the PQ, or if it doesn't exist, add it
|
||||
void 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;
|
||||
}
|
||||
};
|
||||
|
||||
bool CPathfindEngine::isVisited( const CVector2D& coord )
|
||||
{
|
||||
ASNodeHashMap::iterator it = visited.find(coord);
|
||||
return ( it != visited.end() );
|
||||
}
|
||||
|
||||
std::vector<AStarNode*> CPathfindEngine::getNeighbors( AStarNode* node )
|
||||
{
|
||||
std::vector<AStarNode*> vec;
|
||||
|
||||
for( int xdiff = -1; xdiff <= 1; xdiff++ )
|
||||
{
|
||||
for( int ydiff = -1; ydiff <= 1; ydiff++ )
|
||||
{
|
||||
if ( xdiff!=0 || ydiff!=0 )
|
||||
{
|
||||
CVector2D coord = node->coord;
|
||||
coord.x += xdiff; coord.y += ydiff;
|
||||
if ( isVisited(coord) || isPassable(TilespaceToWorldspace(coord)) )
|
||||
{
|
||||
AStarNode* n = getFreeASNode();
|
||||
n->coord = coord;
|
||||
n->f = n->g = n->h = 0;
|
||||
n->parent = 0;
|
||||
vec.push_back(n);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return vec;
|
||||
}
|
||||
|
||||
CPathfindEngine::CPathfindEngine()
|
||||
{
|
||||
/* CConfigValue* sparseDepth = g_ConfigDB.GetValue( CFG_USER, "pathfind.sparse.recursiondepth" );
|
||||
if( sparseDepth )
|
||||
sparseDepth->GetInt( SPF_RECURSION_DEPTH ); */
|
||||
for(int i=0; i<INITNODES; i++)
|
||||
{
|
||||
freeNodes.push_back(new AStarNode);
|
||||
}
|
||||
}
|
||||
|
||||
CPathfindEngine::~CPathfindEngine()
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
void CPathfindEngine::requestPath( HEntity entity, const CVector2D& destination )
|
||||
@ -166,97 +25,32 @@ void CPathfindEngine::requestPath( HEntity entity, const CVector2D& destination
|
||||
|
||||
void CPathfindEngine::requestLowLevelPath( HEntity entity, const CVector2D& destination, bool contact )
|
||||
{
|
||||
/* TODO: Pull out code into AStarEngine */
|
||||
/* TODO: Put a limit on the search space to prevent unreachable destinations from
|
||||
eating the CPU */
|
||||
PROFILE_START("Pathfinding");
|
||||
|
||||
CVector2D source( entity->m_position.X, entity->m_position.Z );
|
||||
|
||||
// If the goal is unreachable, move it towards the start until it is reachable
|
||||
CVector2D goalLoc = destination;
|
||||
CVector2D unitVec = (entity->m_position - goalLoc);
|
||||
unitVec= unitVec.normalize() * CELL_SIZE / 2;
|
||||
while( !isPassable(goalLoc) )
|
||||
if ( mLowPathfinder.findPath(source, destination, entity->m_player) )
|
||||
{
|
||||
goalLoc += unitVec;
|
||||
}
|
||||
|
||||
// Initialize priority queue (PQ) and visited list (V)
|
||||
visited.clear();
|
||||
PriQueue priQ;
|
||||
|
||||
// Construct a dummy node for the goal
|
||||
AStarNode* goal = getFreeASNode();
|
||||
goal->coord = WorldspaceToTilespace(goalLoc);
|
||||
goal->parent = NULL;
|
||||
goal->f = goal->g = goal->h = 0;
|
||||
|
||||
// Assign f,g,h to start location, add to PQ
|
||||
AStarNode* start = getFreeASNode();
|
||||
start->coord = WorldspaceToTilespace(source);
|
||||
start->g = 0;
|
||||
start->f = start->h = (goal->coord-start->coord).length();
|
||||
start->parent = NULL;
|
||||
priQ.push(start);
|
||||
visited[start->coord] = start;
|
||||
|
||||
// Loop until PQ is empty
|
||||
while(!priQ.empty())
|
||||
{
|
||||
// Select best cost node, B, from PQ
|
||||
AStarNode* best = priQ.top();
|
||||
priQ.pop();
|
||||
// If B is the goal, we are done, and found a path
|
||||
if ( best->equals( *goal ) )
|
||||
std::vector<CVector2D> path = mLowPathfinder.getLastPath();
|
||||
std::vector<CVector2D>::iterator it;
|
||||
CEntityOrder node;
|
||||
for( it = path.begin(); (it+1) != path.end(); it++ )
|
||||
{
|
||||
goal->parent = best;
|
||||
goal->g = goal->f = best->g + 1;
|
||||
break;
|
||||
}
|
||||
|
||||
std::vector<AStarNode*> neighbors = getNeighbors(best);
|
||||
// For each neighbor, C, of B
|
||||
std::vector<AStarNode*>::iterator it;
|
||||
for( it = neighbors.begin(); it != neighbors.end(); it++ )
|
||||
{
|
||||
AStarNode* C = *it;
|
||||
// Assign f,g,h to C
|
||||
C->g = best->g + 1;
|
||||
// Penalize for non-straight paths
|
||||
if ( best->parent )
|
||||
if ( !contact )
|
||||
{
|
||||
int dx1 = C->coord.x - best->coord.x;
|
||||
int dy1 = C->coord.y - best->coord.y;
|
||||
int dx2 = best->coord.x - best->parent->coord.x;
|
||||
int dy2 = best->coord.y - best->parent->coord.y;
|
||||
if ( ((dx1 - dx2) + (dy1 - dy2)) != 0 )
|
||||
{
|
||||
C->g += 0.1f;
|
||||
}
|
||||
node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
|
||||
}
|
||||
|
||||
C->h = ((goal->coord) - (C->coord)).length();
|
||||
C->f = C->g + C->h;
|
||||
C->parent = best;
|
||||
|
||||
// If C not in V, add C to V and PQ
|
||||
// If the f of C is less than the f of C in the PQ, promote C in PQ and update V
|
||||
ASNodeHashMap::iterator it2 = visited.find(C->coord);
|
||||
if ( it2 != visited.end() && (C->f < it2->second->f) )
|
||||
else
|
||||
{
|
||||
it2->second = C;
|
||||
priQ.promote(C);
|
||||
}
|
||||
else if ( it2 == visited.end() )
|
||||
{
|
||||
visited[C->coord] = C;
|
||||
priQ.push(C);
|
||||
// TODO: Is this right?
|
||||
node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
|
||||
}
|
||||
node.m_data[0].location = *it;
|
||||
entity->m_orderQueue.push_back(node);
|
||||
}
|
||||
}
|
||||
|
||||
if ( goal->parent )
|
||||
{
|
||||
processPath(entity, goal, contact);
|
||||
node.m_type = CEntityOrder::ORDER_PATH_END_MARKER;
|
||||
node.m_data[0].location = *it;
|
||||
entity->m_orderQueue.push_back(node);
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -264,7 +58,7 @@ void CPathfindEngine::requestLowLevelPath( HEntity entity, const CVector2D& dest
|
||||
// TODO: Figure out what to do in this case
|
||||
}
|
||||
|
||||
cleanup();
|
||||
PROFILE_END("Pathfinding");
|
||||
}
|
||||
|
||||
void CPathfindEngine::requestContactPath( HEntity entity, CEntityOrder* current )
|
||||
@ -289,57 +83,3 @@ void CPathfindEngine::requestContactPath( HEntity entity, CEntityOrder* current
|
||||
// }
|
||||
//}
|
||||
}
|
||||
|
||||
AStarNode* CPathfindEngine::getFreeASNode()
|
||||
{
|
||||
AStarNode* ret;
|
||||
if (!freeNodes.empty())
|
||||
{
|
||||
ret = freeNodes.back();
|
||||
freeNodes.pop_back();
|
||||
}
|
||||
else
|
||||
{
|
||||
ret = new AStarNode;
|
||||
}
|
||||
usedNodes.push_back(ret);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void CPathfindEngine::cleanup()
|
||||
{
|
||||
std::vector<AStarNode*>::iterator it;
|
||||
for( it = usedNodes.begin(); it != usedNodes.end(); it++)
|
||||
{
|
||||
freeNodes.push_back(*it);
|
||||
}
|
||||
|
||||
usedNodes.clear();
|
||||
}
|
||||
|
||||
void processPath(HEntity entity, AStarNode* goal, bool contact)
|
||||
{
|
||||
AStarNode* current = goal;
|
||||
|
||||
CEntityOrder node;
|
||||
node.m_type = CEntityOrder::ORDER_PATH_END_MARKER;
|
||||
entity->m_orderQueue.push_front( node );
|
||||
|
||||
/* TODO: Smoothing for units with a turning radius */
|
||||
while( current != NULL && current->g != 0 )
|
||||
{
|
||||
if ( !contact )
|
||||
{
|
||||
node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
|
||||
}
|
||||
else
|
||||
{
|
||||
// TODO: Is this right?
|
||||
node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
|
||||
}
|
||||
node.m_data[0].location = TilespaceToWorldspace(current->coord);
|
||||
entity->m_orderQueue.push_front( node );
|
||||
current = current->parent;
|
||||
}
|
||||
}
|
@ -14,9 +14,9 @@
|
||||
#include "Singleton.h"
|
||||
#include "EntityHandles.h"
|
||||
#include "Vector2D.h"
|
||||
#include "AStarEngine.h"
|
||||
|
||||
#define g_Pathfinder CPathfindEngine::GetSingleton()
|
||||
#define MAXSIZE 1024
|
||||
|
||||
class CEntityOrder;
|
||||
|
||||
@ -26,40 +26,16 @@ enum EPathType
|
||||
PF_ATTACK_MELEE,
|
||||
};
|
||||
|
||||
class AStarNode;
|
||||
class CVector2D_hash_compare
|
||||
{
|
||||
public:
|
||||
static const size_t bucket_size = 4;
|
||||
static const size_t min_buckets = 16;
|
||||
size_t operator() (const CVector2D& Key) const
|
||||
{
|
||||
return Key.x + Key.y*MAXSIZE;
|
||||
}
|
||||
bool operator() (const CVector2D& _Key1, const CVector2D& _Key2) const
|
||||
{
|
||||
return (_Key1.x < _Key2.x) || (_Key1.x==_Key2.x && _Key1.y < _Key2.y);
|
||||
}
|
||||
};
|
||||
typedef STL_HASH_MAP<CVector2D, AStarNode*, CVector2D_hash_compare> ASNodeHashMap;
|
||||
|
||||
class CPathfindEngine : public Singleton<CPathfindEngine>
|
||||
{
|
||||
public:
|
||||
CPathfindEngine();
|
||||
~CPathfindEngine();
|
||||
void requestPath( HEntity entity, const CVector2D& destination );
|
||||
void requestLowLevelPath( HEntity entity, const CVector2D& destination, bool contact );
|
||||
void requestContactPath( HEntity entity, CEntityOrder* current );
|
||||
private:
|
||||
std::vector<AStarNode*> getNeighbors( AStarNode* node );
|
||||
bool isVisited( const CVector2D& coord );
|
||||
AStarNode* getFreeASNode();
|
||||
void cleanup();
|
||||
|
||||
std::vector<AStarNode*> freeNodes;
|
||||
std::vector<AStarNode*> usedNodes;
|
||||
ASNodeHashMap visited;
|
||||
CAStarEngineLowLevel mLowPathfinder;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
Loading…
Reference in New Issue
Block a user