source-engine/game/server/nav_pathfind.h

//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
//
// Purpose: 
//
// $NoKeywords: $
//
//=============================================================================//
// nav_pathfind.h
// Path-finding mechanisms using the Navigation Mesh
// Author: Michael S. Booth (mike@turtlerockstudios.com), January 2003

#ifndef _NAV_PATHFIND_H_
#define _NAV_PATHFIND_H_

#include "tier0/vprof.h"
#include "mathlib/ssemath.h"
#include "nav_area.h"

extern int g_DebugPathfindCounter;


//-------------------------------------------------------------------------------------------------------------------
/**
 * Used when building a path to determine the kind of path to build
 */
enum RouteType
{
	DEFAULT_ROUTE,
	FASTEST_ROUTE,
	SAFEST_ROUTE,
	RETREAT_ROUTE,
};


//--------------------------------------------------------------------------------------------------------------
/**
 * Functor used with NavAreaBuildPath()
 */
class ShortestPathCost
{
public:
	float operator() ( CNavArea *area, CNavArea *fromArea, const CNavLadder *ladder, const CFuncElevator *elevator, float length )
	{
		if ( fromArea == NULL )
		{
			// first area in path, no cost
			return 0.0f;
		}
		else
		{
			// compute distance traveled along path so far
			float dist;

			if ( ladder )
			{
				dist = ladder->m_length;
			}
			else if ( length > 0.0 )
			{
				dist = length;
			}
			else
			{
				dist = ( area->GetCenter() - fromArea->GetCenter() ).Length();
			}

			float cost = dist + fromArea->GetCostSoFar();

			// if this is a "crouch" area, add penalty
			if ( area->GetAttributes() & NAV_MESH_CROUCH )
			{
				const float crouchPenalty = 20.0f;		// 10
				cost += crouchPenalty * dist;
			}

			// if this is a "jump" area, add penalty
			if ( area->GetAttributes() & NAV_MESH_JUMP )
			{
				const float jumpPenalty = 5.0f;
				cost += jumpPenalty * dist;
			}

			return cost;
		}
	}
};

//--------------------------------------------------------------------------------------------------------------
/**
 * Find path from startArea to goalArea via an A* search, using supplied cost heuristic.
 * If cost functor returns -1 for an area, that area is considered a dead end.
 * This doesn't actually build a path, but the path is defined by following parent
 * pointers back from goalArea to startArea.
 * If 'closestArea' is non-NULL, the closest area to the goal is returned (useful if the path fails).
 * If 'goalArea' is NULL, will compute a path as close as possible to 'goalPos'.
 * If 'goalPos' is NULL, will use the center of 'goalArea' as the goal position.
 * If 'maxPathLength' is nonzero, path building will stop when this length is reached.
 * Returns true if a path exists.
 */
#define IGNORE_NAV_BLOCKERS true
template< typename CostFunctor >
bool NavAreaBuildPath( CNavArea *startArea, CNavArea *goalArea, const Vector *goalPos, CostFunctor &costFunc, CNavArea **closestArea = NULL, float maxPathLength = 0.0f, int teamID = TEAM_ANY, bool ignoreNavBlockers = false )
{
	VPROF_BUDGET( "NavAreaBuildPath", "NextBotSpiky" );

	if ( closestArea )
	{
		*closestArea = startArea;
	}

	bool isDebug = ( g_DebugPathfindCounter-- > 0 );

	if (startArea == NULL)
		return false;

	if (goalArea != NULL && goalArea->IsBlocked( teamID, ignoreNavBlockers ))
		goalArea = NULL;

	if (goalArea == NULL && goalPos == NULL)
		return false;

	startArea->SetParent( NULL );

	// if we are already in the goal area, build trivial path
	if (startArea == goalArea)
	{
		goalArea->SetParent( NULL );
		return true;
	}

	// determine actual goal position
	Vector actualGoalPos = (goalPos) ? *goalPos : goalArea->GetCenter();

	// start search
	CNavArea::ClearSearchLists();

	// compute estimate of path length
	/// @todo Cost might work as "manhattan distance"
	startArea->SetTotalCost( (startArea->GetCenter() - actualGoalPos).Length() );

	float initCost = costFunc( startArea, NULL, NULL, NULL, -1.0f );	
	if (initCost < 0.0f)
		return false;
	startArea->SetCostSoFar( initCost );
	startArea->SetPathLengthSoFar( 0.0 );

	startArea->AddToOpenList();

	// keep track of the area we visit that is closest to the goal
	if (closestArea)
		*closestArea = startArea;
	float closestAreaDist = startArea->GetTotalCost();

	// do A* search
	while( !CNavArea::IsOpenListEmpty() )
	{
		// get next area to check
		CNavArea *area = CNavArea::PopOpenList();

		if ( isDebug )
		{
			area->DrawFilled( 0, 255, 0, 128, 30.0f );
		}

		// don't consider blocked areas
		if ( area->IsBlocked( teamID, ignoreNavBlockers ) )
			continue;

		// check if we have found the goal area or position
		if (area == goalArea || (goalArea == NULL && goalPos && area->Contains( *goalPos )))
		{
			if (closestArea)
			{
				*closestArea = area;
			}

			return true;
		}

		// search adjacent areas
		enum SearchType
		{
			SEARCH_FLOOR, SEARCH_LADDERS, SEARCH_ELEVATORS
		};
		SearchType searchWhere = SEARCH_FLOOR;
		int searchIndex = 0;

		int dir = NORTH;
		const NavConnectVector *floorList = area->GetAdjacentAreas( NORTH );

		bool ladderUp = true;
		const NavLadderConnectVector *ladderList = NULL;
		enum { AHEAD = 0, LEFT, RIGHT, BEHIND, NUM_TOP_DIRECTIONS };
		int ladderTopDir = AHEAD;
		bool bHaveMaxPathLength = ( maxPathLength > 0.0f );
		float length = -1;
		
		while( true )
		{
			CNavArea *newArea = NULL;
			NavTraverseType how;
			const CNavLadder *ladder = NULL;
			const CFuncElevator *elevator = NULL;

			//
			// Get next adjacent area - either on floor or via ladder
			//
			if ( searchWhere == SEARCH_FLOOR )
			{
				// if exhausted adjacent connections in current direction, begin checking next direction
				if ( searchIndex >= floorList->Count() )
				{
					++dir;

					if ( dir == NUM_DIRECTIONS )
					{
						// checked all directions on floor - check ladders next
						searchWhere = SEARCH_LADDERS;

						ladderList = area->GetLadders( CNavLadder::LADDER_UP );
						searchIndex = 0;
						ladderTopDir = AHEAD;
					}
					else
					{
						// start next direction
						floorList = area->GetAdjacentAreas( (NavDirType)dir );
						searchIndex = 0;
					}

					continue;
				}

				const NavConnect &floorConnect = floorList->Element( searchIndex );
				newArea = floorConnect.area;
				length = floorConnect.length;
				how = (NavTraverseType)dir;
				++searchIndex;

				if ( IsX360() && searchIndex < floorList->Count() )
				{
					PREFETCH360( floorList->Element( searchIndex ).area, 0  );
				}
			}
			else if ( searchWhere == SEARCH_LADDERS )
			{
				if ( searchIndex >= ladderList->Count() )
				{
					if ( !ladderUp )
					{
						// checked both ladder directions - check elevators next
						searchWhere = SEARCH_ELEVATORS;
						searchIndex = 0;
						ladder = NULL;
					}
					else
					{
						// check down ladders
						ladderUp = false;
						ladderList = area->GetLadders( CNavLadder::LADDER_DOWN );
						searchIndex = 0;
					}
					continue;
				}

				if ( ladderUp )
				{
					ladder = ladderList->Element( searchIndex ).ladder;

					// do not use BEHIND connection, as its very hard to get to when going up a ladder
					if ( ladderTopDir == AHEAD )
					{
						newArea = ladder->m_topForwardArea;
					}
					else if ( ladderTopDir == LEFT )
					{
						newArea = ladder->m_topLeftArea;
					}
					else if ( ladderTopDir == RIGHT )
					{
						newArea = ladder->m_topRightArea;
					}
					else
					{
						++searchIndex;
						ladderTopDir = AHEAD;
						continue;
					}

					how = GO_LADDER_UP;
					++ladderTopDir;
				}
				else
				{
					newArea = ladderList->Element( searchIndex ).ladder->m_bottomArea;
					how = GO_LADDER_DOWN;
					ladder = ladderList->Element(searchIndex).ladder;
					++searchIndex;
				}

				if ( newArea == NULL )
					continue;

				length = -1.0f;
			}
			else // if ( searchWhere == SEARCH_ELEVATORS )
			{
				const NavConnectVector &elevatorAreas = area->GetElevatorAreas();

				elevator = area->GetElevator();

				if ( elevator == NULL || searchIndex >= elevatorAreas.Count() )
				{
					// done searching connected areas
					elevator = NULL;
					break;
				}

				newArea = elevatorAreas[ searchIndex++ ].area;
				if ( newArea->GetCenter().z > area->GetCenter().z )
				{
					how = GO_ELEVATOR_UP;
				}
				else
				{
					how = GO_ELEVATOR_DOWN;
				}

				length = -1.0f;
			}


			// don't backtrack
			if ( newArea == area )
				continue;

			// don't consider blocked areas
			if ( newArea->IsBlocked( teamID, ignoreNavBlockers ) )
				continue;

			float newCostSoFar = costFunc( newArea, area, ladder, elevator, length );
			
			// check if cost functor says this area is a dead-end
			if ( newCostSoFar < 0.0f )
				continue;
				
			// stop if path length limit reached
			if ( bHaveMaxPathLength )
			{
				// keep track of path length so far
				float deltaLength = ( newArea->GetCenter() - area->GetCenter() ).Length();
				float newLengthSoFar = area->GetPathLengthSoFar() + deltaLength;
				if ( newLengthSoFar > maxPathLength )
					continue;
				
				newArea->SetPathLengthSoFar( newLengthSoFar );
			}

			if ( ( newArea->IsOpen() || newArea->IsClosed() ) && newArea->GetCostSoFar() <= newCostSoFar )
			{
				// this is a worse path - skip it
				continue;
			}
			else
			{
				// compute estimate of distance left to go
				float distSq = ( newArea->GetCenter() - actualGoalPos ).LengthSqr();
				float newCostRemaining = ( distSq > 0.0 ) ? FastSqrt( distSq ) : 0.0 ;

				// track closest area to goal in case path fails
				if ( closestArea && newCostRemaining < closestAreaDist )
				{
					*closestArea = newArea;
					closestAreaDist = newCostRemaining;
				}
				
				newArea->SetCostSoFar( newCostSoFar );
				newArea->SetTotalCost( newCostSoFar + newCostRemaining );

				if ( newArea->IsClosed() )
				{
					newArea->RemoveFromClosedList();
				}

				if ( newArea->IsOpen() )
				{
					// area already on open list, update the list order to keep costs sorted
					newArea->UpdateOnOpenList();
				}
				else
				{
					newArea->AddToOpenList();
				}

				newArea->SetParent( area, how );
			}
		}

		// we have searched this area
		area->AddToClosedList();
	}

	return false;
}


//--------------------------------------------------------------------------------------------------------------
/**
 * Compute distance between two areas. Return -1 if can't reach 'endArea' from 'startArea'.
 */
template< typename CostFunctor >
float NavAreaTravelDistance( CNavArea *startArea, CNavArea *endArea, CostFunctor &costFunc, float maxPathLength = 0.0f )
{
	if (startArea == NULL)
		return -1.0f;

	if (endArea == NULL)
		return -1.0f;

	if (startArea == endArea)
		return 0.0f;

	// compute path between areas using given cost heuristic
	if (NavAreaBuildPath( startArea, endArea, NULL, costFunc, NULL, maxPathLength ) == false)
		return -1.0f;

	// compute distance along path
	float distance = 0.0f;
	for( CNavArea *area = endArea; area->GetParent(); area = area->GetParent() )
	{
		distance += (area->GetCenter() - area->GetParent()->GetCenter()).Length();
	}

	return distance;
}



//--------------------------------------------------------------------------------------------------------------
/**
 * Do a breadth-first search, invoking functor on each area.
 * If functor returns 'true', continue searching from this area. 
 * If functor returns 'false', the area's adjacent areas are not explored (dead end).
 * If 'maxRange' is 0 or less, no range check is done (all areas will be examined).
 *
 * NOTE: Returns all areas that overlap range, even partially
 *
 * @todo Use ladder connections
 */

// helper function
inline void AddAreaToOpenList( CNavArea *area, CNavArea *parent, const Vector &startPos, float maxRange )
{
	if (area == NULL)
		return;

	if (!area->IsMarked())
	{
		area->Mark();
		area->SetTotalCost( 0.0f );
		area->SetParent( parent );

		if (maxRange > 0.0f)
		{
			// make sure this area overlaps range
			Vector closePos;
			area->GetClosestPointOnArea( startPos, &closePos );
			if ((closePos - startPos).AsVector2D().IsLengthLessThan( maxRange ))
			{
				// compute approximate distance along path to limit travel range, too
				float distAlong = parent->GetCostSoFar();
				distAlong += (area->GetCenter() - parent->GetCenter()).Length();
				area->SetCostSoFar( distAlong );

				// allow for some fudge due to large size areas
				if (distAlong <= 1.5f * maxRange)
					area->AddToOpenList();
			}
		}
		else
		{
			// infinite range
			area->AddToOpenList();
		}
	}
}


/****************************************************************
 * DEPRECATED: Use filter-based SearchSurroundingAreas below
 ****************************************************************/
#define INCLUDE_INCOMING_CONNECTIONS	0x1
#define INCLUDE_BLOCKED_AREAS			0x2
#define EXCLUDE_OUTGOING_CONNECTIONS	0x4
#define EXCLUDE_ELEVATORS				0x8
template < typename Functor >
void SearchSurroundingAreas( CNavArea *startArea, const Vector &startPos, Functor &func, float maxRange = -1.0f, unsigned int options = 0, int teamID = TEAM_ANY )
{
	if (startArea == NULL)
		return;

	CNavArea::MakeNewMarker();
	CNavArea::ClearSearchLists();

	startArea->AddToOpenList();
	startArea->SetTotalCost( 0.0f );
	startArea->SetCostSoFar( 0.0f );
	startArea->SetParent( NULL );
	startArea->Mark();

	while( !CNavArea::IsOpenListEmpty() )
	{
		// get next area to check
		CNavArea *area = CNavArea::PopOpenList();

		// don't use blocked areas
		if ( area->IsBlocked( teamID ) && !(options & INCLUDE_BLOCKED_AREAS) )
			continue;

		// invoke functor on area
		if (func( area ))
		{
			// explore adjacent floor areas
			for( int dir=0; dir<NUM_DIRECTIONS; ++dir )
			{
				int count = area->GetAdjacentCount( (NavDirType)dir );
				for( int i=0; i<count; ++i )
				{
					CNavArea *adjArea = area->GetAdjacentArea( (NavDirType)dir, i );
					if ( options & EXCLUDE_OUTGOING_CONNECTIONS )
					{
						if ( !adjArea->IsConnected( area, NUM_DIRECTIONS ) )
						{
							continue;	// skip this outgoing connection
						}
					}
					
					AddAreaToOpenList( adjArea, area, startPos, maxRange );
				}
			}
			
			// potentially include areas that connect TO this area via a one-way link
			if (options & INCLUDE_INCOMING_CONNECTIONS)
			{
				for( int dir=0; dir<NUM_DIRECTIONS; ++dir )
				{
					const NavConnectVector *list = area->GetIncomingConnections( (NavDirType)dir );

					FOR_EACH_VEC( (*list), it )
					{
						NavConnect connect = (*list)[ it ];				
						
						AddAreaToOpenList( connect.area, area, startPos, maxRange );
					}
				}
			}


			// explore adjacent areas connected by ladders

			// check up ladders
			const NavLadderConnectVector *ladderList = area->GetLadders( CNavLadder::LADDER_UP );
			if (ladderList)
			{
				FOR_EACH_VEC( (*ladderList), it )
				{
					const CNavLadder *ladder = (*ladderList)[ it ].ladder;

					// do not use BEHIND connection, as its very hard to get to when going up a ladder
					AddAreaToOpenList( ladder->m_topForwardArea, area, startPos, maxRange );
					AddAreaToOpenList( ladder->m_topLeftArea, area, startPos, maxRange );
					AddAreaToOpenList( ladder->m_topRightArea, area, startPos, maxRange );
				}
			}

			// check down ladders
			ladderList = area->GetLadders( CNavLadder::LADDER_DOWN );
			if (ladderList)
			{
				FOR_EACH_VEC( (*ladderList), it )
				{
					const CNavLadder *ladder = (*ladderList)[ it ].ladder;

					AddAreaToOpenList( ladder->m_bottomArea, area, startPos, maxRange );
				}
			}

			if ( (options & EXCLUDE_ELEVATORS) == 0 )
			{
				const NavConnectVector &elevatorList = area->GetElevatorAreas();
				FOR_EACH_VEC( elevatorList, it )
				{
					CNavArea *elevatorArea = elevatorList[ it ].area;
					AddAreaToOpenList( elevatorArea, area, startPos, maxRange );
				}
			}
		}
	}
}


//--------------------------------------------------------------------------------------------------------------
/**
 * Derive your own custom search functor from this interface method for use with SearchSurroundingAreas below.
 */
class ISearchSurroundingAreasFunctor
{
public:
	virtual ~ISearchSurroundingAreasFunctor() { }

	/**
	 * Perform user-defined action on area.
	 * Return 'false' to end the search (ie: you found what you were looking for)
	 */
	virtual bool operator() ( CNavArea *area, CNavArea *priorArea, float travelDistanceSoFar ) = 0;

	// return true if 'adjArea' should be included in the ongoing search
	virtual bool ShouldSearch( CNavArea *adjArea, CNavArea *currentArea, float travelDistanceSoFar ) 
	{
		return !adjArea->IsBlocked( TEAM_ANY );
	}

	/**
	 * Collect adjacent areas to continue the search by calling 'IncludeInSearch' on each
	 */
	virtual void IterateAdjacentAreas( CNavArea *area, CNavArea *priorArea, float travelDistanceSoFar ) 
	{
		// search adjacent outgoing connections
		for( int dir=0; dir<NUM_DIRECTIONS; ++dir )
		{
			int count = area->GetAdjacentCount( (NavDirType)dir );
			for( int i=0; i<count; ++i )
			{
				CNavArea *adjArea = area->GetAdjacentArea( (NavDirType)dir, i );

				if ( ShouldSearch( adjArea, area, travelDistanceSoFar ) )
				{
					IncludeInSearch( adjArea, area );
				}
			}
		}
	}

	// Invoked after the search has completed
	virtual void PostSearch( void ) { }

	// consider 'area' in upcoming search steps
	void IncludeInSearch( CNavArea *area, CNavArea *priorArea ) 
	{
		if ( area == NULL )
			return;

		if ( !area->IsMarked() )
		{
			area->Mark();
			area->SetTotalCost( 0.0f );
			area->SetParent( priorArea );

			// compute approximate travel distance from start area of search
			if ( priorArea )
			{
				float distAlong = priorArea->GetCostSoFar();
				distAlong += ( area->GetCenter() - priorArea->GetCenter() ).Length();
				area->SetCostSoFar( distAlong );
			}
			else
			{
				area->SetCostSoFar( 0.0f );
			}

			// adding an area to the open list also marks it
			area->AddToOpenList();
		}
	}
};


/**
 * Do a breadth-first search starting from 'startArea' and continuing outward based on
 * adjacent areas that pass the given filter
 */
inline void SearchSurroundingAreas( CNavArea *startArea, ISearchSurroundingAreasFunctor &func )
{
	if ( startArea )
	{
		CNavArea::MakeNewMarker();
		CNavArea::ClearSearchLists();

		startArea->AddToOpenList();
		startArea->SetTotalCost( 0.0f );
		startArea->SetCostSoFar( 0.0f );
		startArea->SetParent( NULL );
		startArea->Mark();

		CUtlVector< CNavArea * > adjVector;

		while( !CNavArea::IsOpenListEmpty() )
		{
			// get next area to check
			CNavArea *area = CNavArea::PopOpenList();

			if ( func( area, area->GetParent(), area->GetCostSoFar() ) )
			{
				func.IterateAdjacentAreas( area, area->GetParent(), area->GetCostSoFar() );
			}
			else
			{
				// search aborted
				break;
			}
		}
	}

	func.PostSearch();
}


//--------------------------------------------------------------------------------------------------------------
/**
 * Fuctor that returns lowest cost for farthest away areas
 * For use with FindMinimumCostArea()
 */
class FarAwayFunctor
{
public:
	float operator() ( CNavArea *area, CNavArea *fromArea, const CNavLadder *ladder )
	{
		if (area == fromArea)
			return 9999999.9f;

		return 1.0f/(fromArea->GetCenter() - area->GetCenter()).Length();
	}
};

/**
 * Fuctor that returns lowest cost for areas farthest from given position
 * For use with FindMinimumCostArea()
 */
class FarAwayFromPositionFunctor 
{
public:
	FarAwayFromPositionFunctor( const Vector &pos ) : m_pos( pos )
	{
	}

	float operator() ( CNavArea *area, CNavArea *fromArea, const CNavLadder *ladder )
	{
		return 1.0f/(m_pos - area->GetCenter()).Length();
	}

private:
	const Vector &m_pos;
};


/**
 * Pick a low-cost area of "decent" size
 */
template< typename CostFunctor >
CNavArea *FindMinimumCostArea( CNavArea *startArea, CostFunctor &costFunc )
{
	const float minSize = 150.0f;

	// collect N low-cost areas of a decent size
	enum { NUM_CHEAP_AREAS = 32 };
	struct 
	{
		CNavArea *area;
		float cost;
	}
	cheapAreaSet[ NUM_CHEAP_AREAS ];
	int cheapAreaSetCount = 0;

	FOR_EACH_VEC( TheNavAreas, iter )
	{
		CNavArea *area = TheNavAreas[iter];

		// skip the small areas
		if ( area->GetSizeX() < minSize || area->GetSizeY() < minSize)
			continue;

		// compute cost of this area
		float cost = costFunc( area, startArea, NULL, NULL );

		if (cheapAreaSetCount < NUM_CHEAP_AREAS)
		{
			cheapAreaSet[ cheapAreaSetCount ].area = area;
			cheapAreaSet[ cheapAreaSetCount++ ].cost = cost;
		}
		else
		{
			// replace most expensive cost if this is cheaper
			int expensive = 0;
			for( int i=1; i<NUM_CHEAP_AREAS; ++i )
				if (cheapAreaSet[i].cost > cheapAreaSet[expensive].cost)
					expensive = i;

			if (cheapAreaSet[expensive].cost > cost)
			{
				cheapAreaSet[expensive].area = area;
				cheapAreaSet[expensive].cost = cost;
			}
		}
	}

	if (cheapAreaSetCount)
	{
		// pick one of the areas at random
		return cheapAreaSet[ RandomInt( 0, cheapAreaSetCount-1 ) ].area;
	}
	else
	{
		// degenerate case - no decent sized areas - pick a random area
		int numAreas = TheNavAreas.Count();
		int which = RandomInt( 0, numAreas-1 );

		FOR_EACH_VEC( TheNavAreas, iter )
		{
			if (which-- == 0)
				return TheNavAreas[iter];
		}

	}
	return cheapAreaSet[ RandomInt( 0, cheapAreaSetCount-1 ) ].area;
}


#endif // _NAV_PATHFIND_H_