diff options
Diffstat (limited to 'game/server/NextBot/Path')
| -rw-r--r-- | game/server/NextBot/Path/NextBotChasePath.cpp | 166 | ||||
| -rw-r--r-- | game/server/NextBot/Path/NextBotChasePath.h | 376 | ||||
| -rw-r--r-- | game/server/NextBot/Path/NextBotPath.cpp | 1094 | ||||
| -rw-r--r-- | game/server/NextBot/Path/NextBotPath.h | 862 | ||||
| -rw-r--r-- | game/server/NextBot/Path/NextBotPathFollow.cpp | 1923 | ||||
| -rw-r--r-- | game/server/NextBot/Path/NextBotPathFollow.h | 106 | ||||
| -rw-r--r-- | game/server/NextBot/Path/NextBotRetreatPath.h | 573 |
7 files changed, 5100 insertions, 0 deletions
diff --git a/game/server/NextBot/Path/NextBotChasePath.cpp b/game/server/NextBot/Path/NextBotChasePath.cpp new file mode 100644 index 0000000..1674fc3 --- /dev/null +++ b/game/server/NextBot/Path/NextBotChasePath.cpp @@ -0,0 +1,166 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//============================================================================= + +#include "cbase.h" + +#include "NextBotChasePath.h" +#include "tier1/fmtstr.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + + +//---------------------------------------------------------------------------------------------- +/** + * Try to cutoff our chase subject + */ +Vector ChasePath::PredictSubjectPosition( INextBot *bot, CBaseEntity *subject ) const +{ + ILocomotion *mover = bot->GetLocomotionInterface(); + + const Vector &subjectPos = subject->GetAbsOrigin(); + + Vector to = subjectPos - bot->GetPosition(); + to.z = 0.0f; + float flRangeSq = to.LengthSqr(); + + // don't lead if subject is very far away + float flLeadRadiusSq = GetLeadRadius(); + flLeadRadiusSq *= flLeadRadiusSq; + if ( flRangeSq > flLeadRadiusSq ) + return subjectPos; + + // Normalize in place + float range = sqrt( flRangeSq ); + to /= ( range + 0.0001f ); // avoid divide by zero + + // estimate time to reach subject, assuming maximum speed + float leadTime = 0.5f + ( range / ( mover->GetRunSpeed() + 0.0001f ) ); + + // estimate amount to lead the subject + Vector lead = leadTime * subject->GetAbsVelocity(); + lead.z = 0.0f; + + if ( DotProduct( to, lead ) < 0.0f ) + { + // the subject is moving towards us - only pay attention + // to his perpendicular velocity for leading + Vector2D to2D = to.AsVector2D(); + to2D.NormalizeInPlace(); + + Vector2D perp( -to2D.y, to2D.x ); + + float enemyGroundSpeed = lead.x * perp.x + lead.y * perp.y; + + lead.x = enemyGroundSpeed * perp.x; + lead.y = enemyGroundSpeed * perp.y; + } + + // compute our desired destination + Vector pathTarget = subjectPos + lead; + + // validate this destination + + // don't lead through walls + if ( lead.LengthSqr() > 36.0f ) + { + float fraction; + if ( !mover->IsPotentiallyTraversable( subjectPos, pathTarget, ILocomotion::IMMEDIATELY, &fraction ) ) + { + // tried to lead through an unwalkable area - clip to walkable space + pathTarget = subjectPos + fraction * ( pathTarget - subjectPos ); + } + } + + // don't lead over cliffs + CNavArea *leadArea = NULL; + +#ifdef NEED_GPGLOBALS_SERVERCOUNT_TO_DO_THIS + CBaseCombatCharacter *pBCC = subject->MyCombatCharacterPointer(); + if ( pBCC && CloseEnough( pathTarget, subjectPos, 3.0 ) ) + { + pathTarget = subjectPos; + leadArea = pBCC->GetLastKnownArea(); // can return null? + } + else + { + struct CacheEntry_t + { + CacheEntry_t() : pArea(NULL) {} + Vector target; + CNavArea *pArea; + }; + + static int iServer; + static CacheEntry_t cache[4]; + static int iNext; + int i; + + bool bFound = false; + if ( iServer != gpGlobals->serverCount ) + { + for ( i = 0; i < ARRAYSIZE(cache); i++ ) + { + cache[i].pArea = NULL; + } + iServer = gpGlobals->serverCount; + } + else + { + for ( i = 0; i < ARRAYSIZE(cache); i++ ) + { + if ( cache[i].pArea && CloseEnough( cache[i].target, pathTarget, 2.0 ) ) + { + pathTarget = cache[i].target; + leadArea = cache[i].pArea; + bFound = true; + break; + } + } + } + + if ( !bFound ) + { + leadArea = TheNavMesh->GetNearestNavArea( pathTarget ); + if ( leadArea ) + { + cache[iNext].target = pathTarget; + cache[iNext].pArea = leadArea; + iNext = ( iNext + 1 ) % ARRAYSIZE( cache ); + } + } + } +#else + leadArea = TheNavMesh->GetNearestNavArea( pathTarget ); +#endif + + + if ( !leadArea || leadArea->GetZ( pathTarget.x, pathTarget.y ) < pathTarget.z - mover->GetMaxJumpHeight() ) + { + // would fall off a cliff + return subjectPos; + } + + /** This needs more thought - it is preventing bots from using dropdowns + if ( mover->HasPotentialGap( subjectPos, pathTarget, &fraction ) ) + { + // tried to lead over a cliff - clip to safe region + pathTarget = subjectPos + fraction * ( pathTarget - subjectPos ); + } + */ + + return pathTarget; +} + +// if the victim is a player, poke them so they know they're being chased +void DirectChasePath::NotifyVictim( INextBot *me, CBaseEntity *victim ) +{ + CBaseCombatCharacter *pBCCVictim = ToBaseCombatCharacter( victim ); + if ( !pBCCVictim ) + return; + + pBCCVictim->OnPursuedBy( me ); +}
\ No newline at end of file diff --git a/game/server/NextBot/Path/NextBotChasePath.h b/game/server/NextBot/Path/NextBotChasePath.h new file mode 100644 index 0000000..1929648 --- /dev/null +++ b/game/server/NextBot/Path/NextBotChasePath.h @@ -0,0 +1,376 @@ +// NextBotChasePath.h +// Maintain and follow a "chase path" to a selected Actor +// Author: Michael Booth, September 2006 +//========= Copyright Valve Corporation, All rights reserved. ============// + +#ifndef _NEXT_BOT_CHASE_PATH_ +#define _NEXT_BOT_CHASE_PATH_ + +#include "nav.h" +#include "NextBotInterface.h" +#include "NextBotLocomotionInterface.h" +#include "NextBotChasePath.h" +#include "NextBotUtil.h" +#include "NextBotPathFollow.h" +#include "tier0/vprof.h" + + +//---------------------------------------------------------------------------------------------- +/** + * A ChasePath extends a PathFollower to periodically recompute a path to a chase + * subject, and to move along the path towards that subject. + */ +class ChasePath : public PathFollower +{ +public: + enum SubjectChaseType + { + LEAD_SUBJECT, + DONT_LEAD_SUBJECT + }; + ChasePath( SubjectChaseType chaseHow = DONT_LEAD_SUBJECT ); + + virtual ~ChasePath() { } + + virtual void Update( INextBot *bot, CBaseEntity *subject, const IPathCost &cost, Vector *pPredictedSubjectPos = NULL ); // update path to chase target and move bot along path + + virtual float GetLeadRadius( void ) const; // range where movement leading begins - beyond this just head right for the subject + virtual float GetMaxPathLength( void ) const; // return maximum path length + virtual Vector PredictSubjectPosition( INextBot *bot, CBaseEntity *subject ) const; // try to cutoff our chase subject, knowing our relative positions and velocities + virtual bool IsRepathNeeded( INextBot *bot, CBaseEntity *subject ) const; // return true if situation has changed enough to warrant recomputing the current path + + virtual float GetLifetime( void ) const; // Return duration this path is valid. Path will become invalid at its earliest opportunity once this duration elapses. Zero = infinite lifetime + + virtual void Invalidate( void ); // (EXTEND) cause the path to become invalid + +private: + void RefreshPath( INextBot *bot, CBaseEntity *subject, const IPathCost &cost, Vector *pPredictedSubjectPos ); + + CountdownTimer m_failTimer; // throttle re-pathing if last path attempt failed + CountdownTimer m_throttleTimer; // require a minimum time between re-paths + CountdownTimer m_lifetimeTimer; + EHANDLE m_lastPathSubject; // the subject used to compute the current/last path + SubjectChaseType m_chaseHow; +}; + +inline ChasePath::ChasePath( SubjectChaseType chaseHow ) +{ + m_failTimer.Invalidate(); + m_throttleTimer.Invalidate(); + m_lifetimeTimer.Invalidate(); + m_lastPathSubject = NULL; + m_chaseHow = chaseHow; +} + +inline float ChasePath::GetLeadRadius( void ) const +{ + return 500.0f; // 1000.0f; +} + +inline float ChasePath::GetMaxPathLength( void ) const +{ + // no limit + return 0.0f; +} + +inline float ChasePath::GetLifetime( void ) const +{ + // infinite duration + return 0.0f; +} + +inline void ChasePath::Invalidate( void ) +{ + // path is gone, repath at earliest opportunity + m_throttleTimer.Invalidate(); + m_lifetimeTimer.Invalidate(); + + // extend + PathFollower::Invalidate(); +} + + + + +//---------------------------------------------------------------------------------------------- +/** + * Maintain a path to our chase subject and move along that path + */ +inline void ChasePath::Update( INextBot *bot, CBaseEntity *subject, const IPathCost &cost, Vector *pPredictedSubjectPos ) +{ + VPROF_BUDGET( "ChasePath::Update", "NextBot" ); + + // maintain the path to the subject + RefreshPath( bot, subject, cost, pPredictedSubjectPos ); + + // move along the path towards the subject + PathFollower::Update( bot ); +} + + +//---------------------------------------------------------------------------------------------- +/** + * Return true if situation has changed enough to warrant recomputing the current path + */ +inline bool ChasePath::IsRepathNeeded( INextBot *bot, CBaseEntity *subject ) const +{ + // the closer we get, the more accurate our path needs to be + Vector to = subject->GetAbsOrigin() - bot->GetPosition(); + + const float minTolerance = 0.0f; // 25.0f; + const float toleranceRate = 0.33f; // 1.0f; // 0.15f; + + float tolerance = minTolerance + toleranceRate * to.Length(); + + return ( subject->GetAbsOrigin() - GetEndPosition() ).IsLengthGreaterThan( tolerance ); +} + + +//---------------------------------------------------------------------------------------------- +/** + * Periodically rebuild the path to our victim + */ +inline void ChasePath::RefreshPath( INextBot *bot, CBaseEntity *subject, const IPathCost &cost, Vector *pPredictedSubjectPos ) +{ + VPROF_BUDGET( "ChasePath::RefreshPath", "NextBot" ); + + ILocomotion *mover = bot->GetLocomotionInterface(); + + // don't change our path if we're on a ladder + if ( IsValid() && mover->IsUsingLadder() ) + { + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "%3.2f: bot(#%d) ChasePath::RefreshPath failed. Bot is on a ladder.\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); + } + + // don't allow repath until a moment AFTER we have left the ladder + m_throttleTimer.Start( 1.0f ); + + return; + } + + if ( subject == NULL ) + { + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "%3.2f: bot(#%d) CasePath::RefreshPath failed. No subject.\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); + } + return; + } + + if ( !m_failTimer.IsElapsed() ) + { +// if ( bot->IsDebugging( NEXTBOT_PATH ) ) +// { +// DevMsg( "%3.2f: bot(#%d) ChasePath::RefreshPath failed. Fail timer not elapsed.\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); +// } + return; + } + + // if our path subject changed, repath immediately + if ( subject != m_lastPathSubject ) + { + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "%3.2f: bot(#%d) Chase path subject changed (from %p to %p).\n", gpGlobals->curtime, bot->GetEntity()->entindex(), m_lastPathSubject.Get(), subject ); + } + + Invalidate(); + + // new subject, fresh attempt + m_failTimer.Invalidate(); + } + + if ( IsValid() && !m_throttleTimer.IsElapsed() ) + { + // require a minimum time between repaths, as long as we have a path to follow +// if ( bot->IsDebugging( NEXTBOT_PATH ) ) +// { +// DevMsg( "%3.2f: bot(#%d) ChasePath::RefreshPath failed. Rate throttled.\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); +// } + return; + } + + if ( IsValid() && m_lifetimeTimer.HasStarted() && m_lifetimeTimer.IsElapsed() ) + { + // this path's lifetime has elapsed + Invalidate(); + } + + if ( !IsValid() || IsRepathNeeded( bot, subject ) ) + { + // the situation has changed - try a new path + bool isPath; + Vector pathTarget = subject->GetAbsOrigin(); + + if ( m_chaseHow == LEAD_SUBJECT ) + { + pathTarget = pPredictedSubjectPos ? *pPredictedSubjectPos : PredictSubjectPosition( bot, subject ); + isPath = Compute( bot, pathTarget, cost, GetMaxPathLength() ); + } + else if ( subject->MyCombatCharacterPointer() && subject->MyCombatCharacterPointer()->GetLastKnownArea() ) + { + isPath = Compute( bot, subject->MyCombatCharacterPointer(), cost, GetMaxPathLength() ); + } + else + { + isPath = Compute( bot, pathTarget, cost, GetMaxPathLength() ); + } + + if ( isPath ) + { + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + //const float size = 20.0f; + //NDebugOverlay::VertArrow( bot->GetPosition() + Vector( 0, 0, size ), bot->GetPosition(), size, 255, RandomInt( 0, 200 ), 255, 255, true, 30.0f ); + + DevMsg( "%3.2f: bot(#%d) REPATH\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); + } + + m_lastPathSubject = subject; + + const float minRepathInterval = 0.5f; + m_throttleTimer.Start( minRepathInterval ); + + // track the lifetime of this new path + float lifetime = GetLifetime(); + if ( lifetime > 0.0f ) + { + m_lifetimeTimer.Start( lifetime ); + } + else + { + m_lifetimeTimer.Invalidate(); + } + } + else + { + // can't reach subject - throttle retry based on range to subject + m_failTimer.Start( 0.005f * ( bot->GetRangeTo( subject ) ) ); + + // allow bot to react to path failure + bot->OnMoveToFailure( this, FAIL_NO_PATH_EXISTS ); + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + //const float size = 20.0f; + const float dT = 90.0f; + int c = RandomInt( 0, 100 ); + //NDebugOverlay::VertArrow( bot->GetPosition() + Vector( 0, 0, size ), bot->GetPosition(), size, 255, c, c, 255, true, dT ); + NDebugOverlay::HorzArrow( bot->GetPosition(), pathTarget, 5.0f, 255, c, c, 255, true, dT ); + + DevMsg( "%3.2f: bot(#%d) REPATH FAILED\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); + } + + Invalidate(); + } + } +} + + +//---------------------------------------------------------------------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------------------------------------------------------------------- +/** + * Directly beeline toward victim if we have a clear shot, otherwise pathfind. + */ +class DirectChasePath : public ChasePath +{ +public: + + DirectChasePath( ChasePath::SubjectChaseType chaseHow = ChasePath::DONT_LEAD_SUBJECT ) : ChasePath( chaseHow ) + { + + } + + //------------------------------------------------------------------------------------------------------- + virtual void Update( INextBot *me, CBaseEntity *victim, const IPathCost &pathCost, Vector *pPredictedSubjectPos = NULL ) // update path to chase target and move bot along path + { + Assert( !pPredictedSubjectPos ); + bool bComputedPredictedPosition; + Vector vecPredictedPosition; + if ( !DirectChase( &bComputedPredictedPosition, &vecPredictedPosition, me, victim ) ) + { + // path around obstacles to reach our victim + ChasePath::Update( me, victim, pathCost, bComputedPredictedPosition ? &vecPredictedPosition : NULL ); + } + NotifyVictim( me, victim ); + } + + //------------------------------------------------------------------------------------------------------- + bool DirectChase( bool *pPredictedPositionComputed, Vector *pPredictedPos, INextBot *me, CBaseEntity *victim ) // if there is nothing between us and our victim, run directly at them + { + *pPredictedPositionComputed = false; + + ILocomotion *mover = me->GetLocomotionInterface(); + + if ( me->IsImmobile() || mover->IsScrambling() ) + { + return false; + } + + if ( IsDiscontinuityAhead( me, CLIMB_UP ) ) + { + return false; + } + + if ( IsDiscontinuityAhead( me, JUMP_OVER_GAP ) ) + { + return false; + } + + Vector leadVictimPos = PredictSubjectPosition( me, victim ); + + // Don't want to have to compute the predicted position twice. + *pPredictedPositionComputed = true; + *pPredictedPos = leadVictimPos; + + if ( !mover->IsPotentiallyTraversable( mover->GetFeet(), leadVictimPos ) ) + { + return false; + } + + // the way is clear - move directly towards our victim + mover->FaceTowards( leadVictimPos ); + mover->Approach( leadVictimPos ); + + me->GetBodyInterface()->AimHeadTowards( victim ); + + // old path is no longer useful since we've moved off of it + Invalidate(); + + return true; + } + + //------------------------------------------------------------------------------------------------------- + virtual bool IsRepathNeeded( INextBot *bot, CBaseEntity *subject ) const // return true if situation has changed enough to warrant recomputing the current path + { + if ( ChasePath::IsRepathNeeded( bot, subject ) ) + { + return true; + } + + return bot->GetLocomotionInterface()->IsStuck() && bot->GetLocomotionInterface()->GetStuckDuration() > 2.0f; + } + + //------------------------------------------------------------------------------------------------------- + /** + * Determine exactly where the path goes between the given two areas + * on the path. Return this point in 'crossPos'. + */ + virtual void ComputeAreaCrossing( INextBot *bot, const CNavArea *from, const Vector &fromPos, const CNavArea *to, NavDirType dir, Vector *crossPos ) const + { + Vector center; + float halfWidth; + from->ComputePortal( to, dir, ¢er, &halfWidth ); + + *crossPos = center; + } + + void NotifyVictim( INextBot *me, CBaseEntity *victim ); +}; + + + + +#endif // _NEXT_BOT_CHASE_PATH_ diff --git a/game/server/NextBot/Path/NextBotPath.cpp b/game/server/NextBot/Path/NextBotPath.cpp new file mode 100644 index 0000000..4514887 --- /dev/null +++ b/game/server/NextBot/Path/NextBotPath.cpp @@ -0,0 +1,1094 @@ +// NextBotPath.cpp +// Encapsulate and manipulate a path through the world +// Author: Michael Booth, February 2006 +//========= Copyright Valve Corporation, All rights reserved. ============// + +#include "cbase.h" + +#include "nav_mesh.h" +#include "fmtstr.h" + +#include "NextBotPath.h" +#include "NextBotInterface.h" +#include "NextBotLocomotionInterface.h" +#include "NextBotBodyInterface.h" +#include "NextBotUtil.h" + +#include "tier0/vprof.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +ConVar NextBotPathDrawIncrement( "nb_path_draw_inc", "100", FCVAR_CHEAT ); +ConVar NextBotPathDrawSegmentCount( "nb_path_draw_segment_count", "100", FCVAR_CHEAT ); +ConVar NextBotPathSegmentInfluenceRadius( "nb_path_segment_influence_radius", "100", FCVAR_CHEAT ); + +//-------------------------------------------------------------------------------------------------------------- +Path::Path( void ) +{ + m_segmentCount = 0; + + m_cursorPos = 0.0f; + m_isCursorDataDirty = true; + m_cursorData.segmentPrior = NULL; + m_ageTimer.Invalidate(); + m_subject = NULL; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Determine actual path positions + */ +bool Path::ComputePathDetails( INextBot *bot, const Vector &start ) +{ + VPROF_BUDGET( "Path::ComputePathDetails", "NextBot" ); + + if (m_segmentCount == 0) + return false; + + IBody *body = bot->GetBodyInterface(); + ILocomotion *mover = bot->GetLocomotionInterface(); + + const float stepHeight = ( mover ) ? mover->GetStepHeight() : 18.0f; + + // inflate hull width slightly as a safety margin + const float hullWidth = ( body ) ? body->GetHullWidth() + 5.0f : 1.0f; + + // set first path position + if ( m_path[0].area->Contains( start ) ) + { + m_path[0].pos = start; + } + else + { + // start in first area's center + m_path[0].pos = m_path[0].area->GetCenter(); + } + m_path[0].ladder = NULL; + m_path[0].how = NUM_TRAVERSE_TYPES; + m_path[0].type = ON_GROUND; + + // set positions along the path + for( int i=1; i<m_segmentCount; ++i ) + { + Segment *from = &m_path[ i-1 ]; + Segment *to = &m_path[ i ]; + + if ( to->how <= GO_WEST ) // walk along the floor to the next area + { + to->ladder = NULL; + + from->area->ComputePortal( to->area, (NavDirType)to->how, &to->m_portalCenter, &to->m_portalHalfWidth ); + + // compute next point + ComputeAreaCrossing( bot, from->area, from->pos, to->area, (NavDirType)to->how, &to->pos ); + + // we need to walk out of "from" area, so keep Z where we can reach it + to->pos.z = from->area->GetZ( to->pos ); + + // if this is a "jump down" connection, we must insert an additional point on the path + //float expectedHeightDrop = from->area->GetZ( from->pos ) - to->area->GetZ( to->pos ); + + // measure the drop distance relative to the actual slope of the ground + Vector fromPos = from->pos; + fromPos.z = from->area->GetZ( fromPos ); + + Vector toPos = to->pos; + toPos.z = to->area->GetZ( toPos ); + + Vector groundNormal; + from->area->ComputeNormal( &groundNormal ); + + Vector alongPath = toPos - fromPos; + + float expectedHeightDrop = -DotProduct( alongPath, groundNormal ); + + if ( expectedHeightDrop > mover->GetStepHeight() ) + { + // NOTE: We can't know this is a drop-down yet, because of subtle interactions + // between nav area links and "portals" and "area crossings" + + // compute direction of path just prior to "jump down" + Vector2D dir; + DirectionToVector2D( (NavDirType)to->how, &dir ); + + // shift top of "jump down" out a bit to "get over the ledge" + const float inc = 10.0f; // 0.25f * hullWidth; + const float maxPushDist = 2.0f * hullWidth; // 75.0f; + float halfWidth = hullWidth/2.0f; + float hullHeight = ( body ) ? body->GetCrouchHullHeight() : 1.0f; + + float pushDist; + for( pushDist = 0.0f; pushDist <= maxPushDist; pushDist += inc ) + { + Vector pos = to->pos + Vector( pushDist * dir.x, pushDist * dir.y, 0.0f ); + Vector lowerPos = Vector( pos.x, pos.y, toPos.z ); + + trace_t result; + NextBotTraceFilterIgnoreActors filter( bot->GetEntity(), COLLISION_GROUP_NONE ); + UTIL_TraceHull( pos, lowerPos, + Vector( -halfWidth, -halfWidth, stepHeight ), Vector( halfWidth, halfWidth, hullHeight ), + bot->GetBodyInterface()->GetSolidMask(), &filter, &result ); + + if ( result.fraction >= 1.0f ) + { + // found clearance to drop + break; + } + } + + Vector startDrop( to->pos.x + pushDist * dir.x, to->pos.y + pushDist * dir.y, to->pos.z ); + Vector endDrop( startDrop.x, startDrop.y, to->area->GetZ( to->pos ) ); + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + NDebugOverlay::Cross3D( startDrop, 5.0f, 255, 0, 255, true, 5.0f ); + NDebugOverlay::Cross3D( endDrop, 5.0f, 255, 255, 0, true, 5.0f ); + NDebugOverlay::VertArrow( startDrop, endDrop, 5.0f, 255, 100, 0, 255, true, 5.0f ); + } + + // verify that there is actually ground down there in case this is a far jump dropdown + float ground; + if ( TheNavMesh->GetGroundHeight( endDrop, &ground ) ) + { + if ( startDrop.z > ground + stepHeight ) + { + // if "ground" is lower than the next segment along the path + // there is a chasm between - this is not a drop down + // NOTE next->pos is not yet valid - this loop is computing it! + // const Segment *next = NextSegment( to ); + // if ( !next || next->area->GetCenter().z < ground + stepHeight ) + { + // this is a "jump down" link + to->pos = startDrop; + to->type = DROP_DOWN; + + // insert a duplicate node to represent the bottom of the fall + if ( m_segmentCount < MAX_PATH_SEGMENTS-1 ) + { + // copy nodes down + for( int j=m_segmentCount; j>i; --j ) + m_path[j] = m_path[j-1]; + + // path is one node longer + ++m_segmentCount; + + // move index ahead into the new node we just duplicated + ++i; + + m_path[i].pos.x = endDrop.x; + m_path[i].pos.y = endDrop.y; + m_path[i].pos.z = ground; + + m_path[i].type = ON_GROUND; + } + } + } + } + } + } + else if ( to->how == GO_LADDER_UP ) // to get to next area, must go up a ladder + { + // find our ladder + const NavLadderConnectVector *ladders = from->area->GetLadders( CNavLadder::LADDER_UP ); + int it; + for( it=0; it<ladders->Count(); ++it ) + { + CNavLadder *ladder = (*ladders)[ it ].ladder; + + // can't use "behind" area when ascending... + if (ladder->m_topForwardArea == to->area || + ladder->m_topLeftArea == to->area || + ladder->m_topRightArea == to->area) + { + to->ladder = ladder; + to->pos = ladder->m_bottom + ladder->GetNormal() * 2.0f * HalfHumanWidth; + to->type = LADDER_UP; + break; + } + } + + if (it == ladders->Count()) + { + //PrintIfWatched( "ERROR: Can't find ladder in path\n" ); + return false; + } + } + else if ( to->how == GO_LADDER_DOWN ) // to get to next area, must go down a ladder + { + // find our ladder + const NavLadderConnectVector *ladders = from->area->GetLadders( CNavLadder::LADDER_DOWN ); + int it; + for( it=0; it<ladders->Count(); ++it ) + { + CNavLadder *ladder = (*ladders)[ it ].ladder; + + if (ladder->m_bottomArea == to->area) + { + to->ladder = ladder; + to->pos = ladder->m_top; + to->pos = ladder->m_top - ladder->GetNormal() * 2.0f * HalfHumanWidth; + to->type = LADDER_DOWN; + break; + } + } + + if (it == ladders->Count()) + { + //PrintIfWatched( "ERROR: Can't find ladder in path\n" ); + return false; + } + } + else if ( to->how == GO_ELEVATOR_UP || to->how == GO_ELEVATOR_DOWN ) + { + to->pos = to->area->GetCenter(); + to->ladder = NULL; + } + } + + + // + // Scan for non-adjacent nav areas and add gap-jump-target nodes + // and jump-up target nodes for adjacent ledge mantling + // @todo Adjacency should be baked into the mesh data + // + for( int i=0; i<m_segmentCount-1; ++i ) + { + Segment *from = &m_path[ i ]; + Segment *to = &m_path[ i+1 ]; + + // first segment doesnt have a direction + if ( from->how != NUM_TRAVERSE_TYPES && from->how > GO_WEST ) + continue; + + if ( to->how > GO_WEST || !to->type == ON_GROUND ) + continue; + + // if areas are separated, we may need to 'gap jump' between them + // add a node to minimize the jump distance + Vector closeFrom, closeTo; + to->area->GetClosestPointOnArea( from->pos, &closeTo ); + from->area->GetClosestPointOnArea( closeTo, &closeFrom ); + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + NDebugOverlay::Line( closeFrom, closeTo, 255, 0, 255, true, 5.0f ); + } + + + const float separationTolerance = 1.9f * GenerationStepSize; + if ( (closeFrom - closeTo).AsVector2D().IsLengthGreaterThan( separationTolerance ) && ( closeTo - closeFrom ).AsVector2D().IsLengthGreaterThan( 0.5f * fabs( closeTo.z - closeFrom.z ) ) ) + { + // areas are disjoint and mostly level - add gap jump target + + // compute landing spot in 'to' area + Vector landingPos; + to->area->GetClosestPointOnArea( to->pos, &landingPos ); + + // compute launch spot in 'from' area + Vector launchPos; + from->area->GetClosestPointOnArea( landingPos, &launchPos ); + + Vector forward = landingPos - launchPos; + forward.NormalizeInPlace(); + + const float halfWidth = hullWidth/2.0f; + + // adjust path position to landing spot + to->pos = landingPos + forward * halfWidth; + + // insert launch position just before that segment to ensure bot is + // positioned for minimal jump distance + Segment newSegment = *from; + + newSegment.pos = launchPos - forward * halfWidth; + newSegment.type = JUMP_OVER_GAP; + + InsertSegment( newSegment, i+1 ); + + ++i; + } + else if ( (closeTo.z - closeFrom.z) > stepHeight ) + { + // areas are adjacent, but need a jump-up - add a jump-to target + + // adjust goal to be at top of ledge + //to->pos.z = to->area->GetZ( to->pos.x, to->pos.y ); + // use center of climb-up destination area to make sure bot moves onto actual ground once they finish their climb + to->pos = to->area->GetCenter(); + + // add launch position at base of jump + Segment newSegment = *from; + + Vector launchPos; + from->area->GetClosestPointOnArea( to->pos, &launchPos ); + + newSegment.pos = launchPos; + newSegment.type = CLIMB_UP; + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + NDebugOverlay::Cross3D( newSegment.pos, 15.0f, 255, 100, 255, true, 3.0f ); + } + + InsertSegment( newSegment, i+1 ); + + ++i; + } + + /** RETHINK THIS. It doesn't work in general cases, and messes up on doorways + else if ( from->type == ON_GROUND && from->how <= GO_WEST ) + { + // if any segment is not directly walkable, add a segment + // fixup corners that are being cut too tightly + if ( mover && !mover->IsPotentiallyTraversable( from->pos, to->pos ) ) + { + Segment newSegment = *from; + + if ( bot->IsDebugging( INextBot::PATH ) ) + { + NDebugOverlay::HorzArrow( from->pos, to->pos, 3.0f, 255, 0, 0, 255, true, 3.0f ); + } + + //newSegment.pos = from->area->GetCenter(); + + Vector2D shift; + DirectionToVector2D( OppositeDirection( (NavDirType)to->how ), &shift ); + + newSegment.pos = to->pos; + newSegment.pos.x += hullWidth * shift.x; + newSegment.pos.y += hullWidth * shift.y; + + newSegment.type = ON_GROUND; + + if ( bot->IsDebugging( INextBot::PATH ) ) + { + NDebugOverlay::Cross3D( newSegment.pos, 15.0f, 255, 0, 255, true, 3.0f ); + } + + InsertSegment( newSegment, i+1 ); + + i += 2; + } + } + */ + } + + return true; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Insert new segment at index i + */ +void Path::InsertSegment( Segment newSegment, int i ) +{ + if (m_segmentCount < MAX_PATH_SEGMENTS-1) + { + // shift segments to make room for new one + for( int j=m_segmentCount; j>i; --j ) + m_path[j] = m_path[j-1]; + + // path is one node longer + ++m_segmentCount; + + m_path[i] = newSegment; + } +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Build trivial path when start and goal are in the same nav area + */ +bool Path::BuildTrivialPath( INextBot *bot, const Vector &goal ) +{ + const Vector &start = bot->GetPosition(); + + m_segmentCount = 0; + + /// @todo Dangerous to use "nearset" nav area - could be far away + CNavArea *startArea = TheNavMesh->GetNearestNavArea( start ); + if (startArea == NULL) + return false; + + CNavArea *goalArea = TheNavMesh->GetNearestNavArea( goal ); + if (goalArea == NULL) + return false; + + m_segmentCount = 2; + + m_path[0].area = startArea; + m_path[0].pos.x = start.x; + m_path[0].pos.y = start.y; + m_path[0].pos.z = startArea->GetZ( start ); + m_path[0].ladder = NULL; + m_path[0].how = NUM_TRAVERSE_TYPES; + m_path[0].type = ON_GROUND; + + m_path[1].area = goalArea; + m_path[1].pos.x = goal.x; + m_path[1].pos.y = goal.y; + m_path[1].pos.z = goalArea->GetZ( goal ); + m_path[1].ladder = NULL; + m_path[1].how = NUM_TRAVERSE_TYPES; + m_path[1].type = ON_GROUND; + + m_path[0].forward = m_path[1].pos - m_path[0].pos; + m_path[0].length = m_path[0].forward.NormalizeInPlace(); + m_path[0].distanceFromStart = 0.0f; + m_path[0].curvature = 0.0f; + + m_path[1].forward = m_path[0].forward; + m_path[1].length = 0.0f; + m_path[1].distanceFromStart = m_path[0].length; + m_path[1].curvature = 0.0f; + + OnPathChanged( bot, COMPLETE_PATH ); + + return true; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Draw the path for debugging. + */ +void Path::Draw( const Path::Segment *start ) const +{ + if ( !IsValid() ) + return; + + CFmtStr msg; + + // limit length of path we draw + int count = NextBotPathDrawSegmentCount.GetInt(); + + const Segment *s = start ? start : FirstSegment(); + int i=0; + while( s && count-- ) + { + const Segment *next = NextSegment( s ); + if ( next == NULL ) + { + // end of the path + break; + } + + Vector to = next->pos - s->pos; + float horiz = MAX( abs(to.x), abs(to.y) ); + float vert = abs( to.z ); + + int r,g,b; + switch( s->type ) + { + case DROP_DOWN: r = 255; g = 0; b = 255; break; + case CLIMB_UP: r = 0; g = 0; b = 255; break; + case JUMP_OVER_GAP: r = 0; g = 255; b = 255; break; + case LADDER_UP: r = 0; g = 255; b = 0; break; + case LADDER_DOWN: r = 0; g = 100; b = 0; break; + default: r = 255; g = 77; b = 0; break; // ON_GROUND + } + + if ( s->ladder ) + { + NDebugOverlay::VertArrow( s->ladder->m_bottom, s->ladder->m_top, 5.0f, r, g, b, 255, true, 0.1f ); + } + else + { + NDebugOverlay::Line( s->pos, next->pos, r, g, b, true, 0.1f ); + } + + const float nodeLength = 25.0f; + if ( horiz > vert ) + { + NDebugOverlay::HorzArrow( s->pos, s->pos + nodeLength * s->forward, 5.0f, r, g, b, 255, true, 0.1f ); + } + else + { + NDebugOverlay::VertArrow( s->pos, s->pos + nodeLength * s->forward, 5.0f, r, g, b, 255, true, 0.1f ); + } + + NDebugOverlay::Text( s->pos, msg.sprintf( "%d", i ), true, 0.1f ); + + //NDebugOverlay::Text( s->pos, msg.sprintf( "%d (%3.2f)", i, s->curvature ), false, 0.1f ); + + s = next; + ++i; + } +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Draw the path for debugging - MODIFIES cursor position + */ +void Path::DrawInterpolated( float from, float to ) +{ + if ( !IsValid() ) + { + return; + } + + float t = from; + + MoveCursor( t ); + const Data &data = GetCursorData(); + Vector lastPos = data.pos; + + do + { + t += NextBotPathDrawIncrement.GetFloat(); + + MoveCursor( t ); + const Data &data = GetCursorData(); + + float curvePower = 3.0f * data.curvature; + + int r = 255 * ( 1.0f - curvePower ); + r = clamp( r, 0, 255 ); + + int g = 255 * ( 1.0f + curvePower ); + g = clamp( g, 0, 255 ); + + NDebugOverlay::Line( lastPos, data.pos, r, g, 0, true, 0.1f ); + + /* + int i = 0xFF & (int)( data.pos.x + data.pos.y + data.pos.z ); + i >>= 1; + i += 128; + + NDebugOverlay::Line( data.pos, data.pos + 10.0f * data.forward, 0, i, i, true, 0.1f ); + */ + + lastPos = data.pos; + } + while( t < to ); +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Check line of sight from 'anchor' node on path to subsequent nodes until + * we find a node that can't been seen from 'anchor'. + */ +int Path::FindNextOccludedNode( INextBot *bot, int anchorIndex ) +{ + ILocomotion *mover = bot->GetLocomotionInterface(); + if ( mover == NULL) + { + return m_segmentCount; + } + + Segment *anchor = &m_path[ anchorIndex ]; + + for( int i=anchorIndex+1; i<m_segmentCount; ++i ) + { + Segment *to = &m_path[i]; + + // if this segment is not on the ground, or is precise, don't skip past it + if ( !to->type == ON_GROUND || (to->area->GetAttributes() & NAV_MESH_PRECISE) ) + { + return i; + } + + if ( !mover->IsPotentiallyTraversable( anchor->pos, to->pos, ILocomotion::IMMEDIATELY ) ) + { + // cant reach this node directly from anchor node + return i; + } + + if ( mover->HasPotentialGap( anchor->pos, to->pos ) ) + { + // we would fall into a gap if we took this cutoff + return i; + } + } + + return m_segmentCount; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Smooth out path, removing redundant nodes + */ +void Path::Optimize( INextBot *bot ) +{ + // this is SUPER expensive - especially the IsGap() check + return; + + VPROF_BUDGET( "Path::Optimize", "NextBot" ); + + if (m_segmentCount < 3) + return; + + int anchor = 0; + + while( anchor < m_segmentCount ) + { + int occluded = FindNextOccludedNode( bot, anchor ); + int nextAnchor = occluded-1; + + if (nextAnchor > anchor) + { + // remove redundant nodes between anchor and nextAnchor + int removeCount = nextAnchor - anchor - 1; + if (removeCount > 0) + { + for( int i=nextAnchor; i<m_segmentCount; ++i ) + { + m_path[i-removeCount] = m_path[i]; + } + m_segmentCount -= removeCount; + } + } + + ++anchor; + } +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Compute final data for completed path + */ +void Path::PostProcess( void ) +{ + VPROF_BUDGET( "Path::PostProcess", "NextBot" ); + + m_ageTimer.Start(); + + if (m_segmentCount == 0) + return; + + if (m_segmentCount == 1) + { + m_path[0].forward = vec3_origin; + m_path[0].length = 0.0f; + m_path[0].distanceFromStart = 0.0f; + m_path[0].curvature = 0.0f; + return; + } + + float distanceSoFar = 0.0f; + int i; + for( i=0; i < m_segmentCount-1; ++i ) + { + Segment *from = &m_path[ i ]; + Segment *to = &m_path[ i+1 ]; + + from->forward = to->pos - from->pos; + from->length = from->forward.NormalizeInPlace(); + + from->distanceFromStart = distanceSoFar; + + distanceSoFar += from->length; + } + + + // compute curvature in XY plane + Vector2D from, to; + for( i=1; i < m_segmentCount-1; ++i ) + { + if (m_path[ i ].type != ON_GROUND) + { + m_path[ i ].curvature = 0.0f; + } + else + { + from = m_path[ i-1 ].forward.AsVector2D(); + from.NormalizeInPlace(); + + to = m_path[ i ].forward.AsVector2D(); + to.NormalizeInPlace(); + + m_path[ i ].curvature = 0.5f * ( 1.0f - from.Dot( to ) ); + + Vector2D right( -from.y, from.x ); + if ( to.Dot( right ) < 0.0f ) + { + m_path[ i ].curvature = -m_path[ i ].curvature; + } + } + } + + // first segment has no curvature + m_path[ 0 ].curvature = 0.0f; + + // last segment maintains direction + m_path[ i ].forward = m_path[ i-1 ].forward; + m_path[ i ].length = 0.0f; + m_path[ i ].distanceFromStart = distanceSoFar; + m_path[ i ].curvature = 0.0f; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return a position on the path at the given distance from the path start + */ +const Vector &Path::GetPosition( float distanceFromStart, const Segment *start ) const +{ + if (!IsValid()) + { + return vec3_origin; + } + + float lengthSoFar; + const Segment *segment; + + if (start) + { + segment = start; + lengthSoFar = start->distanceFromStart; + } + else + { + segment = &m_path[0]; + lengthSoFar = 0.0f; + } + + if (segment->distanceFromStart > distanceFromStart) + { + // clamp to path start + return segment->pos; + } + + + const Segment *next = NextSegment( segment ); + + Vector delta; + float length; + + while( next ) + { + delta = next->pos - segment->pos; + length = segment->length; + + if (lengthSoFar + length >= distanceFromStart) + { + // desired point is on this segment of the path + float overlap = distanceFromStart - lengthSoFar; + float t = overlap / length; + + m_pathPos = segment->pos + t * delta; + + return m_pathPos; + } + + lengthSoFar += length; + + segment = next; + next = NextSegment( next ); + } + + // clamp to path end + return segment->pos; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return the closest point on the path to the given position + */ +const Vector &Path::GetClosestPosition( const Vector &pos, const Segment *start, float alongLimit ) const +{ + const Segment *segment = (start) ? start : &m_path[0]; + + if (segment == NULL) + { + return pos; + } + + m_closePos = pos; + float closeRangeSq = 99999999999.9f; + + float distanceSoFar = 0.0f; + while( alongLimit == 0.0f || distanceSoFar <= alongLimit ) + { + const Segment *nextSegment = NextSegment( segment ); + + if (nextSegment) + { + Vector close; + CalcClosestPointOnLineSegment( pos, segment->pos, nextSegment->pos, close ); + float rangeSq = (close - pos).LengthSqr(); + if (rangeSq < closeRangeSq) + { + m_closePos = close; + closeRangeSq = rangeSq; + } + } + else + { + // end of the path + break; + } + + distanceSoFar += segment->length; + segment = nextSegment; + } + + return m_closePos; +} + + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Replace this path with the given path's data + */ +void Path::Copy( INextBot *bot, const Path &path ) +{ + VPROF_BUDGET( "Path::Copy", "NextBot" ); + + Invalidate(); + + for( int i = 0; i < path.m_segmentCount; ++i ) + { + m_path[i] = path.m_path[i]; + } + m_segmentCount = path.m_segmentCount; + + OnPathChanged( bot, COMPLETE_PATH ); +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Set cursor position to closest point on path to given position + */ +void Path::MoveCursorToClosestPosition( const Vector &pos, SeekType type, float alongLimit ) const +{ + if ( !IsValid() ) + { + return; + } + + if ( type == SEEK_ENTIRE_PATH || type == SEEK_AHEAD ) + { + const Segment *segment; + + if ( type == SEEK_AHEAD ) + { + // continue search from cursor position onward + if ( m_cursorData.segmentPrior ) + { + segment = m_cursorData.segmentPrior; + } + else + { + // no prior segment, start from the start + segment = &m_path[ 0 ]; + } + } + else + { + // search entire path from the start + segment = &m_path[ 0 ]; + } + + m_cursorData.pos = pos; + m_cursorData.segmentPrior = segment; + float closeRangeSq = 99999999999.9f; + + float distanceSoFar = 0.0f; + while( alongLimit == 0.0f || distanceSoFar <= alongLimit ) + { + const Segment *nextSegment = NextSegment( segment ); + + if ( nextSegment ) + { + Vector close; + CalcClosestPointOnLineSegment( pos, segment->pos, nextSegment->pos, close ); + + float rangeSq = ( close - pos ).LengthSqr(); + if ( rangeSq < closeRangeSq ) + { + m_cursorData.pos = close; + m_cursorData.segmentPrior = segment; + + closeRangeSq = rangeSq; + } + } + else + { + // end of the path + break; + } + + distanceSoFar += segment->length; + segment = nextSegment; + } + + // + // Move cursor to closest point on path + // + segment = m_cursorData.segmentPrior; + + float t = ( m_cursorData.pos - segment->pos ).Length() / segment->length; + + m_cursorPos = segment->distanceFromStart + t * segment->length; + m_isCursorDataDirty = true; + } + else + { + AssertMsg( false, "SEEK_BEHIND not implemented" ); + } +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return path state at the current cursor position + */ +const Path::Data &Path::GetCursorData( void ) const +{ + if ( IsValid() ) + { + if ( m_isCursorDataDirty ) + { + const float epsilon = 0.0001f; + if ( m_cursorPos < epsilon || m_segmentCount < 2 ) + { + // start of path + m_cursorData.pos = m_path[0].pos; + m_cursorData.forward = m_path[0].forward; + m_cursorData.curvature = m_path[0].curvature; + m_cursorData.segmentPrior = &m_path[0]; + } + else if ( m_cursorPos > GetLength() - epsilon ) + { + // end of path + m_cursorData.pos = m_path[ m_segmentCount-1 ].pos; + m_cursorData.forward = m_path[ m_segmentCount-1 ].forward; + m_cursorData.curvature = m_path[ m_segmentCount-1 ].curvature; + m_cursorData.segmentPrior = &m_path[ m_segmentCount-1 ]; + } + else + { + // along path + float lengthSoFar = 0.0f; + const Segment *segment = &m_path[0]; + + const Segment *next = NextSegment( segment ); + + while( next ) + { + float length = segment->length; + + if ( lengthSoFar + length >= m_cursorPos ) + { + // desired point is on this segment of the path + float overlap = m_cursorPos - lengthSoFar; + float t = 1.0f; // 0-length segments are assumed to be complete, to avoid NaNs + if ( length > 0.0f ) + { + t = overlap / length; + } + + // interpolate data at this point along the path + m_cursorData.pos = segment->pos + t * ( next->pos - segment->pos ); + m_cursorData.forward = segment->forward + t * ( next->forward - segment->forward ); + m_cursorData.segmentPrior = segment; + + // curvature fades to zero along midpoint of long straight segments + // and is influenced as it nears ends of segment + if ( overlap < NextBotPathSegmentInfluenceRadius.GetFloat() ) + { + if ( length - overlap < NextBotPathSegmentInfluenceRadius.GetFloat() ) + { + // near start and end - interpolate + float startCurvature = segment->curvature * ( 1.0f - ( overlap / NextBotPathSegmentInfluenceRadius.GetFloat() ) ); + float endCurvature = next->curvature * ( 1.0f - ( ( length - overlap ) / NextBotPathSegmentInfluenceRadius.GetFloat() ) ); + + m_cursorData.curvature = ( startCurvature + endCurvature ) / 2.0f; + } + else + { + // near start only + m_cursorData.curvature = segment->curvature * ( 1.0f - ( overlap / NextBotPathSegmentInfluenceRadius.GetFloat() ) ); + } + } + else if ( length - overlap < NextBotPathSegmentInfluenceRadius.GetFloat() ) + { + // near end only + m_cursorData.curvature = next->curvature * ( 1.0f - ( ( length - overlap ) / NextBotPathSegmentInfluenceRadius.GetFloat() ) ); + } + + + break; + } + + lengthSoFar += length; + + segment = next; + next = NextSegment( next ); + } + } + + // data is up to date + m_isCursorDataDirty = false; + } + } + else + { + // path is not valid + m_cursorData.pos = vec3_origin; + m_cursorData.forward = Vector( 1.0f, 0, 0 ); + m_cursorData.curvature = 0.0f; + m_cursorData.segmentPrior = NULL; + } + + return m_cursorData; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Determine exactly where the path goes between the given two areas + * on the path. Return this point in 'crossPos'. + */ +void Path::ComputeAreaCrossing( INextBot *bot, const CNavArea *from, const Vector &fromPos, const CNavArea *to, NavDirType dir, Vector *crossPos ) const +{ + from->ComputeClosestPointInPortal( to, dir, fromPos, crossPos ); + + // move goal position into the goal area a bit to avoid running directly along the edge of an area against a wall, etc + // don't do this unless area is against a wall - and what if our hull is wider than the area? + // AddDirectionVector( crossPos, dir, bot->GetBodyInterface()->GetHullWidth()/2.0f ); +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/game/server/NextBot/Path/NextBotPath.h b/game/server/NextBot/Path/NextBotPath.h new file mode 100644 index 0000000..6669d92 --- /dev/null +++ b/game/server/NextBot/Path/NextBotPath.h @@ -0,0 +1,862 @@ +// NextBotPath.h +// Encapsulate and manipulate a path through the world +// Author: Michael Booth, February 2006 +//========= Copyright Valve Corporation, All rights reserved. ============// + +#ifndef _NEXT_BOT_PATH_H_ +#define _NEXT_BOT_PATH_H_ + +#include "NextBotInterface.h" + +#include "tier0/vprof.h" + +#define PATH_NO_LENGTH_LIMIT 0.0f // non-default argument value for Path::Compute() +#define PATH_TRUNCATE_INCOMPLETE_PATH false // non-default argument value for Path::Compute() + +class INextBot; +class CNavArea; +class CNavLadder; + + +//--------------------------------------------------------------------------------------------------------------- +/** + * The interface for pathfinding costs. + * TODO: Replace all template cost functors with this interface, so we can virtualize and derive from them. + */ +class IPathCost +{ +public: + virtual float operator()( CNavArea *area, CNavArea *fromArea, const CNavLadder *ladder, const CFuncElevator *elevator, float length ) const = 0; +}; + + +//--------------------------------------------------------------------------------------------------------------- +/** + * The interface for selecting a goal area during "open goal" pathfinding + */ +class IPathOpenGoalSelector +{ +public: + // compare "newArea" to "currentGoal" and return the area that is the better goal area + virtual CNavArea *operator() ( CNavArea *currentGoal, CNavArea *newArea ) const = 0; +}; + + +//--------------------------------------------------------------------------------------------------------------- +/** + * A Path through the world. + * Not only does this encapsulate a path to get from point A to point B, + * but also the selecting the decision algorithm for how to build that path. + */ +class Path +{ +public: + Path( void ); + virtual ~Path() { } + + enum SegmentType + { + ON_GROUND, + DROP_DOWN, + CLIMB_UP, + JUMP_OVER_GAP, + LADDER_UP, + LADDER_DOWN, + + NUM_SEGMENT_TYPES + }; + + // @todo Allow custom Segment classes for different kinds of paths + struct Segment + { + CNavArea *area; // the area along the path + NavTraverseType how; // how to enter this area from the previous one + Vector pos; // our movement goal position at this point in the path + const CNavLadder *ladder; // if "how" refers to a ladder, this is it + + SegmentType type; // how to traverse this segment of the path + Vector forward; // unit vector along segment + float length; // length of this segment + float distanceFromStart; // distance of this node from the start of the path + float curvature; // how much the path 'curves' at this point in the XY plane (0 = none, 1 = 180 degree doubleback) + + Vector m_portalCenter; // position of center of 'portal' between previous area and this area + float m_portalHalfWidth; // half width of 'portal' + }; + + virtual float GetLength( void ) const; // return length of path from start to finish + virtual const Vector &GetPosition( float distanceFromStart, const Segment *start = NULL ) const; // return a position on the path at the given distance from the path start + virtual const Vector &GetClosestPosition( const Vector &pos, const Segment *start = NULL, float alongLimit = 0.0f ) const; // return the closest point on the path to the given position + + virtual const Vector &GetStartPosition( void ) const; // return the position where this path starts + virtual const Vector &GetEndPosition( void ) const; // return the position where this path ends + virtual CBaseCombatCharacter *GetSubject( void ) const; // return the actor this path leads to, or NULL if there is no subject + + virtual const Path::Segment *GetCurrentGoal( void ) const; // return current goal along the path we are trying to reach + + virtual float GetAge( void ) const; // return "age" of this path (time since it was built) + + enum SeekType + { + SEEK_ENTIRE_PATH, // search the entire path length + SEEK_AHEAD, // search from current cursor position forward toward end of path + SEEK_BEHIND // search from current cursor position backward toward path start + }; + virtual void MoveCursorToClosestPosition( const Vector &pos, SeekType type = SEEK_ENTIRE_PATH, float alongLimit = 0.0f ) const; // Set cursor position to closest point on path to given position + + enum MoveCursorType + { + PATH_ABSOLUTE_DISTANCE, + PATH_RELATIVE_DISTANCE + }; + virtual void MoveCursorToStart( void ); // set seek cursor to start of path + virtual void MoveCursorToEnd( void ); // set seek cursor to end of path + virtual void MoveCursor( float value, MoveCursorType type = PATH_ABSOLUTE_DISTANCE ); // change seek cursor position + virtual float GetCursorPosition( void ) const; // return position of seek cursor (distance along path) + + struct Data + { + Vector pos; // the position along the path + Vector forward; // unit vector along path direction + float curvature; // how much the path 'curves' at this point in the XY plane (0 = none, 1 = 180 degree doubleback) + const Segment *segmentPrior; // the segment just before this position + }; + virtual const Data &GetCursorData( void ) const; // return path state at the current cursor position + + virtual bool IsValid( void ) const; + virtual void Invalidate( void ); // make path invalid (clear it) + + virtual void Draw( const Path::Segment *start = NULL ) const; // draw the path for debugging + virtual void DrawInterpolated( float from, float to ); // draw the path for debugging - MODIFIES cursor position + + virtual const Segment *FirstSegment( void ) const; // return first segment of path + virtual const Segment *NextSegment( const Segment *currentSegment ) const; // return next segment of path, given current one + virtual const Segment *PriorSegment( const Segment *currentSegment ) const; // return previous segment of path, given current one + virtual const Segment *LastSegment( void ) const; // return last segment of path + + enum ResultType + { + COMPLETE_PATH, + PARTIAL_PATH, + NO_PATH + }; + virtual void OnPathChanged( INextBot *bot, ResultType result ) { } // invoked when the path is (re)computed (path is valid at the time of this call) + + virtual void Copy( INextBot *bot, const Path &path ); // Replace this path with the given path's data + + + //----------------------------------------------------------------------------------------------------------------- + /** + * Compute shortest path from bot to given actor via A* algorithm. + * If returns true, path was found to the subject. + * If returns false, path may either be invalid (use IsValid() to check), or valid but + * doesn't reach all the way to the subject. + */ + template< typename CostFunctor > + bool Compute( INextBot *bot, CBaseCombatCharacter *subject, CostFunctor &costFunc, float maxPathLength = 0.0f, bool includeGoalIfPathFails = true ) + { + VPROF_BUDGET( "Path::Compute(subject)", "NextBot" ); + + Invalidate(); + + m_subject = subject; + + const Vector &start = bot->GetPosition(); + + CNavArea *startArea = bot->GetEntity()->GetLastKnownArea(); + if ( !startArea ) + { + OnPathChanged( bot, NO_PATH ); + return false; + } + + CNavArea *subjectArea = subject->GetLastKnownArea(); + if ( !subjectArea ) + { + OnPathChanged( bot, NO_PATH ); + return false; + } + + Vector subjectPos = subject->GetAbsOrigin(); + + // if we are already in the subject area, build trivial path + if ( startArea == subjectArea ) + { + BuildTrivialPath( bot, subjectPos ); + return true; + } + + // + // Compute shortest path to subject + // + CNavArea *closestArea = NULL; + bool pathResult = NavAreaBuildPath( startArea, subjectArea, &subjectPos, costFunc, &closestArea, maxPathLength, bot->GetEntity()->GetTeamNumber() ); + + // Failed? + if ( closestArea == NULL ) + return false; + + // + // Build actual path by following parent links back from goal area + // + + // get count + int count = 0; + CNavArea *area; + for( area = closestArea; area; area = area->GetParent() ) + { + ++count; + + if ( area == startArea ) + { + // startArea can be re-evaluated during the pathfind and given a parent... + break; + } + if ( count >= MAX_PATH_SEGMENTS-1 ) // save room for endpoint + break; + } + + if ( count == 1 ) + { + BuildTrivialPath( bot, subjectPos ); + return pathResult; + } + + // assemble path + m_segmentCount = count; + for( area = closestArea; count && area; area = area->GetParent() ) + { + --count; + m_path[ count ].area = area; + m_path[ count ].how = area->GetParentHow(); + m_path[ count ].type = ON_GROUND; + } + + if ( pathResult || includeGoalIfPathFails ) + { + // append actual subject position + m_path[ m_segmentCount ].area = closestArea; + m_path[ m_segmentCount ].pos = subjectPos; + m_path[ m_segmentCount ].ladder = NULL; + m_path[ m_segmentCount ].how = NUM_TRAVERSE_TYPES; + m_path[ m_segmentCount ].type = ON_GROUND; + ++m_segmentCount; + } + + // compute path positions + if ( ComputePathDetails( bot, start ) == false ) + { + Invalidate(); + OnPathChanged( bot, NO_PATH ); + return false; + } + + // remove redundant nodes and clean up path + Optimize( bot ); + + PostProcess(); + + OnPathChanged( bot, pathResult ? COMPLETE_PATH : PARTIAL_PATH ); + + return pathResult; + } + + + //----------------------------------------------------------------------------------------------------------------- + /** + * Compute shortest path from bot to 'goal' via A* algorithm. + * If returns true, path was found to the goal position. + * If returns false, path may either be invalid (use IsValid() to check), or valid but + * doesn't reach all the way to the goal. + */ + template< typename CostFunctor > + bool Compute( INextBot *bot, const Vector &goal, CostFunctor &costFunc, float maxPathLength = 0.0f, bool includeGoalIfPathFails = true ) + { + VPROF_BUDGET( "Path::Compute(goal)", "NextBotSpiky" ); + + Invalidate(); + + const Vector &start = bot->GetPosition(); + + CNavArea *startArea = bot->GetEntity()->GetLastKnownArea(); + if ( !startArea ) + { + OnPathChanged( bot, NO_PATH ); + return false; + } + + // check line-of-sight to the goal position when finding it's nav area + const float maxDistanceToArea = 200.0f; + CNavArea *goalArea = TheNavMesh->GetNearestNavArea( goal, true, maxDistanceToArea, true ); + + // if we are already in the goal area, build trivial path + if ( startArea == goalArea ) + { + BuildTrivialPath( bot, goal ); + return true; + } + + // make sure path end position is on the ground + Vector pathEndPosition = goal; + if ( goalArea ) + { + pathEndPosition.z = goalArea->GetZ( pathEndPosition ); + } + else + { + TheNavMesh->GetGroundHeight( pathEndPosition, &pathEndPosition.z ); + } + + // + // Compute shortest path to goal + // + CNavArea *closestArea = NULL; + bool pathResult = NavAreaBuildPath( startArea, goalArea, &goal, costFunc, &closestArea, maxPathLength, bot->GetEntity()->GetTeamNumber() ); + + // Failed? + if ( closestArea == NULL ) + return false; + + // + // Build actual path by following parent links back from goal area + // + + // get count + int count = 0; + CNavArea *area; + for( area = closestArea; area; area = area->GetParent() ) + { + ++count; + + if ( area == startArea ) + { + // startArea can be re-evaluated during the pathfind and given a parent... + break; + } + if ( count >= MAX_PATH_SEGMENTS-1 ) // save room for endpoint + break; + } + + if ( count == 1 ) + { + BuildTrivialPath( bot, goal ); + return pathResult; + } + + // assemble path + m_segmentCount = count; + for( area = closestArea; count && area; area = area->GetParent() ) + { + --count; + m_path[ count ].area = area; + m_path[ count ].how = area->GetParentHow(); + m_path[ count ].type = ON_GROUND; + } + + if ( pathResult || includeGoalIfPathFails ) + { + // append actual goal position + m_path[ m_segmentCount ].area = closestArea; + m_path[ m_segmentCount ].pos = pathEndPosition; + m_path[ m_segmentCount ].ladder = NULL; + m_path[ m_segmentCount ].how = NUM_TRAVERSE_TYPES; + m_path[ m_segmentCount ].type = ON_GROUND; + ++m_segmentCount; + } + + // compute path positions + if ( ComputePathDetails( bot, start ) == false ) + { + Invalidate(); + OnPathChanged( bot, NO_PATH ); + return false; + } + + // remove redundant nodes and clean up path + Optimize( bot ); + + PostProcess(); + + OnPathChanged( bot, pathResult ? COMPLETE_PATH : PARTIAL_PATH ); + + return pathResult; + } + + + //----------------------------------------------------------------------------------------------------------------- + /** + * Build a path from bot's current location to an undetermined goal area + * that minimizes the given cost along the final path and meets the + * goal criteria. + */ + virtual bool ComputeWithOpenGoal( INextBot *bot, const IPathCost &costFunc, const IPathOpenGoalSelector &goalSelector, float maxSearchRadius = 0.0f ) + { + VPROF_BUDGET( "ComputeWithOpenGoal", "NextBot" ); + + int teamID = bot->GetEntity()->GetTeamNumber(); + + CNavArea *startArea = bot->GetEntity()->GetLastKnownArea(); + + if ( startArea == NULL ) + return NULL; + + startArea->SetParent( NULL ); + + // start search + CNavArea::ClearSearchLists(); + + float initCost = costFunc( startArea, NULL, NULL, NULL, -1.0f ); + if ( initCost < 0.0f ) + return NULL; + + startArea->SetTotalCost( initCost ); + startArea->AddToOpenList(); + + // find our goal as we search + CNavArea *goalArea = NULL; + + // + // Dijkstra's algorithm (since we don't know our goal). + // + while( !CNavArea::IsOpenListEmpty() ) + { + // get next area to check + CNavArea *area = CNavArea::PopOpenList(); + + area->AddToClosedList(); + + // don't consider blocked areas + if ( area->IsBlocked( teamID ) ) + continue; + + // build adjacent area array + CollectAdjacentAreas( area ); + + // search adjacent areas + for( int i=0; i<m_adjAreaIndex; ++i ) + { + CNavArea *newArea = m_adjAreaVector[ i ].area; + + // only visit each area once + if ( newArea->IsClosed() ) + continue; + + // don't consider blocked areas + if ( newArea->IsBlocked( teamID ) ) + continue; + + // don't use this area if it is out of range + if ( maxSearchRadius > 0.0f && ( newArea->GetCenter() - bot->GetEntity()->GetAbsOrigin() ).IsLengthGreaterThan( maxSearchRadius ) ) + continue; + + // determine cost of traversing this area + float newCost = costFunc( newArea, area, m_adjAreaVector[ i ].ladder, NULL, -1.0f ); + + // don't use adjacent area if cost functor says it is a dead-end + if ( newCost < 0.0f ) + continue; + + if ( newArea->IsOpen() && newArea->GetTotalCost() <= newCost ) + { + // we have already visited this area, and it has a better path + continue; + } + else + { + // whether this area has been visited or not, we now have a better path to it + newArea->SetParent( area, m_adjAreaVector[ i ].how ); + newArea->SetTotalCost( newCost ); + + // use 'cost so far' to hold cumulative cost + newArea->SetCostSoFar( newCost ); + + // tricky bit here - relying on OpenList being sorted by cost + if ( newArea->IsOpen() ) + { + // area already on open list, update the list order to keep costs sorted + newArea->UpdateOnOpenList(); + } + else + { + newArea->AddToOpenList(); + } + + // keep track of best goal so far + goalArea = goalSelector( goalArea, newArea ); + } + } + } + + if ( goalArea ) + { + // compile the path details into a usable path + AssemblePrecomputedPath( bot, goalArea->GetCenter(), goalArea ); + return true; + } + + // all adjacent areas are likely too far away + return false; + } + + + //----------------------------------------------------------------------------------------------------------------- + /** + * Given the last area in a path with valid parent pointers, + * construct the actual path. + */ + void AssemblePrecomputedPath( INextBot *bot, const Vector &goal, CNavArea *endArea ) + { + VPROF_BUDGET( "AssemblePrecomputedPath", "NextBot" ); + + const Vector &start = bot->GetPosition(); + + // get count + int count = 0; + CNavArea *area; + for( area = endArea; area; area = area->GetParent() ) + { + ++count; + } + + // save room for endpoint + if ( count > MAX_PATH_SEGMENTS-1 ) + { + count = MAX_PATH_SEGMENTS-1; + } + else if ( count == 0 ) + { + return; + } + + if ( count == 1 ) + { + BuildTrivialPath( bot, goal ); + return; + } + + // assemble path + m_segmentCount = count; + for( area = endArea; count && area; area = area->GetParent() ) + { + --count; + m_path[ count ].area = area; + m_path[ count ].how = area->GetParentHow(); + m_path[ count ].type = ON_GROUND; + } + + // append actual goal position + m_path[ m_segmentCount ].area = endArea; + m_path[ m_segmentCount ].pos = goal; + m_path[ m_segmentCount ].ladder = NULL; + m_path[ m_segmentCount ].how = NUM_TRAVERSE_TYPES; + m_path[ m_segmentCount ].type = ON_GROUND; + ++m_segmentCount; + + // compute path positions + if ( ComputePathDetails( bot, start ) == false ) + { + Invalidate(); + OnPathChanged( bot, NO_PATH ); + return; + } + + // remove redundant nodes and clean up path + Optimize( bot ); + + PostProcess(); + + OnPathChanged( bot, COMPLETE_PATH ); + } + + /** + * Utility function for when start and goal are in the same area + */ + bool BuildTrivialPath( INextBot *bot, const Vector &goal ); + + /** + * Determine exactly where the path goes between the given two areas + * on the path. Return this point in 'crossPos'. + */ + virtual void ComputeAreaCrossing( INextBot *bot, const CNavArea *from, const Vector &fromPos, const CNavArea *to, NavDirType dir, Vector *crossPos ) const; + + +private: + enum { MAX_PATH_SEGMENTS = 256 }; + Segment m_path[ MAX_PATH_SEGMENTS ]; + int m_segmentCount; + + bool ComputePathDetails( INextBot *bot, const Vector &start ); // determine actual path positions + + void Optimize( INextBot *bot ); + void PostProcess( void ); + int FindNextOccludedNode( INextBot *bot, int anchor ); // used by Optimize() + + void InsertSegment( Segment newSegment, int i ); // insert new segment at index i + + mutable Vector m_pathPos; // used by GetPosition() + mutable Vector m_closePos; // used by GetClosestPosition() + + mutable float m_cursorPos; // current cursor position (distance along path) + mutable Data m_cursorData; // used by GetCursorData() + mutable bool m_isCursorDataDirty; + + IntervalTimer m_ageTimer; // how old is this path? + CHandle< CBaseCombatCharacter > m_subject; // the subject this path leads to + + /** + * Build a vector of adjacent areas reachable from the given area + */ + void CollectAdjacentAreas( CNavArea *area ) + { + m_adjAreaIndex = 0; + + const NavConnectVector &adjNorth = *area->GetAdjacentAreas( NORTH ); + FOR_EACH_VEC( adjNorth, it ) + { + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + m_adjAreaVector[ m_adjAreaIndex ].area = adjNorth[ it ].area; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_NORTH; + m_adjAreaVector[ m_adjAreaIndex ].ladder = NULL; + ++m_adjAreaIndex; + } + + const NavConnectVector &adjSouth = *area->GetAdjacentAreas( SOUTH ); + FOR_EACH_VEC( adjSouth, it ) + { + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + m_adjAreaVector[ m_adjAreaIndex ].area = adjSouth[ it ].area; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_SOUTH; + m_adjAreaVector[ m_adjAreaIndex ].ladder = NULL; + ++m_adjAreaIndex; + } + + const NavConnectVector &adjWest = *area->GetAdjacentAreas( WEST ); + FOR_EACH_VEC( adjWest, it ) + { + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + m_adjAreaVector[ m_adjAreaIndex ].area = adjWest[ it ].area; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_WEST; + m_adjAreaVector[ m_adjAreaIndex ].ladder = NULL; + ++m_adjAreaIndex; + } + + const NavConnectVector &adjEast = *area->GetAdjacentAreas( EAST ); + FOR_EACH_VEC( adjEast, it ) + { + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + m_adjAreaVector[ m_adjAreaIndex ].area = adjEast[ it ].area; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_EAST; + m_adjAreaVector[ m_adjAreaIndex ].ladder = NULL; + ++m_adjAreaIndex; + } + + const NavLadderConnectVector &adjUpLadder = *area->GetLadders( CNavLadder::LADDER_UP ); + FOR_EACH_VEC( adjUpLadder, it ) + { + CNavLadder *ladder = adjUpLadder[ it ].ladder; + + if ( ladder->m_topForwardArea && m_adjAreaIndex < MAX_ADJ_AREAS ) + { + m_adjAreaVector[ m_adjAreaIndex ].area = ladder->m_topForwardArea; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_LADDER_UP; + m_adjAreaVector[ m_adjAreaIndex ].ladder = ladder; + ++m_adjAreaIndex; + } + + if ( ladder->m_topLeftArea && m_adjAreaIndex < MAX_ADJ_AREAS ) + { + m_adjAreaVector[ m_adjAreaIndex ].area = ladder->m_topLeftArea; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_LADDER_UP; + m_adjAreaVector[ m_adjAreaIndex ].ladder = ladder; + ++m_adjAreaIndex; + } + + if ( ladder->m_topRightArea && m_adjAreaIndex < MAX_ADJ_AREAS ) + { + m_adjAreaVector[ m_adjAreaIndex ].area = ladder->m_topRightArea; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_LADDER_UP; + m_adjAreaVector[ m_adjAreaIndex ].ladder = ladder; + ++m_adjAreaIndex; + } + } + + const NavLadderConnectVector &adjDownLadder = *area->GetLadders( CNavLadder::LADDER_DOWN ); + FOR_EACH_VEC( adjDownLadder, it ) + { + CNavLadder *ladder = adjDownLadder[ it ].ladder; + + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + if ( ladder->m_bottomArea ) + { + m_adjAreaVector[ m_adjAreaIndex ].area = ladder->m_bottomArea; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_LADDER_DOWN; + m_adjAreaVector[ m_adjAreaIndex ].ladder = ladder; + ++m_adjAreaIndex; + } + } + } + + enum { MAX_ADJ_AREAS = 64 }; + + struct AdjInfo + { + CNavArea *area; + CNavLadder *ladder; + NavTraverseType how; + }; + + AdjInfo m_adjAreaVector[ MAX_ADJ_AREAS ]; + int m_adjAreaIndex; + +}; + + +inline float Path::GetLength( void ) const +{ + if (m_segmentCount <= 0) + { + return 0.0f; + } + + return m_path[ m_segmentCount-1 ].distanceFromStart; +} + +inline bool Path::IsValid( void ) const +{ + return (m_segmentCount > 0); +} + +inline void Path::Invalidate( void ) +{ + m_segmentCount = 0; + + m_cursorPos = 0.0f; + + m_cursorData.pos = vec3_origin; + m_cursorData.forward = Vector( 1.0f, 0, 0 ); + m_cursorData.curvature = 0.0f; + m_cursorData.segmentPrior = NULL; + + m_isCursorDataDirty = true; + + m_subject = NULL; +} + +inline const Path::Segment *Path::FirstSegment( void ) const +{ + return (IsValid()) ? &m_path[0] : NULL; +} + +inline const Path::Segment *Path::NextSegment( const Segment *currentSegment ) const +{ + if (currentSegment == NULL || !IsValid()) + return NULL; + + int i = currentSegment - m_path; + + if (i < 0 || i >= m_segmentCount-1) + { + return NULL; + } + + return &m_path[ i+1 ]; +} + +inline const Path::Segment *Path::PriorSegment( const Segment *currentSegment ) const +{ + if (currentSegment == NULL || !IsValid()) + return NULL; + + int i = currentSegment - m_path; + + if (i < 1 || i >= m_segmentCount) + { + return NULL; + } + + return &m_path[ i-1 ]; +} + +inline const Path::Segment *Path::LastSegment( void ) const +{ + return ( IsValid() ) ? &m_path[ m_segmentCount-1 ] : NULL; +} + +inline const Vector &Path::GetStartPosition( void ) const +{ + return ( IsValid() ) ? m_path[ 0 ].pos : vec3_origin; +} + +inline const Vector &Path::GetEndPosition( void ) const +{ + return ( IsValid() ) ? m_path[ m_segmentCount-1 ].pos : vec3_origin; +} + +inline CBaseCombatCharacter *Path::GetSubject( void ) const +{ + return m_subject; +} + +inline void Path::MoveCursorToStart( void ) +{ + m_cursorPos = 0.0f; + m_isCursorDataDirty = true; +} + +inline void Path::MoveCursorToEnd( void ) +{ + m_cursorPos = GetLength(); + m_isCursorDataDirty = true; +} + +inline void Path::MoveCursor( float value, MoveCursorType type ) +{ + if ( type == PATH_ABSOLUTE_DISTANCE ) + { + m_cursorPos = value; + } + else // relative distance + { + m_cursorPos += value; + } + + if ( m_cursorPos < 0.0f ) + { + m_cursorPos = 0.0f; + } + else if ( m_cursorPos > GetLength() ) + { + m_cursorPos = GetLength(); + } + + m_isCursorDataDirty = true; +} + +inline float Path::GetCursorPosition( void ) const +{ + return m_cursorPos; +} + +inline const Path::Segment *Path::GetCurrentGoal( void ) const +{ + return NULL; +} + +inline float Path::GetAge( void ) const +{ + return m_ageTimer.GetElapsedTime(); +} + + +#endif // _NEXT_BOT_PATH_H_ + diff --git a/game/server/NextBot/Path/NextBotPathFollow.cpp b/game/server/NextBot/Path/NextBotPathFollow.cpp new file mode 100644 index 0000000..58e2e85 --- /dev/null +++ b/game/server/NextBot/Path/NextBotPathFollow.cpp @@ -0,0 +1,1923 @@ +// NextBotPathFollow.cpp +// Path following +// Author: Michael Booth, April 2005 +//========= Copyright Valve Corporation, All rights reserved. ============// + +#include "cbase.h" + +#include "BasePropDoor.h" + +#include "nav_mesh.h" +#include "NextBot.h" +#include "NextBotPathFollow.h" +#include "NextBotUtil.h" + +#include "NextBotLocomotionInterface.h" +#include "NextBotBodyInterface.h" +#include "NextBotVisionInterface.h" + +#include "tier0/vprof.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +ConVar NextBotSpeedLookAheadRange( "nb_speed_look_ahead_range", "150", FCVAR_CHEAT ); +ConVar NextBotGoalLookAheadRange( "nb_goal_look_ahead_range", "50", FCVAR_CHEAT ); +ConVar NextBotLadderAlignRange( "nb_ladder_align_range", "50", FCVAR_CHEAT ); + +ConVar NextBotAllowAvoiding( "nb_allow_avoiding", "1", FCVAR_CHEAT ); +ConVar NextBotAllowClimbing( "nb_allow_climbing", "1", FCVAR_CHEAT ); +ConVar NextBotAllowGapJumping( "nb_allow_gap_jumping", "1", FCVAR_CHEAT ); + +ConVar NextBotDebugClimbing( "nb_debug_climbing", "0", FCVAR_CHEAT ); + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Constructor + */ +PathFollower::PathFollower( void ) +{ + m_goal = NULL; + m_didAvoidCheck = false; + + m_avoidTimer.Invalidate(); + m_waitTimer.Invalidate(); + m_hindrance = NULL; + + m_minLookAheadRange = -1.0f; + + // was 10.0f for L4D - need a better solution here (MSB 5/15/09) + m_goalTolerance = 25.0f; +} + + +//-------------------------------------------------------------------------------------------------------------- +class CDetachPath +{ +public: + CDetachPath( PathFollower *path ) + { + m_path = path; + } + + bool operator() ( INextBot *bot ) + { + bot->NotifyPathDestruction( m_path ); + return true; + } + + PathFollower *m_path; +}; + +//-------------------------------------------------------------------------------------------------------------- +PathFollower::~PathFollower() +{ + // allow bots to detach pointer to me + CDetachPath detach( this ); + TheNextBots().ForEachBot( detach ); +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * When the path is invalidated, the follower is also reset + */ +void PathFollower::Invalidate( void ) +{ + // extend + Path::Invalidate(); + + m_goal = NULL; + + m_avoidTimer.Invalidate(); + m_waitTimer.Invalidate(); + m_hindrance = NULL; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Invoked when the path is (re)computed (path is valid at the time of this call) + */ +void PathFollower::OnPathChanged( INextBot *bot, Path::ResultType result ) +{ + // start from the beginning + m_goal = FirstSegment(); +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Adjust speed based on path curvature + */ +void PathFollower::AdjustSpeed( INextBot *bot ) +{ + ILocomotion *mover = bot->GetLocomotionInterface(); + + // if we're coming up on a gap jump, or we're in the air, use maximum speed + if ( ( m_goal && m_goal->type == JUMP_OVER_GAP ) || !mover->IsOnGround() ) + { + mover->SetDesiredSpeed( mover->GetRunSpeed() ); + return; + } + + MoveCursorToClosestPosition( bot->GetPosition() ); + const Path::Data &data = GetCursorData(); + + // speed based on curvature + mover->SetDesiredSpeed( mover->GetRunSpeed() + fabs( data.curvature ) * ( mover->GetWalkSpeed() - mover->GetRunSpeed() ) ); +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return true if reached current goal along path + * NOTE: Ladder goals are handled elsewhere + */ +bool PathFollower::IsAtGoal( INextBot *bot ) const +{ + VPROF_BUDGET( "PathFollower::IsAtGoal", "NextBot" ); + + ILocomotion *mover = bot->GetLocomotionInterface(); + IBody *body = bot->GetBodyInterface(); + + // + // m_goal is the node we are moving toward along the path + // current is the node just behind us + // + const Segment *current = PriorSegment( m_goal ); + Vector toGoal = m_goal->pos - mover->GetFeet(); + +// if ( m_goal->type == JUMP_OVER_GAP && !mover->IsOnGround() ) +// { +// // jumping over a gap, don't skip ahead until we land +// return false; +// } + + if ( current == NULL ) + { + // passed goal + return true; + } + else if ( m_goal->type == DROP_DOWN ) + { + // m_goal is the top of the drop-down, and the following segment is the landing point + const Segment *landing = NextSegment( m_goal ); + + if ( landing == NULL ) + { + // passed goal or corrupt path + return true; + } + else + { + // did we reach the ground + if ( mover->GetFeet().z - landing->pos.z < mover->GetStepHeight() ) + { + // reached goal + return true; + } + } + + /// @todo: it is possible to fall into a bad place and get stuck - should move back onto the path + + } + else if ( m_goal->type == CLIMB_UP ) + { + // once jump is started, assume it is successful, since + // nav mesh may be substantially off from actual ground height at landing + const Segment *landing = NextSegment( m_goal ); + + if ( landing == NULL ) + { + // passed goal or corrupt path + return true; + } + else if ( /*!mover->IsOnGround() && */ mover->GetFeet().z > m_goal->pos.z + mover->GetStepHeight() ) + { + // we're off the ground, presumably climbing - assume we reached the goal + return true; + } + /* This breaks infected climbing up holes in the ceiling - they can get within 2D range of m_goal before finding a ledge to climb up to + else if ( mover->IsOnGround() ) + { + // proximity check + // Z delta can be anything, since we may be climbing over a tall fence, a physics prop, etc. + const float rangeTolerance = 10.0f; + if ( toGoal.AsVector2D().IsLengthLessThan( rangeTolerance ) ) + { + // reached goal + return true; + } + } + */ + } + else + { + const Segment *next = NextSegment( m_goal ); + + if ( next ) + { + // because mover may be off the path, check if it crossed the plane of the goal + // check against average of current and next forward vectors + Vector2D dividingPlane; + + if ( current->ladder ) + { + dividingPlane = m_goal->forward.AsVector2D(); + } + else + { + dividingPlane = current->forward.AsVector2D() + m_goal->forward.AsVector2D(); + } + + if ( DotProduct2D( toGoal.AsVector2D(), dividingPlane ) < 0.0001f && + abs( toGoal.z ) < body->GetStandHullHeight() ) + { + // only skip higher Z goal if next goal is directly reachable + // can't use this for positions below us because we need to be able + // to climb over random objects along our path that we can't actually + // move *through* + if ( toGoal.z < mover->GetStepHeight() && ( mover->IsPotentiallyTraversable( mover->GetFeet(), next->pos ) && !mover->HasPotentialGap( mover->GetFeet(), next->pos ) ) ) + { + // passed goal + return true; + } + } + } + + // proximity check + // Z delta can be anything, since we may be climbing over a tall fence, a physics prop, etc. + if ( toGoal.AsVector2D().IsLengthLessThan( m_goalTolerance ) ) + { + // reached goal + return true; + } + } + + return false; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Move bot along ladder. Return true if ladder motion is in progress, false if complete. + */ +bool PathFollower::LadderUpdate( INextBot *bot ) +{ + VPROF_BUDGET( "PathFollower::LadderUpdate", "NextBot" ); + + ILocomotion *mover = bot->GetLocomotionInterface(); + IBody *body = bot->GetBodyInterface(); + + if ( mover->IsUsingLadder() ) + { + // wait for locomotor to finish traversing ladder + return true; + } + + if ( m_goal->ladder == NULL ) + { + // Check if we have somehow ended up on a ladder, if so, and its a tall down-ladder we are expecting, jump the path ahead. + // This happens for players, who run off ledges and the gamemovement sticks them onto ladders. We only care about + // tall down-ladders, because up ladders work without this, and short ladders aren't dangerous to miss and drop down + // instead of climbing down. + if ( bot->GetEntity()->GetMoveType() == MOVETYPE_LADDER ) + { + // 'current' is the segment we are on/just passed over + const Segment *current = PriorSegment( m_goal ); + if ( current == NULL ) + { + return false; + } + + // Start with current, the segment we are currently traversing. Skip the distance check for that segment, because + // the pos is (hopefully) behind us. And if it's a long path segment, it's already outside the climbLookAheadRange, + // and thus it would prevent us looking at m_goal and further for imminent planned climbs. + // 'current' is the segment we are on/just passed over + const float ladderLookAheadRange = 50.0f; + for( const Segment *s = current; s; s = NextSegment( s ) ) + { + if ( s != current && ( s->pos - mover->GetFeet() ).AsVector2D().IsLengthGreaterThan( ladderLookAheadRange ) ) + { + break; + } + + // Only consider reasonably tall down ladders - if we don't grab onto a short ladder, it hopefully won't be a bad fall. + if ( s->ladder != NULL && s->how == GO_LADDER_DOWN && s->ladder->m_length > mover->GetMaxJumpHeight() ) + { + float destinationHeightDelta = s->pos.z - mover->GetFeet().z; + if ( fabs(destinationHeightDelta) < mover->GetMaxJumpHeight() ) + { + // Advance the goal, and fall through to the normal codepath. + m_goal = s; + break; + } + } + } + } + + if ( m_goal->ladder == NULL ) + { + // no ladder to use + return false; + } + } + + + // start using the ladder + const float mountRange = 25.0f; + + if ( m_goal->how == GO_LADDER_UP ) + { + // check if we're off the ladder and at the top + if ( !mover->IsUsingLadder() && mover->GetFeet().z > m_goal->ladder->m_top.z - mover->GetStepHeight() ) + { + // we're up + m_goal = NextSegment( m_goal ); + return false; + } + + // approach the ladder + Vector2D to = ( m_goal->ladder->m_bottom - mover->GetFeet() ).AsVector2D(); + + body->AimHeadTowards( m_goal->ladder->m_top - 50.0f * m_goal->ladder->GetNormal() + Vector( 0, 0, body->GetCrouchHullHeight() ), + IBody::CRITICAL, + 2.0f, + NULL, + "Mounting upward ladder" ); + + float range = to.NormalizeInPlace(); + if ( range < NextBotLadderAlignRange.GetFloat() ) + { + // getting close - line up + Vector2D ladderNormal2D = m_goal->ladder->GetNormal().AsVector2D(); + float dot = DotProduct2D( ladderNormal2D, to ); + + const float cos5 = 0.9f; + if ( dot < -cos5 ) + { + // lined up - continue approach + mover->Approach( m_goal->ladder->m_bottom ); + + if ( range < mountRange ) + { + // go up ladder + mover->ClimbLadder( m_goal->ladder, m_goal->area ); + } + } + else + { + // rotate around ladder and maintain distance from it + Vector myPerp( -to.y, to.x, 0.0f ); + Vector2D ladderPerp2D( -ladderNormal2D.y, ladderNormal2D.x ); + + Vector goal = m_goal->ladder->m_bottom; + + float alignRange = NextBotLadderAlignRange.GetFloat(); + + if ( dot < 0.0f ) + { + // we are on the correct side of the ladder + // align range should drop off as we reach alignment + alignRange = mountRange + (1.0f + dot) * (alignRange - mountRange); + } + + goal.x -= alignRange * to.x; + goal.y -= alignRange * to.y; + + if ( DotProduct2D( to, ladderPerp2D ) < 0.0f ) + { + goal += 10.0f * myPerp; + } + else + { + goal -= 10.0f * myPerp; + } + + mover->Approach( goal ); + } + } + else + { + // approach the base of the ladder - use normal path following in case there are jumps/climbs on the way to the ladder + return false; + } + } + else // go down ladder + { + // check if we fell off and are now below the ladder + if ( mover->GetFeet().z < m_goal->ladder->m_bottom.z + mover->GetStepHeight() ) + { + // we fell + m_goal = NextSegment( m_goal ); + } + else + { + // approach the ladder + Vector mountPoint = m_goal->ladder->m_top + 0.5f * body->GetHullWidth() * m_goal->ladder->GetNormal(); + Vector2D to = ( mountPoint - mover->GetFeet() ).AsVector2D(); + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + const float size = 5.0f; + NDebugOverlay::Sphere( mountPoint, size, 255, 0, 255, true, 0.1f ); + } + + body->AimHeadTowards( m_goal->ladder->m_bottom + 50.0f * m_goal->ladder->GetNormal() + Vector( 0, 0, body->GetCrouchHullHeight() ), + IBody::CRITICAL, + 1.0f, + NULL, + "Mounting downward ladder" ); + + float range = to.NormalizeInPlace(); + + // Approach the top of the ladder. If we're already on the ladder, start descending. + if ( range < mountRange || bot->GetEntity()->GetMoveType() == MOVETYPE_LADDER ) + { + // go down ladder + mover->DescendLadder( m_goal->ladder, m_goal->area ); + + // increment goal segment since locomotor will move us along the ladder + m_goal = NextSegment( m_goal ); + } + else + { + // approach the top of the ladder - use normal path following in case there are jumps/climbs on the way to the ladder + return false; + } + } + } + + return true; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Check if we have reached our current path goal and + * iterate to next goal or finish the path + */ +bool PathFollower::CheckProgress( INextBot *bot ) +{ + ILocomotion *mover = bot->GetLocomotionInterface(); + + // skip nearby goal points that are redundant to smooth path following motion + const Path::Segment *pSkipToGoal = NULL; + if ( m_minLookAheadRange > 0.0f ) + { + pSkipToGoal = m_goal; + const Vector &myFeet = mover->GetFeet(); + while( pSkipToGoal && pSkipToGoal->type == ON_GROUND && mover->IsOnGround() ) + { + if ( ( pSkipToGoal->pos - myFeet ).IsLengthLessThan( m_minLookAheadRange ) ) + { + // goal is too close - step to next segment + const Path::Segment *nextSegment = NextSegment( pSkipToGoal ); + + if ( !nextSegment || nextSegment->type != ON_GROUND ) + { + // can't skip ahead to next segment - head towards current goal + break; + } + + if ( nextSegment->pos.z > myFeet.z + mover->GetStepHeight() ) + { + // going uphill or up stairs tends to cause problems if we skip ahead, so don't + break; + } + +#ifdef DOTA_DLL + if ( DotProduct( mover->GetMotionVector(), nextSegment->forward ) <= 0.1f ) + { + // don't skip sharp turns + break; + } +#endif + + // can we reach the next path segment directly + if ( mover->IsPotentiallyTraversable( myFeet, nextSegment->pos ) && !mover->HasPotentialGap( myFeet, nextSegment->pos ) ) + { + pSkipToGoal = nextSegment; + } + else + { + // can't directly reach next segment - keep heading towards current goal + break; + } + } + else + { + // goal is farther than min lookahead + break; + } + } + + // didn't find any goal to skip to + if ( pSkipToGoal == m_goal ) + { + pSkipToGoal = NULL; + } + } + + if ( IsAtGoal( bot ) ) + { + // iterate to next segment of the path + const Path::Segment *nextSegment = pSkipToGoal ? pSkipToGoal : NextSegment( m_goal ); + + if ( nextSegment == NULL ) + { + // must be on ground to complete the path + if ( mover->IsOnGround() ) + { + // the end of the path has been reached + mover->GetBot()->OnMoveToSuccess( this ); + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "PathFollower: OnMoveToSuccess\n" ); + } + + // don't invalidate if OnMoveToSuccess just recomputed a new path + if ( GetAge() > 0.0f ) + { + Invalidate(); + } + + return false; + } + } + else + { + // keep moving + m_goal = nextSegment; + + if ( bot->IsDebugging( NEXTBOT_PATH ) && !mover->IsPotentiallyTraversable( mover->GetFeet(), nextSegment->pos ) ) + { + Warning( "PathFollower: path to my goal is blocked by something\n" ); + NDebugOverlay::Sphere( m_goal->pos, 5.f, 255, 0, 0, true, 3.f ); + } + } + } + + return true; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Move mover along path + */ +void PathFollower::Update( INextBot *bot ) +{ + VPROF_BUDGET( "PathFollower::Update", "NextBotSpiky" ); + + // track most recent path followed + bot->SetCurrentPath( this ); + + + ILocomotion *mover = bot->GetLocomotionInterface(); + + if ( !IsValid() || m_goal == NULL ) + { + return; + } + + if ( !m_waitTimer.IsElapsed() ) + { + // still waiting + //mover->ClearStuckStatus( "Waiting for blocker to move" ); + return; + } + +// m_didAvoidCheck = false; + + + if ( LadderUpdate( bot ) ) + { + // we are traversing a ladder + return; + } + + + // adjust speed based on path curvature + AdjustSpeed( bot ); + + if ( CheckProgress( bot ) == false ) + { + // goal reached + return; + } + + // use the direction towards the goal as 'forward' direction + Vector forward = m_goal->pos - mover->GetFeet(); + + if ( m_goal->type == CLIMB_UP ) + { + const Segment *next = NextSegment( m_goal ); + if ( next ) + { + // use landing of climb up as forward to help ledge detection + forward = next->pos - mover->GetFeet(); + } + } + + forward.z = 0.0f; + + float goalRange = forward.NormalizeInPlace(); + + Vector left( -forward.y, forward.x, 0.0f ); + + if ( left.IsZero() ) + { + // if left is zero, forward must also be - path follow failure + mover->GetBot()->OnMoveToFailure( this, FAIL_STUCK ); + + // don't invalidate if OnMoveToFailure just recomputed a new path + if ( GetAge() > 0.0f ) + { + Invalidate(); + } + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "PathFollower: OnMoveToFailure( FAIL_STUCK ) because forward and left are ZERO\n" ); + } + + return; + } + + // unit vectors must follow floor slope + const Vector &normal = mover->GetGroundNormal(); + + // get forward vector along floor + forward = CrossProduct( left, normal ); + + // correct the sideways vector + left = CrossProduct( normal, forward ); + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + float axisSize = 25.0f; + NDebugOverlay::Line( mover->GetFeet(), mover->GetFeet() + axisSize * forward, 255, 0, 0, true, 0.1f ); + NDebugOverlay::Line( mover->GetFeet(), mover->GetFeet() + axisSize * normal, 0, 255, 0, true, 0.1f ); + NDebugOverlay::Line( mover->GetFeet(), mover->GetFeet() + axisSize * left, 0, 0, 255, true, 0.1f ); + } + + // climb up ledges + if ( !Climbing( bot, m_goal, forward, left, goalRange ) ) + { + // a failed climb could mean an invalid path + if ( !IsValid() ) + { + return; + } + + // jump over gaps + JumpOverGaps( bot, m_goal, forward, left, goalRange ); + } + + // event callbacks from the above climbs and jumps may invalidate the path + if ( !IsValid() ) + { + return; + } + + // if our movement goal is high above us, we must have fallen + CNavArea *myArea = bot->GetEntity()->GetLastKnownArea(); + bool isOnStairs = ( myArea && myArea->HasAttributes( NAV_MESH_STAIRS ) ); + + // limit too high distance to reasonable value for bots that can climb very high + float tooHighDistance = mover->GetMaxJumpHeight(); + + if ( !m_goal->ladder && !mover->IsClimbingOrJumping() && !isOnStairs && m_goal->pos.z > mover->GetFeet().z + tooHighDistance ) + { + const float closeRange = 25.0f; // 75.0f; + Vector2D to( mover->GetFeet().x - m_goal->pos.x, mover->GetFeet().y - m_goal->pos.y ); + if ( mover->IsStuck() || to.IsLengthLessThan( closeRange ) ) + { + // the goal is too high to reach + + // check if we can reach the next segment, in case this was a "jump down" situation + const Path::Segment *next = NextSegment( m_goal ); + if ( mover->IsStuck() || !next || ( next->pos.z - mover->GetFeet().z > mover->GetMaxJumpHeight() ) || !mover->IsPotentiallyTraversable( mover->GetFeet(), next->pos ) ) + { + // the next node is too high, too - we really did fall off the path + mover->GetBot()->OnMoveToFailure( this, FAIL_FELL_OFF ); + + // don't invalidate if OnMoveToFailure just recomputed a new path + if ( GetAge() > 0.0f ) + { + Invalidate(); + } + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "PathFollower: OnMoveToFailure( FAIL_FELL_OFF )\n" ); + } + + // reset stuck status since we're (likely) repathing anyways. otherwise, we could be stuck in a loop here and not move + mover->ClearStuckStatus( "Fell off path" ); + + return; + } + } + } + + + Vector goalPos = m_goal->pos; + + // avoid small obstacles + forward = goalPos - mover->GetFeet(); + forward.z = 0.0f; + float rangeToGoal = forward.NormalizeInPlace(); + + left.x = -forward.y; + left.y = forward.x; + left.z = 0.0f; + + if ( true || m_goal != LastSegment() ) // think more about this - we often need to avoid to reach the final goal pos, too (MSB 5/15/09) + { + const float nearLedgeRange = 50.0f; + if ( rangeToGoal > nearLedgeRange || ( m_goal && m_goal->type != CLIMB_UP ) ) + { + goalPos = Avoid( bot, goalPos, forward, left ); + } + } + + // face towards movement goal + if ( mover->IsOnGround() ) + { + mover->FaceTowards( goalPos ); + } + + // move bot along path + mover->Approach( goalPos ); + + // Currently, Approach determines STAND or CROUCH. + // Override this if we're approaching a climb or a jump + if ( m_goal && ( m_goal->type == CLIMB_UP || m_goal->type == JUMP_OVER_GAP ) ) + { + bot->GetBodyInterface()->SetDesiredPosture( IBody::STAND ); + } + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + const Segment *start = GetCurrentGoal(); + if ( start ) + { + start = PriorSegment( start ); + } + + Draw( start ); + + /* + else + { + DrawInterpolated( 0.0f, GetLength() ); + } + */ + + NDebugOverlay::Cross3D( goalPos, 5.0f, 150, 150, 255, true, 0.1f ); + NDebugOverlay::Line( bot->GetEntity()->WorldSpaceCenter(), goalPos, 255, 255, 0, true, 0.1f ); + } +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * If entity is returned, it is blocking us from continuing along our path + */ +CBaseEntity *PathFollower::FindBlocker( INextBot *bot ) +{ + IIntention *think = bot->GetIntentionInterface(); + + // if we don't care about hindrances, don't do the expensive tests + if ( think->IsHindrance( bot, IS_ANY_HINDRANCE_POSSIBLE ) != ANSWER_YES ) + return NULL; + + ILocomotion *mover = bot->GetLocomotionInterface(); + IBody *body = bot->GetBodyInterface(); + + trace_t result; + NextBotTraceFilterOnlyActors filter( bot->GetEntity(), COLLISION_GROUP_NONE ); + + const float size = body->GetHullWidth()/4.0f; // keep this small to avoid lockups when groups of bots get close + Vector blockerMins( -size, -size, mover->GetStepHeight() ); + Vector blockerMaxs( size, size, body->GetCrouchHullHeight() ); + + Vector from = mover->GetFeet(); + float range = 0.0f; + + const float maxHindranceRangeAlong = 750.0f; + + // because our path goal may be far ahead of us if the way to there is unobstructed, we + // need to start looking from the point of the path we are actually standing on + MoveCursorToClosestPosition( mover->GetFeet() ); + + for( const Segment *s = GetCursorData().segmentPrior; s && range < maxHindranceRangeAlong; s = NextSegment( s ) ) + { + // trace along direction toward goal a minimum range, in case goal and hindrance are + // very close, but goal is closer + + Vector traceForward = s->pos - from; + float traceRange = traceForward.NormalizeInPlace(); + + const float minTraceRange = 2.0f * body->GetHullWidth(); + if ( traceRange < minTraceRange ) + { + traceRange = minTraceRange; + } + + mover->TraceHull( from, from + traceRange * traceForward, blockerMins, blockerMaxs, body->GetSolidMask(), &filter, &result ); + + if ( result.DidHitNonWorldEntity() ) + { + // if blocker is close, they could be behind us - check + Vector toBlocker = result.m_pEnt->GetAbsOrigin() - bot->GetLocomotionInterface()->GetFeet(); + + Vector alongPath = s->pos - from; + alongPath.z = 0.0f; + + if ( DotProduct( toBlocker, alongPath ) > 0.0f ) + { + // ask the bot if this really is a hindrance + if ( think->IsHindrance( bot, result.m_pEnt ) == ANSWER_YES ) + { + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + NDebugOverlay::Circle( bot->GetLocomotionInterface()->GetFeet(), QAngle( -90.0f, 0, 0 ), 10.0f, 255, 0, 0, 255, true, 1.0f ); + NDebugOverlay::HorzArrow( bot->GetLocomotionInterface()->GetFeet(), result.m_pEnt->GetAbsOrigin(), 1.0f, 255, 0, 0, 255, true, 1.0f ); + } + + // we are blocked + return result.m_pEnt; + } + } + } + + from = s->pos; + range += s->length; + } + + return NULL; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Do reflex avoidance movements of very nearby obstacles. + * Return adjusted goal. + */ +Vector PathFollower::Avoid( INextBot *bot, const Vector &goalPos, const Vector &forward, const Vector &left ) +{ + VPROF_BUDGET( "PathFollower::Avoid", "NextBotExpensive" ); + + if ( !NextBotAllowAvoiding.GetBool() ) + { + return goalPos; + } + + if ( !m_avoidTimer.IsElapsed() ) + { + return goalPos; + } + + // low frequency check until we actually hit something we need to avoid + const float avoidInterval = 0.5f; // 1.0f; + m_avoidTimer.Start( avoidInterval ); + + ILocomotion *mover = bot->GetLocomotionInterface(); + + if ( mover->IsClimbingOrJumping() || !mover->IsOnGround() ) + { + return goalPos; + } + + // + // Check for potential blockers along our path and wait if we're blocked + // + m_hindrance = FindBlocker( bot ); + if ( m_hindrance != NULL ) + { + // wait + m_waitTimer.Start( avoidInterval * RandomFloat( 1.0f, 2.0f ) ); + + return mover->GetFeet(); + } + + + // if we are in a "precise" area, do not use avoid volumes + CNavArea *area = bot->GetEntity()->GetLastKnownArea(); + if ( area && ( area->GetAttributes() & NAV_MESH_PRECISE ) ) + { + return goalPos; + } + + m_didAvoidCheck = true; + + // we want to avoid other players, etc + trace_t result; + NextBotTraceFilterOnlyActors filter( bot->GetEntity(), COLLISION_GROUP_NONE ); + + IBody *body = bot->GetBodyInterface(); + unsigned int mask = body->GetSolidMask(); + + const float size = body->GetHullWidth()/4.0f; + const float offset = size + 2.0f; + + float range = mover->IsRunning() ? 50.0f : 30.0f; + range *= bot->GetEntity()->GetModelScale(); + + m_hullMin = Vector( -size, -size, mover->GetStepHeight()+0.1f ); + + // only use crouch-high avoid volumes, since we'll just crouch if higher obstacles are near + m_hullMax = Vector( size, size, body->GetCrouchHullHeight() ); + + Vector nextStepHullMin( -size, -size, 2.0f * mover->GetStepHeight() + 0.1f ); + + // avoid any open doors in our way + CBasePropDoor *door = NULL; + + // check left side + m_leftFrom = mover->GetFeet() + offset * left; + m_leftTo = m_leftFrom + range * forward; + + m_isLeftClear = true; + float leftAvoid = 0.0f; + + NextBotTraversableTraceFilter traverseFilter( bot ); + mover->TraceHull( m_leftFrom, m_leftTo, m_hullMin, m_hullMax, mask, &traverseFilter, &result ); + if ( result.fraction < 1.0f || result.startsolid ) + { + // if this sensor is starting in a solid, set fraction to emulate being against a wall + if ( result.startsolid ) + { + result.fraction = 0.0f; + } + + leftAvoid = clamp( 1.0f - result.fraction, 0.0f, 1.0f ); + + m_isLeftClear = false; + + // track any doors we need to avoid + if ( result.DidHitNonWorldEntity() ) + { + door = dynamic_cast< CBasePropDoor * >( result.m_pEnt ); + } + + // check for steps +// float firstHit = result.fraction; +// mover->TraceHull( m_leftFrom, m_leftTo, nextStepHullMin, m_hullMax, mask, &filter, &result ); +// if ( result.fraction <= firstHit ) //+ mover->GetStepHeight()/2.0f ) +// { +// // it's not a step - we hit something +// m_isLeftClear = false; +// } + } + + // check right side + m_rightFrom = mover->GetFeet() - offset * left; + m_rightTo = m_rightFrom + range * forward; + + m_isRightClear = true; + float rightAvoid = 0.0f; + + mover->TraceHull( m_rightFrom, m_rightTo, m_hullMin, m_hullMax, mask, &traverseFilter, &result ); + if ( result.fraction < 1.0f || result.startsolid ) + { + // if this sensor is starting in a solid, set fraction to emulate being against a wall + if ( result.startsolid ) + { + result.fraction = 0.0f; + } + + rightAvoid = clamp( 1.0f - result.fraction, 0.0f, 1.0f ); + + m_isRightClear = false; + + // track any doors we need to avoid + if ( !door && result.DidHitNonWorldEntity() ) + { + door = dynamic_cast< CBasePropDoor * >( result.m_pEnt ); + } + + // check for steps +// float firstHit = result.fraction; +// mover->TraceHull( m_rightFrom, m_rightTo, nextStepHullMin, m_hullMax, mask, &filter, &result ); +// if ( result.fraction <= firstHit ) // + mover->GetStepHeight()/2.0f) +// { +// // it's not a step - we hit something +// m_isRightClear = false; +// } + } + + Vector adjustedGoal = goalPos; + + // avoid doors directly in our way + if ( door && !m_isLeftClear && !m_isRightClear ) + { + Vector forward, right, up; + AngleVectors( door->GetAbsAngles(), &forward, &right, &up ); + + const float doorWidth = 100.0f; + Vector doorEdge = door->GetAbsOrigin() - doorWidth * right; + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + NDebugOverlay::Axis( door->GetAbsOrigin(), door->GetAbsAngles(), 20.0f, true, 10.0f ); + NDebugOverlay::Line( door->GetAbsOrigin(), doorEdge, 255, 255, 0, true, 10.0f ); + } + + // move around door + adjustedGoal.x = doorEdge.x; + adjustedGoal.y = doorEdge.y; + + // do avoid check again next frame + m_avoidTimer.Invalidate(); + } + else if ( !m_isLeftClear || !m_isRightClear ) + { + // adjust goal to avoid small obstacle + float avoidResult = 0.0f; + if ( m_isLeftClear ) + { + avoidResult = -rightAvoid; + } + else if (m_isRightClear) + { + avoidResult = leftAvoid; + } + else + { + // both left and right are blocked, avoid nearest + const float equalTolerance = 0.01f; + if ( fabs( rightAvoid - leftAvoid ) < equalTolerance ) + { + // squarely against a wall, etc + return adjustedGoal; + } + else if ( rightAvoid > leftAvoid ) + { + avoidResult = -rightAvoid; + } + else + { + avoidResult = leftAvoid; + } + } + + // adjust goal to avoid obstacle + Vector avoidDir = 0.5f * forward - left * avoidResult; + avoidDir.NormalizeInPlace(); + + adjustedGoal = mover->GetFeet() + 100.0f * avoidDir; + + // do avoid check again next frame + m_avoidTimer.Invalidate(); + } + + return adjustedGoal; +} + + +#ifdef EXPERIMENTAL_LEDGE_FINDER +//-------------------------------------------------------------------------------------------------------------- +/** + * Given a hull that defines the area of space that may contain a climbable ledge, + * subdivide it until we find the ledge. + */ +bool PathFollower::FindClimbLedge( INextBot *bot, Vector startTracePos, Vector ledgeRegionMins, Vector ledgeRegionMaxs ) +{ + float deltaZ = ledgeRegionMaxs.z - ledgeRegionMins.z; + + if ( deltaZ <= bot->GetLocomotionInterface()->GetStepHeight() ) + { + // reached minimum subdivision limit - stop + return false; + } + + trace_t result; + NextBotTraversableTraceFilter filter( bot, ILocomotion::IMMEDIATELY ); + + mover->TraceHull( startTracePos, startTracePos, + ledgeRegionMins, ledgeRegionMaxs, + bot->GetBodyInterface()->GetSolidMask(), &filter, &result ); + + + if ( result.DidHit() ) + { + // volume is blocked - split into upper and lower volumes and try again + float midZ = ( ledgeRegionMins.z + ledgeRegionMaxs.z ) / 2.0f; + + Vector upperLedgeRegionMins( ledgeRegionMins.x, ledgeRegionMins.y, midZ ); + Vector upperLedgeRegionMaxs = ledgeRegionMaxs; + FindClimbLedge( bot, startTracePos, upperLedgeRegionMins, upperLedgeRegionMaxs ); + + + Vector lowerLedgeRegionMins = ledgeRegionMins; + Vector lowerLedgeRegionMaxs( ledgeRegionMaxs.x, ledgeRegionMaxs.y, midZ ); + FindClimbLedge( bot, startTracePos, lowerLedgeRegionMins, lowerLedgeRegionMaxs ); + } + else + { + // volume is clear, trace straight down to find ledge and keep lowest one we've found + mover->TraceHull( startTracePos, + startTracePos + Vector( 0, 0, -100.0f ), + ledgeRegionMins, ledgeRegionMaxs, + bot->GetBodyInterface()->GetSolidMask(), &filter, &result ); + } +} +#endif // _DEBUG + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Climb up ledges + */ +bool PathFollower::Climbing( INextBot *bot, const Path::Segment *goal, const Vector &forward, const Vector &right, float goalRange ) +{ + VPROF_BUDGET( "PathFollower::Climbing", "NextBot" ); + + ILocomotion *mover = bot->GetLocomotionInterface(); + IBody *body = bot->GetBodyInterface(); + CNavArea *myArea = bot->GetEntity()->GetLastKnownArea(); + + if ( !mover->IsAbleToClimb() || !NextBotAllowClimbing.GetBool() ) + { + return false; + } + + // use the 2D direction towards our goal + Vector climbDirection = forward; + climbDirection.z = 0.0f; + climbDirection.NormalizeInPlace(); + + // we can't have this as large as our hull width, or we'll find ledges ahead of us + // that we will fall from when we climb up because our hull wont actually touch at the top. + const float ledgeLookAheadRange = body->GetHullWidth() - 1; + + if ( mover->IsClimbingOrJumping() || mover->IsAscendingOrDescendingLadder() || !mover->IsOnGround() ) + { + return false; + } + + // can be in any posture when we climb + + if ( m_goal == NULL ) + { + return false; + } + + if ( TheNavMesh->IsAuthoritative() ) + { + // + // Trust what that nav mesh tells us. + // No need for expensive ledge-finding for games with simpler geometry (like TF2) + // + if ( m_goal->type == CLIMB_UP ) + { + const Segment *afterClimb = NextSegment( m_goal ); + if ( afterClimb && afterClimb->area ) + { + // find closest point on climb-destination area + Vector nearClimbGoal; + afterClimb->area->GetClosestPointOnArea( mover->GetFeet(), &nearClimbGoal ); + + climbDirection = nearClimbGoal - mover->GetFeet(); + climbDirection.z = 0.0f; + climbDirection.NormalizeInPlace(); + + if ( mover->ClimbUpToLedge( nearClimbGoal, climbDirection, NULL ) ) + return true; + } + } + + return false; + } + + + // If we're approaching a CLIMB_UP link, save off the height delta for it, and trust the nav *just* enough + // to climb up to that ledge and only that ledge. We keep as large a tolerance as possible, to trust + // the nav as little as possible. There's no valid way to have another CLIMB_UP link within crouch height, + // because we can't actually fit in between the two areas, so one climb is invalid. + float climbUpLedgeHeightDelta = -1.0f; + const float climbUpLedgeTolerance = body->GetCrouchHullHeight(); + + if ( m_goal->type == CLIMB_UP ) + { + const Segment *afterClimb = NextSegment( m_goal ); + if ( afterClimb && afterClimb->area ) + { + // find closest point on climb-destination area + Vector nearClimbGoal; + afterClimb->area->GetClosestPointOnArea( mover->GetFeet(), &nearClimbGoal ); + + climbDirection = nearClimbGoal - mover->GetFeet(); + climbUpLedgeHeightDelta = climbDirection.z; + climbDirection.z = 0.0f; + climbDirection.NormalizeInPlace(); + } + } + + // don't try to climb up stairs + if ( m_goal->area->HasAttributes( NAV_MESH_STAIRS ) || ( myArea && myArea->HasAttributes( NAV_MESH_STAIRS ) ) ) + { + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + NDebugOverlay::Cross3D( mover->GetFeet(), 5.0f, 0, 255, 255, true, 5.0f ); + DevMsg( "%3.2f: %s ON STAIRS\n", gpGlobals->curtime, bot->GetDebugIdentifier() ); + } + return false; + } + + // 'current' is the segment we are on/just passed over + const Segment *current = PriorSegment( m_goal ); + if ( current == NULL ) + { + return false; + } + + // If path segment immediately ahead of us is not obstructed, don't try to climb. + // This is required to try to avoid accidentally climbing onto valid high ledges when we really want to run UNDER them to our destination. + // We need to check "immediate" traversability to pay attention to breakable objects in our way that we should climb over. + // We also need to check traversability out to 2 * ledgeLookAheadRange in case our goal is just before a tricky ledge climb and once we pass the goal it will be too late. + // When we're in a CLIMB_UP segment, allow us to look for ledges - we know the destination ledge height, and will only grab the correct ledge. + Vector toGoal = m_goal->pos - mover->GetFeet(); + toGoal.NormalizeInPlace(); + + if ( toGoal.z < mover->GetTraversableSlopeLimit() && + !mover->IsStuck() && m_goal->type != CLIMB_UP && + mover->IsPotentiallyTraversable( mover->GetFeet(), mover->GetFeet() + 2.0f * ledgeLookAheadRange * toGoal, ILocomotion::IMMEDIATELY ) ) + { + return false; + } + + + // can't do this - we have to find the ledge to deal with breakable railings +#if 0 + // If our path requires a climb, do the climb. + // This solves some issues where there are several possible climbable ledges at a given + // location, and we need to know which ledge to climb - just use the preplanned path's choice. + const Segment *ledge = NextSegment( m_goal ); + if ( m_goal->type == CLIMB_UP && ledge ) + { + const float startClimbRange = body->GetHullWidth(); + if ( ( m_goal->pos - mover->GetFeet() ).IsLengthLessThan( startClimbRange ) ) + { + mover->ClimbUpToLedge( ledge->pos, climbDirection ); + return true; + } + } +#endif + + + + // Determine if we're approaching a planned climb. + // Start with current, the segment we are currently traversing. Skip the distance check for that segment, because + // the pos is (hopefully) behind us. And if it's a long path segment, it's already outside the climbLookAheadRange, + // and thus it would prevent us looking at m_goal and further for imminent planned climbs. + const float climbLookAheadRange = 150.0f; + bool isPlannedClimbImminent = false; + float plannedClimbZ = 0.0f; + for( const Segment *s = current; s; s = NextSegment( s ) ) + { + if ( s != current && ( s->pos - mover->GetFeet() ).AsVector2D().IsLengthGreaterThan( climbLookAheadRange ) ) + { + break; + } + + if ( s->type == CLIMB_UP ) + { + isPlannedClimbImminent = true; + + const Segment *next = NextSegment( s ); + if ( next ) + { + plannedClimbZ = next->pos.z; + } + break; + } + } + + unsigned int mask = body->GetSolidMask(); + trace_t result; + NextBotTraversableTraceFilter filter( bot, ILocomotion::IMMEDIATELY ); + + const float hullWidth = body->GetHullWidth(); + const float halfSize = hullWidth / 2.0f; + const float minHullHeight = body->GetCrouchHullHeight(); + const float minLedgeHeight = mover->GetStepHeight() + 0.1f; + + Vector skipStepHeightHullMin( -halfSize, -halfSize, minLedgeHeight ); + + // need to use minimum actual hull height here to catch porous fences and railings + Vector skipStepHeightHullMax( halfSize, halfSize, minHullHeight + 0.1f ); + + + // Find the highest height we can stand at our current location. + // Using the full width hull catches on small lips/ledges, so back up and try again. + float ceilingFraction; + + // We can't use IsPotentiallyTraversable to test for ledges, because it's smaller Hull can cause the + // next trace (trace the ceiling height forward) to start solid. + // mover->IsPotentiallyTraversable( mover->GetFeet(), mover->GetFeet() + Vector( 0, 0, mover->GetMaxJumpHeight() ), ILocomotion::IMMEDIATELY, &ceilingFraction ); + + // Instead of IsPotentiallyTraversable, we back up the same distance and use a second upward trace + // to see if that one finds a higher ceiling. If so, we use that ceiling height, and use the + // backed-up feet position for the ledge finding traces. + Vector feet( mover->GetFeet() ); + Vector ceiling( feet + Vector( 0, 0, mover->GetMaxJumpHeight() ) ); + mover->TraceHull( feet, ceiling, + skipStepHeightHullMin, skipStepHeightHullMax, mask, &filter, &result ); + ceilingFraction = result.fraction; + bool isBackupTraceUsed = false; + if ( ceilingFraction < 1.0f || result.startsolid ) + { + trace_t backupTrace; + const float backupDistance = hullWidth * 0.25f; // The IsPotentiallyTraversable check this replaces uses a 1/4 hull width trace + Vector backupFeet( feet - climbDirection * backupDistance ); + Vector backupCeiling( backupFeet + Vector( 0, 0, mover->GetMaxJumpHeight() ) ); + mover->TraceHull( backupFeet, backupCeiling, + skipStepHeightHullMin, skipStepHeightHullMax, mask, &filter, &backupTrace ); + if ( !backupTrace.startsolid && backupTrace.fraction > ceilingFraction ) + { + bot->DebugConColorMsg( NEXTBOT_PATH, Color( 255, 255, 255, 255 ), "%s backing up when looking for max ledge height\n", bot->GetDebugIdentifier() ); + result = backupTrace; + ceilingFraction = result.fraction; + feet = backupFeet; + ceiling = backupCeiling; + isBackupTraceUsed = true; + } + } + + float maxLedgeHeight = ceilingFraction * mover->GetMaxJumpHeight(); + + if ( maxLedgeHeight <= mover->GetStepHeight() ) + { + return false; // early out when we can't even climb StepHeight. + } + + // + // Check for ledge climbs over things in our way. + // Even if we have a CLIMB_UP link in our path, we still need + // to find the actual ledge by tracing the local geometry. + // + Vector climbHullMax( halfSize, halfSize, maxLedgeHeight ); + + Vector ledgePos = feet; // to be computed below + + mover->TraceHull( feet, + feet + climbDirection * ledgeLookAheadRange, + skipStepHeightHullMin, climbHullMax, mask, &filter, &result ); + + if ( bot->IsDebugging( NEXTBOT_PATH ) && NextBotDebugClimbing.GetBool() ) + { + // show ledge-finding hull as we move + NDebugOverlay::SweptBox( feet, + feet + climbDirection * ledgeLookAheadRange, + skipStepHeightHullMin, climbHullMax, vec3_angle, + 255, 100, 0, 255, 0.1f ); + } + + bool wasPotentialLedgeFound = result.DidHit() && !result.startsolid; + // To test climbing up past small lips on walls, we can force the bot to run past the overhang and use the backup trace: + // wasPotentialLedgeFound = wasPotentialLedgeFound && (result.fraction == 0 || isBackupTraceUsed); + if ( wasPotentialLedgeFound ) + { + VPROF_BUDGET( "PathFollower::Climbing( Search for ledge to climb )", "NextBot" ); + + if ( bot->IsDebugging( NEXTBOT_PATH ) && NextBotDebugClimbing.GetBool() ) + { + // show ledge-finding hull that found a ledge candidate + NDebugOverlay::SweptBox( feet, + feet + climbDirection * ledgeLookAheadRange, + skipStepHeightHullMin, climbHullMax, vec3_angle, + 255, 100, 0, 100, 999.9f ); + + // show primary climb direction + NDebugOverlay::HorzArrow( feet, feet + 50.0f * climbDirection, 2.0f, 0, 255, 0, 255, true, 9999.9f ); + } + + // what are we climbing over? + CBaseEntity *obstacle = result.m_pEnt; + + if ( !result.DidHitNonWorldEntity() || bot->IsAbleToClimbOnto( obstacle ) ) + { + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "%3.2f: %s at potential ledge climb\n", gpGlobals->curtime, bot->GetDebugIdentifier() ); + } + + // the low hull sweep hit an obstacle - note how 'far in' this is + float ledgeFrontWallDepth = ledgeLookAheadRange * result.fraction; + + float minLedgeDepth = body->GetHullWidth()/2.0f; // 5.0f; + if ( m_goal->type == CLIMB_UP ) + { + // Climbing up to a narrow nav area indicates a narrow ledge. We need to reduce our minLedgeDepth + // here or our path will say we should climb but we'll forever fail to find a wide enough ledge. + const Segment *afterClimb = NextSegment( m_goal ); + if ( afterClimb && afterClimb->area ) + { + Vector depthVector = climbDirection * minLedgeDepth; + depthVector.z = 0; + if ( fabs( depthVector.x ) > afterClimb->area->GetSizeX() ) + { + depthVector.x = (depthVector.x > 0) ? afterClimb->area->GetSizeX() : -afterClimb->area->GetSizeX(); + } + if ( fabs( depthVector.y ) > afterClimb->area->GetSizeY() ) + { + depthVector.y = (depthVector.y > 0) ? afterClimb->area->GetSizeY() : -afterClimb->area->GetSizeY(); + } + + float areaDepth = depthVector.NormalizeInPlace(); + minLedgeDepth = MIN( minLedgeDepth, areaDepth ); + } + } + + // + // Find the ledge. Start at the lowest jump we can make + // and step up until we find the actual ledge. + // + // The scan is limited to maxLedgeHeight in case our max + // jump/climb height is so tall the highest horizontal hull + // trace could be on the other side of the ceiling above us + // + + float ledgeHeight = minLedgeHeight; + const float ledgeHeightIncrement = 0.5f * mover->GetStepHeight(); + + bool foundWall = false; + bool foundLedge = false; + + // once we have found the ledge's front wall, we must look at least minLedgeDepth farther in to verify it is a ledge + // NOTE: This *must* be ledgeLookAheadRange since ledges are compared against the initial trace which was ledgeLookAheadRange deep + float ledgeTopLookAheadRange = ledgeLookAheadRange; + + // TODO: we also must look far enough ahead in case the ledge we actually find is "deeper" than the initial wall at the base + + Vector climbHullMin( -halfSize, -halfSize, 0.0f ); + Vector climbHullMax( halfSize, halfSize, minHullHeight ); + + Vector wallPos; + float wallDepth = 0.0f; + + bool isLastIteration = false; + while( true ) + { + // trace forward to find the wall in front of us, or the empty space of the ledge above us + mover->TraceHull( feet + Vector( 0, 0, ledgeHeight ), + feet + Vector( 0, 0, ledgeHeight ) + climbDirection * ledgeTopLookAheadRange, + climbHullMin, climbHullMax, mask, &filter, &result ); + + float traceDepth = ledgeTopLookAheadRange * result.fraction; + + if ( !result.startsolid ) + { + // if trace reached minLedgeDepth farther, this is a potential ledge + if ( foundWall ) + { + // TODO: test that potential ledge is flat enough to stand on + if ( ( traceDepth - ledgeFrontWallDepth ) > minLedgeDepth ) + { + bool isUsable = true; + + // initialize ledgePos from result of last trace + ledgePos = result.endpos; + + // Find the actual ground level on the potential ledge + // Only trace back down to the previous ledge height trace. + // The ledge can be no lower, or we would've found it in the last iteration. + mover->TraceHull( ledgePos, + ledgePos + Vector( 0, 0, -ledgeHeightIncrement ), + climbHullMin, climbHullMax, mask, &filter, &result ); + + ledgePos = result.endpos; + + // if the whole trace is in solid, we're out of luck, but + // if the trace just started solid, 'ledgePos' should still be valid + // since the trace left the solid and then hit. + // if the trace hit nothing, the potential ledge is actually deeper in + const float MinGroundNormal = 0.7f; // players can't stand on ground steeper than 0.7 + if ( result.allsolid || !result.DidHit() || result.plane.normal.z < MinGroundNormal ) + { + // not a usable ledge, try again + isUsable = false; + } + else + { + if ( climbUpLedgeHeightDelta > 0.0f ) + { + // if we're climbing to a specific ledge via a CLIMB_UP link, only climb to that ledge. + // Do this only for the world (which includes static props) so we can still opportunistically + // climb up onto breakable railings and physics props. + if ( result.DidHitWorld() ) + { + float potentialLedgeHeight = result.endpos.z - feet.z; + if ( fabs(potentialLedgeHeight - climbUpLedgeHeightDelta) > climbUpLedgeTolerance ) + { + isUsable = false; + } + } + } + } + + if ( isUsable ) + { + // back up until we no longer are hitting the ledge to determine the + // exact ledge edge position + Vector validLedgePos = ledgePos; // save off a valid ledge pos + const float edgeTolerance = 4.0f; + const float maxBackUp = hullWidth; + float backUpSoFar = edgeTolerance; + Vector testPos = ledgePos; + + while( backUpSoFar < maxBackUp ) + { + testPos -= edgeTolerance * climbDirection; + backUpSoFar += edgeTolerance; + + mover->TraceHull( testPos, + testPos + Vector( 0, 0, -ledgeHeightIncrement ), + climbHullMin, climbHullMax, mask, &filter, &result ); + + + if ( bot->IsDebugging( NEXTBOT_PATH ) && NextBotDebugClimbing.GetBool() ) + { + // show edge-finder hulls + NDebugOverlay::SweptBox( testPos, + testPos + Vector( 0, 0, -mover->GetStepHeight() ), + climbHullMin, climbHullMax, vec3_angle, 255, 0, 0, 255, 999.9f ); + } + + if ( result.DidHit() && result.plane.normal.z >= MinGroundNormal ) + { + // we hit, this is closer to the actual ledge edge + ledgePos = result.endpos; + } + else + { + // nothing but air or a steep slope below us, we have found the edge + break; + } + } + + // we want ledgePos to be right on the edge itself, so move + // it ahead by half of the hull width + ledgePos += climbDirection * halfSize; + + // Make sure this doesn't embed us in the far wall if the ledge is narrow, since we would + // have backed up less than halfSize. + Vector climbHullMinStep( climbHullMin ); // skip StepHeight for sloped ledges + mover->TraceHull( validLedgePos, + ledgePos, + climbHullMinStep, climbHullMax, mask, &filter, &result ); + + ledgePos = result.endpos; + + // Now since ledgePos + StepHeight is valid, trace down to find ground on sloped ledges. + mover->TraceHull( ledgePos + Vector( 0, 0, StepHeight ), + ledgePos, + climbHullMin, climbHullMax, mask, &filter, &result ); + if ( !result.startsolid ) + { + ledgePos = result.endpos; + } + } + + +/*** NOTE: While this saves us from climbing into a window below the window we want to get in, + *** it also causes us to climb in midair high over crates sitting against walls we need to climb over. + if ( isUsable && m_goal->type == CLIMB_UP ) + { + // we can only accept ledges at least as high as our current CLIMB_UP destination + // NOTE: Can't use plannedClimbZ here, since that could be 2 or 3 short climbs ahead + const Segment *ledge = NextSegment( m_goal ); + + if ( !ledge || ledgeHeight < ledge->pos.z - feet.z - mover->GetStepHeight() ) + { + // this ledge is below the CLIMB_UP destination - can't use it + isUsable = false; + } + } +*/ + + + if ( isUsable ) + { + // found a usable ledge here + foundLedge = true; + break; + } + } + } + else if ( result.DidHit() ) + { + // this iteration hit the wall under the ledge, + // meaning the next iteration that reaches far enough will be our ledge + + // Since we know that our desired route is likely blocked (via the + // IsTraversable check above) - any ledge we hit we must climb. + + // found a valid ledge wall + foundWall = true; + wallDepth = traceDepth; + + // make sure the subsequent traces are at least minLedgeDepth deeper than + // the wall we just found, or all ledge checks will fail + float minTraceDepth = traceDepth + minLedgeDepth + 0.1f; + + if ( ledgeTopLookAheadRange < minTraceDepth ) + { + ledgeTopLookAheadRange = minTraceDepth; + } + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "%3.2f: Climbing - found wall.\n", gpGlobals->curtime ); + if ( NextBotDebugClimbing.GetBool() ) + { + NDebugOverlay::HorzArrow( result.endpos, result.endpos + 20.0f * result.plane.normal, 5.0f, 255, 100, 0, 255, true, 9999.9f ); + } + wallPos = result.endpos; + } + } + else if ( ledgeHeight > body->GetCrouchHullHeight() && !isPlannedClimbImminent ) + { + // we haven't hit anything yet, and we're already above our heads - no obstacle + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "%3.2f: Climbing - skipping overhead climb we can walk/crawl under.\n", gpGlobals->curtime ); + } + break; + } + } + + ledgeHeight += ledgeHeightIncrement; + + if ( ledgeHeight >= maxLedgeHeight ) + { + if ( isLastIteration ) + { + // tested at max height + break; + } + + // check one more time at max jump height + isLastIteration = true; + ledgeHeight = maxLedgeHeight; + } + } + + if ( foundLedge ) + { + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "%3.2f: STARTING LEDGE CLIMB UP\n", gpGlobals->curtime ); + + if ( NextBotDebugClimbing.GetBool() ) + { + NDebugOverlay::Cross3D( ledgePos, 10.0f, 0, 255, 0, true, 9999.9f ); + + // display approximation of idealized ledge that has been found + Vector side( -climbDirection.y, climbDirection.x, 0.0f ); + + // this is an approximation, since AABB can hit at any angle + Vector base = feet + halfSize * climbDirection; + + Vector wallBottomLeft = base + halfSize * side; + Vector wallBottomRight = base - halfSize * side; + Vector wallTopLeft = wallBottomLeft + Vector( 0, 0, ledgeHeight ); + Vector wallTopRight = wallBottomRight + Vector( 0, 0, ledgeHeight ); + + NDebugOverlay::Triangle( wallBottomRight, wallBottomLeft, wallTopLeft, 255, 100, 0, 100, true, 9999.9f ); + NDebugOverlay::Triangle( wallBottomRight, wallTopLeft, wallTopRight, 255, 100, 0, 100, true, 9999.9f ); + + Vector ledgeLeft = ledgePos + halfSize * side; + Vector ledgeRight = ledgePos - halfSize * side; + + NDebugOverlay::Triangle( wallTopRight, wallTopLeft, ledgeLeft, 0, 100, 255, 100, true, 9999.9f ); + NDebugOverlay::Triangle( wallTopRight, ledgeLeft, ledgeRight, 0, 100, 255, 100, true, 9999.9f ); + } + } + + if ( !mover->ClimbUpToLedge( ledgePos, climbDirection, obstacle ) ) + { + // climb failed - build a new path in case we're now stuck + //Invalidate(); + return false; + } + + return true; + } + else if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + DevMsg( "%3.2f: CANT FIND LEDGE TO CLIMB\n", gpGlobals->curtime ); + } + } + } + + return false; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Jump over gaps + */ +bool PathFollower::JumpOverGaps( INextBot *bot, const Path::Segment *goal, const Vector &forward, const Vector &right, float goalRange ) +{ + VPROF_BUDGET( "PathFollower::JumpOverGaps", "NextBot" ); + + ILocomotion *mover = bot->GetLocomotionInterface(); + IBody *body = bot->GetBodyInterface(); + + if ( !mover->IsAbleToJumpAcrossGaps() || !NextBotAllowGapJumping.GetBool() ) + { + return false; + } + + if ( mover->IsClimbingOrJumping() || mover->IsAscendingOrDescendingLadder() || !mover->IsOnGround() ) + { + return false; + } + + if ( !body->IsActualPosture( IBody::STAND ) ) + { + // can't jump if we're not standing + return false; + } + + if ( m_goal == NULL ) + { + return false; + } + + trace_t result; + NextBotTraversableTraceFilter filter( bot, ILocomotion::IMMEDIATELY ); + + const float hullWidth = ( body ) ? body->GetHullWidth() : 1.0f; + + // 'current' is the segment we are on/just passed over + const Segment *current = PriorSegment( m_goal ); + if ( current == NULL ) + { + return false; + } + + const float minGapJumpRange = 2.0f * hullWidth; + + const Segment *gap = NULL; + + if ( current->type == JUMP_OVER_GAP ) + { + gap = current; + } + else + { + float searchRange = goalRange; + for( const Segment *s = m_goal; s; s = NextSegment( s ) ) + { + if ( searchRange > minGapJumpRange ) + { + break; + } + + if ( s->type == JUMP_OVER_GAP ) + { + gap = s; + break; + } + + searchRange += s->length; + } + } + + if ( gap ) + { + VPROF_BUDGET( "PathFollower::GapJumping", "NextBot" ); + + float halfWidth = hullWidth/2.0f; + + if ( mover->IsGap( mover->GetFeet() + halfWidth * gap->forward, gap->forward ) ) + { + // there is a gap to jump over + const Segment *landing = NextSegment( gap ); + if ( landing ) + { + mover->JumpAcrossGap( landing->pos, landing->forward ); + + // if we're jumping over this gap, make sure our goal is the landing so we aim for it + m_goal = landing; + + if ( bot->IsDebugging( NEXTBOT_PATH ) ) + { + NDebugOverlay::Cross3D( m_goal->pos, 5.0f, 0, 255, 255, true, 5.0f ); + DevMsg( "%3.2f: GAP JUMP\n", gpGlobals->curtime ); + } + return true; + } + } + } + + return false; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Draw the path for debugging + */ +void PathFollower::Draw( const Path::Segment *start ) const +{ + if ( m_goal == NULL ) + return; + + // show avoid volumes + if ( m_didAvoidCheck ) + { + QAngle angles( 0, 0, 0 ); + + if (m_isLeftClear) + NDebugOverlay::SweptBox( m_leftFrom, m_leftTo, m_hullMin, m_hullMax, angles, 0, 255, 0, 255, 0.1f ); + else + NDebugOverlay::SweptBox( m_leftFrom, m_leftTo, m_hullMin, m_hullMax, angles, 255, 0, 0, 255, 0.1f ); + + if (m_isRightClear) + NDebugOverlay::SweptBox( m_rightFrom, m_rightTo, m_hullMin, m_hullMax, angles, 0, 255, 0, 255, 0.1f ); + else + NDebugOverlay::SweptBox( m_rightFrom, m_rightTo, m_hullMin, m_hullMax, angles, 255, 0, 0, 255, 0.1f ); + + const_cast< PathFollower * >( this )->m_didAvoidCheck = false; + } + + // highlight current goal segment + if ( m_goal ) + { + const float size = 5.0f; + NDebugOverlay::Sphere( m_goal->pos, size, 255, 255, 0, true, 0.1f ); + + switch( m_goal->how ) + { + case GO_NORTH: + case GO_SOUTH: + NDebugOverlay::Line( m_goal->m_portalCenter - Vector( m_goal->m_portalHalfWidth, 0, 0 ), m_goal->m_portalCenter + Vector( m_goal->m_portalHalfWidth, 0, 0 ), 255, 0, 255, true, 0.1f ); + break; + + default: + NDebugOverlay::Line( m_goal->m_portalCenter - Vector( 0, m_goal->m_portalHalfWidth, 0 ), m_goal->m_portalCenter + Vector( 0, m_goal->m_portalHalfWidth, 0 ), 255, 0, 255, true, 0.1f ); + break; + } + + // 'current' is the segment we are on/just passed over + const Segment *current = PriorSegment( m_goal ); + if ( current ) + { + NDebugOverlay::Line( current->pos, m_goal->pos, 255, 255, 0, true, 0.1f ); + } + } + + // extend + Path::Draw(); +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return true if there is a the given discontinuity ahead in the path within the given range (-1 = entire remaining path) + */ +bool PathFollower::IsDiscontinuityAhead( INextBot *bot, Path::SegmentType type, float range ) const +{ + if ( m_goal ) + { + const Path::Segment *current = PriorSegment( m_goal ); + if ( current && current->type == type ) + { + // we're on the discontinuity now + return true; + } + + float rangeSoFar = ( m_goal->pos - bot->GetLocomotionInterface()->GetFeet() ).Length(); + + for( const Segment *s = m_goal; s; s = NextSegment( s ) ) + { + if ( rangeSoFar >= range ) + { + break; + } + + if ( s->type == type ) + { + return true; + } + + rangeSoFar += s->length; + } + } + + return false; +} + + diff --git a/game/server/NextBot/Path/NextBotPathFollow.h b/game/server/NextBot/Path/NextBotPathFollow.h new file mode 100644 index 0000000..edb27b5 --- /dev/null +++ b/game/server/NextBot/Path/NextBotPathFollow.h @@ -0,0 +1,106 @@ +// NextBotPathFollow.h +// Path following +// Author: Michael Booth, April 2005 +//========= Copyright Valve Corporation, All rights reserved. ============// + +#ifndef _NEXT_BOT_PATH_FOLLOWER_ +#define _NEXT_BOT_PATH_FOLLOWER_ + +#include "nav_mesh.h" +#include "nav_pathfind.h" +#include "NextBotPath.h" + +class INextBot; +class ILocomotion; + + +//-------------------------------------------------------------------------------------------------------- +/** + * A PathFollower extends a Path to include mechanisms to move along (follow) it + */ +class PathFollower : public Path +{ +public: + PathFollower( void ); + virtual ~PathFollower(); + + virtual void Invalidate( void ); // (EXTEND) cause the path to become invalid + virtual void Draw( const Path::Segment *start = NULL ) const; // (EXTEND) draw the path for debugging + virtual void OnPathChanged( INextBot *bot, Path::ResultType result ); // invoked when the path is (re)computed (path is valid at the time of this call) + + virtual void Update( INextBot *bot ); // move bot along path + + virtual const Path::Segment *GetCurrentGoal( void ) const; // return current goal along the path we are trying to reach + + virtual void SetMinLookAheadDistance( float value ); // minimum range movement goal must be along path + + virtual CBaseEntity *GetHindrance( void ) const; // returns entity that is hindering our progress along the path + + virtual bool IsDiscontinuityAhead( INextBot *bot, Path::SegmentType type, float range = -1.0f ) const; // return true if there is a the given discontinuity ahead in the path within the given range (-1 = entire remaining path) + + void SetGoalTolerance( float range ); // set tolerance within at which we're considered to be at our goal + +private: + const Path::Segment *m_goal; // our current goal along the path + float m_minLookAheadRange; + + bool CheckProgress( INextBot *bot ); + bool IsAtGoal( INextBot *bot ) const; // return true if reached current path goal + + //bool IsOnStairs( INextBot *bot ) const; // return true if bot is standing on a stairway + bool m_isOnStairs; + + CountdownTimer m_avoidTimer; // do avoid check more often if we recently avoided + + CountdownTimer m_waitTimer; // for waiting for a blocker to move off our path + CHandle< CBaseEntity > m_hindrance; + + // debug display data for avoid volumes + bool m_didAvoidCheck; + Vector m_leftFrom; + Vector m_leftTo; + bool m_isLeftClear; + Vector m_rightFrom; + Vector m_rightTo; + bool m_isRightClear; + Vector m_hullMin, m_hullMax; + + void AdjustSpeed( INextBot *bot ); // adjust speed based on path curvature + + Vector Avoid( INextBot *bot, const Vector &goalPos, const Vector &forward, const Vector &left ); // avoidance movements for very nearby obstacles. returns modified goal position + bool Climbing( INextBot *bot, const Path::Segment *goal, const Vector &forward, const Vector &left, float goalRange ); // climb up ledges + bool JumpOverGaps( INextBot *bot, const Path::Segment *goal, const Vector &forward, const Vector &left, float goalRange ); // jump over gaps + + bool LadderUpdate( INextBot *bot ); // move bot along ladder + CBaseEntity *FindBlocker( INextBot *bot ); // if entity is returned, it is blocking us from continuing along our path + + float m_goalTolerance; +}; + + +inline void PathFollower::SetGoalTolerance( float range ) +{ + m_goalTolerance = range; +} + + +inline const Path::Segment *PathFollower::GetCurrentGoal( void ) const +{ + return m_goal; +} + + +inline void PathFollower::SetMinLookAheadDistance( float value ) +{ + m_minLookAheadRange = value; +} + +inline CBaseEntity *PathFollower::GetHindrance( void ) const +{ + return m_hindrance; +} + + +#endif // _NEXT_BOT_PATH_FOLLOWER_ + + diff --git a/game/server/NextBot/Path/NextBotRetreatPath.h b/game/server/NextBot/Path/NextBotRetreatPath.h new file mode 100644 index 0000000..8d57cb4 --- /dev/null +++ b/game/server/NextBot/Path/NextBotRetreatPath.h @@ -0,0 +1,573 @@ +// NextBotRetreatPath.h +// Maintain and follow a path that leads safely away from the given Actor +// Author: Michael Booth, February 2007 +//========= Copyright Valve Corporation, All rights reserved. ============// + +#ifndef _NEXT_BOT_RETREAT_PATH_ +#define _NEXT_BOT_RETREAT_PATH_ + +#include "nav.h" +#include "NextBotInterface.h" +#include "NextBotLocomotionInterface.h" +#include "NextBotRetreatPath.h" +#include "NextBotUtil.h" +#include "NextBotPathFollow.h" +#include "tier0/vprof.h" + + +//---------------------------------------------------------------------------------------------- +/** + * A RetreatPath extends a PathFollower to periodically recompute a path + * away from a threat, and to move along the path away from that threat. + */ +class RetreatPath : public PathFollower +{ +public: + RetreatPath( void ); + virtual ~RetreatPath() { } + + void Update( INextBot *bot, CBaseEntity *threat ); // update path away from threat and move bot along path + + virtual float GetMaxPathLength( void ) const; // return maximum path length + + virtual void Invalidate( void ); // (EXTEND) cause the path to become invalid + +private: + void RefreshPath( INextBot *bot, CBaseEntity *threat ); + + CountdownTimer m_throttleTimer; // require a minimum time between re-paths + EHANDLE m_pathThreat; // the threat of our existing path + Vector m_pathThreatPos; // where the threat was when the path was built +}; + +inline RetreatPath::RetreatPath( void ) +{ + m_throttleTimer.Invalidate(); + m_pathThreat = NULL; +} + +inline float RetreatPath::GetMaxPathLength( void ) const +{ + return 1000.0f; +} + +inline void RetreatPath::Invalidate( void ) +{ + // path is gone, repath at earliest opportunity + m_throttleTimer.Invalidate(); + m_pathThreat = NULL; + + // extend + PathFollower::Invalidate(); +} + + + +//---------------------------------------------------------------------------------------------- +/** + * Maintain a path to our chase threat and move along that path + */ +inline void RetreatPath::Update( INextBot *bot, CBaseEntity *threat ) +{ + VPROF_BUDGET( "RetreatPath::Update", "NextBot" ); + + if ( threat == NULL ) + { + return; + } + + // if our path threat changed, repath immediately + if ( threat != m_pathThreat ) + { + if ( bot->IsDebugging( INextBot::PATH ) ) + { + DevMsg( "%3.2f: bot(#%d) Chase path threat changed (from %X to %X).\n", gpGlobals->curtime, bot->GetEntity()->entindex(), m_pathThreat.Get(), threat ); + } + + Invalidate(); + } + + // maintain the path away from the threat + RefreshPath( bot, threat ); + + // move along the path towards the threat + PathFollower::Update( bot ); +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Build a path away from retreatFromArea up to retreatRange in length. + */ +class RetreatPathBuilder +{ +public: + RetreatPathBuilder( INextBot *me, CBaseEntity *threat, float retreatRange = 500.0f ) + { + m_me = me; + m_mover = me->GetLocomotionInterface(); + + m_threat = threat; + m_retreatRange = retreatRange; + } + + CNavArea *ComputePath( void ) + { + VPROF_BUDGET( "NavAreaBuildRetreatPath", "NextBot" ); + + if ( m_mover == NULL ) + return NULL; + + CNavArea *startArea = m_me->GetEntity()->GetLastKnownArea(); + + if ( startArea == NULL ) + return NULL; + + CNavArea *retreatFromArea = TheNavMesh->GetNearestNavArea( m_threat->GetAbsOrigin() ); + if ( retreatFromArea == NULL ) + return NULL; + + startArea->SetParent( NULL ); + + // start search + CNavArea::ClearSearchLists(); + + float initCost = Cost( startArea, NULL, NULL ); + if ( initCost < 0.0f ) + return NULL; + + int teamID = m_me->GetEntity()->GetTeamNumber(); + + startArea->SetTotalCost( initCost ); + + startArea->AddToOpenList(); + + // keep track of the area farthest away from the threat + CNavArea *farthestArea = NULL; + float farthestRange = 0.0f; + + // + // Dijkstra's algorithm (since we don't know our goal). + // Build a path as far away from the retreat area as possible. + // Minimize total path length and danger. + // Maximize distance to threat of end of path. + // + while( !CNavArea::IsOpenListEmpty() ) + { + // get next area to check + CNavArea *area = CNavArea::PopOpenList(); + + area->AddToClosedList(); + + // don't consider blocked areas + if ( area->IsBlocked( teamID ) ) + continue; + + // build adjacent area array + CollectAdjacentAreas( area ); + + // search adjacent areas + for( int i=0; i<m_adjAreaIndex; ++i ) + { + CNavArea *newArea = m_adjAreaVector[ i ].area; + + // only visit each area once + if ( newArea->IsClosed() ) + continue; + + // don't consider blocked areas + if ( newArea->IsBlocked( teamID ) ) + continue; + + // don't use this area if it is out of range + if ( ( newArea->GetCenter() - m_me->GetEntity()->GetAbsOrigin() ).IsLengthGreaterThan( m_retreatRange ) ) + continue; + + // determine cost of traversing this area + float newCost = Cost( newArea, area, m_adjAreaVector[ i ].ladder ); + + // don't use adjacent area if cost functor says it is a dead-end + if ( newCost < 0.0f ) + continue; + + if ( newArea->IsOpen() && newArea->GetTotalCost() <= newCost ) + { + // we have already visited this area, and it has a better path + continue; + } + else + { + // whether this area has been visited or not, we now have a better path + newArea->SetParent( area, m_adjAreaVector[ i ].how ); + newArea->SetTotalCost( newCost ); + + // use 'cost so far' to hold cumulative cost + newArea->SetCostSoFar( newCost ); + + // tricky bit here - relying on OpenList being sorted by cost + if ( newArea->IsOpen() ) + { + // area already on open list, update the list order to keep costs sorted + newArea->UpdateOnOpenList(); + } + else + { + newArea->AddToOpenList(); + } + + // keep track of area farthest from threat + float threatRange = ( newArea->GetCenter() - m_threat->GetAbsOrigin() ).Length(); + if ( threatRange > farthestRange ) + { + farthestArea = newArea; + farthestRange = threatRange; + } + } + } + } + + return farthestArea; + } + + + /** + * Build a vector of adjacent areas reachable from the given area + */ + void CollectAdjacentAreas( CNavArea *area ) + { + m_adjAreaIndex = 0; + + const NavConnectVector &adjNorth = *area->GetAdjacentAreas( NORTH ); + FOR_EACH_VEC( adjNorth, it ) + { + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + m_adjAreaVector[ m_adjAreaIndex ].area = adjNorth[ it ].area; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_NORTH; + m_adjAreaVector[ m_adjAreaIndex ].ladder = NULL; + ++m_adjAreaIndex; + } + + const NavConnectVector &adjSouth = *area->GetAdjacentAreas( SOUTH ); + FOR_EACH_VEC( adjSouth, it ) + { + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + m_adjAreaVector[ m_adjAreaIndex ].area = adjSouth[ it ].area; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_SOUTH; + m_adjAreaVector[ m_adjAreaIndex ].ladder = NULL; + ++m_adjAreaIndex; + } + + const NavConnectVector &adjWest = *area->GetAdjacentAreas( WEST ); + FOR_EACH_VEC( adjWest, it ) + { + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + m_adjAreaVector[ m_adjAreaIndex ].area = adjWest[ it ].area; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_WEST; + m_adjAreaVector[ m_adjAreaIndex ].ladder = NULL; + ++m_adjAreaIndex; + } + + const NavConnectVector &adjEast = *area->GetAdjacentAreas( EAST ); + FOR_EACH_VEC( adjEast, it ) + { + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + m_adjAreaVector[ m_adjAreaIndex ].area = adjEast[ it ].area; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_EAST; + m_adjAreaVector[ m_adjAreaIndex ].ladder = NULL; + ++m_adjAreaIndex; + } + + const NavLadderConnectVector &adjUpLadder = *area->GetLadders( CNavLadder::LADDER_UP ); + FOR_EACH_VEC( adjUpLadder, it ) + { + CNavLadder *ladder = adjUpLadder[ it ].ladder; + + if ( ladder->m_topForwardArea && m_adjAreaIndex < MAX_ADJ_AREAS ) + { + m_adjAreaVector[ m_adjAreaIndex ].area = ladder->m_topForwardArea; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_LADDER_UP; + m_adjAreaVector[ m_adjAreaIndex ].ladder = ladder; + ++m_adjAreaIndex; + } + + if ( ladder->m_topLeftArea && m_adjAreaIndex < MAX_ADJ_AREAS ) + { + m_adjAreaVector[ m_adjAreaIndex ].area = ladder->m_topLeftArea; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_LADDER_UP; + m_adjAreaVector[ m_adjAreaIndex ].ladder = ladder; + ++m_adjAreaIndex; + } + + if ( ladder->m_topRightArea && m_adjAreaIndex < MAX_ADJ_AREAS ) + { + m_adjAreaVector[ m_adjAreaIndex ].area = ladder->m_topRightArea; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_LADDER_UP; + m_adjAreaVector[ m_adjAreaIndex ].ladder = ladder; + ++m_adjAreaIndex; + } + } + + const NavLadderConnectVector &adjDownLadder = *area->GetLadders( CNavLadder::LADDER_DOWN ); + FOR_EACH_VEC( adjDownLadder, it ) + { + CNavLadder *ladder = adjDownLadder[ it ].ladder; + + if ( m_adjAreaIndex >= MAX_ADJ_AREAS ) + break; + + if ( ladder->m_bottomArea ) + { + m_adjAreaVector[ m_adjAreaIndex ].area = ladder->m_bottomArea; + m_adjAreaVector[ m_adjAreaIndex ].how = GO_LADDER_DOWN; + m_adjAreaVector[ m_adjAreaIndex ].ladder = ladder; + ++m_adjAreaIndex; + } + } + } + + /** + * Cost minimizes path length traveled thus far and "danger" (proximity to threat(s)) + */ + float Cost( CNavArea *area, CNavArea *fromArea, const CNavLadder *ladder ) + { + // check if we can use this area + if ( !m_mover->IsAreaTraversable( area ) ) + { + return -1.0f; + } + + int teamID = m_me->GetEntity()->GetTeamNumber(); + if ( area->IsBlocked( teamID ) ) + { + return -1.0f; + } + + const float debugDeltaT = 3.0f; + + float cost; + + const float maxThreatRange = 500.0f; + const float dangerDensity = 1000.0f; + + if ( fromArea == NULL ) + { + cost = 0.0f; + + if ( area->Contains( m_threat->GetAbsOrigin() ) ) + { + // maximum danger - threat is in the area with us + cost += 10.0f * dangerDensity; + + if ( m_me->IsDebugging( INextBot::PATH ) ) + { + area->DrawFilled( 255, 0, 0, 128 ); + } + } + else + { + // danger proportional to range to us + float rangeToThreat = ( m_threat->GetAbsOrigin() - m_me->GetEntity()->GetAbsOrigin() ).Length(); + + if ( rangeToThreat < maxThreatRange ) + { + cost += dangerDensity * ( 1.0f - ( rangeToThreat / maxThreatRange ) ); + + if ( m_me->IsDebugging( INextBot::PATH ) ) + { + NDebugOverlay::Line( m_me->GetEntity()->GetAbsOrigin(), m_threat->GetAbsOrigin(), 255, 0, 0, true, debugDeltaT ); + } + } + } + } + else + { + // compute distance traveled along path so far + float dist; + + if ( ladder ) + { + const float ladderCostFactor = 100.0f; + dist = ladderCostFactor * ladder->m_length; + } + else + { + Vector to = area->GetCenter() - fromArea->GetCenter(); + + dist = to.Length(); + + // check for vertical discontinuities + Vector closeFrom, closeTo; + area->GetClosestPointOnArea( fromArea->GetCenter(), &closeTo ); + fromArea->GetClosestPointOnArea( area->GetCenter(), &closeFrom ); + + float deltaZ = closeTo.z - closeFrom.z; + + if ( deltaZ > m_mover->GetMaxJumpHeight() ) + { + // too high to jump + return -1.0f; + } + else if ( -deltaZ > m_mover->GetDeathDropHeight() ) + { + // too far down to drop + return -1.0f; + } + + // prefer to maintain our level + const float climbCost = 10.0f; + dist += climbCost * fabs( deltaZ ); + } + + cost = dist + fromArea->GetTotalCost(); + + + // Add in danger cost due to threat + // Assume straight line between areas and find closest point + // to the threat along that line segment. The distance between + // the threat and closest point on the line is the danger cost. + + // path danger is CUMULATIVE + float dangerCost = fromArea->GetCostSoFar(); + + Vector close; + float t; + CalcClosestPointOnLineSegment( m_threat->GetAbsOrigin(), area->GetCenter(), fromArea->GetCenter(), close, &t ); + if ( t < 0.0f ) + { + close = area->GetCenter(); + } + else if ( t > 1.0f ) + { + close = fromArea->GetCenter(); + } + + float rangeToThreat = ( m_threat->GetAbsOrigin() - close ).Length(); + + if ( rangeToThreat < maxThreatRange ) + { + float dangerFactor = 1.0f - ( rangeToThreat / maxThreatRange ); + dangerCost = dangerDensity * dangerFactor; + + if ( m_me->IsDebugging( INextBot::PATH ) ) + { + NDebugOverlay::HorzArrow( fromArea->GetCenter(), area->GetCenter(), 5, 255 * dangerFactor, 0, 0, 255, true, debugDeltaT ); + + Vector to = close - m_threat->GetAbsOrigin(); + to.NormalizeInPlace(); + + NDebugOverlay::Line( close, close - 50.0f * to, 255, 0, 0, true, debugDeltaT ); + } + } + + cost += dangerCost; + } + + return cost; + } + +private: + INextBot *m_me; + ILocomotion *m_mover; + + CBaseEntity *m_threat; + float m_retreatRange; + + enum { MAX_ADJ_AREAS = 64 }; + + struct AdjInfo + { + CNavArea *area; + CNavLadder *ladder; + NavTraverseType how; + }; + + AdjInfo m_adjAreaVector[ MAX_ADJ_AREAS ]; + int m_adjAreaIndex; + +}; + + +//---------------------------------------------------------------------------------------------- +/** + * Periodically rebuild the path away from our threat + */ +inline void RetreatPath::RefreshPath( INextBot *bot, CBaseEntity *threat ) +{ + VPROF_BUDGET( "RetreatPath::RefreshPath", "NextBot" ); + + if ( threat == NULL ) + { + if ( bot->IsDebugging( INextBot::PATH ) ) + { + DevMsg( "%3.2f: bot(#%d) CasePath::RefreshPath failed. No threat.\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); + } + return; + } + + // don't change our path if we're on a ladder + ILocomotion *mover = bot->GetLocomotionInterface(); + if ( IsValid() && mover && mover->IsUsingLadder() ) + { + if ( bot->IsDebugging( INextBot::PATH ) ) + { + DevMsg( "%3.2f: bot(#%d) RetreatPath::RefreshPath failed. Bot is on a ladder.\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); + } + return; + } + + // the closer we get, the more accurate our path needs to be + Vector to = threat->GetAbsOrigin() - bot->GetPosition(); + + const float minTolerance = 0.0f; + const float toleranceRate = 0.33f; + + float tolerance = minTolerance + toleranceRate * to.Length(); + + if ( !IsValid() || ( threat->GetAbsOrigin() - m_pathThreatPos ).IsLengthGreaterThan( tolerance ) ) + { + if ( !m_throttleTimer.IsElapsed() ) + { + // require a minimum time between repaths, as long as we have a path to follow + if ( bot->IsDebugging( INextBot::PATH ) ) + { + DevMsg( "%3.2f: bot(#%d) RetreatPath::RefreshPath failed. Rate throttled.\n", gpGlobals->curtime, bot->GetEntity()->entindex() ); + } + return; + } + + // remember our path threat + m_pathThreat = threat; + m_pathThreatPos = threat->GetAbsOrigin(); + + RetreatPathBuilder retreat( bot, threat, GetMaxPathLength() ); + + CNavArea *goalArea = retreat.ComputePath(); + + if ( goalArea ) + { + AssemblePrecomputedPath( bot, goalArea->GetCenter(), goalArea ); + } + else + { + // all adjacent areas are too far away - just move directly away from threat + Vector to = threat->GetAbsOrigin() - bot->GetPosition(); + + BuildTrivialPath( bot, bot->GetPosition() - to ); + } + + const float minRepathInterval = 0.5f; + m_throttleTimer.Start( minRepathInterval ); + } +} + + + +#endif // _NEXT_BOT_RETREAT_PATH_ |