First version of the SOurce SDK 2013

1 files changed, 2137 insertions, 0 deletions

diff --git a/mp/src/game/server/ai_pathfinder.cpp b/mp/src/game/server/ai_pathfinder.cpp
new file mode 100644
index 00000000..709a129b
--- /dev/null
+++ b/mp/src/game/server/ai_pathfinder.cpp
@@ -0,0 +1,2137 @@
+//========= Copyright Valve Corporation, All rights reserved. ============//

+//

+// Purpose:

+//

+// $NoKeywords: $

+//=============================================================================//

+

+#include "cbase.h"

+

+#include "ndebugoverlay.h"

+

+#include "ai_pathfinder.h"

+

+#include "ai_basenpc.h"

+#include "ai_node.h"

+#include "ai_network.h"

+#include "ai_waypoint.h"

+#include "ai_link.h"

+#include "ai_routedist.h"

+#include "ai_moveprobe.h"

+#include "ai_dynamiclink.h"

+#include "ai_hint.h"

+#include "bitstring.h"

+

+//@todo: bad dependency!

+#include "ai_navigator.h"

+

+// memdbgon must be the last include file in a .cpp file!!!

+#include "tier0/memdbgon.h"

+

+#define NUM_NPC_DEBUG_OVERLAYS	  50

+

+const float MAX_LOCAL_NAV_DIST_GROUND[2] = { (50*12), (25*12) };

+const float MAX_LOCAL_NAV_DIST_FLY[2] = { (750*12), (750*12) };

+

+//-----------------------------------------------------------------------------

+// CAI_Pathfinder

+//

+

+BEGIN_SIMPLE_DATADESC( CAI_Pathfinder )

+

+	//								m_TriDebugOverlay

+	//								m_bIgnoreStaleLinks

+  	DEFINE_FIELD( m_flLastStaleLinkCheckTime,		FIELD_TIME ),

+	//								m_pNetwork

+

+END_DATADESC()

+

+//-----------------------------------------------------------------------------

+// Compute move type bits to nav type

+//-----------------------------------------------------------------------------

+Navigation_t MoveBitsToNavType( int fBits )

+{

+	switch (fBits)

+	{

+	case bits_CAP_MOVE_GROUND:

+		return NAV_GROUND;

+

+	case bits_CAP_MOVE_FLY:

+		return NAV_FLY;

+

+	case bits_CAP_MOVE_CLIMB:

+		return NAV_CLIMB;

+

+	case bits_CAP_MOVE_JUMP:

+		return NAV_JUMP;

+

+	default:

+		// This will only happen if more than one bit is set

+		Assert(0);

+		return NAV_NONE;

+	}

+}

+

+

+//-----------------------------------------------------------------------------

+

+void CAI_Pathfinder::Init( CAI_Network *pNetwork )

+{

+	Assert( pNetwork );

+	m_pNetwork = pNetwork;

+}

+	

+//-----------------------------------------------------------------------------

+//

+//-----------------------------------------------------------------------------

+bool CAI_Pathfinder::UseStrongOptimizations()

+{

+	if ( !AIStrongOpt() )

+	{

+		return false;

+	}

+

+#ifdef HL2_DLL

+	if( GetOuter()->Classify() == CLASS_PLAYER_ALLY_VITAL )

+	{

+		return false;

+	}

+#endif//HL2_DLL

+	return true;

+}

+

+//-----------------------------------------------------------------------------

+// Computes the link type

+//-----------------------------------------------------------------------------

+Navigation_t CAI_Pathfinder::ComputeWaypointType( CAI_Node **ppNodes, int parentID, int destID )

+{

+	Navigation_t navType = NAV_NONE;

+

+	CAI_Node *pNode = ppNodes[parentID];

+	for (int link=0; link < pNode->NumLinks();link++) 

+	{

+		if (pNode->GetLinkByIndex(link)->DestNodeID(parentID) == destID)

+		{

+			// BRJ 10/1/02

+			// FIXME: pNPC->CapabilitiesGet() is actually the mechanism by which fliers

+			// filter out the bitfields in the waypoint type (most importantly, bits_MOVE_CAP_GROUND)

+			// that would cause the waypoint distance to be computed in a 2D, as opposed to 3D fashion

+			// This is a super-scary weak link if you ask me.

+			int linkMoveTypeBits = pNode->GetLinkByIndex(link)->m_iAcceptedMoveTypes[GetHullType()];

+			int moveTypeBits = ( linkMoveTypeBits & CapabilitiesGet());

+			if ( !moveTypeBits && linkMoveTypeBits == bits_CAP_MOVE_JUMP )

+			{

+				Assert( pNode->GetHint() && pNode->GetHint()->HintType() == HINT_JUMP_OVERRIDE );

+				ppNodes[destID]->Lock(0.3);

+				moveTypeBits = linkMoveTypeBits;

+			}

+			Navigation_t linkType = MoveBitsToNavType( moveTypeBits );

+

+			// This will only trigger if the links disagree about their nav type

+			Assert( (navType == NAV_NONE) || (navType == linkType) );

+			navType = linkType; 

+			break;

+		}

+	}

+

+	// @TODO (toml 10-15-02): one would not expect to come out of the above logic

+	// with NAV_NONE. However, if a graph is newly built, it can contain malformed

+	// links that are referred to by the destination node, not the source node.

+	// This has to be fixed

+	if ( navType == NAV_NONE )

+	{

+		pNode = ppNodes[destID];

+		for (int link=0; link < pNode->NumLinks();link++) 

+		{

+			if (pNode->GetLinkByIndex(link)->DestNodeID(parentID) == destID)

+			{

+				int npcMoveBits = CapabilitiesGet();

+				int nodeMoveBits = pNode->GetLinkByIndex(link)->m_iAcceptedMoveTypes[GetHullType()];

+				int moveTypeBits = ( npcMoveBits & nodeMoveBits );

+				Navigation_t linkType = MoveBitsToNavType( moveTypeBits );

+

+				Assert( (navType == NAV_NONE) || (navType == linkType) );

+				navType = linkType; 

+

+				DevMsg( "Note: Strange link found between nodes in AI node graph\n" );

+				break;

+			}

+		}

+	}

+

+	AssertMsg( navType != NAV_NONE, "Pathfinder appears to have output a path with consecutive nodes thate are not actually connected\n" );

+

+	return navType;

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Given an array of parentID's and endID, contruct a linked 

+//			list of waypoints through those parents

+//-----------------------------------------------------------------------------

+AI_Waypoint_t* CAI_Pathfinder::MakeRouteFromParents( int *parentArray, int endID ) 

+{

+	AI_Waypoint_t *pOldWaypoint = NULL;

+	AI_Waypoint_t *pNewWaypoint = NULL;

+	int	currentID = endID;

+

+	CAI_Node **pAInode = GetNetwork()->AccessNodes();

+

+	while (currentID != NO_NODE) 

+	{

+		// Try to link it to the previous waypoint

+		int prevID = parentArray[currentID];

+

+		int destID; 

+		if (prevID != NO_NODE)

+		{

+			destID = prevID;

+		}

+		else

+		{		   

+			// If we have no previous node, then use the next node

+			if ( !pOldWaypoint )

+				return NULL;

+			destID = pOldWaypoint->iNodeID;

+		}

+

+		Navigation_t waypointType = ComputeWaypointType( pAInode, currentID, destID );

+

+		// BRJ 10/1/02

+		// FIXME: It appears potentially possible for us to compute waypoints 

+		// here which the NPC is not capable of traversing (because 

+		// pNPC->CapabilitiesGet() in ComputeWaypointType() above filters it out). 

+		// It's also possible if none of the lines have an appropriate DestNodeID.

+		// Um, shouldn't such a waypoint not be allowed?!?!?

+		Assert( waypointType != NAV_NONE );

+

+		pNewWaypoint = new AI_Waypoint_t( pAInode[currentID]->GetPosition(GetHullType()),

+			pAInode[currentID]->GetYaw(), waypointType, bits_WP_TO_NODE, currentID );

+

+		// Link it up...

+		pNewWaypoint->SetNext( pOldWaypoint );

+		pOldWaypoint = pNewWaypoint;

+

+		currentID = prevID;

+	}

+

+	return pOldWaypoint;

+}

+

+

+//------------------------------------------------------------------------------

+// Purpose : Test if stale link is no longer stale

+//------------------------------------------------------------------------------

+

+bool CAI_Pathfinder::IsLinkStillStale(int moveType, CAI_Link *nodeLink)

+{

+	if ( m_bIgnoreStaleLinks )

+		return false;

+

+	if ( !(nodeLink->m_LinkInfo & bits_LINK_STALE_SUGGESTED ) )

+		return false;

+

+	if ( gpGlobals->curtime < nodeLink->m_timeStaleExpires )

+		return true;

+

+	// NPC should only check one stale link per think

+	if (gpGlobals->curtime == m_flLastStaleLinkCheckTime)

+	{

+		return true;

+	}

+	else

+	{

+		m_flLastStaleLinkCheckTime = gpGlobals->curtime;

+	}

+	

+	// Test movement, if suceeds, clear the stale bit

+	if (CheckStaleRoute(GetNetwork()->GetNode(nodeLink->m_iSrcID)->GetPosition(GetHullType()),

+		GetNetwork()->GetNode(nodeLink->m_iDestID)->GetPosition(GetHullType()), moveType))

+	{

+		nodeLink->m_LinkInfo &= ~bits_LINK_STALE_SUGGESTED;

+		return false;

+	}

+

+	nodeLink->m_timeStaleExpires = gpGlobals->curtime + 1.0;

+

+	return true;

+}

+

+

+//-----------------------------------------------------------------------------

+//-----------------------------------------------------------------------------

+int	CAI_Pathfinder::NearestNodeToNPC()

+{

+	return GetNetwork()->NearestNodeToPoint( GetOuter(), GetAbsOrigin() );

+}

+

+//-----------------------------------------------------------------------------

+//-----------------------------------------------------------------------------

+int CAI_Pathfinder::NearestNodeToPoint( const Vector &vecOrigin )

+{

+	return GetNetwork()->NearestNodeToPoint( GetOuter(), vecOrigin );

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Build a path between two nodes

+//-----------------------------------------------------------------------------

+

+AI_Waypoint_t *CAI_Pathfinder::FindBestPath(int startID, int endID) 

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_FindBestPath );

+	

+	if ( !GetNetwork()->NumNodes() )

+		return NULL;

+

+#ifdef AI_PERF_MON

+	m_nPerfStatPB++;

+#endif

+

+	int nNodes = GetNetwork()->NumNodes();

+	CAI_Node **pAInode = GetNetwork()->AccessNodes();

+

+	CVarBitVec	openBS(nNodes);

+	CVarBitVec	closeBS(nNodes);

+

+	// ------------- INITIALIZE ------------------------

+	float* nodeG = (float *)stackalloc( nNodes * sizeof(float) );

+	float* nodeH = (float *)stackalloc( nNodes * sizeof(float) );

+	float* nodeF = (float *)stackalloc( nNodes * sizeof(float) );

+	int*   nodeP = (int *)stackalloc( nNodes * sizeof(int) );		// Node parent 

+

+	for (int node=0;node<nNodes;node++)

+	{

+		nodeG[node] = FLT_MAX;

+		nodeP[node] = -1;

+	}

+

+	nodeG[startID] = 0;

+

+	nodeH[startID] = 0.1*(pAInode[startID]->GetPosition(GetHullType())-pAInode[endID]->GetPosition(GetHullType())).Length(); // Don't want to over estimate

+	nodeF[startID] = nodeG[startID] + nodeH[startID];

+

+	openBS.Set(startID);

+	closeBS.Set( startID );

+

+	// --------------- FIND BEST PATH ------------------

+	while (!openBS.IsAllClear()) 

+	{

+		int smallestID = CAI_Network::FindBSSmallest(&openBS,nodeF,nNodes);

+	

+		openBS.Clear(smallestID);

+

+		CAI_Node *pSmallestNode = pAInode[smallestID];

+		

+		if (GetOuter()->IsUnusableNode(smallestID, pSmallestNode->GetHint()))

+			continue;

+

+		if (smallestID == endID) 

+		{

+			AI_Waypoint_t* route = MakeRouteFromParents(&nodeP[0], endID);

+			return route;

+		}

+

+		// Check this if the node is immediately in the path after the startNode 

+		// that it isn't blocked

+		for (int link=0; link < pSmallestNode->NumLinks();link++) 

+		{

+			CAI_Link *nodeLink = pSmallestNode->GetLinkByIndex(link);

+			

+			if (!IsLinkUsable(nodeLink,smallestID))

+				continue;

+

+			// FIXME: the cost function should take into account Node costs (danger, flanking, etc).

+			int moveType = nodeLink->m_iAcceptedMoveTypes[GetHullType()] & CapabilitiesGet();

+			int testID	 = nodeLink->DestNodeID(smallestID);

+

+			Vector r1 = pSmallestNode->GetPosition(GetHullType());

+			Vector r2 = pAInode[testID]->GetPosition(GetHullType());

+			float dist   = GetOuter()->GetNavigator()->MovementCost( moveType, r1, r2 ); // MovementCost takes ref parameters!!

+

+			if ( dist == FLT_MAX )

+				continue;

+

+			float new_g  = nodeG[smallestID] + dist;

+

+			if ( !closeBS.IsBitSet(testID) || (new_g < nodeG[testID]) ) 

+			{

+				nodeP[testID] = smallestID;

+				nodeG[testID] = new_g;

+				nodeH[testID] = (pAInode[testID]->GetPosition(GetHullType())-pAInode[endID]->GetPosition(GetHullType())).Length();

+				nodeF[testID] = nodeG[testID] + nodeH[testID];

+

+				closeBS.Set( testID );

+				openBS.Set( testID );

+			}

+		}

+	}

+

+	return NULL;   

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Find a short random path of at least pathLength distance.  If

+//			vDirection is given random path will expand in the given direction,

+//			and then attempt to go generally straight

+//-----------------------------------------------------------------------------

+

+AI_Waypoint_t* CAI_Pathfinder::FindShortRandomPath(int startID, float minPathLength, const Vector &directionIn) 

+{

+	int				pNeighbor[AI_MAX_NODE_LINKS];

+	int				pStaleNeighbor[AI_MAX_NODE_LINKS];

+	int				numNeighbors		= 1;	// The start node

+	int				numStaleNeighbors	= 0;

+	int				neighborID			= NO_NODE;

+

+	int nNodes = GetNetwork()->NumNodes();

+	CAI_Node **pAInode = GetNetwork()->AccessNodes();

+

+	if ( !nNodes )

+		return NULL;

+	

+	MARK_TASK_EXPENSIVE();

+

+	int *nodeParent	= (int *)stackalloc( sizeof(int) * nNodes );

+	CVarBitVec closeBS(nNodes);

+	Vector vDirection = directionIn;

+

+	// ------------------------------------------

+	// Bail immediately if node has no neighbors

+	// ------------------------------------------

+	if (pAInode[startID]->NumLinks() == 0)

+	{

+		return NULL;

+	}

+

+	// ------------- INITIALIZE ------------------------

+	nodeParent[startID] = NO_NODE;

+	pNeighbor[0]		= startID;

+

+	// --------------- FIND PATH ---------------------------------------------------------------

+	// Quit when path is long enough, and I've run out of neighbors unless I'm on a climb node

+	// in which case I'm not allowed to stop

+	// -----------------------------------------------------------------------------------------

+	float	pathLength	 = 0;

+	int		nSearchCount = 0;

+	while ( (pathLength < minPathLength) || 

+			(neighborID != NO_NODE && pAInode[neighborID]->GetType() == NODE_CLIMB))

+	{

+		nSearchCount++;

+

+		// If no neighbors try circling back to last node

+		if (neighborID			!= NO_NODE	&&

+			numNeighbors		== 0		&& 

+			numStaleNeighbors	== 0		)

+		{

+			// If we dead ended on a climb node we've failed as we

+			// aren't allowed to stop on a climb node

+			if (pAInode[neighborID]->GetType() == NODE_CLIMB)

+			{

+				// If no neighbors exist we've failed.

+				return NULL;

+			}

+			// Otherwise accept this path to a dead end

+			else

+			{

+				AI_Waypoint_t* route = MakeRouteFromParents(&nodeParent[0], neighborID);

+				return route;

+			}

+		}

+

+		// ----------------------

+		//  Pick a neighbor

+		// ----------------------

+		int lastID = neighborID;

+

+		// If vDirection is non-zero attempt to expand close to current direction

+		if (vDirection != vec3_origin)

+		{

+			float	bestDot		= -1;

+			Vector	vLastPos;

+

+			if (lastID == NO_NODE)

+			{

+				vLastPos = GetLocalOrigin();

+			}

+			else

+			{

+				vLastPos = pAInode[lastID]->GetOrigin();

+			}

+

+			// If no neighbors, try using a stale one

+			if (numNeighbors == 0)

+			{

+				neighborID = pStaleNeighbor[random->RandomInt(0,numStaleNeighbors-1)];

+			}

+			else

+			{

+				for (int i=0;i<numNeighbors;i++)

+				{

+					Vector nodeDir = vLastPos - pAInode[pNeighbor[i]]->GetOrigin();

+					VectorNormalize(nodeDir);

+					float fDotPr = DotProduct(vDirection,nodeDir);

+					if (fDotPr > bestDot)

+					{

+						bestDot = fDotPr;

+						neighborID = pNeighbor[i];

+					}

+				}

+			}

+

+			if (neighborID != NO_NODE)

+			{

+				vDirection = vLastPos - pAInode[neighborID]->GetOrigin();

+				VectorNormalize(vDirection);

+			}

+

+		}

+		// Pick random neighbor 

+		else if (numNeighbors != 0)

+		{

+			neighborID = pNeighbor[random->RandomInt(0,numNeighbors-1)];

+		}

+		// If no neighbors, try using a stale one

+		else

+		{

+			neighborID = pStaleNeighbor[random->RandomInt(0,numStaleNeighbors-1)];

+		}

+

+		// BUGBUG: This routine is totally hosed!

+		if ( neighborID < 0 )

+			return NULL;

+

+		// Set previous nodes parent

+		nodeParent[neighborID] = lastID;

+		closeBS.Set(neighborID);

+

+		// Add the new length

+		if (lastID != NO_NODE)

+		{

+			pathLength += (pAInode[lastID]->GetOrigin() - pAInode[neighborID]->GetOrigin()).Length();

+		}

+

+		// If path is long enough or we've hit a maximum number of search nodes,

+		// we're done unless we've ended on a climb node

+		if ((pathLength >= minPathLength || nSearchCount > 20) &&

+			pAInode[neighborID]->GetType() != NODE_CLIMB)

+		{

+			return MakeRouteFromParents(&nodeParent[0], neighborID);

+		}

+

+		// Clear neighbors

+		numNeighbors		= 0;

+		numStaleNeighbors	= 0;

+

+		// Now add in new neighbors, pick links in different order ever time

+		pAInode[neighborID]->ShuffleLinks();

+		for (int link=0; link < pAInode[neighborID]->NumLinks();link++) 

+		{

+			if ( numStaleNeighbors == ARRAYSIZE(pStaleNeighbor) )

+			{

+				AssertMsg( 0, "Array overflow" );

+				return NULL;

+			}

+			if ( numNeighbors == ARRAYSIZE(pStaleNeighbor) )

+			{

+				AssertMsg( 0, "Array overflow" );

+				return NULL;

+			}

+

+			CAI_Link*	nodeLink = pAInode[neighborID]->GetShuffeledLink(link);

+			int			testID	 = nodeLink->DestNodeID(neighborID);

+

+			// --------------------------------------------------------------------------

+			//  Don't loop

+			// --------------------------------------------------------------------------

+			if (closeBS.IsBitSet(testID))

+			{

+				continue;

+			}

+

+			// --------------------------------------------------------------------------

+			// Don't go back to the node I just visited

+			// --------------------------------------------------------------------------

+			if (testID == lastID)

+			{

+				continue; 

+			}

+

+			// --------------------------------------------------------------------------

+			// Make sure link is valid

+			// --------------------------------------------------------------------------

+			if (!IsLinkUsable(nodeLink,neighborID))

+			{

+				continue;

+			}

+

+			// --------------------------------------------------------------------------

+			// If its a stale node add to stale list

+			// --------------------------------------------------------------------------

+			if (pAInode[testID]->IsLocked())

+			{

+				pStaleNeighbor[numStaleNeighbors]=testID;

+				numStaleNeighbors++;

+			}

+

+			// --------------------------------------

+			//  Add to list of non-stale neighbors

+			// --------------------------------------

+			else

+			{

+				pNeighbor[numNeighbors]=testID;

+				numNeighbors++;

+			}

+		}

+	}

+	// Failed to get a path of full length, but return what we have

+	return MakeRouteFromParents(&nodeParent[0], neighborID);

+}

+

+//------------------------------------------------------------------------------

+// Purpose : Returns true is link us usable by the given NPC from the

+//			 startID node.

+//------------------------------------------------------------------------------

+

+bool CAI_Pathfinder::IsLinkUsable(CAI_Link *pLink, int startID)

+{

+	// --------------------------------------------------------------------------

+	// Skip if link turned off

+	// --------------------------------------------------------------------------

+	if (pLink->m_LinkInfo & bits_LINK_OFF)

+	{

+		CAI_DynamicLink *pDynamicLink = pLink->m_pDynamicLink;

+

+		if ( !pDynamicLink || pDynamicLink->m_strAllowUse == NULL_STRING )

+			return false;

+

+		const char *pszAllowUse = STRING( pDynamicLink->m_strAllowUse );

+		if ( pDynamicLink->m_bInvertAllow )

+		{

+			// Exlude only the specified entity name or classname

+			if ( GetOuter()->NameMatches(pszAllowUse) || GetOuter()->ClassMatches( pszAllowUse ) )

+				return false;

+		}

+		else

+		{

+			// Exclude everything but the allowed entity name or classname

+			if ( !GetOuter()->NameMatches( pszAllowUse) && !GetOuter()->ClassMatches( pszAllowUse ) )

+				return false;

+		}

+	}

+

+	// --------------------------------------------------------------------------			

+	//  Get the destination nodeID

+	// --------------------------------------------------------------------------			

+	int endID = pLink->DestNodeID(startID);

+

+	// --------------------------------------------------------------------------

+	// Make sure I have the ability to do the type of movement specified by the link

+	// --------------------------------------------------------------------------

+	int linkMoveTypes = pLink->m_iAcceptedMoveTypes[GetHullType()];

+	int moveType = ( linkMoveTypes & CapabilitiesGet() );

+

+	CAI_Node *pStartNode,*pEndNode;

+

+	pStartNode = GetNetwork()->GetNode(startID);

+	pEndNode = GetNetwork()->GetNode(endID);

+

+	if ( (linkMoveTypes & bits_CAP_MOVE_JUMP) && !moveType )

+	{

+		CAI_Hint *pStartHint = pStartNode->GetHint();

+		CAI_Hint *pEndHint = pEndNode->GetHint();

+		if ( pStartHint && pEndHint )

+		{

+			if ( pStartHint->HintType() == HINT_JUMP_OVERRIDE && 

+				 pEndHint->HintType() == HINT_JUMP_OVERRIDE &&

+				 ( ( ( pStartHint->GetSpawnFlags() | pEndHint->GetSpawnFlags() ) & SF_ALLOW_JUMP_UP ) || pStartHint->GetAbsOrigin().z > pEndHint->GetAbsOrigin().z ) )

+			{

+				if ( !pStartNode->IsLocked() )

+				{

+					if ( pStartHint->GetTargetNode() == -1 || pStartHint->GetTargetNode() == endID )

+						moveType = bits_CAP_MOVE_JUMP;

+				}

+			}

+		}

+	}

+

+	if (!moveType)

+	{

+		return false;

+	}

+

+	// --------------------------------------------------------------------------

+	// Check if NPC has a reason not to use the desintion node

+	// --------------------------------------------------------------------------

+	if (GetOuter()->IsUnusableNode(endID, pEndNode->GetHint()))

+	{

+		return false;

+	}	

+

+	// --------------------------------------------------------------------------

+	// If a jump make sure the jump is within NPC's legal parameters for jumping

+	// --------------------------------------------------------------------------

+	if (moveType == bits_CAP_MOVE_JUMP)

+	{	

+		if (!GetOuter()->IsJumpLegal(pStartNode->GetPosition(GetHullType()), 

+									 pEndNode->GetPosition(GetHullType()),

+									 pEndNode->GetPosition(GetHullType())))

+		{

+			return false;

+		}

+	}

+

+	// --------------------------------------------------------------------------

+	// If an NPC suggested that this link is stale and I haven't checked it yet

+	// I should make sure the link is still valid before proceeding

+	// --------------------------------------------------------------------------

+	if (pLink->m_LinkInfo & bits_LINK_STALE_SUGGESTED)

+	{

+		if (IsLinkStillStale(moveType, pLink))

+		{

+			return false;

+		}

+	}

+	return true;

+}

+

+//-----------------------------------------------------------------------------

+

+static int NPCBuildFlags( CAI_BaseNPC *pNPC, const Vector &vecOrigin )

+{

+	// If vecOrigin the the npc's position and npc is climbing only climb nodes allowed

+	if (pNPC->GetLocalOrigin() == vecOrigin && pNPC->GetNavType() == NAV_CLIMB) 

+	{

+		return bits_BUILD_CLIMB;

+	}

+	else if (pNPC->CapabilitiesGet() & bits_CAP_MOVE_FLY)

+	{

+		return bits_BUILD_FLY | bits_BUILD_GIVEWAY;

+	}

+	else if (pNPC->CapabilitiesGet() & bits_CAP_MOVE_GROUND)

+	{

+		int buildFlags = bits_BUILD_GROUND | bits_BUILD_GIVEWAY;

+		if (pNPC->CapabilitiesGet() & bits_CAP_MOVE_JUMP)

+		{

+			buildFlags |= bits_BUILD_JUMP;

+		}

+

+		return buildFlags;

+	}

+	return 0;

+}

+

+

+//-----------------------------------------------------------------------------

+// Creates a node waypoint

+//-----------------------------------------------------------------------------

+AI_Waypoint_t* CAI_Pathfinder::CreateNodeWaypoint( Hull_t hullType, int nodeID, int nodeFlags )

+{

+	CAI_Node *pNode = GetNetwork()->GetNode(nodeID);

+

+	Navigation_t navType;

+	switch(pNode->GetType())

+	{

+	case NODE_CLIMB:

+		navType = NAV_CLIMB;

+		break;

+

+	case NODE_AIR:

+		navType = NAV_FLY;

+		break;

+

+	default:

+		navType = NAV_GROUND;

+		break;

+	}

+

+	return new AI_Waypoint_t( pNode->GetPosition(hullType), pNode->GetYaw(), navType, ( bits_WP_TO_NODE | nodeFlags) , nodeID );

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Returns a route to a node for the given npc with the given 

+//			build flags

+//-----------------------------------------------------------------------------

+AI_Waypoint_t* CAI_Pathfinder::RouteToNode(const Vector &vecOrigin, int buildFlags, int nodeID, float goalTolerance)

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_RouteToNode );

+	

+	buildFlags |= NPCBuildFlags( GetOuter(), vecOrigin );

+	buildFlags &= ~bits_BUILD_GET_CLOSE;

+

+	// Check if vecOrigin is already at the smallest node

+

+	// FIXME: an equals check is a bit sloppy, this should be a tolerance

+	const Vector &vecNodePosition = GetNetwork()->GetNode(nodeID)->GetPosition(GetHullType());

+	if (vecOrigin == vecNodePosition)

+	{

+		return CreateNodeWaypoint( GetHullType(), nodeID, bits_WP_TO_GOAL );

+	}

+

+	// Otherwise try to build a local route to the node

+	AI_Waypoint_t *pResult = BuildLocalRoute(vecOrigin, 

+		vecNodePosition, NULL, bits_WP_TO_NODE, nodeID, buildFlags, goalTolerance);

+	if ( pResult )

+		pResult->iNodeID = nodeID;

+	return pResult;

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Returns a route to a node for the given npc with the given 

+//			build flags

+//-----------------------------------------------------------------------------

+

+AI_Waypoint_t* CAI_Pathfinder::RouteFromNode(const Vector &vecOrigin, int buildFlags, int nodeID, float goalTolerance)

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_RouteFromNode );

+	

+	buildFlags |= NPCBuildFlags( GetOuter(), vecOrigin );

+	buildFlags |= bits_BUILD_GET_CLOSE;

+

+	// Check if vecOrigin is already at the smallest node

+	// FIXME: an equals check is a bit sloppy, this should be a tolerance

+	CAI_Node *pNode = GetNetwork()->GetNode(nodeID);

+	const Vector &vecNodePosition = pNode->GetPosition(GetHullType());

+

+	if (vecOrigin == vecNodePosition)

+	{

+		return CreateNodeWaypoint( GetHullType(), nodeID, bits_WP_TO_GOAL );

+	}

+

+	// Otherwise try to build a local route from the node

+	AI_Waypoint_t* pResult = BuildLocalRoute( vecNodePosition, 

+											  vecOrigin, NULL, bits_WP_TO_GOAL, NO_NODE, buildFlags, goalTolerance);

+

+	// Handle case of target hanging over edge near climb dismount

+	if ( !pResult &&

+		 pNode->GetType() == NODE_CLIMB && 

+		 ( vecOrigin - vecNodePosition ).Length2DSqr() < 32.0*32.0 &&

+		 GetOuter()->GetMoveProbe()->CheckStandPosition(vecNodePosition, MASK_NPCSOLID_BRUSHONLY ) )

+	{

+		pResult = new AI_Waypoint_t( vecOrigin, 0, NAV_GROUND, bits_WP_TO_GOAL, nodeID );

+	}

+

+	return pResult;

+}

+

+//-----------------------------------------------------------------------------

+// Builds a simple route (no triangulation, no making way)

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildSimpleRoute( Navigation_t navType, const Vector &vStart, 

+	const Vector &vEnd, const CBaseEntity *pTarget, int endFlags, int nodeID, 

+	int nodeTargetType, float flYaw )

+{

+	Assert( navType == NAV_JUMP || navType == NAV_CLIMB ); // this is what this here function is for

+	// Only allowed to jump to ground nodes

+	if ((nodeID == NO_NODE)	|| (GetNetwork()->GetNode(nodeID)->GetType() == nodeTargetType) )

+	{

+		AIMoveTrace_t moveTrace;

+		GetOuter()->GetMoveProbe()->MoveLimit( navType, vStart, vEnd, MASK_NPCSOLID, pTarget, &moveTrace );

+

+		// If I was able to make the move, or the vEnd is the

+		// goal and I'm within tolerance, just move to vEnd

+		if (!IsMoveBlocked(moveTrace))

+		{

+			// It worked so return a route of length one to the endpoint

+			return new AI_Waypoint_t( vEnd, flYaw, navType, endFlags, nodeID );

+		}

+	}

+	return NULL;

+}

+

+

+//-----------------------------------------------------------------------------

+// Builds a complex route (triangulation, making way)

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildComplexRoute( Navigation_t navType, const Vector &vStart, 

+	const Vector &vEnd, const CBaseEntity *pTarget, int endFlags, int nodeID, 

+	int buildFlags, float flYaw, float goalTolerance, float maxLocalNavDistance )

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_BuildComplexRoute );

+	

+	float flTotalDist = ComputePathDistance( navType, vStart, vEnd );

+	if ( flTotalDist < 0.0625 )

+	{

+		return new AI_Waypoint_t( vEnd, flYaw, navType, endFlags, nodeID );

+	}

+	

+	unsigned int collideFlags = (buildFlags & bits_BUILD_IGNORE_NPCS) ? MASK_NPCSOLID_BRUSHONLY : MASK_NPCSOLID;

+

+	bool bCheckGround = (GetOuter()->CapabilitiesGet() & bits_CAP_SKIP_NAV_GROUND_CHECK) ? false : true;

+

+	if ( flTotalDist <= maxLocalNavDistance )

+	{

+		AIMoveTrace_t moveTrace;

+

+		AI_PROFILE_SCOPE_BEGIN( CAI_Pathfinder_BuildComplexRoute_Direct );

+	

+		GetOuter()->GetMoveProbe()->MoveLimit( navType, vStart, vEnd, collideFlags, pTarget, (bCheckGround) ? 100 : 0, &moveTrace);

+

+		// If I was able to make the move...

+		if (!IsMoveBlocked(moveTrace))

+		{

+			// It worked so return a route of length one to the endpoint

+			return new AI_Waypoint_t( vEnd, flYaw, navType, endFlags, nodeID );

+		}

+	

+		// ...or the vEnd is thegoal and I'm within tolerance, just move to vEnd

+		if ( (buildFlags & bits_BUILD_GET_CLOSE) && 

+			 (endFlags & bits_WP_TO_GOAL) && 

+			 moveTrace.flDistObstructed <= goalTolerance ) 

+		{

+			return new AI_Waypoint_t( vEnd, flYaw, navType, endFlags, nodeID );

+		}

+

+		AI_PROFILE_SCOPE_END();

+

+		// -------------------------------------------------------------------

+		// Try to triangulate if requested

+		// -------------------------------------------------------------------

+

+		AI_PROFILE_SCOPE_BEGIN( CAI_Pathfinder_BuildComplexRoute_Triangulate );

+		

+		if (buildFlags & bits_BUILD_TRIANG)

+		{

+			if ( !UseStrongOptimizations() || ( GetOuter()->GetState() == NPC_STATE_SCRIPT || GetOuter()->IsCurSchedule( SCHED_SCENE_GENERIC, false ) ) )

+			{

+				float flTotalDist = ComputePathDistance( navType, vStart, vEnd );

+

+				AI_Waypoint_t *triangRoute = BuildTriangulationRoute(vStart, vEnd, pTarget, 

+					endFlags, nodeID, flYaw, flTotalDist - moveTrace.flDistObstructed, navType);

+

+				if (triangRoute)

+				{

+					return triangRoute;

+				}

+			}

+		}

+		

+		AI_PROFILE_SCOPE_END();

+

+		// -------------------------------------------------------------------

+		// Try to giveway if requested

+		// -------------------------------------------------------------------

+		if (moveTrace.fStatus == AIMR_BLOCKED_NPC && (buildFlags & bits_BUILD_GIVEWAY))

+		{

+			// If I can't get there even ignoring NPCs, don't bother to request a giveway

+			AIMoveTrace_t moveTrace2;

+			GetOuter()->GetMoveProbe()->MoveLimit( navType, vStart, vEnd, MASK_NPCSOLID_BRUSHONLY, pTarget, (bCheckGround) ? 100 : 0, &moveTrace2 );

+

+			if (!IsMoveBlocked(moveTrace2))

+			{							

+				// If I can clear the way return a route of length one to the target location

+				if ( CanGiveWay(vStart, vEnd, moveTrace.pObstruction) )

+				{

+					return new AI_Waypoint_t( vEnd, flYaw, navType, endFlags, nodeID );

+				}

+			}

+		}

+	}

+	return NULL;

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempts to build a jump route between vStart

+//			and vEnd, ignoring entity pTarget

+// Input  :

+// Output : Returns a route if sucessful or NULL if no local route was possible

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildJumpRoute(const Vector &vStart, const Vector &vEnd, 

+	const CBaseEntity *pTarget, int endFlags, int nodeID, int buildFlags, float flYaw)

+{

+	// Only allowed to jump to ground nodes

+	return BuildSimpleRoute( NAV_JUMP, vStart, vEnd, pTarget, 

+		endFlags, nodeID, NODE_GROUND, flYaw );

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempts to build a climb route between vStart

+//			and vEnd, ignoring entity pTarget

+// Input  :

+// Output : Returns a route if sucessful or NULL if no climb route was possible

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildClimbRoute(const Vector &vStart, const Vector &vEnd, const CBaseEntity *pTarget, int endFlags, int nodeID, int buildFlags, float flYaw)

+{

+	// Only allowed to climb to climb nodes

+	return BuildSimpleRoute( NAV_CLIMB, vStart, vEnd, pTarget, 

+		endFlags, nodeID, NODE_CLIMB, flYaw );

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempts to build a ground route between vStart

+//			and vEnd, ignoring entity pTarget the the given tolerance

+// Input  :

+// Output : Returns a route if sucessful or NULL if no ground route was possible

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildGroundRoute(const Vector &vStart, const Vector &vEnd, 

+	const CBaseEntity *pTarget, int endFlags, int nodeID, int buildFlags, float flYaw, float goalTolerance)

+{

+	return BuildComplexRoute( NAV_GROUND, vStart, vEnd, pTarget, 

+		endFlags, nodeID, buildFlags, flYaw, goalTolerance, MAX_LOCAL_NAV_DIST_GROUND[UseStrongOptimizations()] );

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempts to build a fly route between vStart

+//			and vEnd, ignoring entity pTarget the the given tolerance

+// Input  :

+// Output : Returns a route if sucessful or NULL if no ground route was possible

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildFlyRoute(const Vector &vStart, const Vector &vEnd, 

+	const CBaseEntity *pTarget, int endFlags, int nodeID, int buildFlags, float flYaw, float goalTolerance)

+{

+	return BuildComplexRoute( NAV_FLY, vStart, vEnd, pTarget, 

+		endFlags, nodeID, buildFlags, flYaw, goalTolerance, MAX_LOCAL_NAV_DIST_FLY[UseStrongOptimizations()] );

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempts to build a route between vStart and vEnd, requesting the

+//			pNPCBlocker to get out of the way

+// Input  :

+// Output : Returns a route if sucessful or NULL if giveway failed

+//-----------------------------------------------------------------------------

+bool CAI_Pathfinder::CanGiveWay( const Vector& vStart, const Vector& vEnd, CBaseEntity *pBlocker)

+{

+	// FIXME: make this a CAI_BaseNPC member function

+	CAI_BaseNPC *pNPCBlocker = pBlocker->MyNPCPointer();

+	if (pNPCBlocker && pNPCBlocker->edict()) 

+	{

+		Disposition_t eDispBlockerToMe = pNPCBlocker->IRelationType( GetOuter() );

+		if ( ( eDispBlockerToMe == D_LI ) || ( eDispBlockerToMe == D_NU ) )

+		{

+			return true;

+		}

+		

+		return false;

+

+		// FIXME: this is called in route creation, not navigation.  It shouldn't actually make

+		// anyone get out of their way, just see if they'll honor the request.

+		// things like locked doors, enemies and such should refuse, all others shouldn't.

+		// things like breakables should know who is trying to break them, though a door hidden behind

+		// some boxes shouldn't be known to the AI even though a route should connect through them but

+		// be turned off.

+

+		/*

+		Vector moveDir	   = (vEnd - vStart).Normalize();

+		Vector blockerDir  = (pNPCBlocker->GetLocalOrigin()  - vStart);

+		float  blockerDist = DotProduct(moveDir,blockerDir);

+		Vector blockPos	   = vStart + (moveDir*blockerDist);

+

+		if (pNPCBlocker->RequestGiveWay ( m_owner->GetLocalOrigin(), blockPos, moveDir, m_owner->m_eHull))

+		{

+			return true;

+		}

+		*/

+	}

+	return false;

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempts to build a triangulation route between vStart

+//			and vEnd, ignoring entity pTarget the the given tolerance and

+//			triangulating around a blocking object at blockDist

+// Input  :

+// Output : Returns a route if sucessful or NULL if no local route was possible

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildTriangulationRoute(

+	  const Vector &vStart, // from where

+	  const Vector &vEnd,	// to where

+	  const CBaseEntity *pTarget,	// an entity I can ignore

+	  int endFlags,			// add these WP flags to the last waypoint

+	  int nodeID,			// node id for the last waypoint

+	  float flYaw,			// ideal yaw for the last waypoint

+	  float flDistToBlocker,// how far away is the obstruction from the start?

+	  Navigation_t navType)

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_BuildTriangulationRoute );

+	

+	Vector vApex;

+	if (!Triangulate(navType, vStart, vEnd, flDistToBlocker, pTarget, &vApex ))

+		return NULL;

+

+	//-----------------------------------------------------------------------------

+	// it worked, create a route

+	//-----------------------------------------------------------------------------

+	AI_Waypoint_t *pWayPoint2 = new AI_Waypoint_t( vEnd, flYaw, navType, endFlags, nodeID );

+

+	// FIXME: Compute a reasonable yaw here

+	AI_Waypoint_t *waypoint1 = new AI_Waypoint_t( vApex, 0, navType, bits_WP_TO_DETOUR, NO_NODE );

+	waypoint1->SetNext(pWayPoint2);

+

+	return waypoint1;

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Get the next node (with wrapping) around a circularly wound path

+// Input  : nLastNode - The starting node

+//			nDirection - Direction we're moving

+//			nNumNodes - Total nodes in the chain

+//-----------------------------------------------------------------------------

+inline int GetNextPoint( int nLastNode, int nDirection, int nNumNodes )

+{

+	int nNextNode = nLastNode + nDirection;

+	if ( nNextNode > (nNumNodes-1) )

+		nNextNode = 0;

+	else if ( nNextNode < 0 )

+		nNextNode = (nNumNodes-1);

+

+	return nNextNode;

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempt to wind a route through a series of node points in a specified direction.

+// Input  : *vecCorners - Points to test between

+//			nNumCorners - Number of points to test

+//			&vecStart - Starting position

+//			&vecEnd - Ending position

+// Output : Route through the points

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildRouteThroughPoints( Vector *vecPoints, int nNumPoints, int nDirection, int nStartIndex, int nEndIndex, Navigation_t navType, CBaseEntity *pTarget )

+{

+	AIMoveTrace_t endTrace;

+	endTrace.fStatus = AIMR_OK;

+

+	CAI_MoveProbe *pMoveProbe = GetOuter()->GetMoveProbe();

+

+	AI_Waypoint_t *pFirstRoute = NULL;

+	AI_Waypoint_t *pHeadRoute = NULL;

+

+	int nCurIndex = nStartIndex;

+	int nNextIndex;

+

+	// FIXME: Must be able to move to the first position (these needs some parameterization) 

+	pMoveProbe->MoveLimit( navType, GetOuter()->GetAbsOrigin(), vecPoints[nStartIndex], MASK_NPCSOLID, pTarget, &endTrace );

+	if ( IsMoveBlocked( endTrace ) )

+	{

+		// NDebugOverlay::HorzArrow( GetOuter()->GetAbsOrigin(), vecPoints[nStartIndex], 8.0f, 255, 0, 0, 0, true, 4.0f );

+		return NULL;

+	}

+

+	// NDebugOverlay::HorzArrow( GetOuter()->GetAbsOrigin(), vecPoints[nStartIndex], 8.0f, 0, 255, 0, 0, true, 4.0f );

+

+	int nRunAwayCount = 0;

+	while ( nRunAwayCount++ < nNumPoints )

+	{

+		// Advance our index in the specified direction

+		nNextIndex = GetNextPoint( nCurIndex, nDirection, nNumPoints );

+

+		// Try and build a local route between the current and next point

+		pMoveProbe->MoveLimit( navType, vecPoints[nCurIndex], vecPoints[nNextIndex], MASK_NPCSOLID, pTarget, &endTrace );

+		if ( IsMoveBlocked( endTrace ) )

+		{

+			// TODO: Triangulate here if we failed?

+

+			// We failed, so give up

+			if ( pHeadRoute )

+			{

+				DeleteAll( pHeadRoute );

+			}

+

+			// NDebugOverlay::HorzArrow( vecPoints[nCurIndex], vecPoints[nNextIndex], 8.0f, 255, 0, 0, 0, true, 4.0f );

+			return NULL;

+		}

+

+		// NDebugOverlay::HorzArrow( vecPoints[nCurIndex], vecPoints[nNextIndex], 8.0f, 0, 255, 0, 0, true, 4.0f );

+

+		if ( pHeadRoute == NULL )

+		{

+			// Start a new route head

+			pFirstRoute = pHeadRoute = new AI_Waypoint_t( vecPoints[nCurIndex], 0.0f, navType, bits_WP_TO_DETOUR, NO_NODE );

+		}

+		else

+		{

+			// Link a new waypoint into the path

+			AI_Waypoint_t *pNewNode = new AI_Waypoint_t( vecPoints[nCurIndex], 0.0f, navType, bits_WP_TO_DETOUR|bits_WP_DONT_SIMPLIFY, NO_NODE );

+			pHeadRoute->SetNext( pNewNode );

+			pHeadRoute = pNewNode;

+		}

+

+		// See if we're done

+		if ( nNextIndex == nEndIndex )

+		{

+			AI_Waypoint_t *pNewNode = new AI_Waypoint_t( vecPoints[nEndIndex], 0.0f, navType, bits_WP_TO_DETOUR, NO_NODE );

+			pHeadRoute->SetNext( pNewNode );

+			pHeadRoute = pNewNode;

+			break;

+		}

+

+		// Advance one node

+		nCurIndex = nNextIndex;

+	}

+

+	return pFirstRoute;

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Find the closest point in a list of points, to a specified position

+// Input  : &vecPosition - Position to test against

+//			*vecPoints - List of vectors we'll check

+//			nNumPoints - Number of points in the list

+// Output : Index to the closest point in the list

+//-----------------------------------------------------------------------------

+inline int ClosestPointToPosition( const Vector &vecPosition, Vector *vecPoints, int nNumPoints )

+{

+	int   nBestNode = -1;

+	float flBestDistSqr = FLT_MAX;

+	float flDistSqr;

+	for ( int i = 0; i < nNumPoints; i++ )

+	{

+		flDistSqr = ( vecPoints[i] - vecPosition ).LengthSqr();

+		if ( flDistSqr < flBestDistSqr )

+		{

+			flBestDistSqr = flDistSqr;

+			nBestNode = i;

+		}

+	}

+

+	return nBestNode;

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Find which winding through a circular list is shortest in physical distance travelled

+// Input  : &vecStart - Where we started from

+//			nStartPoint - Starting index into the points

+//			nEndPoint - Ending index into the points

+//			nNumPoints - Number of points in the list

+//			*vecPoints - List of vectors making up a list of points

+//-----------------------------------------------------------------------------

+inline int ShortestDirectionThroughPoints( const Vector &vecStart, int nStartPoint, int nEndPoint, Vector *vecPoints, int nNumPoints )

+{

+	const int nClockwise = 1;

+	const int nCounterClockwise = -1;

+

+	// Find the quickest direction around the object

+	int nCurPoint = nStartPoint;

+	int nNextPoint = GetNextPoint( nStartPoint, 1, nNumPoints );

+

+	float flStartDistSqr = ( vecStart - vecPoints[nStartPoint] ).LengthSqr();

+	float flDistanceSqr = flStartDistSqr;

+

+	// Try going clockwise first

+	for ( int i = 0; i < nNumPoints; i++ )

+	{

+		flDistanceSqr += ( vecPoints[nCurPoint] - vecPoints[nNextPoint] ).LengthSqr();

+

+		if ( nNextPoint == nEndPoint )

+			break;

+

+		nNextPoint = GetNextPoint( nNextPoint, 1, nNumPoints );

+	}

+

+	// Save this to test against

+	float flBestDistanceSqr = flDistanceSqr;

+

+	// Start from the beginning again

+	flDistanceSqr = flStartDistSqr;

+

+	nCurPoint = nStartPoint;

+	nNextPoint = GetNextPoint( nStartPoint, -1, nNumPoints );

+

+	// Now go the other way and see if it's shorter to do so

+	for ( int i = 0; i < nNumPoints; i++ )

+	{

+		flDistanceSqr += ( vecPoints[nCurPoint] - vecPoints[nNextPoint] ).LengthSqr();

+

+		// We've gone over our maximum so we can't be shorter

+		if ( flDistanceSqr > flBestDistanceSqr )

+			break;

+

+		// We hit the end, we're shorter

+		if ( nNextPoint == nEndPoint )

+			return nCounterClockwise;

+

+		nNextPoint = GetNextPoint( nNextPoint, -1, nNumPoints );

+	}

+

+	return nClockwise;

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempt to build an avoidance route around an object using its OBB

+//			Currently this function is meant for NPCs moving around a vehicle, 

+//			and is very specialized as such

+//

+// Output : Returns a route if successful or NULL if no local route was possible

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildOBBAvoidanceRoute(	const Vector &vStart, const Vector &vEnd,

+														const CBaseEntity *pObstruction, // obstruction to avoid

+														const CBaseEntity *pTarget,		 // target to ignore

+														Navigation_t navType )

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_BuildOBBAvoidanceRoute );

+

+	// If the point we're navigating to is within our OBB, then fail

+	// TODO: We could potentially also just try to get as near as possible

+	if ( pObstruction->CollisionProp()->IsPointInBounds( vEnd ) )

+		return NULL;

+

+	// Find out how much we'll need to inflate the collision bounds to let us move past

+	Vector vecSize = pObstruction->CollisionProp()->OBBSize();

+	float flWidth = GetOuter()->GetHullWidth() * 0.5f;

+

+	float flWidthPercX = ( flWidth / vecSize.x );

+	float flWidthPercY = ( flWidth / vecSize.y );

+

+	// Find the points around the object, bloating it by our hull width

+	// The ordering of these corners wind clockwise around the object, starting at the top left

+	Vector vecPoints[4];

+	pObstruction->CollisionProp()->NormalizedToWorldSpace( Vector(  -flWidthPercX, 1+flWidthPercY, 0.25f ), &vecPoints[0] );

+	pObstruction->CollisionProp()->NormalizedToWorldSpace( Vector( 1+flWidthPercX, 1+flWidthPercY, 0.25f ), &vecPoints[1] );

+	pObstruction->CollisionProp()->NormalizedToWorldSpace( Vector( 1+flWidthPercX,  -flWidthPercY, 0.25f ), &vecPoints[2] );

+	pObstruction->CollisionProp()->NormalizedToWorldSpace( Vector(  -flWidthPercX,  -flWidthPercY, 0.25f ), &vecPoints[3] );

+

+	// Find the two points nearest our goals

+	int nStartPoint = ClosestPointToPosition( vStart, vecPoints, ARRAYSIZE( vecPoints ) );

+	int nEndPoint = ClosestPointToPosition( vEnd, vecPoints, ARRAYSIZE( vecPoints ) );

+

+	// We won't be able to build a route if we're moving no distance between points

+	if ( nStartPoint == nEndPoint )

+		return NULL;

+

+	// Find the shortest path around this wound polygon (direction is how to step through array)

+	int nDirection = ShortestDirectionThroughPoints( vStart, nStartPoint, nEndPoint, vecPoints, ARRAYSIZE( vecPoints ) );

+

+	// Attempt to build a route in our direction

+	AI_Waypoint_t *pRoute = BuildRouteThroughPoints( vecPoints, ARRAYSIZE(vecPoints), nDirection, nStartPoint, nEndPoint, navType, (CBaseEntity *) pTarget );

+	if ( pRoute == NULL )

+	{

+		// Failed that way, so try the opposite

+		pRoute = BuildRouteThroughPoints( vecPoints, ARRAYSIZE(vecPoints), (-nDirection), nStartPoint, nEndPoint, navType, (CBaseEntity *) pTarget );

+		if ( pRoute == NULL )

+			return NULL;

+	}

+

+	return pRoute;

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempts to build a local route (not using nodes) between vStart

+//			and vEnd, ignoring entity pTarget the the given tolerance

+// Input  :

+// Output : Returns a route if successful or NULL if no local route was possible

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildLocalRoute(const Vector &vStart, const Vector &vEnd, const CBaseEntity *pTarget, int endFlags, int nodeID, int buildFlags, float goalTolerance)

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_BuildLocalRoute );

+

+	// Get waypoint yaw

+	float flYaw;

+	if (nodeID != NO_NODE)

+	{

+		flYaw = GetNetwork()->GetNode(nodeID)->GetYaw();

+	}

+	else

+	{

+		flYaw = 0;

+	}

+

+	// Try a ground route if requested

+	if (buildFlags & bits_BUILD_GROUND)

+	{

+		AI_Waypoint_t *groundRoute = BuildGroundRoute(vStart,vEnd,pTarget,endFlags,nodeID,buildFlags,flYaw,goalTolerance);

+

+		if (groundRoute)

+		{

+			return groundRoute;

+		}

+	}

+

+	// Try a fly route if requested

+	if ( buildFlags & bits_BUILD_FLY )

+	{

+		AI_Waypoint_t *flyRoute = BuildFlyRoute(vStart,vEnd,pTarget,endFlags,nodeID,buildFlags,flYaw,goalTolerance);

+

+		if (flyRoute)

+		{

+			return flyRoute;

+		}

+	}

+

+	// Try a jump route if NPC can jump and requested

+	if ((buildFlags & bits_BUILD_JUMP) && (CapabilitiesGet() & bits_CAP_MOVE_JUMP))

+	{

+		AI_Waypoint_t *jumpRoute = BuildJumpRoute(vStart,vEnd,pTarget,endFlags,nodeID,buildFlags,flYaw);

+

+		if (jumpRoute)

+		{

+			return jumpRoute;

+		}

+	}

+

+	// Try a climb route if NPC can climb and requested

+	if ((buildFlags & bits_BUILD_CLIMB)	&& (CapabilitiesGet() & bits_CAP_MOVE_CLIMB))

+	{

+		AI_Waypoint_t *climbRoute = BuildClimbRoute(vStart,vEnd,pTarget,endFlags,nodeID,buildFlags,flYaw);

+		

+		if (climbRoute)

+		{

+			return climbRoute;

+		}

+	}

+

+	// Everything failed so return a NULL route

+	return NULL;

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Builds a route to the given vecGoal using either local movement

+//			or nodes

+//-----------------------------------------------------------------------------

+

+ConVar ai_no_local_paths( "ai_no_local_paths", "0" );

+

+AI_Waypoint_t *CAI_Pathfinder::BuildRoute( const Vector &vStart, const Vector &vEnd, CBaseEntity *pTarget, float goalTolerance, Navigation_t curNavType, bool bLocalSucceedOnWithinTolerance )

+{

+	int buildFlags = 0;

+	bool bTryLocal = !ai_no_local_paths.GetBool();

+

+	// Set up build flags

+	if (curNavType == NAV_CLIMB)

+	{

+		 // if I'm climbing, then only allow routes that are also climb routes

+		buildFlags = bits_BUILD_CLIMB;

+		bTryLocal = false;

+	}

+	else if ( (CapabilitiesGet() & bits_CAP_MOVE_FLY) || (CapabilitiesGet() & bits_CAP_MOVE_SWIM) )

+	{

+		buildFlags = (bits_BUILD_FLY | bits_BUILD_GIVEWAY | bits_BUILD_TRIANG);

+	}

+	else if (CapabilitiesGet() & bits_CAP_MOVE_GROUND)

+	{

+		buildFlags = (bits_BUILD_GROUND | bits_BUILD_GIVEWAY | bits_BUILD_TRIANG);

+		if ( CapabilitiesGet() & bits_CAP_MOVE_JUMP )

+		{

+			buildFlags |= bits_BUILD_JUMP;

+		}

+	}

+

+	// If our local moves can succeed if we get within the goaltolerance, set the flag

+	if ( bLocalSucceedOnWithinTolerance )

+	{

+		buildFlags |= bits_BUILD_GET_CLOSE;

+	}

+

+	AI_Waypoint_t *pResult = NULL;

+

+	//  First try a local route 

+	if ( bTryLocal && CanUseLocalNavigation() )

+	{

+		pResult = BuildLocalRoute(vStart, vEnd, pTarget, 

+								  bits_WP_TO_GOAL, NO_NODE, 

+								  buildFlags, goalTolerance);

+	}

+

+	//  If the fails, try a node route

+	if ( !pResult )

+	{

+		pResult = BuildNodeRoute( vStart, vEnd, buildFlags, goalTolerance );

+	}

+

+	m_bIgnoreStaleLinks = false;

+

+	return pResult;

+}

+

+void CAI_Pathfinder::UnlockRouteNodes( AI_Waypoint_t *pPath )

+{

+	CAI_Node *pNode;

+	while ( pPath )

+	{

+		if ( pPath->iNodeID != NO_NODE && ( pNode = GetNetwork()->GetNode(pPath->iNodeID) ) != NULL && pNode->IsLocked() )

+			pNode->Unlock();

+		pPath = pPath->GetNext();

+	}

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Attempts to build a radial route around the given center position

+//			over a given arc size

+//

+// Input  : vStartPos	- where route should start from

+//			vCenterPos	- the center of the arc

+//			vGoalPos	- ultimate goal position

+//			flRadius	- radius of the arc

+//			flArc		- how long should the path be (in degrees)

+//			bClockwise	- the direction we are heading

+// Output : The route

+//-----------------------------------------------------------------------------

+AI_Waypoint_t *CAI_Pathfinder::BuildRadialRoute( const Vector &vStartPos, const Vector &vCenterPos, const Vector &vGoalPos, float flRadius, float flArc, float flStepDist, bool bClockwise, float goalTolerance, bool bAirRoute /*= false*/ )

+{

+	MARK_TASK_EXPENSIVE();

+

+	// ------------------------------------------------------------------------------

+	// Make sure we have a minimum distance between nodes.  For the given 

+	// radius, calculate the angular step necessary for this distance.

+	// IMPORTANT: flStepDist must be large enough that given the 

+	//			  NPC's movment speed that it can come to a stop

+	// ------------------------------------------------------------------------------

+	float flAngleStep = 2.0f * atan((0.5f*flStepDist)/flRadius);	

+

+	// Flip direction if clockwise

+	if ( bClockwise )

+	{

+		flArc		*= -1;

+		flAngleStep *= -1;

+	}

+

+	// Calculate the start angle on the arc in world coordinates

+	Vector vStartDir = ( vStartPos - vCenterPos );

+	VectorNormalize( vStartDir );

+

+	// Get our control angles

+	float flStartAngle	= DEG2RAD(UTIL_VecToYaw(vStartDir));

+	float flEndAngle	= flStartAngle + DEG2RAD(flArc);

+

+	//  Offset set our first node by one arc step so NPC doesn't run perpendicular to the arc when starting a different radius

+	flStartAngle += flAngleStep;

+

+	AI_Waypoint_t*	pHeadRoute	= NULL;	// Pointer to the beginning of the route chains

+	AI_Waypoint_t*	pNextRoute	= NULL; // Next leg of the route

+	AI_Waypoint_t*  pLastRoute	= NULL; // The last route chain added to the head

+	Vector			vLastPos	= vStartPos; // Last position along the arc in worldspace

+	int				fRouteBits = ( bAirRoute ) ? bits_BUILD_FLY : bits_BUILD_GROUND; // Whether this is an air route or not

+	float			flCurAngle = flStartAngle; // Starting angle

+	Vector			vNextPos;

+	

+	// Make sure that we've got somewhere to go.  This generally means your trying to walk too small an arc.

+	Assert( ( bClockwise && flCurAngle > flEndAngle ) || ( !bClockwise && flCurAngle < flEndAngle ) );

+

+	// Start iterating through our arc

+	while( 1 )

+	{

+		// See if we've ended our run

+		if ( ( bClockwise && flCurAngle <= flEndAngle ) || ( !bClockwise && flCurAngle >= flEndAngle ) )

+			break;

+		

+		// Get our next position along the arc

+		vNextPos	= vCenterPos;

+		vNextPos.x	+= flRadius * cos( flCurAngle );

+		vNextPos.y	+= flRadius * sin( flCurAngle );

+

+		// Build a route from the last position to the current one

+		pNextRoute = BuildLocalRoute( vLastPos, vNextPos, NULL, NULL, NO_NODE, fRouteBits, goalTolerance);

+		

+		// If we can't find a route, we failed

+		if ( pNextRoute == NULL )

+			return NULL;

+

+		// Don't simplify the route (otherwise we'll cut corners where we don't want to!

+		pNextRoute->ModifyFlags( bits_WP_DONT_SIMPLIFY, true );

+			

+		if ( pHeadRoute )

+		{

+			// Tack the routes together

+			AddWaypointLists( pHeadRoute, pNextRoute );

+		}

+		else

+		{

+			// Otherwise we're now the previous route

+			pHeadRoute = pNextRoute;

+		}

+		

+		// Move our position

+		vLastPos  = vNextPos;

+		pLastRoute = pNextRoute;

+

+		// Move our current angle

+		flCurAngle += flAngleStep;

+	}

+

+	// NOTE: We could also simply build a local route with no curve, but it's unlikely that's what was intended by the caller

+	if ( pHeadRoute == NULL )

+		return NULL;

+

+	// Append a path to the final position

+	pLastRoute = BuildLocalRoute( vLastPos, vGoalPos, NULL, NULL, NO_NODE, bAirRoute ? bits_BUILD_FLY : bits_BUILD_GROUND, goalTolerance );	

+	if ( pLastRoute == NULL )

+		return NULL;

+

+	// Allow us to simplify the last leg of the route

+	pLastRoute->ModifyFlags( bits_WP_DONT_SIMPLIFY, false );

+	pLastRoute->ModifyFlags( bits_WP_TO_GOAL, true );

+

+	// Add them together

+	AddWaypointLists( pHeadRoute, pLastRoute );

+	

+	// Give back the complete route

+	return pHeadRoute;

+}

+

+

+//-----------------------------------------------------------------------------

+// Checks a stale navtype route 

+//-----------------------------------------------------------------------------

+bool CAI_Pathfinder::CheckStaleNavTypeRoute( Navigation_t navType, const Vector &vStart, const Vector &vEnd )

+{

+	AIMoveTrace_t moveTrace;

+	GetOuter()->GetMoveProbe()->MoveLimit( navType, vStart, vEnd, MASK_NPCSOLID, NULL, 100, AIMLF_IGNORE_TRANSIENTS, &moveTrace);

+

+	// Is the direct route clear?

+	if (!IsMoveBlocked(moveTrace))

+	{

+		return true;

+	}

+

+	// Next try to triangulate

+	// FIXME: Since blocked dist is an unreliable number, this computation is bogus

+	Vector vecDelta;

+	VectorSubtract( vEnd, vStart, vecDelta );

+	float flTotalDist = vecDelta.Length();

+

+	Vector vApex;

+	if (Triangulate( navType, vStart, vEnd, flTotalDist - moveTrace.flDistObstructed, NULL, &vApex ))

+	{

+		return true;

+	}

+

+	// Try a giveway request, if I can get there ignoring NPCs

+	if ( moveTrace.pObstruction && moveTrace.pObstruction->MyNPCPointer() )

+	{

+		GetOuter()->GetMoveProbe()->MoveLimit( navType, vStart, vEnd, MASK_NPCSOLID_BRUSHONLY, NULL, &moveTrace);

+

+		if (!IsMoveBlocked(moveTrace))

+		{

+			return true;

+		}

+	}

+

+	return false;

+}

+

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Checks if a local route (not using nodes) between vStart

+//			and vEnd exists using the moveType

+// Input  :

+// Output : Returns a route if sucessful or NULL if no local route was possible

+//-----------------------------------------------------------------------------

+bool CAI_Pathfinder::CheckStaleRoute(const Vector &vStart, const Vector &vEnd, int moveTypes)

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_CheckStaleRoute );

+

+	// -------------------------------------------------------------------

+	// First try to go there directly

+	// -------------------------------------------------------------------

+	if (moveTypes & bits_CAP_MOVE_GROUND) 

+	{

+		if (CheckStaleNavTypeRoute( NAV_GROUND, vStart, vEnd ))

+			return true;

+	}

+

+	// -------------------------------------------------------------------

+	// First try to go there directly

+	// -------------------------------------------------------------------

+	if (moveTypes & bits_CAP_MOVE_FLY) 

+	{

+		if (CheckStaleNavTypeRoute( NAV_FLY, vStart, vEnd ))

+			return true;

+	}

+

+	// --------------------------------------------------------------

+	//  Try to jump if we can jump to a node

+	// --------------------------------------------------------------

+	if (moveTypes & bits_CAP_MOVE_JUMP)

+	{

+		AIMoveTrace_t moveTrace;

+		GetOuter()->GetMoveProbe()->MoveLimit( NAV_JUMP, vStart, vEnd, MASK_NPCSOLID, NULL, &moveTrace);

+		if (!IsMoveBlocked(moveTrace))

+		{

+			return true;

+		}

+		else

+		{

+			// Can't tell jump up from jump down at this point

+			GetOuter()->GetMoveProbe()->MoveLimit( NAV_JUMP, vEnd, vStart, MASK_NPCSOLID, NULL, &moveTrace);

+			if (!IsMoveBlocked(moveTrace))

+				return true;

+		}

+	}

+

+	// --------------------------------------------------------------

+	//  Try to climb if we can climb to a node

+	// --------------------------------------------------------------

+	if (moveTypes & bits_CAP_MOVE_CLIMB)

+	{

+		AIMoveTrace_t moveTrace;

+		GetOuter()->GetMoveProbe()->MoveLimit( NAV_CLIMB, vStart, vEnd, MASK_NPCSOLID, NULL, &moveTrace);

+		if (!IsMoveBlocked(moveTrace))

+		{	

+			return true;

+		}

+	}

+

+	// Man do we suck! Couldn't get there by any route

+	return false;

+}

+

+//-----------------------------------------------------------------------------

+

+#define MAX_NODE_TRIES 4

+#define MAX_TRIANGULATIONS 2

+

+class CPathfindNearestNodeFilter : public INearestNodeFilter

+{

+public:

+	CPathfindNearestNodeFilter( CAI_Pathfinder *pPathfinder, const Vector &vGoal, bool bToNode, int buildFlags, float goalTolerance )

+	 :	m_pPathfinder( pPathfinder ),

+		m_nTries(0),

+		m_vGoal( vGoal ),

+		m_bToNode( bToNode ),

+		m_goalTolerance( goalTolerance ),

+		m_moveTypes( buildFlags & ( bits_BUILD_GROUND | bits_BUILD_FLY | bits_BUILD_JUMP | bits_BUILD_CLIMB ) ),

+		m_pRoute( NULL )

+	{

+		// Cast to int in order to indicate that we are intentionally comparing different

+		// enum types, to suppress gcc warnings.

+		COMPILE_TIME_ASSERT( bits_BUILD_GROUND == (int)bits_CAP_MOVE_GROUND && bits_BUILD_FLY == (int)bits_CAP_MOVE_FLY && bits_BUILD_JUMP == (int)bits_CAP_MOVE_JUMP && bits_BUILD_CLIMB == (int)bits_CAP_MOVE_CLIMB );

+	}

+

+	bool IsValid( CAI_Node *pNode )

+	{

+		int nStaleLinks = 0;

+		if ( !m_pPathfinder->m_bIgnoreStaleLinks )

+		{

+			int hull = m_pPathfinder->GetOuter()->GetHullType();

+			for ( int i = 0; i < pNode->NumLinks(); i++ )

+			{

+				CAI_Link *pLink = pNode->GetLinkByIndex( i );

+				if ( pLink->m_LinkInfo & ( bits_LINK_STALE_SUGGESTED | bits_LINK_OFF ) )

+				{

+					nStaleLinks++;

+				}

+				else if ( ( pLink->m_iAcceptedMoveTypes[hull] & m_moveTypes ) == 0 )

+				{

+					nStaleLinks++;

+				}

+			}

+		}

+

+		if ( nStaleLinks && nStaleLinks == pNode->NumLinks() )

+			return false;

+

+		int buildFlags = ( m_nTries < MAX_TRIANGULATIONS ) ? ( bits_BUILD_IGNORE_NPCS | bits_BUILD_TRIANG ) : bits_BUILD_IGNORE_NPCS;

+

+		if ( m_bToNode )

+			m_pRoute = m_pPathfinder->RouteToNode( m_vGoal, buildFlags, pNode->GetId(), m_goalTolerance );

+		else

+			m_pRoute = m_pPathfinder->RouteFromNode( m_vGoal, buildFlags, pNode->GetId(), m_goalTolerance );

+

+		m_nTries++;

+

+		return ( m_pRoute != NULL );

+	}

+

+	bool ShouldContinue()

+	{

+		return ( !m_pRoute && m_nTries < MAX_NODE_TRIES );

+	}

+

+	CAI_Pathfinder *m_pPathfinder;

+	int				m_nTries;

+	Vector			m_vGoal;

+	bool			m_bToNode;

+	float			m_goalTolerance;

+	int				m_moveTypes;

+

+	AI_Waypoint_t *	m_pRoute;

+};

+

+

+AI_Waypoint_t *CAI_Pathfinder::BuildNearestNodeRoute( const Vector &vGoal, bool bToNode, int buildFlags, float goalTolerance, int *pNearestNode )

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_BuildNearestNodeRoute );

+

+	CPathfindNearestNodeFilter filter( this, vGoal, bToNode, buildFlags, goalTolerance );

+	*pNearestNode  = GetNetwork()->NearestNodeToPoint( GetOuter(), vGoal, true, &filter );

+

+	return filter.m_pRoute;

+}

+

+//-----------------------------------------------------------------------------

+// Purpose: Attemps to build a node route between vStart and vEnd

+// Input  :

+// Output : Returns a route if sucessful or NULL if no node route was possible

+//-----------------------------------------------------------------------------

+

+AI_Waypoint_t *CAI_Pathfinder::BuildNodeRoute(const Vector &vStart, const Vector &vEnd, int buildFlags, float goalTolerance)

+{

+	AI_PROFILE_SCOPE( CAI_Pathfinder_BuildNodeRoute );

+

+	// ----------------------------------------------------------------------

+	//  Make sure network has nodes

+	// ----------------------------------------------------------------------

+	if (GetNetwork()->NumNodes() == 0)

+		return NULL;

+

+	// ----------------------------------------------------------------------

+	//	Find the nearest source node

+	// ----------------------------------------------------------------------

+	int srcID;

+	AI_Waypoint_t *srcRoute = BuildNearestNodeRoute( vStart, true, buildFlags, goalTolerance, &srcID );

+	if ( !srcRoute )

+	{

+		DbgNavMsg1( GetOuter(), "Node pathfind failed, no route to source %d\n", srcID );

+		return NULL;

+	}

+

+	// ----------------------------------------------------------------------

+	//	Find the nearest destination node

+	// ----------------------------------------------------------------------

+	int destID;

+	AI_Waypoint_t *destRoute = BuildNearestNodeRoute( vEnd, false, buildFlags, goalTolerance, &destID );

+	if ( !destRoute )

+	{

+		DeleteAll( srcRoute );

+		DbgNavMsg1( GetOuter(), "Node pathfind failed, no route to dest %d\n", destID );

+		return NULL;

+	}

+

+	// ----------------------------------------------------------------------

+	// If source and destination are the same, we can bypass finding a route

+	// ----------------------------------------------------------------------

+	if (destID == srcID)

+	{

+		AddWaypointLists(srcRoute,destRoute);

+		DbgNavMsg(  GetOuter(), "Node pathfind succeeded: dest == source\n");

+		return srcRoute;

+	}

+

+	// If nodes are not connected by network graph, no route is possible

+	if (!GetNetwork()->IsConnected(srcID, destID))

+		return NULL;

+

+	AI_Waypoint_t *path = FindBestPath(srcID, destID);

+

+	if (!path)

+	{

+		DeleteAll(srcRoute);

+		DeleteAll(destRoute);

+		DbgNavMsg2( GetOuter(), "Node pathfind failed, no route between %d and %d\n", srcID, destID );

+		return NULL;

+	}

+

+	// Now put all the pieces together to form our route

+	AddWaypointLists(srcRoute,path);

+	AddWaypointLists(srcRoute,destRoute);

+

+	DbgNavMsg(  GetOuter(), "Node pathfind succeeded\n");

+	return srcRoute;

+}

+

+//-----------------------------------------------------------------------------

+// Test the triangulation route...

+//-----------------------------------------------------------------------------

+#ifdef _WIN32

+#pragma warning (disable:4701)

+#endif

+

+bool CAI_Pathfinder::TestTriangulationRoute( Navigation_t navType, const Vector& vecStart, 

+	const Vector &vecApex, const Vector &vecEnd, const CBaseEntity *pTargetEnt, AIMoveTrace_t *pStartTrace )

+{

+	AIMoveTrace_t endTrace;

+	endTrace.fStatus = AIMR_OK;	// just to make the debug overlay code easy

+

+	// Check the triangulation

+	CAI_MoveProbe *pMoveProbe = GetOuter()->GetMoveProbe();

+

+	bool bPathClear = false;

+

+	// See if we can get from the start point to the triangle apex

+	if ( pMoveProbe->MoveLimit(navType, vecStart, vecApex, MASK_NPCSOLID, pTargetEnt, pStartTrace ) )

+	{

+		// Ok, we got from the start to the triangle apex, now try

+		// the triangle apex to the end

+		if ( pMoveProbe->MoveLimit(navType, vecApex, vecEnd, MASK_NPCSOLID, pTargetEnt, &endTrace ) )

+		{

+			bPathClear = true;

+		}

+	}

+

+	// Debug mode: display the tested path...

+	if (GetOuter()->m_debugOverlays & OVERLAY_NPC_TRIANGULATE_BIT) 

+		m_TriDebugOverlay.AddTriOverlayLines( vecStart, vecApex, vecEnd, *pStartTrace, endTrace, bPathClear);

+

+	return bPathClear;

+}

+

+#ifdef _WIN32

+#pragma warning (default:4701)

+#endif

+

+

+//-----------------------------------------------------------------------------

+// Purpose: tries to overcome local obstacles by triangulating a path around them.

+// Input  : flDist is is how far the obstruction that we are trying

+//			to triangulate around is from the npc

+// Output :

+//-----------------------------------------------------------------------------

+

+// FIXME: this has no concept that the blocker may not be exactly along the vecStart, vecEnd vector.

+// FIXME: it should take a position (and size?) to avoid

+// FIXME: this does the position checks in the same order as GiveWay() so they tend to fight each other when both are active

+#define MAX_TRIAGULATION_DIST (32*12)

+bool CAI_Pathfinder::Triangulate( Navigation_t navType, const Vector &vecStart, const Vector &vecEndIn, 

+	float flDistToBlocker, const CBaseEntity *pTargetEnt, Vector *pApex )

+{

+	if ( GetOuter()->IsFlaggedEfficient() )

+		return false;

+

+	Assert( pApex );

+

+	AI_PROFILE_SCOPE( CAI_Pathfinder_Triangulate );

+

+	Vector vecForward, vecUp, vecPerpendicular;

+	VectorSubtract( vecEndIn, vecStart, vecForward );

+	float flTotalDist = VectorNormalize( vecForward );

+

+	Vector vecEnd;

+

+	// If we're walking, then don't try to triangulate over large distances

+	if ( navType != NAV_FLY && flTotalDist > MAX_TRIAGULATION_DIST)

+	{

+		vecEnd = vecForward * MAX_TRIAGULATION_DIST;

+		flTotalDist = MAX_TRIAGULATION_DIST;

+		if ( !GetOuter()->GetMoveProbe()->MoveLimit(navType, vecEnd, vecEndIn, MASK_NPCSOLID, pTargetEnt) )

+		{

+			return false;

+		}

+

+	}

+	else

+		vecEnd = vecEndIn;

+

+	// Compute a direction vector perpendicular to the desired motion direction

+	if ( 1.0f - fabs(vecForward.z) > 1e-3 )

+	{

+		vecUp.Init( 0, 0, 1 );

+		CrossProduct( vecForward, vecUp, vecPerpendicular );	// Orthogonal to facing

+	}

+	else

+	{

+		vecUp.Init( 0, 1, 0 );

+		vecPerpendicular.Init( 1, 0, 0 ); 

+	}

+

+	// Grab the size of the navigation bounding box

+	float sizeX = 0.5f * NAI_Hull::Length(GetHullType());

+	float sizeZ = 0.5f * NAI_Hull::Height(GetHullType());

+

+	// start checking right about where the object is, picking two equidistant

+	// starting points, one on the left, one on the right. As we progress 

+	// through the loop, we'll push these away from the obstacle, hoping to 

+	// find a way around on either side. m_vecSize.x is added to the ApexDist 

+	// in order to help select an apex point that insures that the NPC is 

+	// sufficiently past the obstacle before trying to turn back onto its original course.

+

+	if (GetOuter()->m_debugOverlays & OVERLAY_NPC_TRIANGULATE_BIT) 

+	{

+		m_TriDebugOverlay.FadeTriOverlayLines();

+	}

+

+	float flApexDist = flDistToBlocker + sizeX;

+	if (flApexDist > flTotalDist) 

+	{

+		flApexDist = flTotalDist;

+	}

+

+	// Compute triangulation apex points (NAV_FLY attempts vertical triangulation too)

+	Vector vecDelta[2];

+	Vector vecApex[4];

+	float pApexDist[4];

+

+	Vector vecCenter;

+	int nNumToTest = 2;

+	VectorMultiply( vecPerpendicular, sizeX, vecDelta[0] );

+

+	VectorMA( vecStart, flApexDist, vecForward, vecCenter );

+	VectorSubtract( vecCenter, vecDelta[0], vecApex[0] );

+	VectorAdd( vecCenter, vecDelta[0], vecApex[1] );

+ 	vecDelta[0] *= 2.0f;

+	pApexDist[0] = pApexDist[1] = flApexDist;

+

+	if (navType == NAV_FLY)

+	{

+		VectorMultiply( vecUp, 3.0f * sizeZ, vecDelta[1] );

+		VectorSubtract( vecCenter, vecDelta[1], vecApex[2] );

+		VectorAdd( vecCenter, vecDelta[1], vecApex[3] );

+		pApexDist[2] = pApexDist[3] = flApexDist;

+		nNumToTest = 4;

+	}

+

+	AIMoveTrace_t moveTrace;

+	for (int i = 0; i < 2; ++i )

+	{

+		// NOTE: Do reverse order so fliers try to move over the top first 

+		for (int j = nNumToTest; --j >= 0; )

+		{

+			if (TestTriangulationRoute(navType, vecStart, vecApex[j], vecEnd, pTargetEnt, &moveTrace))

+			{

+				*pApex  = vecApex[j];

+				return true;

+			}

+

+			// Here, the starting half of the triangle was blocked. Lets

+			// pull back the apex toward the start...

+			if (IsMoveBlocked(moveTrace))

+			{

+				Vector vecStartToObstruction;

+				VectorSubtract( moveTrace.vEndPosition, vecStart, vecStartToObstruction );

+				float flDistToObstruction = DotProduct( vecStartToObstruction, vecForward );

+

+				float flNewApexDist = pApexDist[j];

+				if (pApexDist[j] > flDistToObstruction)

+					flNewApexDist = flDistToObstruction;

+

+				VectorMA( vecApex[j], flNewApexDist - pApexDist[j], vecForward, vecApex[j] );

+				pApexDist[j] = flNewApexDist;

+			}

+

+			// NOTE: This has to occur *after* the code above because

+			// the above code uses vecApex for some distance computations

+			if (j & 0x1)

+				vecApex[j] += vecDelta[j >> 1];

+			else

+				vecApex[j] -= vecDelta[j >> 1];

+		}

+	}

+

+	return false;

+}

+

+

+//-----------------------------------------------------------------------------

+// Purpose: Triangulation debugging 

+//-----------------------------------------------------------------------------

+

+void CAI_Pathfinder::DrawDebugGeometryOverlays(int npcDebugOverlays) 

+{

+	m_TriDebugOverlay.Draw(npcDebugOverlays);

+}

+

+void CAI_Pathfinder::CTriDebugOverlay::Draw(int npcDebugOverlays) 

+{

+	if (m_debugTriOverlayLine) 

+	{

+		if ( npcDebugOverlays & OVERLAY_NPC_TRIANGULATE_BIT) 

+		{

+			for (int i=0;i<NUM_NPC_DEBUG_OVERLAYS;i++)

+			{

+				if (m_debugTriOverlayLine[i]->draw)

+				{

+					NDebugOverlay::Line(m_debugTriOverlayLine[i]->origin,

+										 m_debugTriOverlayLine[i]->dest,

+										 m_debugTriOverlayLine[i]->r,

+										 m_debugTriOverlayLine[i]->g,

+										 m_debugTriOverlayLine[i]->b,

+										 m_debugTriOverlayLine[i]->noDepthTest,

+										 0);

+				}

+			}

+		}

+		else

+		{

+			ClearTriOverlayLines();

+		}

+	}

+}

+

+void CAI_Pathfinder::CTriDebugOverlay::AddTriOverlayLines( const Vector &vecStart, const Vector &vecApex, const Vector &vecEnd, const AIMoveTrace_t &startTrace, const AIMoveTrace_t &endTrace, bool bPathClear )

+{

+	static unsigned char s_TriangulationColor[2][3] = 

+	{

+		{ 255,   0, 0 },

+		{   0, 255, 0 }

+	};

+

+	unsigned char *c = s_TriangulationColor[bPathClear];

+

+	AddTriOverlayLine(vecStart, vecApex, c[0],c[1],c[2], false);

+	AddTriOverlayLine(vecApex, vecEnd, c[0],c[1],c[2], false);

+

+	// If we've blocked, draw an X where we were blocked...

+	if (IsMoveBlocked(startTrace.fStatus))

+	{

+		Vector pt1, pt2;

+		pt1 = pt2 = startTrace.vEndPosition;

+

+		pt1.x -= 10; pt1.y -= 10;

+		pt2.x += 10; pt2.y += 10;

+		AddTriOverlayLine(pt1, pt2, c[0],c[1],c[2], false);

+

+		pt1.x += 20;

+		pt2.x -= 20;

+		AddTriOverlayLine(pt1, pt2, c[0],c[1],c[2], false);

+	}

+

+	if (IsMoveBlocked(endTrace.fStatus))

+	{

+		Vector pt1, pt2;

+		pt1 = pt2 = endTrace.vEndPosition;

+

+		pt1.x -= 10; pt1.y -= 10;

+		pt2.x += 10; pt2.y += 10;

+		AddTriOverlayLine(pt1, pt2, c[0],c[1],c[2], false);

+

+		pt1.x += 20;

+		pt2.x -= 20;

+		AddTriOverlayLine(pt1, pt2, c[0],c[1],c[2], false);

+	}

+}

+

+void CAI_Pathfinder::CTriDebugOverlay::ClearTriOverlayLines(void) 

+{

+	if (m_debugTriOverlayLine)

+	{

+		for (int i=0;i<NUM_NPC_DEBUG_OVERLAYS;i++)

+		{

+			m_debugTriOverlayLine[i]->draw = false;

+		}

+	}

+}

+void CAI_Pathfinder::CTriDebugOverlay::FadeTriOverlayLines(void) 

+{

+	if (m_debugTriOverlayLine)

+	{

+		for (int i=0;i<NUM_NPC_DEBUG_OVERLAYS;i++)

+		{

+			m_debugTriOverlayLine[i]->r *= 0.5;

+			m_debugTriOverlayLine[i]->g *= 0.5;				

+			m_debugTriOverlayLine[i]->b *= 0.5;				

+		}

+	}

+}

+void CAI_Pathfinder::CTriDebugOverlay::AddTriOverlayLine(const Vector &origin, const Vector &dest, int r, int g, int b, bool noDepthTest)

+{

+	if (!m_debugTriOverlayLine)

+	{

+		m_debugTriOverlayLine = new OverlayLine_t*[NUM_NPC_DEBUG_OVERLAYS];

+		for (int i=0;i<NUM_NPC_DEBUG_OVERLAYS;i++)

+		{

+			m_debugTriOverlayLine[i] = new OverlayLine_t;

+		}

+	}

+	static int overCounter = 0;

+

+	if (overCounter >= NUM_NPC_DEBUG_OVERLAYS)

+	{

+		overCounter = 0;

+	}

+	

+	m_debugTriOverlayLine[overCounter]->origin			= origin;

+	m_debugTriOverlayLine[overCounter]->dest			= dest;

+	m_debugTriOverlayLine[overCounter]->r				= r;

+	m_debugTriOverlayLine[overCounter]->g				= g;

+	m_debugTriOverlayLine[overCounter]->b				= b;

+	m_debugTriOverlayLine[overCounter]->noDepthTest		= noDepthTest;

+	m_debugTriOverlayLine[overCounter]->draw			= true;

+	overCounter++;

+

+}

+

+//-----------------------------------------------------------------------------