diff options
Diffstat (limited to 'game/server/NextBot/Path/NextBotPathFollow.cpp')
| -rw-r--r-- | game/server/NextBot/Path/NextBotPathFollow.cpp | 1923 |
1 files changed, 1923 insertions, 0 deletions
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; +} + + |