//-----------------------------------------------------------------------------
// Purpose:
//
//
// Output :
//-----------------------------------------------------------------------------
void CNPC_RollerDozer::RunTask( const Task_t *pTask )
{
switch( pTask->iTask )
{
case TASK_ROLLERDOZER_CLEAR_DEBRIS:
if( gpGlobals->curtime > m_flWaitFinished )
{
m_hDebris = NULL;
m_flTimeDebrisSearch = gpGlobals->curtime;
TaskComplete();
}
else if( m_hDebris != NULL )
{
float yaw = UTIL_VecToYaw( m_hDebris->GetLocalOrigin() - GetLocalOrigin() );
Vector vecRight, vecForward;
AngleVectors( QAngle( 0, yaw, 0 ), &vecForward, &vecRight, NULL );
//Stop pushing if I'm going to push this object sideways or back towards the center of the cleanup area.
Vector vecCleanupDir = m_hDebris->GetLocalOrigin() - m_vecCleanupPoint;
VectorNormalize( vecCleanupDir );
if( DotProduct( vecForward, vecCleanupDir ) < -0.5 )
{
// HACKHACK !!!HACKHACK - right now forcing an unstick. Do this better (sjb)
// Clear the debris, suspend the search for debris, trick base class into unsticking me.
m_hDebris = NULL;
m_flTimeDebrisSearch = gpGlobals->curtime + 4;
m_iFail = 10;
TaskFail("Pushing Wrong Way");
}
m_RollerController.m_vecAngular = WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecRight, ROLLERDOZER_FORWARD_SPEED * 2 );
}
else
{
TaskFail("No debris!!");
}
break;
default:
BaseClass::RunTask( pTask );
break;
}
}
IMotionEvent::simresult_e CGravControllerPoint::Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular )
{
Vector vel;
AngularImpulse angVel;
float fracRemainingSimTime = 1.0;
if ( m_timeToArrive > 0 )
{
fracRemainingSimTime *= deltaTime / m_timeToArrive;
if ( fracRemainingSimTime > 1 )
{
fracRemainingSimTime = 1;
}
}
m_timeToArrive -= deltaTime;
if ( m_timeToArrive < 0 )
{
m_timeToArrive = 0;
}
float invDeltaTime = (1.0f / deltaTime);
Vector world;
pObject->LocalToWorld( &world, m_localPosition );
m_worldPosition = world;
pObject->GetVelocity( &vel, &angVel );
//pObject->GetVelocityAtPoint( world, &vel );
float damping = 1.0;
world += vel * deltaTime * damping;
Vector delta = (m_targetPosition - world) * fracRemainingSimTime * invDeltaTime;
Vector alignDir;
linear = vec3_origin;
angular = vec3_origin;
if ( m_align )
{
QAngle angles;
Vector origin;
Vector axis;
AngularImpulse torque;
pObject->GetShadowPosition( &origin, &angles );
// align local normal to target normal
VMatrix tmp = SetupMatrixOrgAngles( origin, angles );
Vector worldNormal = tmp.VMul3x3( m_localAlignNormal );
axis = CrossProduct( worldNormal, m_targetAlignNormal );
float trig = VectorNormalize(axis);
float alignRotation = RAD2DEG(asin(trig));
axis *= alignRotation;
if ( alignRotation < 10 )
{
float dot = DotProduct( worldNormal, m_targetAlignNormal );
// probably 180 degrees off
if ( dot < 0 )
{
if ( worldNormal.x < 0.5 )
{
axis.Init(10,0,0);
}
else
{
axis.Init(0,0,10);
}
alignRotation = 10;
}
}
// Solve for the rotation around the target normal (at the local align pos) that will
// move the grabbed spot to the destination.
Vector worldRotCenter = tmp.VMul4x3( m_localAlignPosition );
Vector rotSrc = world - worldRotCenter;
Vector rotDest = m_targetPosition - worldRotCenter;
// Get a basis in the plane perpendicular to m_targetAlignNormal
Vector srcN = rotSrc;
VectorNormalize( srcN );
Vector tangent = CrossProduct( srcN, m_targetAlignNormal );
float len = VectorNormalize( tangent );
// needs at least ~5 degrees, or forget rotation (0.08 ~= sin(5))
if ( len > 0.08 )
{
Vector binormal = CrossProduct( m_targetAlignNormal, tangent );
// Now project the src & dest positions into that plane
Vector planeSrc( DotProduct( rotSrc, tangent ), DotProduct( rotSrc, binormal ), 0 );
Vector planeDest( DotProduct( rotDest, tangent ), DotProduct( rotDest, binormal ), 0 );
float rotRadius = VectorNormalize( planeSrc );
float destRadius = VectorNormalize( planeDest );
if ( rotRadius > 0.1 )
{
if ( destRadius < rotRadius )
{
destRadius = rotRadius;
}
//float ratio = rotRadius / destRadius;
float angleSrc = atan2( planeSrc.y, planeSrc.x );
float angleDest = atan2( planeDest.y, planeDest.x );
float angleDiff = angleDest - angleSrc;
angleDiff = RAD2DEG(angleDiff);
axis += m_targetAlignNormal * angleDiff;
//world = m_targetPosition;// + rotDest * (1-ratio);
// NDebugOverlay::Line( worldRotCenter, worldRotCenter-m_targetAlignNormal*50, 255, 0, 0, false, 0.1 );
// NDebugOverlay::Line( worldRotCenter, worldRotCenter+tangent*50, 0, 255, 0, false, 0.1 );
// NDebugOverlay::Line( worldRotCenter, worldRotCenter+binormal*50, 0, 0, 255, false, 0.1 );
}
}
torque = WorldToLocalRotation( tmp, axis, 1 );
torque *= fracRemainingSimTime * invDeltaTime;
torque -= angVel * 1.0; // damping
for ( int i = 0; i < 3; i++ )
{
if ( torque[i] > 0 )
{
if ( torque[i] > m_maxAngularAcceleration[i] )
torque[i] = m_maxAngularAcceleration[i];
}
else
{
if ( torque[i] < -m_maxAngularAcceleration[i] )
torque[i] = -m_maxAngularAcceleration[i];
}
}
torque *= invDeltaTime;
angular += torque;
// Calculate an acceleration that pulls the object toward the constraint
// When you're out of alignment, don't pull very hard
float factor = fabsf(alignRotation);
if ( factor < 5 )
{
factor = clamp( factor, 0, 5 ) * (1/5);
alignDir = m_targetAlignPosition - worldRotCenter;
// Limit movement to the part along m_targetAlignNormal if worldRotCenter is on the backside of
// of the target plane (one inch epsilon)!
float planeForward = DotProduct( alignDir, m_targetAlignNormal );
if ( planeForward > 1 )
{
alignDir = m_targetAlignNormal * planeForward;
}
Vector accel = alignDir * invDeltaTime * fracRemainingSimTime * (1-factor) * 0.20 * invDeltaTime;
float mag = accel.Length();
if ( mag > m_maxAcceleration )
{
accel *= (m_maxAcceleration/mag);
}
linear += accel;
}
linear -= vel*damping*invDeltaTime;
// UNDONE: Factor in the change in worldRotCenter due to applied torque!
}
else
{
// clamp future velocity to max speed
Vector nextVel = delta + vel;
float nextSpeed = nextVel.Length();
if ( nextSpeed > m_maxVel )
{
nextVel *= (m_maxVel / nextSpeed);
delta = nextVel - vel;
}
delta *= invDeltaTime;
float linearAccel = delta.Length();
if ( linearAccel > m_maxAcceleration )
{
delta *= m_maxAcceleration / linearAccel;
}
Vector accel;
AngularImpulse angAccel;
pObject->CalculateForceOffset( delta, world, &accel, &angAccel );
linear += accel;
angular += angAccel;
}
return SIM_GLOBAL_ACCELERATION;
}
//---------------------------------------------------------
//---------------------------------------------------------
void CNPC_Roller::RunTask( const Task_t *pTask )
{
switch( pTask->iTask )
{
case TASK_ROLLER_UNSTICK:
{
float yaw = UTIL_VecToYaw( m_vecUnstickDirection );
Vector vecRight;
AngleVectors( QAngle( 0, yaw, 0 ), NULL, &vecRight, NULL );
m_RollerController.m_vecAngular = WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecRight, m_flForwardSpeed );
}
if( gpGlobals->curtime > m_flWaitFinished )
{
TaskComplete();
}
break;
case TASK_ROLLER_WAIT_FOR_PHYSICS:
{
Vector vecVelocity;
VPhysicsGetObject()->GetVelocity( &vecVelocity, NULL );
if( VPhysicsGetObject()->IsAsleep() )
{
TaskComplete();
}
}
break;
case TASK_RUN_PATH:
case TASK_WALK_PATH:
// Start turning early
if( (GetLocalOrigin() - GetNavigator()->GetCurWaypointPos() ).Length() <= 64 )
{
if( GetNavigator()->CurWaypointIsGoal() )
{
// Hit the brakes a bit.
float yaw = UTIL_VecToYaw( GetNavigator()->GetCurWaypointPos() - GetLocalOrigin() );
Vector vecRight;
AngleVectors( QAngle( 0, yaw, 0 ), NULL, &vecRight, NULL );
m_RollerController.m_vecAngular += WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecRight, -m_flForwardSpeed * 5 );
TaskComplete();
return;
}
GetNavigator()->AdvancePath();
}
{
float yaw = UTIL_VecToYaw( GetNavigator()->GetCurWaypointPos() - GetLocalOrigin() );
Vector vecRight;
Vector vecToPath; // points at the path
AngleVectors( QAngle( 0, yaw, 0 ), &vecToPath, &vecRight, NULL );
// figure out if the roller is turning. If so, cut the throttle a little.
float flDot;
Vector vecVelocity;
VPhysicsGetObject()->GetVelocity( &vecVelocity, NULL );
VectorNormalize( vecVelocity );
vecVelocity.z = 0;
flDot = DotProduct( vecVelocity, vecToPath );
m_RollerController.m_vecAngular = vec3_origin;
if( flDot > 0.25 && flDot < 0.7 )
{
// Feed a little torque backwards into the axis perpendicular to the velocity.
// This will help get rid of momentum that would otherwise make us overshoot our goal.
Vector vecCompensate;
vecCompensate.x = vecVelocity.y;
vecCompensate.y = -vecVelocity.x;
vecCompensate.z = 0;
m_RollerController.m_vecAngular = WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecCompensate, m_flForwardSpeed * -0.75 );
}
m_RollerController.m_vecAngular += WorldToLocalRotation( SetupMatrixAngles(GetLocalAngles()), vecRight, m_flForwardSpeed );
}
break;
case TASK_ROLLER_ISSUE_CODE:
if( gpGlobals->curtime >= m_flWaitFinished )
{
if( m_iCodeProgress == ROLLER_CODE_DIGITS )
{
TaskComplete();
}
else
{
m_flWaitFinished = gpGlobals->curtime + ROLLER_TONE_TIME;
CPASAttenuationFilter filter( this );
EmitSound( filter, entindex(), CHAN_BODY, pCodeSounds[ m_iAccessCode[ m_iCodeProgress ] ], 1.0, ATTN_NORM );
m_iCodeProgress++;
}
}
break;
default:
BaseClass::RunTask( pTask );
break;
}
}
