/**
* 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 )
{
int lastVisible = 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;
}
/**
* 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_PRECISE)
return;
Vector dir( BotCOS( m_improv->GetMoveAngle() ), BotSIN( m_improv->GetMoveAngle() ), 0.0f );
dir.z = 0.0f;
dir.NormalizeInPlace();
Vector lat( -dir.y, dir.x, 0.0f );
const float feelerOffset = (m_improv->IsCrouching()) ? 20.0f : 25.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();
}
}
/**
* 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).Make2D().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).Make2D().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_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_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.Normalize();
// 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_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 (DotProduct( toPoint, initDir.Make2D() ) < 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_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;
}
/**
* 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).LengthSquared();
// 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;
}
// 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 CCSBot::FindOurPositionOnPath(Vector *close, bool local) const
{
if (!HasPath())
return -1;
Vector along, toFeet;
Vector feet(pev->origin.x, pev->origin.y, GetFeetZ());
Vector eyes = feet + Vector(0, 0, HalfHumanHeight); // in case we're crouching
Vector pos;
const Vector *from, *to;
real_t length;
float closeLength;
float closeDistSq = 9999999999.9;
int closeIndex = -1;
real_t distSq;
int start, end;
if (local)
{
start = m_pathIndex - 3;
if (start < 1)
start = 1;
end = m_pathIndex + 3;
if (end > m_pathLength)
end = m_pathLength;
}
else
{
start = 1;
end = m_pathLength;
}
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).LengthSquared();
// 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_EVERYTHING))
continue;
// don't use points we cant reach
if (!IsStraightLinePathWalkable(&pos))
continue;
closeDistSq = distSq;
if (close)
*close = pos;
closeIndex = i - 1;
}
}
return closeIndex;
}
// Do reflex avoidance movements if our "feelers" are touched
void CCSBot::FeelerReflexAdjustment(Vector *goalPosition)
{
// if we are in a "precise" area, do not do feeler adjustments
if (m_lastKnownArea && (m_lastKnownArea->GetAttributes() & NAV_PRECISE))
return;
Vector dir(BotCOS(m_forwardAngle), BotSIN(m_forwardAngle), 0.0f);
Vector lat(-dir.y, dir.x, 0.0f);
const float feelerOffset = (IsCrouching()) ? 15.0f : 20.0f;
const float feelerLengthRun = 50.0f; // 100 - too long for tight hallways (cs_747)
const float feelerLengthWalk = 30.0f;
const float feelerHeight = StepHeight + 0.1f; // if obstacle is lower than StepHeight, we'll walk right over it
float feelerLength = (IsRunning()) ? feelerLengthRun : feelerLengthWalk;
feelerLength = (IsCrouching()) ? 20.0f : feelerLength;
// Feelers must follow floor slope
float ground;
Vector normal;
//m_eyePos = EyePosition();
m_eyePos.x = pev->origin.x + pev->view_ofs.x;
m_eyePos.y = pev->origin.y + pev->view_ofs.y;
m_eyePos.z = pev->origin.z + pev->view_ofs.z;
if (GetSimpleGroundHeightWithFloor(&m_eyePos, &ground, &normal) == false)
return;
// get forward vector along floor
dir = CrossProduct(lat, normal);
// correct the sideways vector
lat = CrossProduct(dir, normal);
Vector feet(pev->origin.x, pev->origin.y, GetFeetZ());
feet.z += feelerHeight;
Vector from = feet + feelerOffset * lat;
Vector to = from + feelerLength * dir;
bool leftClear = IsWalkableTraceLineClear(from, to, WALK_THRU_EVERYTHING);
// avoid ledges, too
// use 'from' so it doesn't interfere with legitimate gap jumping (its at our feet)
// TODO: Rethink this - it causes lots of wiggling when bots jump down from vents, etc
/*
float ground;
if (GetSimpleGroundHeightWithFloor(&from, &ground))
{
if (GetFeetZ() - ground > JumpHeight)
leftClear = false;
}
*/
if ((cv_bot_traceview.value == 1.0f && IsLocalPlayerWatchingMe()) || cv_bot_traceview.value == 10.0f)
{
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_EVERYTHING);
/*
// avoid ledges, too
if (GetSimpleGroundHeightWithFloor(&from, &ground))
{
if (GetFeetZ() - ground > JumpHeight)
rightClear = false;
}
*/
if ((cv_bot_traceview.value == 1.0f && IsLocalPlayerWatchingMe()) || cv_bot_traceview.value == 10.0f)
{
if (rightClear)
UTIL_DrawBeamPoints(from, to, 1, 0, 255, 0);
else
UTIL_DrawBeamPoints(from, to, 1, 255, 0, 0);
}
const float avoidRange = (IsCrouching()) ? 150.0f : 300.0f; // 50.0f : 300.0f
if (!rightClear)
{
if (leftClear)
{
// right hit, left clear - veer left
*goalPosition = *goalPosition + avoidRange * lat;
}
}
else if (!leftClear)
{
// right clear, left hit - veer right
*goalPosition = *goalPosition - avoidRange * lat;
}
}
