//-----------------------------------------------------------------------------
// Purpose: Adds addRoute to the end of oldRoute
//-----------------------------------------------------------------------------
void AddWaypointLists(AI_Waypoint_t *oldRoute, AI_Waypoint_t *addRoute)
{
// Add to the end of the route
AI_Waypoint_t *waypoint = oldRoute;
while (waypoint->GetNext())
{
waypoint = waypoint->GetNext();
}
waypoint->ModifyFlags( bits_WP_TO_GOAL, false );
// Check for duplication, but copy the type
if (waypoint->iNodeID != NO_NODE &&
waypoint->iNodeID == addRoute->iNodeID )
{
// waypoint->iWPType = addRoute->iWPType; <<TODO>> found case where this was bad
AI_Waypoint_t *pNext = addRoute->GetNext();
delete addRoute;
waypoint->SetNext(pNext);
}
else
{
waypoint->SetNext(addRoute);
}
while (waypoint->GetNext())
{
waypoint = waypoint->GetNext();
}
waypoint->ModifyFlags( bits_WP_TO_GOAL, true );
}
void CASW_Alien_Jumper::LockJumpNode( void )
{
if ( HasSpawnFlags( SF_ANTLION_USE_GROUNDCHECKS ) == false )
return;
if ( GetNavigator()->GetPath() == NULL )
return;
if ( asw_test_new_alien_jump.GetBool() == false )
return;
AI_Waypoint_t *pWaypoint = GetNavigator()->GetPath()->GetCurWaypoint();
while ( pWaypoint )
{
AI_Waypoint_t *pNextWaypoint = pWaypoint->GetNext();
if ( pNextWaypoint && pNextWaypoint->NavType() == NAV_JUMP && pWaypoint->iNodeID != NO_NODE )
{
CAI_Node *pNode = GetNavigator()->GetNetwork()->GetNode( pWaypoint->iNodeID );
if ( pNode )
{
//NDebugOverlay::Box( pNode->GetOrigin(), Vector( -16, -16, -16 ), Vector( 16, 16, 16 ), 255, 0, 0, 0, 2 );
pNode->Lock( 0.5f );
break;
}
}
else
{
pWaypoint = pWaypoint->GetNext();
}
}
}
AI_Waypoint_t *CAI_WaypointList::GetLast()
{
AI_Waypoint_t *p = GetFirst();
if (!p)
return NULL;
while ( p->GetNext() )
p = p->GetNext();
return p;
}
AI_Waypoint_t * AI_Waypoint_t::GetLast()
{
Assert( !pNext || pNext->pPrev == this );
AI_Waypoint_t *pCurr = this;
while (pCurr->GetNext())
{
pCurr = pCurr->GetNext();
}
return pCurr;
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CNPC_VehicleDriver::CalculatePostPoints( void )
{
m_vecPostPoint = m_vecDesiredPosition;
m_vecPostPostPoint = m_vecDesiredPosition;
// If we have a waypoint beyond our current, use it instead.
if ( !GetNavigator()->CurWaypointIsGoal() )
{
AI_Waypoint_t *pCurWaypoint = GetNavigator()->GetPath()->GetCurWaypoint();
m_vecPostPoint = pCurWaypoint->GetNext()->GetPos();
if ( pCurWaypoint->GetNext()->GetNext() )
{
m_vecPostPostPoint = pCurWaypoint->GetNext()->GetNext()->GetPos();
}
else
{
m_vecPostPostPoint = m_vecPostPoint;
}
}
}
void AssertRouteValid( AI_Waypoint_t* route )
{
// Check that the goal wasn't just clobbered
if (route)
{
AI_Waypoint_t* waypoint = route;
while (waypoint)
{
#ifdef _GOALDEBUG
if (!waypoint->GetNext() && !(waypoint->Flags() & (bits_WP_TO_GOAL|bits_WP_TO_PATHCORNER)))
{
DevMsg( "!!ERROR!! Final waypoint is not a goal!\n");
}
#endif
waypoint->AssertValid();
waypoint = waypoint->GetNext();
}
}
}
int CASW_Marine_Hint_Manager::FindHints(const Vector &position, CASW_Marine *pLeader, const float flMinDistance, const float flMaxDistance, CUtlVector<HintData_t *> &result)
{
VPROF_BUDGET("CASW_Marine_Hint_Manager::FindHints", "SquadFormation");
Assert(pLeader);
const float flMinDistSqr = Square(flMinDistance);
const float flMaxDistSqr = Square(flMaxDistance);
int nCount = m_Hints.Count();
for ( int i = 0; i < nCount; i++ )
{
HintData_t *pHint = m_Hints[i];
float flDistSqr = position.DistToSqr(pHint->m_vecPosition);
if (flDistSqr < flMinDistSqr || flDistSqr > flMaxDistSqr)
{
continue;
}
AI_Waypoint_t *pPath = pLeader->GetPathfinder()->BuildRoute(position, pHint->m_vecPosition, NULL, 0, NAV_GROUND, bits_BUILD_NO_LOCAL_NAV);
if (!pPath)
{
continue;
}
pHint->m_flDistance = position.DistTo(pPath->GetPos());
AI_Waypoint_t *pCur = pPath;
while (pCur->GetNext())
{
AI_Waypoint_t *pNext = pCur->GetNext();
pHint->m_flDistance += pCur->GetPos().DistTo(pNext->GetPos());
pCur = pNext;
}
DeleteAll(pPath);
if (pHint->m_flDistance > flMaxDistance)
{
continue;
}
result.AddToTail( pHint );
}
return result.Count();
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
bool CAI_LeadBehavior::GetClosestPointOnRoute( const Vector &targetPos, Vector *pVecClosestPoint )
{
AI_Waypoint_t *waypoint = GetOuter()->GetNavigator()->GetPath()->GetCurWaypoint();
AI_Waypoint_t *builtwaypoints = NULL;
if ( !waypoint )
{
// We arrive here twice when lead behaviour starts:
// - When the lead behaviour is first enabled. We have no schedule. We want to know if the player is ahead of us.
// - A frame later when we've chosen to lead the player, but we still haven't built our route. We know that the
// the player isn't lagging, so it's safe to go ahead and simply say he's ahead of us. This avoids building
// the temp route twice.
if ( IsCurSchedule( SCHED_LEAD_PLAYER, false ) )
return true;
// Build a temp route to the gold and use that
builtwaypoints = GetOuter()->GetPathfinder()->BuildRoute( GetOuter()->GetAbsOrigin(), m_goal, NULL, GetOuter()->GetDefaultNavGoalTolerance(), GetOuter()->GetNavType(), true );
if ( !builtwaypoints )
return false;
GetOuter()->GetPathfinder()->UnlockRouteNodes( builtwaypoints );
waypoint = builtwaypoints;
}
// Find the nearest node to the target (going forward)
float flNearestDist2D = 999999999;
float flNearestDist = 999999999;
float flPathDist, flPathDist2D;
Vector vecNearestPoint(0, 0, 0);
Vector vecPrevPos = GetOuter()->GetAbsOrigin();
for ( ; (waypoint != NULL) ; waypoint = waypoint->GetNext() )
{
// Find the closest point on the line segment on the path
Vector vecClosest;
CalcClosestPointOnLineSegment( targetPos, vecPrevPos, waypoint->GetPos(), vecClosest );
/*
if ( builtwaypoints )
{
NDebugOverlay::Line( vecPrevPos, waypoint->GetPos(), 0,0,255,true, 10.0 );
}
*/
vecPrevPos = waypoint->GetPos();
// Find the distance between this test point and our goal point
flPathDist2D = vecClosest.AsVector2D().DistToSqr( targetPos.AsVector2D() );
if ( flPathDist2D > flNearestDist2D )
continue;
flPathDist = vecClosest.z - targetPos.z;
flPathDist *= flPathDist;
flPathDist += flPathDist2D;
if (( flPathDist2D == flNearestDist2D ) && ( flPathDist >= flNearestDist ))
continue;
flNearestDist2D = flPathDist2D;
flNearestDist = flPathDist;
vecNearestPoint = vecClosest;
}
if ( builtwaypoints )
{
//NDebugOverlay::Line( vecNearestPoint, targetPos, 0,255,0,true, 10.0 );
DeleteAll( builtwaypoints );
}
*pVecClosestPoint = vecNearestPoint;
return true;
}
void CAI_BlendedMotor::BuildTurnScript( const AILocalMoveGoal_t &move )
{
int i;
AI_Movementscript_t script;
script.Init();
// current location
script.vecLocation = GetAbsOrigin();
script.flYaw = GetAbsAngles().y;
m_scriptTurn.AddToTail( script );
//-------------------------
// insert default turn parameters, try to turn 80% to goal at all corners before getting there
int prev = 0;
for (i = 0; i < m_scriptMove.Count(); i++)
{
AI_Waypoint_t *pCurWaypoint = m_scriptMove[i].pWaypoint;
if (pCurWaypoint)
{
script.Init();
script.vecLocation = pCurWaypoint->vecLocation;
script.pWaypoint = pCurWaypoint;
script.flElapsedTime = m_scriptMove[i].flElapsedTime;
m_scriptTurn[prev].flTime = script.flElapsedTime - m_scriptTurn[prev].flElapsedTime;
if (pCurWaypoint->GetNext())
{
Vector d1 = pCurWaypoint->GetNext()->vecLocation - script.vecLocation;
Vector d2 = script.vecLocation - m_scriptTurn[prev].vecLocation;
d1.z = 0;
VectorNormalize( d1 );
d2.z = 0;
VectorNormalize( d2 );
float y1 = UTIL_VecToYaw( d1 );
float y2 = UTIL_VecToYaw( d2 );
float deltaYaw = fabs( UTIL_AngleDiff( y1, y2 ) );
if (deltaYaw > 0.1)
{
// turn to 80% of goal
script.flYaw = UTIL_ApproachAngle( y1, y2, deltaYaw * 0.8 );
m_scriptTurn.AddToTail( script );
// DevMsg("turn waypoint %.1f %.1f %.1f\n", script.vecLocation.x, script.vecLocation.y, script.vecLocation.z );
prev++;
}
}
else
{
Vector vecDir = GetNavigator()->GetArrivalDirection();
script.flYaw = UTIL_VecToYaw( vecDir );
m_scriptTurn.AddToTail( script );
// DevMsg("turn waypoint %.1f %.1f %.1f\n", script.vecLocation.x, script.vecLocation.y, script.vecLocation.z );
prev++;
}
}
}
// propagate ending facing back over any nearby nodes
// FIXME: this needs to minimize total turning, not just local/end turning.
// depending on waypoint spacing, complexity, it may turn the wrong way!
for (i = m_scriptTurn.Count()-1; i > 1; i--)
{
float deltaYaw = UTIL_AngleDiff( m_scriptTurn[i-1].flYaw, m_scriptTurn[i].flYaw );
float maxYaw = YAWSPEED * m_scriptTurn[i-1].flTime;
if (fabs(deltaYaw) > maxYaw)
{
m_scriptTurn[i-1].flYaw = UTIL_ApproachAngle( m_scriptTurn[i-1].flYaw, m_scriptTurn[i].flYaw, maxYaw );
}
}
for (i = 0; i < m_scriptTurn.Count() - 1; )
{
i = i + BuildTurnScript( i, i + 1 ) + 1;
}
//-------------------------
}
void CAI_BlendedMotor::BuildVelocityScript( const AILocalMoveGoal_t &move )
{
int i;
float a;
float idealVelocity = GetIdealSpeed();
if (idealVelocity == 0)
{
idealVelocity = 50;
}
float idealAccel = GetIdealAccel();
if (idealAccel == 0)
{
idealAccel = 100;
}
AI_Movementscript_t script;
// set current location as start of script
script.vecLocation = GetAbsOrigin();
script.flMaxVelocity = GetCurSpeed();
m_scriptMove.AddToTail( script );
//-------------------------
extern ConVar *npc_height_adjust;
if (npc_height_adjust->GetBool() && move.bHasTraced && move.directTrace.flTotalDist != move.thinkTrace.flTotalDist)
{
float flDist = (move.directTrace.vEndPosition - m_scriptMove[0].vecLocation).Length2D();
float flHeight = move.directTrace.vEndPosition.z - m_scriptMove[0].vecLocation.z;
float flDelta;
if (flDist > 0)
{
flDelta = flHeight / flDist;
}
else
{
flDelta = 0;
}
m_flPredictiveSpeedAdjust = 1.1 - fabs( flDelta );
m_flPredictiveSpeedAdjust = clamp( m_flPredictiveSpeedAdjust, (flHeight > 0.0) ? 0.5 : 0.8, 1.0 );
/*
if ((GetOuter()->m_debugOverlays & OVERLAY_NPC_SELECTED_BIT))
{
Msg("m_flPredictiveSpeedAdjust %.3f %.1f %.1f\n", m_flPredictiveSpeedAdjust, flHeight, flDist );
NDebugOverlay::Box( move.directTrace.vEndPosition, Vector( -2, -2, -2 ), Vector( 2, 2, 2 ), 0,255,255, 0, 0.12 );
}
*/
}
if (npc_height_adjust->GetBool())
{
float flDist = (move.thinkTrace.vEndPosition - m_vecPrevOrigin2).Length2D();
float flHeight = move.thinkTrace.vEndPosition.z - m_vecPrevOrigin2.z;
float flDelta;
if (flDist > 0)
{
flDelta = flHeight / flDist;
}
else
{
flDelta = 0;
}
float newSpeedAdjust = 1.1 - fabs( flDelta );
newSpeedAdjust = clamp( newSpeedAdjust, (flHeight > 0.0) ? 0.5 : 0.8, 1.0 );
// debounce speed adjust
if (newSpeedAdjust < m_flReactiveSpeedAdjust)
{
m_flReactiveSpeedAdjust = m_flReactiveSpeedAdjust * 0.2 + newSpeedAdjust * 0.8;
}
else
{
m_flReactiveSpeedAdjust = m_flReactiveSpeedAdjust * 0.5 + newSpeedAdjust * 0.5;
}
// filter through origins
m_vecPrevOrigin2 = m_vecPrevOrigin1;
m_vecPrevOrigin1 = GetAbsOrigin();
/*
if ((GetOuter()->m_debugOverlays & OVERLAY_NPC_SELECTED_BIT))
{
NDebugOverlay::Box( m_vecPrevOrigin2, Vector( -2, -2, -2 ), Vector( 2, 2, 2 ), 255,0,255, 0, 0.12 );
NDebugOverlay::Box( move.thinkTrace.vEndPosition, Vector( -2, -2, -2 ), Vector( 2, 2, 2 ), 255,0,255, 0, 0.12 );
Msg("m_flReactiveSpeedAdjust %.3f %.1f %.1f\n", m_flReactiveSpeedAdjust, flHeight, flDist );
}
*/
}
idealVelocity = idealVelocity * min( m_flReactiveSpeedAdjust, m_flPredictiveSpeedAdjust );
//-------------------------
bool bAddedExpected = false;
// add all waypoint locations and velocities
AI_Waypoint_t *pCurWaypoint = GetNavigator()->GetPath()->GetCurWaypoint();
// there has to be at least one waypoint
Assert( pCurWaypoint );
while (pCurWaypoint && (pCurWaypoint->NavType() == NAV_GROUND || pCurWaypoint->NavType() == NAV_FLY) /*&& flTotalDist / idealVelocity < 3.0*/) // limit lookahead to 3 seconds
{
script.Init();
AI_Waypoint_t *pNext = pCurWaypoint->GetNext();
if (ai_path_adjust_speed_on_immediate_turns->GetBool() && !bAddedExpected)
{
// hack in next expected immediate location for move
script.vecLocation = GetAbsOrigin() + move.dir * move.curExpectedDist;
bAddedExpected = true;
pNext = pCurWaypoint;
}
else
{
script.vecLocation = pCurWaypoint->vecLocation;
script.pWaypoint = pCurWaypoint;
}
//DevMsg("waypoint %.1f %.1f %.1f\n", script.vecLocation.x, script.vecLocation.y, script.vecLocation.z );
if (pNext)
{
switch( pNext->NavType())
{
case NAV_GROUND:
case NAV_FLY:
{
Vector d1 = pNext->vecLocation - script.vecLocation;
Vector d2 = script.vecLocation - m_scriptMove[m_scriptMove.Count()-1].vecLocation;
// remove very short, non terminal ground links
// FIXME: is this safe? Maybe just check for co-located ground points?
if (d1.Length2D() < 1.0)
{
/*
if (m_scriptMove.Count() > 1)
{
int i = m_scriptMove.Count() - 1;
m_scriptMove[i].vecLocation = pCurWaypoint->vecLocation;
m_scriptMove[i].pWaypoint = pCurWaypoint;
}
*/
pCurWaypoint = pNext;
continue;
}
d1.z = 0;
VectorNormalize( d1 );
d2.z = 0;
VectorNormalize( d2 );
// figure velocity
float dot = (DotProduct( d1, d2 ) + 0.2);
if (dot > 0)
{
dot = clamp( dot, 0.0f, 1.0f );
script.flMaxVelocity = idealVelocity * dot;
}
else
{
script.flMaxVelocity = 0;
}
}
break;
case NAV_JUMP:
// FIXME: information about what the jump should look like isn't stored in the waypoints
// this'll need to call
// GetMoveProbe()->MoveLimit( NAV_JUMP, GetLocalOrigin(), GetPath()->CurWaypointPos(), MASK_NPCSOLID, GetNavTargetEntity(), &moveTrace );
// to get how far/fast the jump will be, but this is also stateless, so it'd call it per frame.
// So far it's not clear that the moveprobe doesn't also call this.....
{
float minJumpHeight = 0;
float maxHorzVel = max( GetCurSpeed(), 100 );
float gravity = sv_gravity->GetFloat() * GetOuter()->GetGravity();
Vector vecApex;
Vector rawJumpVel = GetMoveProbe()->CalcJumpLaunchVelocity(script.vecLocation, pNext->vecLocation, gravity, &minJumpHeight, maxHorzVel, &vecApex );
script.flMaxVelocity = rawJumpVel.Length2D();
// Msg("%.1f\n", script.flMaxVelocity );
}
break;
case NAV_CLIMB:
{
/*
CAI_Node *pClimbNode = GetNavigator()->GetNetwork()->GetNode(pNext->iNodeID);
check: pClimbNode->m_eNodeInfo
bits_NODE_CLIMB_BOTTOM,
bits_NODE_CLIMB_ON,
bits_NODE_CLIMB_OFF_FORWARD,
bits_NODE_CLIMB_OFF_LEFT,
bits_NODE_CLIMB_OFF_RIGHT
*/
script.flMaxVelocity = 0;
}
break;
/*
case NAV_FLY:
// FIXME: can there be a NAV_GROUND -> NAV_FLY transition?
script.flMaxVelocity = 0;
break;
*/
default:
break;
}
}
else
{
script.flMaxVelocity = GetNavigator()->GetArrivalSpeed();
// Assert( script.flMaxVelocity == 0 );
}
m_scriptMove.AddToTail( script );
pCurWaypoint = pNext;
}
//-------------------------
// update distances
float flTotalDist = 0;
for (i = 0; i < m_scriptMove.Count() - 1; i++ )
{
flTotalDist += m_scriptMove[i].flDist = (m_scriptMove[i+1].vecLocation - m_scriptMove[i].vecLocation).Length2D();
}
//-------------------------
if ( !m_bDeceleratingToGoal && m_scriptMove.Count() && flTotalDist > 0 )
{
float flNeededAccel = DeltaV( m_scriptMove[0].flMaxVelocity, m_scriptMove[m_scriptMove.Count() - 1].flMaxVelocity, flTotalDist );
m_bDeceleratingToGoal = (flNeededAccel < -idealAccel);
//Assert( flNeededAccel != idealAccel);
}
//-------------------------
// insert slowdown points due to blocking
if (ai_path_insert_pause_at_obstruction->GetBool() && move.directTrace.pObstruction)
{
float distToObstruction = (move.directTrace.vEndPosition - m_scriptMove[0].vecLocation).Length2D();
// HACK move obstruction out "stepsize" to account for it being based on stand position and not a trace
distToObstruction = distToObstruction + 16;
InsertSlowdown( distToObstruction, idealAccel, false );
}
if (ai_path_insert_pause_at_est_end->GetBool() && GetNavigator()->GetArrivalDistance() > 0.0)
{
InsertSlowdown( flTotalDist - GetNavigator()->GetArrivalDistance(), idealAccel, true );
}
// calc initial velocity based on immediate direction changes
if ( ai_path_adjust_speed_on_immediate_turns->GetBool() && m_scriptMove.Count() > 1)
{
/*
if ((GetOuter()->m_debugOverlays & OVERLAY_NPC_SELECTED_BIT))
{
Vector tmp = m_scriptMove[1].vecLocation - m_scriptMove[0].vecLocation;
VectorNormalize( tmp );
NDebugOverlay::Line( m_scriptMove[0].vecLocation + Vector( 0, 0, 10 ), m_scriptMove[0].vecLocation + tmp * 32 + Vector( 0, 0, 10 ), 255,255,255, true, 0.1 );
NDebugOverlay::Line( m_scriptMove[0].vecLocation + Vector( 0, 0, 10 ), m_scriptMove[1].vecLocation + Vector( 0, 0, 10 ), 255,0,0, true, 0.1 );
tmp = GetCurVel();
VectorNormalize( tmp );
NDebugOverlay::Line( m_scriptMove[0].vecLocation + Vector( 0, 0, 10 ), m_scriptMove[0].vecLocation + tmp * 32 + Vector( 0, 0, 10 ), 0,0,255, true, 0.1 );
}
*/
Vector d1 = m_scriptMove[1].vecLocation - m_scriptMove[0].vecLocation;
d1.z = 0;
VectorNormalize( d1 );
Vector d2 = GetCurVel();
d2.z = 0;
VectorNormalize( d2 );
float dot = (DotProduct( d1, d2 ) + MIN_STEER_DOT);
dot = clamp( dot, 0.0f, 1.0f );
m_scriptMove[0].flMaxVelocity = m_scriptMove[0].flMaxVelocity * dot;
}
// clamp forward velocities
for (i = 0; i < m_scriptMove.Count() - 1; i++ )
{
// find needed acceleration
float dv = m_scriptMove[i+1].flMaxVelocity - m_scriptMove[i].flMaxVelocity;
if (dv > 0.0)
{
// find time, distance to accel to next max vel
float t1 = dv / idealAccel;
float d1 = m_scriptMove[i].flMaxVelocity * t1 + 0.5 * (idealAccel) * t1 * t1;
// is there enough distance
if (d1 > m_scriptMove[i].flDist)
{
float r1, r2;
// clamp the next velocity to the possible accel in the given distance
if (SolveQuadratic( 0.5 * idealAccel, m_scriptMove[i].flMaxVelocity, -m_scriptMove[i].flDist, r1, r2 ))
{
m_scriptMove[i+1].flMaxVelocity = m_scriptMove[i].flMaxVelocity + idealAccel * r1;
}
}
}
}
// clamp decel velocities
for (i = m_scriptMove.Count() - 1; i > 0; i-- )
{
// find needed deceleration
float dv = m_scriptMove[i].flMaxVelocity - m_scriptMove[i-1].flMaxVelocity;
if (dv < 0.0)
{
// find time, distance to decal to next max vel
float t1 = -dv / idealAccel;
float d1 = m_scriptMove[i].flMaxVelocity * t1 + 0.5 * (idealAccel) * t1 * t1;
// is there enough distance
if (d1 > m_scriptMove[i-1].flDist)
{
float r1, r2;
// clamp the next velocity to the possible decal in the given distance
if (SolveQuadratic( 0.5 * idealAccel, m_scriptMove[i].flMaxVelocity, -m_scriptMove[i-1].flDist, r1, r2 ))
{
m_scriptMove[i-1].flMaxVelocity = m_scriptMove[i].flMaxVelocity + idealAccel * r1;
}
}
}
}
/*
for (i = 0; i < m_scriptMove.Count(); i++)
{
NDebugOverlay::Text( m_scriptMove[i].vecLocation, (const char *)CFmtStr( "%.2f ", m_scriptMove[i].flMaxVelocity ), false, 0.1 );
// DevMsg("%.2f ", m_scriptMove[i].flMaxVelocity );
}
// DevMsg("\n");
*/
// insert intermediate ideal velocities
for (i = 0; i < m_scriptMove.Count() - 1;)
{
// accel to ideal
float t1 = (idealVelocity - m_scriptMove[i].flMaxVelocity) / idealAccel;
float d1 = m_scriptMove[i].flMaxVelocity * t1 + 0.5 * (idealAccel) * t1 * t1;
// decel from ideal
float t2 = (idealVelocity - m_scriptMove[i+1].flMaxVelocity) / idealAccel;
float d2 = m_scriptMove[i+1].flMaxVelocity * t2 + 0.5 * (idealAccel) * t2 * t2;
m_scriptMove[i].flDist = (m_scriptMove[i+1].vecLocation - m_scriptMove[i].vecLocation).Length2D();
// is it possible to accel and decal to idealVelocity between next two nodes
if (d1 + d2 < m_scriptMove[i].flDist)
{
Vector start = m_scriptMove[i].vecLocation;
Vector end = m_scriptMove[i+1].vecLocation;
float dist = m_scriptMove[i].flDist;
// insert the two points needed to end accel and start decel
if (d1 > 1.0 && t1 > 0.1)
{
a = d1 / dist;
script.Init();
script.vecLocation = end * a + start * (1 - a);
script.flMaxVelocity = idealVelocity;
m_scriptMove.InsertAfter( i, script );
i++;
}
if (dist - d2 > 1.0 && t2 > 0.1)
{
// DevMsg("%.2f : ", a );
a = (dist - d2) / dist;
script.Init();
script.vecLocation = end * a + start * (1 - a);
script.flMaxVelocity = idealVelocity;
m_scriptMove.InsertAfter( i, script );
i++;
}
i++;
}
else
{
// check to see if the amount of change needed to reach target is less than the ideal acceleration
float flNeededAccel = fabs( DeltaV( m_scriptMove[i].flMaxVelocity, m_scriptMove[i+1].flMaxVelocity, m_scriptMove[i].flDist ) );
if (flNeededAccel < idealAccel)
{
// if so, they it's possible to get a bit towards the ideal velocity
float v1 = m_scriptMove[i].flMaxVelocity;
float v2 = m_scriptMove[i+1].flMaxVelocity;
float dist = m_scriptMove[i].flDist;
// based on solving:
// v1+A*t1-v2-A*t2=0
// v1*t1+0.5*A*t1*t1+v2*t2+0.5*A*t2*t2-D=0
float tmp = idealAccel*dist+0.5*v1*v1+0.5*v2*v2;
Assert( tmp >= 0 );
t1 = (-v1+sqrt( tmp )) / idealAccel;
t2 = (v1+idealAccel*t1-v2)/idealAccel;
// if this assert hits, write down the v1, v2, dist, and idealAccel numbers and send them to me (Ken).
// go ahead the comment it out, it's safe, but I'd like to know a test case where it's happening
//Assert( t1 > 0 && t2 > 0 );
// check to make sure it's really worth it
if (t1 > 0.0 && t2 > 0.0)
{
d1 = v1 * t1 + 0.5 * idealAccel * t1 * t1;
/*
d2 = v2 * t2 + 0.5 * idealAccel * t2 * t2;
Assert( fabs( d1 + d2 - dist ) < 0.001 );
*/
float a = d1 / m_scriptMove[i].flDist;
script.Init();
script.vecLocation = m_scriptMove[i+1].vecLocation * a + m_scriptMove[i].vecLocation * (1 - a);
script.flMaxVelocity = m_scriptMove[i].flMaxVelocity + idealAccel * t1;
if (script.flMaxVelocity < idealVelocity)
{
// DevMsg("insert %.2f %.2f %.2f\n", m_scriptMove[i].flMaxVelocity, script.flMaxVelocity, m_scriptMove[i+1].flMaxVelocity );
m_scriptMove.InsertAfter( i, script );
i += 1;
}
}
}
i += 1;
}
}
// clamp min velocities
for (i = 0; i < m_scriptMove.Count(); i++)
{
m_scriptMove[i].flMaxVelocity = max( m_scriptMove[i].flMaxVelocity, MIN_VELOCITY );
}
// rebuild fields
m_scriptMove[0].flElapsedTime = 0;
for (i = 0; i < m_scriptMove.Count() - 1; )
{
m_scriptMove[i].flDist = (m_scriptMove[i+1].vecLocation - m_scriptMove[i].vecLocation).Length2D();
if (m_scriptMove[i].flMaxVelocity == 0 && m_scriptMove[i+1].flMaxVelocity == 0)
{
// force a minimum velocity
//CE_assert
//Assert( 0 );
m_scriptMove[i+1].flMaxVelocity = 1.0;
}
float t = m_scriptMove[i].flDist / (0.5 * (m_scriptMove[i].flMaxVelocity + m_scriptMove[i+1].flMaxVelocity));
m_scriptMove[i].flTime = t;
/*
if (m_scriptMove[i].flDist < 0.01)
{
// Assert( m_scriptMove[i+1].pWaypoint == NULL );
m_scriptMove.Remove( i + 1 );
continue;
}
*/
m_scriptMove[i+1].flElapsedTime = m_scriptMove[i].flElapsedTime + m_scriptMove[i].flTime;
i++;
}
/*
for (i = 0; i < m_scriptMove.Count(); i++)
{
DevMsg("(%.2f : %.2f : %.2f)", m_scriptMove[i].flMaxVelocity, m_scriptMove[i].flDist, m_scriptMove[i].flTime );
// DevMsg("(%.2f:%.2f)", m_scriptMove[i].flTime, m_scriptMove[i].flElapsedTime );
}
DevMsg("\n");
*/
}