diff options
Diffstat (limited to 'game/server/NextBot/Path/NextBotPath.cpp')
| -rw-r--r-- | game/server/NextBot/Path/NextBotPath.cpp | 1094 |
1 files changed, 1094 insertions, 0 deletions
diff --git a/game/server/NextBot/Path/NextBotPath.cpp b/game/server/NextBot/Path/NextBotPath.cpp new file mode 100644 index 0000000..4514887 --- /dev/null +++ b/game/server/NextBot/Path/NextBotPath.cpp @@ -0,0 +1,1094 @@ +// 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 ); +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + |