# 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.

source/simulation/AStarEngine.cpp

@@ -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;
+}

source/simulation/AStarEngine.h

@@ -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

source/simulation/Collision.cpp

@@ -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;

source/simulation/Collision.h

@@ -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 );

source/simulation/PathfindEngine.cpp

@@ -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;
-	}
-}

source/simulation/PathfindEngine.h

@@ -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