diff options
Diffstat (limited to 'game/shared/cstrike/bot/nav_path.cpp')
| -rw-r--r-- | game/shared/cstrike/bot/nav_path.cpp | 1208 |
1 files changed, 1208 insertions, 0 deletions
diff --git a/game/shared/cstrike/bot/nav_path.cpp b/game/shared/cstrike/bot/nav_path.cpp new file mode 100644 index 0000000..92285bc --- /dev/null +++ b/game/shared/cstrike/bot/nav_path.cpp @@ -0,0 +1,1208 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +// nav_path.cpp +// Encapsulation of a path through space +// Author: Michael S. Booth ([email protected]), November 2003 + +#include "cbase.h" +#include "cs_gamerules.h" +#include "cs_player.h" + +#include "nav_mesh.h" +#include "nav_path.h" +#include "bot_util.h" +#include "improv_locomotor.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +#ifdef _WIN32 +#pragma warning (disable:4701) // disable warning that variable *may* not be initialized +#endif + + +#define DrawLine( from, to, duration, red, green, blue ) NDebugOverlay::Line( from, to, red, green, blue, true, 0.1f ) + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Determine actual path positions + */ +bool CNavPath::ComputePathPositions( void ) +{ + if (m_segmentCount == 0) + return false; + + // 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; + + for( int i=1; i<m_segmentCount; ++i ) + { + const PathSegment *from = &m_path[ i-1 ]; + PathSegment *to = &m_path[ i ]; + + if (to->how <= GO_WEST) // walk along the floor to the next area + { + to->ladder = NULL; + + // compute next point, keeping path as straight as possible + from->area->ComputeClosestPointInPortal( to->area, (NavDirType)to->how, from->pos, &to->pos ); + + // move goal position into the goal area a bit + const float stepInDist = 5.0f; // how far to "step into" an area - must be less than min area size + AddDirectionVector( &to->pos, (NavDirType)to->how, stepInDist ); + + // 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 + if (to->area->IsConnected( from->area, NUM_DIRECTIONS ) == false) + { + // this is a "jump down" link + + // 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 pushDist = 25.0f; + to->pos.x += pushDist * dir.x; + to->pos.y += pushDist * dir.y; + + // 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 = to->pos.x + pushDist * dir.x; + m_path[i].pos.y = to->pos.y + pushDist * dir.y; + + // put this one at the bottom of the fall + m_path[i].pos.z = to->area->GetZ( m_path[i].pos ); + } + } + } + else if (to->how == GO_LADDER_UP) // to get to next area, must go up a ladder + { + // find our ladder + const NavLadderConnectList *list = from->area->GetLadderList( CSNavLadder::LADDER_UP ); + int it; + for( it = list->Head(); it != list->InvalidIndex(); it = list->Next(it)) + { + CSNavLadder *ladder = (*list)[ 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; + break; + } + } + + if (it == list->InvalidIndex()) + { + //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 NavLadderConnectList *list = from->area->GetLadderList( CSNavLadder::LADDER_DOWN ); + int it; + for( it = list->Head(); it != list->InvalidIndex(); it = list->Next(it)) + { + CSNavLadder *ladder = (*list)[ 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; + break; + } + } + + if (it == list->InvalidIndex()) + { + //PrintIfWatched( "ERROR: Can't find ladder in path\n" ); + return false; + } + } + } + + return true; +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return true if position is at the end of the path + */ +bool CNavPath::IsAtEnd( const Vector &pos ) const +{ + if (!IsValid()) + return false; + + const float epsilon = 20.0f; + return (pos - GetEndpoint()).IsLengthLessThan( epsilon ); +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return length of path from start to finish + */ +float CNavPath::GetLength( void ) const +{ + float length = 0.0f; + for( int i=1; i<GetSegmentCount(); ++i ) + { + length += (m_path[i].pos - m_path[i-1].pos).Length(); + } + + return length; +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return point a given distance along the path - if distance is out of path bounds, point is clamped to start/end + * @todo Be careful of returning "positions" along one-way drops, ladders, etc. + */ +bool CNavPath::GetPointAlongPath( float distAlong, Vector *pointOnPath ) const +{ + if (!IsValid() || pointOnPath == NULL) + return false; + + if (distAlong <= 0.0f) + { + *pointOnPath = m_path[0].pos; + return true; + } + + float lengthSoFar = 0.0f; + float segmentLength; + Vector dir; + for( int i=1; i<GetSegmentCount(); ++i ) + { + dir = m_path[i].pos - m_path[i-1].pos; + segmentLength = dir.Length(); + + if (segmentLength + lengthSoFar >= distAlong) + { + // desired point is on this segment of the path + float delta = distAlong - lengthSoFar; + float t = delta / segmentLength; + + *pointOnPath = m_path[i].pos + t * dir; + + return true; + } + + lengthSoFar += segmentLength; + } + + *pointOnPath = m_path[ GetSegmentCount()-1 ].pos; + return true; +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return the node index closest to the given distance along the path without going over - returns (-1) if error + */ +int CNavPath::GetSegmentIndexAlongPath( float distAlong ) const +{ + if (!IsValid()) + return -1; + + if (distAlong <= 0.0f) + { + return 0; + } + + float lengthSoFar = 0.0f; + Vector dir; + for( int i=1; i<GetSegmentCount(); ++i ) + { + lengthSoFar += (m_path[i].pos - m_path[i-1].pos).Length(); + + if (lengthSoFar > distAlong) + { + return i-1; + } + } + + return GetSegmentCount()-1; +} + + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Compute closest point on path to given point + * NOTE: This does not do line-of-sight tests, so closest point may be thru the floor, etc + */ +bool CNavPath::FindClosestPointOnPath( const Vector *worldPos, int startIndex, int endIndex, Vector *close ) const +{ + if (!IsValid() || close == NULL) + return false; + + Vector along, toWorldPos; + Vector pos; + const Vector *from, *to; + float length; + float closeLength; + float closeDistSq = 9999999999.9; + float distSq; + + for( int i=startIndex; i<=endIndex; ++i ) + { + from = &m_path[i-1].pos; + to = &m_path[i].pos; + + // compute ray along this path segment + along = *to - *from; + + // make it a unit vector along the path + length = along.NormalizeInPlace(); + + // compute vector from start of segment to our point + toWorldPos = *worldPos - *from; + + // find distance of closest point on ray + closeLength = DotProduct( toWorldPos, along ); + + // constrain point to be on path segment + if (closeLength <= 0.0f) + pos = *from; + else if (closeLength >= length) + pos = *to; + else + pos = *from + closeLength * along; + + distSq = (pos - *worldPos).LengthSqr(); + + // keep the closest point so far + if (distSq < closeDistSq) + { + closeDistSq = distSq; + *close = pos; + } + } + + return true; +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Build trivial path when start and goal are in the same nav area + */ +bool CNavPath::BuildTrivialPath( const Vector &start, const Vector &goal ) +{ + m_segmentCount = 0; + + 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[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; + + return true; +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Draw the path for debugging. + */ +void CNavPath::Draw( const Vector &color ) +{ + if (!IsValid()) + return; + + for( int i=1; i<m_segmentCount; ++i ) + { + DrawLine( m_path[i-1].pos + Vector( 0, 0, HalfHumanHeight ), + m_path[i].pos + Vector( 0, 0, HalfHumanHeight ), 2, 255 * color.x, 255 * color.y, 255 * color.z ); + } +} + + + +//-------------------------------------------------------------------------------------------------------------- +/** + * 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 CNavPath::FindNextOccludedNode( int anchor ) +{ + for( int i=anchor+1; i<m_segmentCount; ++i ) + { + // don't remove ladder nodes + if (m_path[i].ladder) + return i; + + if (!IsWalkableTraceLineClear( m_path[ anchor ].pos, m_path[ i ].pos )) + { + // cant see this node from anchor node + return i; + } + + Vector anchorPlusHalf = m_path[ anchor ].pos + Vector( 0, 0, HalfHumanHeight ); + Vector iPlusHalf = m_path[ i ].pos +Vector( 0, 0, HalfHumanHeight ); + if (!IsWalkableTraceLineClear( anchorPlusHalf, iPlusHalf) ) + { + // cant see this node from anchor node + return i; + } + + Vector anchorPlusFull = m_path[ anchor ].pos + Vector( 0, 0, HumanHeight ); + Vector iPlusFull = m_path[ i ].pos + Vector( 0, 0, HumanHeight ); + if (!IsWalkableTraceLineClear( anchorPlusFull, iPlusFull )) + { + // cant see this node from anchor node + return i; + } + } + + return m_segmentCount; +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Smooth out path, removing redundant nodes + */ +void CNavPath::Optimize( void ) +{ +// DONT USE THIS: Optimizing the path results in cutting thru obstacles +return; + + if (m_segmentCount < 3) + return; + + int anchor = 0; + + while( anchor < m_segmentCount ) + { + int occluded = FindNextOccludedNode( 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; + } +} + + +//-------------------------------------------------------------------------------------------------------------- +//-------------------------------------------------------------------------------------------------------------- + +/** + * Constructor + */ +CNavPathFollower::CNavPathFollower( void ) +{ + m_improv = NULL; + m_path = NULL; + + m_segmentIndex = 0; + m_isLadderStarted = false; + + m_isDebug = false; +} + +void CNavPathFollower::Reset( void ) +{ + m_segmentIndex = 1; + m_isLadderStarted = false; + + m_stuckMonitor.Reset(); +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Move improv along path + */ +void CNavPathFollower::Update( float deltaT, bool avoidObstacles ) +{ + if (m_path == NULL || m_path->IsValid() == false) + return; + + const CNavPath::PathSegment *node = (*m_path)[ m_segmentIndex ]; + + if (node == NULL) + { + m_improv->OnMoveToFailure( m_path->GetEndpoint(), CImprovLocomotor::FAIL_INVALID_PATH ); + m_path->Invalidate(); + return; + } + + // handle ladders + /* + if (node->ladder) + { + const Vector *approachPos = NULL; + const Vector *departPos = NULL; + + if (m_segmentIndex) + approachPos = &(*m_path)[ m_segmentIndex-1 ]->pos; + + if (m_segmentIndex < m_path->GetSegmentCount()-1) + departPos = &(*m_path)[ m_segmentIndex+1 ]->pos; + + if (!m_isLadderStarted) + { + // set up ladder movement + m_improv->StartLadder( node->ladder, node->how, approachPos, departPos ); + m_isLadderStarted = true; + } + + // move improv along ladder + if (m_improv->TraverseLadder( node->ladder, node->how, approachPos, departPos, deltaT )) + { + // completed ladder + ++m_segmentIndex; + } + return; + } + */ + + // reset ladder init flag + m_isLadderStarted = false; + + // + // Check if we reached the end of the path + // + const float closeRange = 20.0f; + if ((m_improv->GetFeet() - node->pos).IsLengthLessThan( closeRange )) + { + ++m_segmentIndex; + + if (m_segmentIndex >= m_path->GetSegmentCount()) + { + m_improv->OnMoveToSuccess( m_path->GetEndpoint() ); + m_path->Invalidate(); + return; + } + } + + + m_goal = node->pos; + + const float aheadRange = 300.0f; + m_segmentIndex = FindPathPoint( aheadRange, &m_goal, &m_behindIndex ); + if (m_segmentIndex >= m_path->GetSegmentCount()) + m_segmentIndex = m_path->GetSegmentCount()-1; + + + bool isApproachingJumpArea = false; + + // + // Crouching + // + if (!m_improv->IsUsingLadder()) + { + // because hostage crouching is not really supported by the engine, + // if we are standing in a crouch area, we must crouch to avoid collisions + if (m_improv->GetLastKnownArea() && + m_improv->GetLastKnownArea()->GetAttributes() & NAV_MESH_CROUCH && + !(m_improv->GetLastKnownArea()->GetAttributes() & NAV_MESH_JUMP)) + { + m_improv->Crouch(); + } + + // if we are approaching a crouch area, crouch + // if there are no crouch areas coming up, stand + const float crouchRange = 50.0f; + bool didCrouch = false; + for( int i=m_segmentIndex; i<m_path->GetSegmentCount(); ++i ) + { + const CNavArea *to = (*m_path)[i]->area; + + // if there is a jump area on the way to the crouch area, don't crouch as it messes up the jump + if (to->GetAttributes() & NAV_MESH_JUMP) + { + isApproachingJumpArea = true; + break; + } + + Vector close; + to->GetClosestPointOnArea( m_improv->GetCentroid(), &close ); + + if ((close - m_improv->GetFeet()).AsVector2D().IsLengthGreaterThan( crouchRange )) + break; + + if (to->GetAttributes() & NAV_MESH_CROUCH) + { + m_improv->Crouch(); + didCrouch = true; + break; + } + + } + + if (!didCrouch && !m_improv->IsJumping()) + { + // no crouch areas coming up + m_improv->StandUp(); + } + + } // end crouching logic + + + if (m_isDebug) + { + m_path->Draw(); + UTIL_DrawBeamPoints( m_improv->GetCentroid(), m_goal + Vector( 0, 0, StepHeight ), 1, 255, 0, 255 ); + UTIL_DrawBeamPoints( m_goal + Vector( 0, 0, StepHeight ), m_improv->GetCentroid(), 1, 255, 0, 255 ); + } + + // check if improv becomes stuck + m_stuckMonitor.Update( m_improv ); + + + // if improv has been stuck for too long, give up + const float giveUpTime = 2.0f; + if (m_stuckMonitor.GetDuration() > giveUpTime) + { + m_improv->OnMoveToFailure( m_path->GetEndpoint(), CImprovLocomotor::FAIL_STUCK ); + m_path->Invalidate(); + return; + } + + + // if our goal is high above us, we must have fallen + if (m_goal.z - m_improv->GetFeet().z > JumpCrouchHeight) + { + const float closeRange = 75.0f; + Vector2D to( m_improv->GetFeet().x - m_goal.x, m_improv->GetFeet().y - m_goal.y ); + if (to.IsLengthLessThan( closeRange )) + { + // we can't reach the goal position + // check if we can reach the next node, in case this was a "jump down" situation + const CNavPath::PathSegment *nextNode = (*m_path)[ m_behindIndex+1 ]; + if (m_behindIndex >=0 && nextNode) + { + if (nextNode->pos.z - m_improv->GetFeet().z > JumpCrouchHeight) + { + // the next node is too high, too - we really did fall of the path + m_improv->OnMoveToFailure( m_path->GetEndpoint(), CImprovLocomotor::FAIL_FELL_OFF ); + m_path->Invalidate(); + return; + } + } + else + { + // fell trying to get to the last node in the path + m_improv->OnMoveToFailure( m_path->GetEndpoint(), CImprovLocomotor::FAIL_FELL_OFF ); + m_path->Invalidate(); + return; + } + } + } + + + // avoid small obstacles + if (avoidObstacles && !isApproachingJumpArea && !m_improv->IsJumping() && m_segmentIndex < m_path->GetSegmentCount()-1) + { + FeelerReflexAdjustment( &m_goal ); + + // currently, this is only used for hostages, and their collision physics stinks + // do more feeler checks to avoid short obstacles + /* + const float inc = 0.25f; + for( float t = 0.5f; t < 1.0f; t += inc ) + { + FeelerReflexAdjustment( &m_goal, t * StepHeight ); + } + */ + + } + + // move improv along path + m_improv->TrackPath( m_goal, deltaT ); +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Return the closest point to our current position on our current path + * If "local" is true, only check the portion of the path surrounding m_pathIndex. + */ +int CNavPathFollower::FindOurPositionOnPath( Vector *close, bool local ) const +{ + if (!m_path->IsValid()) + return -1; + + Vector along, toFeet; + Vector feet = m_improv->GetFeet(); + Vector eyes = m_improv->GetEyes(); + Vector pos; + const Vector *from, *to; + float length; + float closeLength; + float closeDistSq = 9999999999.9; + int closeIndex = -1; + float distSq; + + int start, end; + + if (local) + { + start = m_segmentIndex - 3; + if (start < 1) + start = 1; + + end = m_segmentIndex + 3; + if (end > m_path->GetSegmentCount()) + end = m_path->GetSegmentCount(); + } + else + { + start = 1; + end = m_path->GetSegmentCount(); + } + + for( int i=start; i<end; ++i ) + { + from = &(*m_path)[i-1]->pos; + to = &(*m_path)[i]->pos; + + // compute ray along this path segment + along = *to - *from; + + // make it a unit vector along the path + length = along.NormalizeInPlace(); + + // compute vector from start of segment to our point + toFeet = feet - *from; + + // find distance of closest point on ray + closeLength = DotProduct( toFeet, along ); + + // constrain point to be on path segment + if (closeLength <= 0.0f) + pos = *from; + else if (closeLength >= length) + pos = *to; + else + pos = *from + closeLength * along; + + distSq = (pos - feet).LengthSqr(); + + // keep the closest point so far + if (distSq < closeDistSq) + { + // don't use points we cant see + Vector probe = pos + Vector( 0, 0, HalfHumanHeight ); + if (!IsWalkableTraceLineClear( eyes, probe, WALK_THRU_DOORS | WALK_THRU_BREAKABLES )) + continue; + + // don't use points we cant reach + //if (!IsStraightLinePathWalkable( &pos )) + // continue; + + closeDistSq = distSq; + if (close) + *close = pos; + closeIndex = i-1; + } + } + + return closeIndex; +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Compute a point a fixed distance ahead along our path. + * Returns path index just after point. + */ +int CNavPathFollower::FindPathPoint( float aheadRange, Vector *point, int *prevIndex ) +{ + // find path index just past aheadRange + int afterIndex; + + // finds the closest point on local area of path, and returns the path index just prior to it + Vector close; + int startIndex = FindOurPositionOnPath( &close, true ); + + if (prevIndex) + *prevIndex = startIndex; + + if (startIndex <= 0) + { + // went off the end of the path + // or next point in path is unwalkable (ie: jump-down) + // keep same point + return m_segmentIndex; + } + + // if we are crouching, just follow the path exactly + if (m_improv->IsCrouching()) + { + // we want to move to the immediately next point along the path from where we are now + int index = startIndex+1; + if (index >= m_path->GetSegmentCount()) + index = m_path->GetSegmentCount()-1; + + *point = (*m_path)[ index ]->pos; + + // if we are very close to the next point in the path, skip ahead to the next one to avoid wiggling + // we must do a 2D check here, in case the goal point is floating in space due to jump down, etc + const float closeEpsilon = 20.0f; // 10 + while ((*point - close).AsVector2D().IsLengthLessThan( closeEpsilon )) + { + ++index; + + if (index >= m_path->GetSegmentCount()) + { + index = m_path->GetSegmentCount()-1; + break; + } + + *point = (*m_path)[ index ]->pos; + } + + return index; + } + + // make sure we use a node a minimum distance ahead of us, to avoid wiggling + while (startIndex < m_path->GetSegmentCount()-1) + { + Vector pos = (*m_path)[ startIndex+1 ]->pos; + + // we must do a 2D check here, in case the goal point is floating in space due to jump down, etc + const float closeEpsilon = 20.0f; + if ((pos - close).AsVector2D().IsLengthLessThan( closeEpsilon )) + { + ++startIndex; + } + else + { + break; + } + } + + // if we hit a ladder or jump area, must stop (dont use ladder behind us) + if (startIndex > m_segmentIndex && startIndex < m_path->GetSegmentCount() && + ((*m_path)[ startIndex ]->ladder || (*m_path)[ startIndex ]->area->GetAttributes() & NAV_MESH_JUMP)) + { + *point = (*m_path)[ startIndex ]->pos; + return startIndex; + } + + // we need the point just *ahead* of us + ++startIndex; + if (startIndex >= m_path->GetSegmentCount()) + startIndex = m_path->GetSegmentCount()-1; + + // if we hit a ladder or jump area, must stop + if (startIndex < m_path->GetSegmentCount() && + ((*m_path)[ startIndex ]->ladder || (*m_path)[ startIndex ]->area->GetAttributes() & NAV_MESH_JUMP)) + { + *point = (*m_path)[ startIndex ]->pos; + return startIndex; + } + + // note direction of path segment we are standing on + Vector initDir = (*m_path)[ startIndex ]->pos - (*m_path)[ startIndex-1 ]->pos; + initDir.NormalizeInPlace(); + + Vector feet = m_improv->GetFeet(); + Vector eyes = m_improv->GetEyes(); + float rangeSoFar = 0; + + // this flag is true if our ahead point is visible + bool visible = true; + + Vector prevDir = initDir; + + // step along the path until we pass aheadRange + bool isCorner = false; + int i; + for( i=startIndex; i<m_path->GetSegmentCount(); ++i ) + { + Vector pos = (*m_path)[i]->pos; + Vector to = pos - (*m_path)[i-1]->pos; + Vector dir = to; + dir.NormalizeInPlace(); + + // don't allow path to double-back from our starting direction (going upstairs, down curved passages, etc) + if (DotProduct( dir, initDir ) < 0.0f) // -0.25f + { + --i; + break; + } + + // if the path turns a corner, we want to move towards the corner, not into the wall/stairs/etc + if (DotProduct( dir, prevDir ) < 0.5f) + { + isCorner = true; + --i; + break; + } + prevDir = dir; + + // don't use points we cant see + Vector probe = pos + Vector( 0, 0, HalfHumanHeight ); + if (!IsWalkableTraceLineClear( eyes, probe, WALK_THRU_BREAKABLES )) + { + // presumably, the previous point is visible, so we will interpolate + visible = false; + break; + } + + // if we encounter a ladder or jump area, we must stop + if (i < m_path->GetSegmentCount() && + ((*m_path)[ i ]->ladder || (*m_path)[ i ]->area->GetAttributes() & NAV_MESH_JUMP)) + break; + + // Check straight-line path from our current position to this position + // Test for un-jumpable height change, or unrecoverable fall + //if (!IsStraightLinePathWalkable( &pos )) + //{ + // --i; + // break; + //} + + Vector along = (i == startIndex) ? (pos - feet) : (pos - (*m_path)[i-1]->pos); + rangeSoFar += along.Length2D(); + + // stop if we have gone farther than aheadRange + if (rangeSoFar >= aheadRange) + break; + } + + if (i < startIndex) + afterIndex = startIndex; + else if (i < m_path->GetSegmentCount()) + afterIndex = i; + else + afterIndex = m_path->GetSegmentCount()-1; + + + // compute point on the path at aheadRange + if (afterIndex == 0) + { + *point = (*m_path)[0]->pos; + } + else + { + // interpolate point along path segment + const Vector *afterPoint = &(*m_path)[ afterIndex ]->pos; + const Vector *beforePoint = &(*m_path)[ afterIndex-1 ]->pos; + + Vector to = *afterPoint - *beforePoint; + float length = to.Length2D(); + + float t = 1.0f - ((rangeSoFar - aheadRange) / length); + + if (t < 0.0f) + t = 0.0f; + else if (t > 1.0f) + t = 1.0f; + + *point = *beforePoint + t * to; + + // if afterPoint wasn't visible, slide point backwards towards beforePoint until it is + if (!visible) + { + const float sightStepSize = 25.0f; + float dt = sightStepSize / length; + + Vector probe = *point + Vector( 0, 0, HalfHumanHeight ); + while( t > 0.0f && !IsWalkableTraceLineClear( eyes, probe, WALK_THRU_BREAKABLES ) ) + { + t -= dt; + *point = *beforePoint + t * to; + } + + if (t <= 0.0f) + *point = *beforePoint; + } + } + + // if position found is too close to us, or behind us, force it farther down the path so we don't stop and wiggle + if (!isCorner) + { + const float epsilon = 50.0f; + Vector2D toPoint; + Vector2D centroid( m_improv->GetCentroid().x, m_improv->GetCentroid().y ); + + toPoint.x = point->x - centroid.x; + toPoint.y = point->y - centroid.y; + + if (DotProduct2D( toPoint, initDir.AsVector2D() ) < 0.0f || toPoint.IsLengthLessThan( epsilon )) + { + int i; + for( i=startIndex; i<m_path->GetSegmentCount(); ++i ) + { + toPoint.x = (*m_path)[i]->pos.x - centroid.x; + toPoint.y = (*m_path)[i]->pos.y - centroid.y; + if ((*m_path)[i]->ladder || (*m_path)[i]->area->GetAttributes() & NAV_MESH_JUMP || toPoint.IsLengthGreaterThan( epsilon )) + { + *point = (*m_path)[i]->pos; + startIndex = i; + break; + } + } + + if (i == m_path->GetSegmentCount()) + { + *point = m_path->GetEndpoint(); + startIndex = m_path->GetSegmentCount()-1; + } + } + } + + // m_pathIndex should always be the next point on the path, even if we're not moving directly towards it + if (startIndex < m_path->GetSegmentCount()) + return startIndex; + + return m_path->GetSegmentCount()-1; +} + + +//-------------------------------------------------------------------------------------------------------------- +/** + * Do reflex avoidance movements if our "feelers" are touched + * @todo Parameterize feeler spacing + */ +void CNavPathFollower::FeelerReflexAdjustment( Vector *goalPosition, float height ) +{ + // if we are in a "precise" area, do not do feeler adjustments + if (m_improv->GetLastKnownArea() && m_improv->GetLastKnownArea()->GetAttributes() & NAV_MESH_PRECISE) + return; + + // use the direction towards the goal + Vector dir = *goalPosition - m_improv->GetFeet(); + dir.z = 0.0f; + dir.NormalizeInPlace(); + + Vector lat( -dir.y, dir.x, 0.0f ); + + const float feelerOffset = (m_improv->IsCrouching()) ? 15.0f : 20.0f; // 15, 20 + const float feelerLengthRun = 50.0f; // 100 - too long for tight hallways (cs_747) + const float feelerLengthWalk = 30.0f; + + const float feelerHeight = (height > 0.0f) ? height : StepHeight + 0.1f; // if obstacle is lower than StepHeight, we'll walk right over it + + float feelerLength = (m_improv->IsRunning()) ? feelerLengthRun : feelerLengthWalk; + + feelerLength = (m_improv->IsCrouching()) ? 20.0f : feelerLength; + + // + // Feelers must follow floor slope + // + float ground; + Vector normal; + if (m_improv->GetSimpleGroundHeightWithFloor( m_improv->GetEyes(), &ground, &normal ) == false) + return; + + // get forward vector along floor + dir = CrossProduct( lat, normal ); + + // correct the sideways vector + lat = CrossProduct( dir, normal ); + + + Vector feet = m_improv->GetFeet(); + feet.z += feelerHeight; + + Vector from = feet + feelerOffset * lat; + Vector to = from + feelerLength * dir; + + bool leftClear = IsWalkableTraceLineClear( from, to, WALK_THRU_DOORS | WALK_THRU_BREAKABLES ); + + // draw debug beams + if (m_isDebug) + { + if (leftClear) + UTIL_DrawBeamPoints( from, to, 1, 0, 255, 0 ); + else + UTIL_DrawBeamPoints( from, to, 1, 255, 0, 0 ); + } + + from = feet - feelerOffset * lat; + to = from + feelerLength * dir; + + bool rightClear = IsWalkableTraceLineClear( from, to, WALK_THRU_DOORS | WALK_THRU_BREAKABLES ); + + // draw debug beams + if (m_isDebug) + { + if (rightClear) + UTIL_DrawBeamPoints( from, to, 1, 0, 255, 0 ); + else + UTIL_DrawBeamPoints( from, to, 1, 255, 0, 0 ); + } + + + + const float avoidRange = (m_improv->IsCrouching()) ? 150.0f : 300.0f; + + if (!rightClear) + { + if (leftClear) + { + // right hit, left clear - veer left + *goalPosition = *goalPosition + avoidRange * lat; + //*goalPosition = m_improv->GetFeet() + avoidRange * lat; + + //m_improv->StrafeLeft(); + } + } + else if (!leftClear) + { + // right clear, left hit - veer right + *goalPosition = *goalPosition - avoidRange * lat; + //*goalPosition = m_improv->GetFeet() - avoidRange * lat; + + //m_improv->StrafeRight(); + } + +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Reset the stuck-checker. + */ +CStuckMonitor::CStuckMonitor( void ) +{ + m_isStuck = false; + m_avgVelIndex = 0; + m_avgVelCount = 0; +} + +/** + * Reset the stuck-checker. + */ +void CStuckMonitor::Reset( void ) +{ + m_isStuck = false; + m_avgVelIndex = 0; + m_avgVelCount = 0; +} + +//-------------------------------------------------------------------------------------------------------------- +/** + * Test if the improv has become stuck + */ +void CStuckMonitor::Update( CImprovLocomotor *improv ) +{ + if (m_isStuck) + { + // improv is stuck - see if it has moved far enough to be considered unstuck + const float unstuckRange = 75.0f; + if ((improv->GetCentroid() - m_stuckSpot).IsLengthGreaterThan( unstuckRange )) + { + // no longer stuck + Reset(); + //PrintIfWatched( "UN-STUCK\n" ); + } + } + else + { + // check if improv has become stuck + + // compute average velocity over a short period (for stuck check) + Vector vel = improv->GetCentroid() - m_lastCentroid; + + // if we are jumping, ignore Z + //if (improv->IsJumping()) + // vel.z = 0.0f; + + // ignore Z unless we are on a ladder (which is only Z) + if (!improv->IsUsingLadder()) + vel.z = 0.0f; + + // cannot be Length2D, or will break ladder movement (they are only Z) + float moveDist = vel.Length(); + + float deltaT = gpGlobals->curtime - m_lastTime; + if (deltaT <= 0.0f) + return; + + m_lastTime = gpGlobals->curtime; + + // compute current velocity + m_avgVel[ m_avgVelIndex++ ] = moveDist/deltaT; + + if (m_avgVelIndex == MAX_VEL_SAMPLES) + m_avgVelIndex = 0; + + if (m_avgVelCount < MAX_VEL_SAMPLES) + { + ++m_avgVelCount; + } + else + { + // we have enough samples to know if we're stuck + + float avgVel = 0.0f; + for( int t=0; t<m_avgVelCount; ++t ) + avgVel += m_avgVel[t]; + + avgVel /= m_avgVelCount; + + // cannot make this velocity too high, or actors will get "stuck" when going down ladders + float stuckVel = (improv->IsUsingLadder()) ? 10.0f : 20.0f; + + if (avgVel < stuckVel) + { + // note when and where we initially become stuck + m_stuckTimer.Start(); + m_stuckSpot = improv->GetCentroid(); + m_isStuck = true; + } + } + } + + // always need to track this + m_lastCentroid = improv->GetCentroid(); +} + |