2138 lines
69 KiB
C++
2138 lines
69 KiB
C++
//========= 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++;
|
|
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|