# First iteration of low-level pathfinder
PathfindSparse replaced with an A* implementation. Low-level pathfinding done on the tiles of the map, high level pathfinding is currently a placeholder, and only gives the final waypoint. This was SVN commit r3747.
This commit is contained in:
parent
7e6fd7f199
commit
bd431d25f6
@ -472,7 +472,12 @@ void CEntity::update( size_t timestep )
|
||||
updateCollisionPatch();
|
||||
return;
|
||||
case CEntityOrder::ORDER_GOTO_WAYPOINT:
|
||||
if ( processGotoWaypoint( current, timestep ) )
|
||||
if ( processGotoWaypoint( current, timestep, false ) )
|
||||
break;
|
||||
updateCollisionPatch();
|
||||
return;
|
||||
case CEntityOrder::ORDER_GOTO_WAYPOINT_CONTACT:
|
||||
if ( processGotoWaypoint( current, timestep, true ) )
|
||||
break;
|
||||
updateCollisionPatch();
|
||||
return;
|
||||
|
@ -225,7 +225,7 @@ private:
|
||||
|
||||
bool processGotoNoPathing( CEntityOrder* current, size_t timestep_milli );
|
||||
bool processGoto( CEntityOrder* current, size_t timestep_milli );
|
||||
bool processGotoWaypoint( CEntityOrder* current, size_t timestep_milli );
|
||||
bool processGotoWaypoint( CEntityOrder* current, size_t timestep_milli, bool contact );
|
||||
|
||||
bool processPatrol( CEntityOrder* current, size_t timestep_milli );
|
||||
|
||||
|
@ -86,6 +86,7 @@ public:
|
||||
ORDER_GOTO_SMOOTHED,
|
||||
ORDER_GOTO_COLLISION,
|
||||
ORDER_GOTO_WAYPOINT,
|
||||
ORDER_GOTO_WAYPOINT_CONTACT,
|
||||
ORDER_GOTO,
|
||||
ORDER_RUN,
|
||||
ORDER_PATROL,
|
||||
|
@ -592,7 +592,7 @@ bool CEntity::processGoto( CEntityOrder* current, size_t UNUSED(timestep_millis)
|
||||
return( true );
|
||||
}
|
||||
|
||||
bool CEntity::processGotoWaypoint( CEntityOrder* current, size_t UNUSED(timestep_milli) )
|
||||
bool CEntity::processGotoWaypoint( CEntityOrder* current, size_t UNUSED(timestep_milli), bool contact )
|
||||
{
|
||||
CVector2D pos( m_position.X, m_position.Z );
|
||||
CVector2D path_to = current->m_data[0].location;
|
||||
@ -607,7 +607,9 @@ bool CEntity::processGotoWaypoint( CEntityOrder* current, size_t UNUSED(timestep
|
||||
return( false );
|
||||
}
|
||||
|
||||
g_Pathfinder.requestLowLevelPath( me, path_to );
|
||||
processChooseMovement( Distance );
|
||||
|
||||
g_Pathfinder.requestLowLevelPath( me, path_to, contact );
|
||||
|
||||
return( true );
|
||||
}
|
||||
|
@ -1,19 +1,157 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
#include "PathfindEngine.h"
|
||||
#include "PathfindSparse.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" );
|
||||
/* CConfigValue* sparseDepth = g_ConfigDB.GetValue( CFG_USER, "pathfind.sparse.recursiondepth" );
|
||||
if( sparseDepth )
|
||||
sparseDepth->GetInt( SPF_RECURSION_DEPTH );
|
||||
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 )
|
||||
{
|
||||
// pathSparse( entity, destination );
|
||||
/* TODO: Add code to generate high level path
|
||||
For now, just the one high level waypoint to the final
|
||||
destination is added
|
||||
@ -24,25 +162,182 @@ void CPathfindEngine::requestPath( HEntity entity, const CVector2D& destination
|
||||
entity->m_orderQueue.push_front( waypoint );
|
||||
}
|
||||
|
||||
void CPathfindEngine::requestLowLevelPath( HEntity entity, const CVector2D& destination )
|
||||
void CPathfindEngine::requestLowLevelPath( HEntity entity, const CVector2D& destination, bool contact )
|
||||
{
|
||||
/* Temporary - will be replaced by low-level A* */
|
||||
pathSparse( entity, destination );
|
||||
/* TODO: Pull out code into AStarEngine */
|
||||
/* TODO: Put a limit on the search space to prevent unreachable destinations from
|
||||
eating the CPU */
|
||||
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) )
|
||||
{
|
||||
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 ) )
|
||||
{
|
||||
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 )
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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) )
|
||||
{
|
||||
it2->second = C;
|
||||
priQ.promote(C);
|
||||
}
|
||||
else if ( it2 == visited.end() )
|
||||
{
|
||||
visited[C->coord] = C;
|
||||
priQ.push(C);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ( goal->parent )
|
||||
{
|
||||
processPath(entity, goal, contact);
|
||||
}
|
||||
else
|
||||
{
|
||||
// If no path was found, then unsolvable
|
||||
// TODO: Figure out what to do in this case
|
||||
}
|
||||
|
||||
cleanup();
|
||||
}
|
||||
|
||||
void CPathfindEngine::requestContactPath( HEntity entity, CEntityOrder* current )
|
||||
{
|
||||
pathSparse( entity, current->m_data[0].entity->m_position );
|
||||
// For attack orders, do some additional postprocessing (replace goto/nopathing
|
||||
// with attack/nopathing, up until the attack order marker)
|
||||
std::deque<CEntityOrder>::iterator it;
|
||||
for( it = entity->m_orderQueue.begin(); it != entity->m_orderQueue.end(); it++ )
|
||||
{
|
||||
if( it->m_type == CEntityOrder::ORDER_PATH_END_MARKER )
|
||||
break;
|
||||
if( it->m_type == CEntityOrder::ORDER_GOTO_NOPATHING )
|
||||
{
|
||||
*it = *current;
|
||||
}
|
||||
}
|
||||
/* TODO: Same as non-contact: need high-level planner */
|
||||
CEntityOrder waypoint;
|
||||
waypoint.m_type = CEntityOrder::ORDER_GOTO_WAYPOINT_CONTACT;
|
||||
waypoint.m_data[0].location = current->m_data[0].entity->m_position;
|
||||
entity->m_orderQueue.push_front( waypoint );
|
||||
|
||||
//pathSparse( entity, current->m_data[0].entity->m_position );
|
||||
//// For attack orders, do some additional postprocessing (replace goto/nopathing
|
||||
//// with attack/nopathing, up until the attack order marker)
|
||||
//std::deque<CEntityOrder>::iterator it;
|
||||
//for( it = entity->m_orderQueue.begin(); it != entity->m_orderQueue.end(); it++ )
|
||||
//{
|
||||
// if( it->m_type == CEntityOrder::ORDER_PATH_END_MARKER )
|
||||
// break;
|
||||
// if( it->m_type == CEntityOrder::ORDER_GOTO_NOPATHING )
|
||||
// {
|
||||
// *it = *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;
|
||||
}
|
||||
}
|
@ -16,6 +16,7 @@
|
||||
#include "Vector2D.h"
|
||||
|
||||
#define g_Pathfinder CPathfindEngine::GetSingleton()
|
||||
#define MAXSIZE 1024
|
||||
|
||||
class CEntityOrder;
|
||||
|
||||
@ -25,13 +26,40 @@ 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 );
|
||||
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;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
Loading…
Reference in New Issue
Block a user