forked from 0ad/0ad
# 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();
|
updateCollisionPatch();
|
||||||
return;
|
return;
|
||||||
case CEntityOrder::ORDER_GOTO_WAYPOINT:
|
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;
|
break;
|
||||||
updateCollisionPatch();
|
updateCollisionPatch();
|
||||||
return;
|
return;
|
||||||
|
@ -225,7 +225,7 @@ private:
|
|||||||
|
|
||||||
bool processGotoNoPathing( CEntityOrder* current, size_t timestep_milli );
|
bool processGotoNoPathing( CEntityOrder* current, size_t timestep_milli );
|
||||||
bool processGoto( 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 );
|
bool processPatrol( CEntityOrder* current, size_t timestep_milli );
|
||||||
|
|
||||||
|
@ -86,6 +86,7 @@ public:
|
|||||||
ORDER_GOTO_SMOOTHED,
|
ORDER_GOTO_SMOOTHED,
|
||||||
ORDER_GOTO_COLLISION,
|
ORDER_GOTO_COLLISION,
|
||||||
ORDER_GOTO_WAYPOINT,
|
ORDER_GOTO_WAYPOINT,
|
||||||
|
ORDER_GOTO_WAYPOINT_CONTACT,
|
||||||
ORDER_GOTO,
|
ORDER_GOTO,
|
||||||
ORDER_RUN,
|
ORDER_RUN,
|
||||||
ORDER_PATROL,
|
ORDER_PATROL,
|
||||||
|
@ -592,7 +592,7 @@ bool CEntity::processGoto( CEntityOrder* current, size_t UNUSED(timestep_millis)
|
|||||||
return( true );
|
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 pos( m_position.X, m_position.Z );
|
||||||
CVector2D path_to = current->m_data[0].location;
|
CVector2D path_to = current->m_data[0].location;
|
||||||
@ -607,7 +607,9 @@ bool CEntity::processGotoWaypoint( CEntityOrder* current, size_t UNUSED(timestep
|
|||||||
return( false );
|
return( false );
|
||||||
}
|
}
|
||||||
|
|
||||||
g_Pathfinder.requestLowLevelPath( me, path_to );
|
processChooseMovement( Distance );
|
||||||
|
|
||||||
|
g_Pathfinder.requestLowLevelPath( me, path_to, contact );
|
||||||
|
|
||||||
return( true );
|
return( true );
|
||||||
}
|
}
|
||||||
|
@ -1,19 +1,157 @@
|
|||||||
#include "precompiled.h"
|
#include "precompiled.h"
|
||||||
|
|
||||||
#include "PathfindEngine.h"
|
#include "PathfindEngine.h"
|
||||||
#include "PathfindSparse.h"
|
//#include "PathfindSparse.h"
|
||||||
#include "ConfigDB.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()
|
CPathfindEngine::CPathfindEngine()
|
||||||
{
|
{
|
||||||
CConfigValue* sparseDepth = g_ConfigDB.GetValue( CFG_USER, "pathfind.sparse.recursiondepth" );
|
/* CConfigValue* sparseDepth = g_ConfigDB.GetValue( CFG_USER, "pathfind.sparse.recursiondepth" );
|
||||||
if( sparseDepth )
|
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 )
|
void CPathfindEngine::requestPath( HEntity entity, const CVector2D& destination )
|
||||||
{
|
{
|
||||||
// pathSparse( entity, destination );
|
|
||||||
/* TODO: Add code to generate high level path
|
/* TODO: Add code to generate high level path
|
||||||
For now, just the one high level waypoint to the final
|
For now, just the one high level waypoint to the final
|
||||||
destination is added
|
destination is added
|
||||||
@ -24,25 +162,182 @@ void CPathfindEngine::requestPath( HEntity entity, const CVector2D& destination
|
|||||||
entity->m_orderQueue.push_front( waypoint );
|
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* */
|
/* TODO: Pull out code into AStarEngine */
|
||||||
pathSparse( entity, destination );
|
/* 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 )
|
void CPathfindEngine::requestContactPath( HEntity entity, CEntityOrder* current )
|
||||||
{
|
{
|
||||||
pathSparse( entity, current->m_data[0].entity->m_position );
|
/* TODO: Same as non-contact: need high-level planner */
|
||||||
// For attack orders, do some additional postprocessing (replace goto/nopathing
|
CEntityOrder waypoint;
|
||||||
// with attack/nopathing, up until the attack order marker)
|
waypoint.m_type = CEntityOrder::ORDER_GOTO_WAYPOINT_CONTACT;
|
||||||
std::deque<CEntityOrder>::iterator it;
|
waypoint.m_data[0].location = current->m_data[0].entity->m_position;
|
||||||
for( it = entity->m_orderQueue.begin(); it != entity->m_orderQueue.end(); it++ )
|
entity->m_orderQueue.push_front( waypoint );
|
||||||
{
|
|
||||||
if( it->m_type == CEntityOrder::ORDER_PATH_END_MARKER )
|
//pathSparse( entity, current->m_data[0].entity->m_position );
|
||||||
break;
|
//// For attack orders, do some additional postprocessing (replace goto/nopathing
|
||||||
if( it->m_type == CEntityOrder::ORDER_GOTO_NOPATHING )
|
//// with attack/nopathing, up until the attack order marker)
|
||||||
{
|
//std::deque<CEntityOrder>::iterator it;
|
||||||
*it = *current;
|
//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"
|
#include "Vector2D.h"
|
||||||
|
|
||||||
#define g_Pathfinder CPathfindEngine::GetSingleton()
|
#define g_Pathfinder CPathfindEngine::GetSingleton()
|
||||||
|
#define MAXSIZE 1024
|
||||||
|
|
||||||
class CEntityOrder;
|
class CEntityOrder;
|
||||||
|
|
||||||
@ -25,13 +26,40 @@ enum EPathType
|
|||||||
PF_ATTACK_MELEE,
|
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>
|
class CPathfindEngine : public Singleton<CPathfindEngine>
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
CPathfindEngine();
|
CPathfindEngine();
|
||||||
|
~CPathfindEngine();
|
||||||
void requestPath( HEntity entity, const CVector2D& destination );
|
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 );
|
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
|
#endif
|
||||||
|
Loading…
Reference in New Issue
Block a user