You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1094 lines
28 KiB
1094 lines
28 KiB
// NextBotPath.cpp |
|
// Encapsulate and manipulate a path through the world |
|
// Author: Michael Booth, February 2006 |
|
//========= Copyright Valve Corporation, All rights reserved. ============// |
|
|
|
#include "cbase.h" |
|
|
|
#include "nav_mesh.h" |
|
#include "fmtstr.h" |
|
|
|
#include "NextBotPath.h" |
|
#include "NextBotInterface.h" |
|
#include "NextBotLocomotionInterface.h" |
|
#include "NextBotBodyInterface.h" |
|
#include "NextBotUtil.h" |
|
|
|
#include "tier0/vprof.h" |
|
|
|
// memdbgon must be the last include file in a .cpp file!!! |
|
#include "tier0/memdbgon.h" |
|
|
|
ConVar NextBotPathDrawIncrement( "nb_path_draw_inc", "100", FCVAR_CHEAT ); |
|
ConVar NextBotPathDrawSegmentCount( "nb_path_draw_segment_count", "100", FCVAR_CHEAT ); |
|
ConVar NextBotPathSegmentInfluenceRadius( "nb_path_segment_influence_radius", "100", FCVAR_CHEAT ); |
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
Path::Path( void ) |
|
{ |
|
m_segmentCount = 0; |
|
|
|
m_cursorPos = 0.0f; |
|
m_isCursorDataDirty = true; |
|
m_cursorData.segmentPrior = NULL; |
|
m_ageTimer.Invalidate(); |
|
m_subject = NULL; |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Determine actual path positions |
|
*/ |
|
bool Path::ComputePathDetails( INextBot *bot, const Vector &start ) |
|
{ |
|
VPROF_BUDGET( "Path::ComputePathDetails", "NextBot" ); |
|
|
|
if (m_segmentCount == 0) |
|
return false; |
|
|
|
IBody *body = bot->GetBodyInterface(); |
|
ILocomotion *mover = bot->GetLocomotionInterface(); |
|
|
|
const float stepHeight = ( mover ) ? mover->GetStepHeight() : 18.0f; |
|
|
|
// inflate hull width slightly as a safety margin |
|
const float hullWidth = ( body ) ? body->GetHullWidth() + 5.0f : 1.0f; |
|
|
|
// set first path position |
|
if ( m_path[0].area->Contains( start ) ) |
|
{ |
|
m_path[0].pos = start; |
|
} |
|
else |
|
{ |
|
// start in first area's center |
|
m_path[0].pos = m_path[0].area->GetCenter(); |
|
} |
|
m_path[0].ladder = NULL; |
|
m_path[0].how = NUM_TRAVERSE_TYPES; |
|
m_path[0].type = ON_GROUND; |
|
|
|
// set positions along the path |
|
for( int i=1; i<m_segmentCount; ++i ) |
|
{ |
|
Segment *from = &m_path[ i-1 ]; |
|
Segment *to = &m_path[ i ]; |
|
|
|
if ( to->how <= GO_WEST ) // walk along the floor to the next area |
|
{ |
|
to->ladder = NULL; |
|
|
|
from->area->ComputePortal( to->area, (NavDirType)to->how, &to->m_portalCenter, &to->m_portalHalfWidth ); |
|
|
|
// compute next point |
|
ComputeAreaCrossing( bot, from->area, from->pos, to->area, (NavDirType)to->how, &to->pos ); |
|
|
|
// we need to walk out of "from" area, so keep Z where we can reach it |
|
to->pos.z = from->area->GetZ( to->pos ); |
|
|
|
// if this is a "jump down" connection, we must insert an additional point on the path |
|
//float expectedHeightDrop = from->area->GetZ( from->pos ) - to->area->GetZ( to->pos ); |
|
|
|
// measure the drop distance relative to the actual slope of the ground |
|
Vector fromPos = from->pos; |
|
fromPos.z = from->area->GetZ( fromPos ); |
|
|
|
Vector toPos = to->pos; |
|
toPos.z = to->area->GetZ( toPos ); |
|
|
|
Vector groundNormal; |
|
from->area->ComputeNormal( &groundNormal ); |
|
|
|
Vector alongPath = toPos - fromPos; |
|
|
|
float expectedHeightDrop = -DotProduct( alongPath, groundNormal ); |
|
|
|
if ( expectedHeightDrop > mover->GetStepHeight() ) |
|
{ |
|
// NOTE: We can't know this is a drop-down yet, because of subtle interactions |
|
// between nav area links and "portals" and "area crossings" |
|
|
|
// compute direction of path just prior to "jump down" |
|
Vector2D dir; |
|
DirectionToVector2D( (NavDirType)to->how, &dir ); |
|
|
|
// shift top of "jump down" out a bit to "get over the ledge" |
|
const float inc = 10.0f; // 0.25f * hullWidth; |
|
const float maxPushDist = 2.0f * hullWidth; // 75.0f; |
|
float halfWidth = hullWidth/2.0f; |
|
float hullHeight = ( body ) ? body->GetCrouchHullHeight() : 1.0f; |
|
|
|
float pushDist; |
|
for( pushDist = 0.0f; pushDist <= maxPushDist; pushDist += inc ) |
|
{ |
|
Vector pos = to->pos + Vector( pushDist * dir.x, pushDist * dir.y, 0.0f ); |
|
Vector lowerPos = Vector( pos.x, pos.y, toPos.z ); |
|
|
|
trace_t result; |
|
NextBotTraceFilterIgnoreActors filter( bot->GetEntity(), COLLISION_GROUP_NONE ); |
|
UTIL_TraceHull( pos, lowerPos, |
|
Vector( -halfWidth, -halfWidth, stepHeight ), Vector( halfWidth, halfWidth, hullHeight ), |
|
bot->GetBodyInterface()->GetSolidMask(), &filter, &result ); |
|
|
|
if ( result.fraction >= 1.0f ) |
|
{ |
|
// found clearance to drop |
|
break; |
|
} |
|
} |
|
|
|
Vector startDrop( to->pos.x + pushDist * dir.x, to->pos.y + pushDist * dir.y, to->pos.z ); |
|
Vector endDrop( startDrop.x, startDrop.y, to->area->GetZ( to->pos ) ); |
|
|
|
if ( bot->IsDebugging( NEXTBOT_PATH ) ) |
|
{ |
|
NDebugOverlay::Cross3D( startDrop, 5.0f, 255, 0, 255, true, 5.0f ); |
|
NDebugOverlay::Cross3D( endDrop, 5.0f, 255, 255, 0, true, 5.0f ); |
|
NDebugOverlay::VertArrow( startDrop, endDrop, 5.0f, 255, 100, 0, 255, true, 5.0f ); |
|
} |
|
|
|
// verify that there is actually ground down there in case this is a far jump dropdown |
|
float ground; |
|
if ( TheNavMesh->GetGroundHeight( endDrop, &ground ) ) |
|
{ |
|
if ( startDrop.z > ground + stepHeight ) |
|
{ |
|
// if "ground" is lower than the next segment along the path |
|
// there is a chasm between - this is not a drop down |
|
// NOTE next->pos is not yet valid - this loop is computing it! |
|
// const Segment *next = NextSegment( to ); |
|
// if ( !next || next->area->GetCenter().z < ground + stepHeight ) |
|
{ |
|
// this is a "jump down" link |
|
to->pos = startDrop; |
|
to->type = DROP_DOWN; |
|
|
|
// insert a duplicate node to represent the bottom of the fall |
|
if ( m_segmentCount < MAX_PATH_SEGMENTS-1 ) |
|
{ |
|
// copy nodes down |
|
for( int j=m_segmentCount; j>i; --j ) |
|
m_path[j] = m_path[j-1]; |
|
|
|
// path is one node longer |
|
++m_segmentCount; |
|
|
|
// move index ahead into the new node we just duplicated |
|
++i; |
|
|
|
m_path[i].pos.x = endDrop.x; |
|
m_path[i].pos.y = endDrop.y; |
|
m_path[i].pos.z = ground; |
|
|
|
m_path[i].type = ON_GROUND; |
|
} |
|
} |
|
} |
|
} |
|
} |
|
} |
|
else if ( to->how == GO_LADDER_UP ) // to get to next area, must go up a ladder |
|
{ |
|
// find our ladder |
|
const NavLadderConnectVector *ladders = from->area->GetLadders( CNavLadder::LADDER_UP ); |
|
int it; |
|
for( it=0; it<ladders->Count(); ++it ) |
|
{ |
|
CNavLadder *ladder = (*ladders)[ it ].ladder; |
|
|
|
// can't use "behind" area when ascending... |
|
if (ladder->m_topForwardArea == to->area || |
|
ladder->m_topLeftArea == to->area || |
|
ladder->m_topRightArea == to->area) |
|
{ |
|
to->ladder = ladder; |
|
to->pos = ladder->m_bottom + ladder->GetNormal() * 2.0f * HalfHumanWidth; |
|
to->type = LADDER_UP; |
|
break; |
|
} |
|
} |
|
|
|
if (it == ladders->Count()) |
|
{ |
|
//PrintIfWatched( "ERROR: Can't find ladder in path\n" ); |
|
return false; |
|
} |
|
} |
|
else if ( to->how == GO_LADDER_DOWN ) // to get to next area, must go down a ladder |
|
{ |
|
// find our ladder |
|
const NavLadderConnectVector *ladders = from->area->GetLadders( CNavLadder::LADDER_DOWN ); |
|
int it; |
|
for( it=0; it<ladders->Count(); ++it ) |
|
{ |
|
CNavLadder *ladder = (*ladders)[ it ].ladder; |
|
|
|
if (ladder->m_bottomArea == to->area) |
|
{ |
|
to->ladder = ladder; |
|
to->pos = ladder->m_top; |
|
to->pos = ladder->m_top - ladder->GetNormal() * 2.0f * HalfHumanWidth; |
|
to->type = LADDER_DOWN; |
|
break; |
|
} |
|
} |
|
|
|
if (it == ladders->Count()) |
|
{ |
|
//PrintIfWatched( "ERROR: Can't find ladder in path\n" ); |
|
return false; |
|
} |
|
} |
|
else if ( to->how == GO_ELEVATOR_UP || to->how == GO_ELEVATOR_DOWN ) |
|
{ |
|
to->pos = to->area->GetCenter(); |
|
to->ladder = NULL; |
|
} |
|
} |
|
|
|
|
|
// |
|
// Scan for non-adjacent nav areas and add gap-jump-target nodes |
|
// and jump-up target nodes for adjacent ledge mantling |
|
// @todo Adjacency should be baked into the mesh data |
|
// |
|
for( int i=0; i<m_segmentCount-1; ++i ) |
|
{ |
|
Segment *from = &m_path[ i ]; |
|
Segment *to = &m_path[ i+1 ]; |
|
|
|
// first segment doesnt have a direction |
|
if ( from->how != NUM_TRAVERSE_TYPES && from->how > GO_WEST ) |
|
continue; |
|
|
|
if ( to->how > GO_WEST || !to->type == ON_GROUND ) |
|
continue; |
|
|
|
// if areas are separated, we may need to 'gap jump' between them |
|
// add a node to minimize the jump distance |
|
Vector closeFrom, closeTo; |
|
to->area->GetClosestPointOnArea( from->pos, &closeTo ); |
|
from->area->GetClosestPointOnArea( closeTo, &closeFrom ); |
|
|
|
if ( bot->IsDebugging( NEXTBOT_PATH ) ) |
|
{ |
|
NDebugOverlay::Line( closeFrom, closeTo, 255, 0, 255, true, 5.0f ); |
|
} |
|
|
|
|
|
const float separationTolerance = 1.9f * GenerationStepSize; |
|
if ( (closeFrom - closeTo).AsVector2D().IsLengthGreaterThan( separationTolerance ) && ( closeTo - closeFrom ).AsVector2D().IsLengthGreaterThan( 0.5f * fabs( closeTo.z - closeFrom.z ) ) ) |
|
{ |
|
// areas are disjoint and mostly level - add gap jump target |
|
|
|
// compute landing spot in 'to' area |
|
Vector landingPos; |
|
to->area->GetClosestPointOnArea( to->pos, &landingPos ); |
|
|
|
// compute launch spot in 'from' area |
|
Vector launchPos; |
|
from->area->GetClosestPointOnArea( landingPos, &launchPos ); |
|
|
|
Vector forward = landingPos - launchPos; |
|
forward.NormalizeInPlace(); |
|
|
|
const float halfWidth = hullWidth/2.0f; |
|
|
|
// adjust path position to landing spot |
|
to->pos = landingPos + forward * halfWidth; |
|
|
|
// insert launch position just before that segment to ensure bot is |
|
// positioned for minimal jump distance |
|
Segment newSegment = *from; |
|
|
|
newSegment.pos = launchPos - forward * halfWidth; |
|
newSegment.type = JUMP_OVER_GAP; |
|
|
|
InsertSegment( newSegment, i+1 ); |
|
|
|
++i; |
|
} |
|
else if ( (closeTo.z - closeFrom.z) > stepHeight ) |
|
{ |
|
// areas are adjacent, but need a jump-up - add a jump-to target |
|
|
|
// adjust goal to be at top of ledge |
|
//to->pos.z = to->area->GetZ( to->pos.x, to->pos.y ); |
|
// use center of climb-up destination area to make sure bot moves onto actual ground once they finish their climb |
|
to->pos = to->area->GetCenter(); |
|
|
|
// add launch position at base of jump |
|
Segment newSegment = *from; |
|
|
|
Vector launchPos; |
|
from->area->GetClosestPointOnArea( to->pos, &launchPos ); |
|
|
|
newSegment.pos = launchPos; |
|
newSegment.type = CLIMB_UP; |
|
|
|
if ( bot->IsDebugging( NEXTBOT_PATH ) ) |
|
{ |
|
NDebugOverlay::Cross3D( newSegment.pos, 15.0f, 255, 100, 255, true, 3.0f ); |
|
} |
|
|
|
InsertSegment( newSegment, i+1 ); |
|
|
|
++i; |
|
} |
|
|
|
/** RETHINK THIS. It doesn't work in general cases, and messes up on doorways |
|
else if ( from->type == ON_GROUND && from->how <= GO_WEST ) |
|
{ |
|
// if any segment is not directly walkable, add a segment |
|
// fixup corners that are being cut too tightly |
|
if ( mover && !mover->IsPotentiallyTraversable( from->pos, to->pos ) ) |
|
{ |
|
Segment newSegment = *from; |
|
|
|
if ( bot->IsDebugging( INextBot::PATH ) ) |
|
{ |
|
NDebugOverlay::HorzArrow( from->pos, to->pos, 3.0f, 255, 0, 0, 255, true, 3.0f ); |
|
} |
|
|
|
//newSegment.pos = from->area->GetCenter(); |
|
|
|
Vector2D shift; |
|
DirectionToVector2D( OppositeDirection( (NavDirType)to->how ), &shift ); |
|
|
|
newSegment.pos = to->pos; |
|
newSegment.pos.x += hullWidth * shift.x; |
|
newSegment.pos.y += hullWidth * shift.y; |
|
|
|
newSegment.type = ON_GROUND; |
|
|
|
if ( bot->IsDebugging( INextBot::PATH ) ) |
|
{ |
|
NDebugOverlay::Cross3D( newSegment.pos, 15.0f, 255, 0, 255, true, 3.0f ); |
|
} |
|
|
|
InsertSegment( newSegment, i+1 ); |
|
|
|
i += 2; |
|
} |
|
} |
|
*/ |
|
} |
|
|
|
return true; |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Insert new segment at index i |
|
*/ |
|
void Path::InsertSegment( Segment newSegment, int i ) |
|
{ |
|
if (m_segmentCount < MAX_PATH_SEGMENTS-1) |
|
{ |
|
// shift segments to make room for new one |
|
for( int j=m_segmentCount; j>i; --j ) |
|
m_path[j] = m_path[j-1]; |
|
|
|
// path is one node longer |
|
++m_segmentCount; |
|
|
|
m_path[i] = newSegment; |
|
} |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Build trivial path when start and goal are in the same nav area |
|
*/ |
|
bool Path::BuildTrivialPath( INextBot *bot, const Vector &goal ) |
|
{ |
|
const Vector &start = bot->GetPosition(); |
|
|
|
m_segmentCount = 0; |
|
|
|
/// @todo Dangerous to use "nearset" nav area - could be far away |
|
CNavArea *startArea = TheNavMesh->GetNearestNavArea( start ); |
|
if (startArea == NULL) |
|
return false; |
|
|
|
CNavArea *goalArea = TheNavMesh->GetNearestNavArea( goal ); |
|
if (goalArea == NULL) |
|
return false; |
|
|
|
m_segmentCount = 2; |
|
|
|
m_path[0].area = startArea; |
|
m_path[0].pos.x = start.x; |
|
m_path[0].pos.y = start.y; |
|
m_path[0].pos.z = startArea->GetZ( start ); |
|
m_path[0].ladder = NULL; |
|
m_path[0].how = NUM_TRAVERSE_TYPES; |
|
m_path[0].type = ON_GROUND; |
|
|
|
m_path[1].area = goalArea; |
|
m_path[1].pos.x = goal.x; |
|
m_path[1].pos.y = goal.y; |
|
m_path[1].pos.z = goalArea->GetZ( goal ); |
|
m_path[1].ladder = NULL; |
|
m_path[1].how = NUM_TRAVERSE_TYPES; |
|
m_path[1].type = ON_GROUND; |
|
|
|
m_path[0].forward = m_path[1].pos - m_path[0].pos; |
|
m_path[0].length = m_path[0].forward.NormalizeInPlace(); |
|
m_path[0].distanceFromStart = 0.0f; |
|
m_path[0].curvature = 0.0f; |
|
|
|
m_path[1].forward = m_path[0].forward; |
|
m_path[1].length = 0.0f; |
|
m_path[1].distanceFromStart = m_path[0].length; |
|
m_path[1].curvature = 0.0f; |
|
|
|
OnPathChanged( bot, COMPLETE_PATH ); |
|
|
|
return true; |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Draw the path for debugging. |
|
*/ |
|
void Path::Draw( const Path::Segment *start ) const |
|
{ |
|
if ( !IsValid() ) |
|
return; |
|
|
|
CFmtStr msg; |
|
|
|
// limit length of path we draw |
|
int count = NextBotPathDrawSegmentCount.GetInt(); |
|
|
|
const Segment *s = start ? start : FirstSegment(); |
|
int i=0; |
|
while( s && count-- ) |
|
{ |
|
const Segment *next = NextSegment( s ); |
|
if ( next == NULL ) |
|
{ |
|
// end of the path |
|
break; |
|
} |
|
|
|
Vector to = next->pos - s->pos; |
|
float horiz = MAX( abs(to.x), abs(to.y) ); |
|
float vert = abs( to.z ); |
|
|
|
int r,g,b; |
|
switch( s->type ) |
|
{ |
|
case DROP_DOWN: r = 255; g = 0; b = 255; break; |
|
case CLIMB_UP: r = 0; g = 0; b = 255; break; |
|
case JUMP_OVER_GAP: r = 0; g = 255; b = 255; break; |
|
case LADDER_UP: r = 0; g = 255; b = 0; break; |
|
case LADDER_DOWN: r = 0; g = 100; b = 0; break; |
|
default: r = 255; g = 77; b = 0; break; // ON_GROUND |
|
} |
|
|
|
if ( s->ladder ) |
|
{ |
|
NDebugOverlay::VertArrow( s->ladder->m_bottom, s->ladder->m_top, 5.0f, r, g, b, 255, true, 0.1f ); |
|
} |
|
else |
|
{ |
|
NDebugOverlay::Line( s->pos, next->pos, r, g, b, true, 0.1f ); |
|
} |
|
|
|
const float nodeLength = 25.0f; |
|
if ( horiz > vert ) |
|
{ |
|
NDebugOverlay::HorzArrow( s->pos, s->pos + nodeLength * s->forward, 5.0f, r, g, b, 255, true, 0.1f ); |
|
} |
|
else |
|
{ |
|
NDebugOverlay::VertArrow( s->pos, s->pos + nodeLength * s->forward, 5.0f, r, g, b, 255, true, 0.1f ); |
|
} |
|
|
|
NDebugOverlay::Text( s->pos, msg.sprintf( "%d", i ), true, 0.1f ); |
|
|
|
//NDebugOverlay::Text( s->pos, msg.sprintf( "%d (%3.2f)", i, s->curvature ), false, 0.1f ); |
|
|
|
s = next; |
|
++i; |
|
} |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Draw the path for debugging - MODIFIES cursor position |
|
*/ |
|
void Path::DrawInterpolated( float from, float to ) |
|
{ |
|
if ( !IsValid() ) |
|
{ |
|
return; |
|
} |
|
|
|
float t = from; |
|
|
|
MoveCursor( t ); |
|
const Data &data = GetCursorData(); |
|
Vector lastPos = data.pos; |
|
|
|
do |
|
{ |
|
t += NextBotPathDrawIncrement.GetFloat(); |
|
|
|
MoveCursor( t ); |
|
const Data &data = GetCursorData(); |
|
|
|
float curvePower = 3.0f * data.curvature; |
|
|
|
int r = 255 * ( 1.0f - curvePower ); |
|
r = clamp( r, 0, 255 ); |
|
|
|
int g = 255 * ( 1.0f + curvePower ); |
|
g = clamp( g, 0, 255 ); |
|
|
|
NDebugOverlay::Line( lastPos, data.pos, r, g, 0, true, 0.1f ); |
|
|
|
/* |
|
int i = 0xFF & (int)( data.pos.x + data.pos.y + data.pos.z ); |
|
i >>= 1; |
|
i += 128; |
|
|
|
NDebugOverlay::Line( data.pos, data.pos + 10.0f * data.forward, 0, i, i, true, 0.1f ); |
|
*/ |
|
|
|
lastPos = data.pos; |
|
} |
|
while( t < to ); |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Check line of sight from 'anchor' node on path to subsequent nodes until |
|
* we find a node that can't been seen from 'anchor'. |
|
*/ |
|
int Path::FindNextOccludedNode( INextBot *bot, int anchorIndex ) |
|
{ |
|
ILocomotion *mover = bot->GetLocomotionInterface(); |
|
if ( mover == NULL) |
|
{ |
|
return m_segmentCount; |
|
} |
|
|
|
Segment *anchor = &m_path[ anchorIndex ]; |
|
|
|
for( int i=anchorIndex+1; i<m_segmentCount; ++i ) |
|
{ |
|
Segment *to = &m_path[i]; |
|
|
|
// if this segment is not on the ground, or is precise, don't skip past it |
|
if ( !to->type == ON_GROUND || (to->area->GetAttributes() & NAV_MESH_PRECISE) ) |
|
{ |
|
return i; |
|
} |
|
|
|
if ( !mover->IsPotentiallyTraversable( anchor->pos, to->pos, ILocomotion::IMMEDIATELY ) ) |
|
{ |
|
// cant reach this node directly from anchor node |
|
return i; |
|
} |
|
|
|
if ( mover->HasPotentialGap( anchor->pos, to->pos ) ) |
|
{ |
|
// we would fall into a gap if we took this cutoff |
|
return i; |
|
} |
|
} |
|
|
|
return m_segmentCount; |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Smooth out path, removing redundant nodes |
|
*/ |
|
void Path::Optimize( INextBot *bot ) |
|
{ |
|
// this is SUPER expensive - especially the IsGap() check |
|
return; |
|
|
|
VPROF_BUDGET( "Path::Optimize", "NextBot" ); |
|
|
|
if (m_segmentCount < 3) |
|
return; |
|
|
|
int anchor = 0; |
|
|
|
while( anchor < m_segmentCount ) |
|
{ |
|
int occluded = FindNextOccludedNode( bot, anchor ); |
|
int nextAnchor = occluded-1; |
|
|
|
if (nextAnchor > anchor) |
|
{ |
|
// remove redundant nodes between anchor and nextAnchor |
|
int removeCount = nextAnchor - anchor - 1; |
|
if (removeCount > 0) |
|
{ |
|
for( int i=nextAnchor; i<m_segmentCount; ++i ) |
|
{ |
|
m_path[i-removeCount] = m_path[i]; |
|
} |
|
m_segmentCount -= removeCount; |
|
} |
|
} |
|
|
|
++anchor; |
|
} |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Compute final data for completed path |
|
*/ |
|
void Path::PostProcess( void ) |
|
{ |
|
VPROF_BUDGET( "Path::PostProcess", "NextBot" ); |
|
|
|
m_ageTimer.Start(); |
|
|
|
if (m_segmentCount == 0) |
|
return; |
|
|
|
if (m_segmentCount == 1) |
|
{ |
|
m_path[0].forward = vec3_origin; |
|
m_path[0].length = 0.0f; |
|
m_path[0].distanceFromStart = 0.0f; |
|
m_path[0].curvature = 0.0f; |
|
return; |
|
} |
|
|
|
float distanceSoFar = 0.0f; |
|
int i; |
|
for( i=0; i < m_segmentCount-1; ++i ) |
|
{ |
|
Segment *from = &m_path[ i ]; |
|
Segment *to = &m_path[ i+1 ]; |
|
|
|
from->forward = to->pos - from->pos; |
|
from->length = from->forward.NormalizeInPlace(); |
|
|
|
from->distanceFromStart = distanceSoFar; |
|
|
|
distanceSoFar += from->length; |
|
} |
|
|
|
|
|
// compute curvature in XY plane |
|
Vector2D from, to; |
|
for( i=1; i < m_segmentCount-1; ++i ) |
|
{ |
|
if (m_path[ i ].type != ON_GROUND) |
|
{ |
|
m_path[ i ].curvature = 0.0f; |
|
} |
|
else |
|
{ |
|
from = m_path[ i-1 ].forward.AsVector2D(); |
|
from.NormalizeInPlace(); |
|
|
|
to = m_path[ i ].forward.AsVector2D(); |
|
to.NormalizeInPlace(); |
|
|
|
m_path[ i ].curvature = 0.5f * ( 1.0f - from.Dot( to ) ); |
|
|
|
Vector2D right( -from.y, from.x ); |
|
if ( to.Dot( right ) < 0.0f ) |
|
{ |
|
m_path[ i ].curvature = -m_path[ i ].curvature; |
|
} |
|
} |
|
} |
|
|
|
// first segment has no curvature |
|
m_path[ 0 ].curvature = 0.0f; |
|
|
|
// last segment maintains direction |
|
m_path[ i ].forward = m_path[ i-1 ].forward; |
|
m_path[ i ].length = 0.0f; |
|
m_path[ i ].distanceFromStart = distanceSoFar; |
|
m_path[ i ].curvature = 0.0f; |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Return a position on the path at the given distance from the path start |
|
*/ |
|
const Vector &Path::GetPosition( float distanceFromStart, const Segment *start ) const |
|
{ |
|
if (!IsValid()) |
|
{ |
|
return vec3_origin; |
|
} |
|
|
|
float lengthSoFar; |
|
const Segment *segment; |
|
|
|
if (start) |
|
{ |
|
segment = start; |
|
lengthSoFar = start->distanceFromStart; |
|
} |
|
else |
|
{ |
|
segment = &m_path[0]; |
|
lengthSoFar = 0.0f; |
|
} |
|
|
|
if (segment->distanceFromStart > distanceFromStart) |
|
{ |
|
// clamp to path start |
|
return segment->pos; |
|
} |
|
|
|
|
|
const Segment *next = NextSegment( segment ); |
|
|
|
Vector delta; |
|
float length; |
|
|
|
while( next ) |
|
{ |
|
delta = next->pos - segment->pos; |
|
length = segment->length; |
|
|
|
if (lengthSoFar + length >= distanceFromStart) |
|
{ |
|
// desired point is on this segment of the path |
|
float overlap = distanceFromStart - lengthSoFar; |
|
float t = overlap / length; |
|
|
|
m_pathPos = segment->pos + t * delta; |
|
|
|
return m_pathPos; |
|
} |
|
|
|
lengthSoFar += length; |
|
|
|
segment = next; |
|
next = NextSegment( next ); |
|
} |
|
|
|
// clamp to path end |
|
return segment->pos; |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Return the closest point on the path to the given position |
|
*/ |
|
const Vector &Path::GetClosestPosition( const Vector &pos, const Segment *start, float alongLimit ) const |
|
{ |
|
const Segment *segment = (start) ? start : &m_path[0]; |
|
|
|
if (segment == NULL) |
|
{ |
|
return pos; |
|
} |
|
|
|
m_closePos = pos; |
|
float closeRangeSq = 99999999999.9f; |
|
|
|
float distanceSoFar = 0.0f; |
|
while( alongLimit == 0.0f || distanceSoFar <= alongLimit ) |
|
{ |
|
const Segment *nextSegment = NextSegment( segment ); |
|
|
|
if (nextSegment) |
|
{ |
|
Vector close; |
|
CalcClosestPointOnLineSegment( pos, segment->pos, nextSegment->pos, close ); |
|
float rangeSq = (close - pos).LengthSqr(); |
|
if (rangeSq < closeRangeSq) |
|
{ |
|
m_closePos = close; |
|
closeRangeSq = rangeSq; |
|
} |
|
} |
|
else |
|
{ |
|
// end of the path |
|
break; |
|
} |
|
|
|
distanceSoFar += segment->length; |
|
segment = nextSegment; |
|
} |
|
|
|
return m_closePos; |
|
} |
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Replace this path with the given path's data |
|
*/ |
|
void Path::Copy( INextBot *bot, const Path &path ) |
|
{ |
|
VPROF_BUDGET( "Path::Copy", "NextBot" ); |
|
|
|
Invalidate(); |
|
|
|
for( int i = 0; i < path.m_segmentCount; ++i ) |
|
{ |
|
m_path[i] = path.m_path[i]; |
|
} |
|
m_segmentCount = path.m_segmentCount; |
|
|
|
OnPathChanged( bot, COMPLETE_PATH ); |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Set cursor position to closest point on path to given position |
|
*/ |
|
void Path::MoveCursorToClosestPosition( const Vector &pos, SeekType type, float alongLimit ) const |
|
{ |
|
if ( !IsValid() ) |
|
{ |
|
return; |
|
} |
|
|
|
if ( type == SEEK_ENTIRE_PATH || type == SEEK_AHEAD ) |
|
{ |
|
const Segment *segment; |
|
|
|
if ( type == SEEK_AHEAD ) |
|
{ |
|
// continue search from cursor position onward |
|
if ( m_cursorData.segmentPrior ) |
|
{ |
|
segment = m_cursorData.segmentPrior; |
|
} |
|
else |
|
{ |
|
// no prior segment, start from the start |
|
segment = &m_path[ 0 ]; |
|
} |
|
} |
|
else |
|
{ |
|
// search entire path from the start |
|
segment = &m_path[ 0 ]; |
|
} |
|
|
|
m_cursorData.pos = pos; |
|
m_cursorData.segmentPrior = segment; |
|
float closeRangeSq = 99999999999.9f; |
|
|
|
float distanceSoFar = 0.0f; |
|
while( alongLimit == 0.0f || distanceSoFar <= alongLimit ) |
|
{ |
|
const Segment *nextSegment = NextSegment( segment ); |
|
|
|
if ( nextSegment ) |
|
{ |
|
Vector close; |
|
CalcClosestPointOnLineSegment( pos, segment->pos, nextSegment->pos, close ); |
|
|
|
float rangeSq = ( close - pos ).LengthSqr(); |
|
if ( rangeSq < closeRangeSq ) |
|
{ |
|
m_cursorData.pos = close; |
|
m_cursorData.segmentPrior = segment; |
|
|
|
closeRangeSq = rangeSq; |
|
} |
|
} |
|
else |
|
{ |
|
// end of the path |
|
break; |
|
} |
|
|
|
distanceSoFar += segment->length; |
|
segment = nextSegment; |
|
} |
|
|
|
// |
|
// Move cursor to closest point on path |
|
// |
|
segment = m_cursorData.segmentPrior; |
|
|
|
float t = ( m_cursorData.pos - segment->pos ).Length() / segment->length; |
|
|
|
m_cursorPos = segment->distanceFromStart + t * segment->length; |
|
m_isCursorDataDirty = true; |
|
} |
|
else |
|
{ |
|
AssertMsg( false, "SEEK_BEHIND not implemented" ); |
|
} |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Return path state at the current cursor position |
|
*/ |
|
const Path::Data &Path::GetCursorData( void ) const |
|
{ |
|
if ( IsValid() ) |
|
{ |
|
if ( m_isCursorDataDirty ) |
|
{ |
|
const float epsilon = 0.0001f; |
|
if ( m_cursorPos < epsilon || m_segmentCount < 2 ) |
|
{ |
|
// start of path |
|
m_cursorData.pos = m_path[0].pos; |
|
m_cursorData.forward = m_path[0].forward; |
|
m_cursorData.curvature = m_path[0].curvature; |
|
m_cursorData.segmentPrior = &m_path[0]; |
|
} |
|
else if ( m_cursorPos > GetLength() - epsilon ) |
|
{ |
|
// end of path |
|
m_cursorData.pos = m_path[ m_segmentCount-1 ].pos; |
|
m_cursorData.forward = m_path[ m_segmentCount-1 ].forward; |
|
m_cursorData.curvature = m_path[ m_segmentCount-1 ].curvature; |
|
m_cursorData.segmentPrior = &m_path[ m_segmentCount-1 ]; |
|
} |
|
else |
|
{ |
|
// along path |
|
float lengthSoFar = 0.0f; |
|
const Segment *segment = &m_path[0]; |
|
|
|
const Segment *next = NextSegment( segment ); |
|
|
|
while( next ) |
|
{ |
|
float length = segment->length; |
|
|
|
if ( lengthSoFar + length >= m_cursorPos ) |
|
{ |
|
// desired point is on this segment of the path |
|
float overlap = m_cursorPos - lengthSoFar; |
|
float t = 1.0f; // 0-length segments are assumed to be complete, to avoid NaNs |
|
if ( length > 0.0f ) |
|
{ |
|
t = overlap / length; |
|
} |
|
|
|
// interpolate data at this point along the path |
|
m_cursorData.pos = segment->pos + t * ( next->pos - segment->pos ); |
|
m_cursorData.forward = segment->forward + t * ( next->forward - segment->forward ); |
|
m_cursorData.segmentPrior = segment; |
|
|
|
// curvature fades to zero along midpoint of long straight segments |
|
// and is influenced as it nears ends of segment |
|
if ( overlap < NextBotPathSegmentInfluenceRadius.GetFloat() ) |
|
{ |
|
if ( length - overlap < NextBotPathSegmentInfluenceRadius.GetFloat() ) |
|
{ |
|
// near start and end - interpolate |
|
float startCurvature = segment->curvature * ( 1.0f - ( overlap / NextBotPathSegmentInfluenceRadius.GetFloat() ) ); |
|
float endCurvature = next->curvature * ( 1.0f - ( ( length - overlap ) / NextBotPathSegmentInfluenceRadius.GetFloat() ) ); |
|
|
|
m_cursorData.curvature = ( startCurvature + endCurvature ) / 2.0f; |
|
} |
|
else |
|
{ |
|
// near start only |
|
m_cursorData.curvature = segment->curvature * ( 1.0f - ( overlap / NextBotPathSegmentInfluenceRadius.GetFloat() ) ); |
|
} |
|
} |
|
else if ( length - overlap < NextBotPathSegmentInfluenceRadius.GetFloat() ) |
|
{ |
|
// near end only |
|
m_cursorData.curvature = next->curvature * ( 1.0f - ( ( length - overlap ) / NextBotPathSegmentInfluenceRadius.GetFloat() ) ); |
|
} |
|
|
|
|
|
break; |
|
} |
|
|
|
lengthSoFar += length; |
|
|
|
segment = next; |
|
next = NextSegment( next ); |
|
} |
|
} |
|
|
|
// data is up to date |
|
m_isCursorDataDirty = false; |
|
} |
|
} |
|
else |
|
{ |
|
// path is not valid |
|
m_cursorData.pos = vec3_origin; |
|
m_cursorData.forward = Vector( 1.0f, 0, 0 ); |
|
m_cursorData.curvature = 0.0f; |
|
m_cursorData.segmentPrior = NULL; |
|
} |
|
|
|
return m_cursorData; |
|
} |
|
|
|
|
|
//-------------------------------------------------------------------------------------------------------------- |
|
/** |
|
* Determine exactly where the path goes between the given two areas |
|
* on the path. Return this point in 'crossPos'. |
|
*/ |
|
void Path::ComputeAreaCrossing( INextBot *bot, const CNavArea *from, const Vector &fromPos, const CNavArea *to, NavDirType dir, Vector *crossPos ) const |
|
{ |
|
from->ComputeClosestPointInPortal( to, dir, fromPos, crossPos ); |
|
|
|
// move goal position into the goal area a bit to avoid running directly along the edge of an area against a wall, etc |
|
// don't do this unless area is against a wall - and what if our hull is wider than the area? |
|
// AddDirectionVector( crossPos, dir, bot->GetBodyInterface()->GetHullWidth()/2.0f ); |
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|