1HEADmaster

1 files changed, 1208 insertions, 0 deletions

diff --git a/game/server/cstrike/cs_nav_path.cpp b/game/server/cstrike/cs_nav_path.cpp
new file mode 100644
index 0000000..5a1a9ce
--- /dev/null
+++ b/game/server/cstrike/cs_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 "cs_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 CCSNavPath::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 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;
+					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;
+					break;
+				}
+			}
+
+			if (it == ladders->Count())
+			{
+				//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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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 CCSNavPath::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();
+}
+