//========= 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;nodeGetPosition(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;iGetOrigin(); 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;idraw) { 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;idraw = false; } } } void CAI_Pathfinder::CTriDebugOverlay::FadeTriOverlayLines(void) { if (m_debugTriOverlayLine) { for (int i=0;ir *= 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) { 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++; } //-----------------------------------------------------------------------------