void CSDKPlayerAnimState::ConvergeYawAnglesThroughZero( float flGoalYaw, float flYawRate, float flDeltaTime, float &flCurrentYaw )
{
float flFadeTurnDegrees = 60;
float flEyeGoalYaw = AngleDiff(flGoalYaw, m_flEyeYaw);
float flEyeCurrentYaw = AngleDiff(flCurrentYaw, m_flEyeYaw);
// Find the yaw delta.
float flDeltaYaw = flEyeGoalYaw - flEyeCurrentYaw;
float flDeltaYawAbs = fabs( flDeltaYaw );
// Always do at least a bit of the turn (1%).
float flScale = 1.0f;
flScale = flDeltaYawAbs / flFadeTurnDegrees;
flScale = clamp( flScale, 0.01f, 1.0f );
float flYaw = flYawRate * flDeltaTime * flScale;
if ( flDeltaYawAbs < flYaw )
{
flCurrentYaw = flGoalYaw;
}
else
{
float flSide = ( flDeltaYaw < 0.0f ) ? -1.0f : 1.0f;
flCurrentYaw += ( flYaw * flSide );
}
flCurrentYaw = AngleNormalize( flCurrentYaw );
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void C_PropVehicleDriveable::RestrictView( float *pYawBounds, float *pPitchBounds,
float *pRollBounds, QAngle &vecViewAngles )
{
int eyeAttachmentIndex = LookupAttachment( "vehicle_driver_eyes" );
Vector vehicleEyeOrigin;
QAngle vehicleEyeAngles;
GetAttachmentLocal( eyeAttachmentIndex, vehicleEyeOrigin, vehicleEyeAngles );
// Limit the yaw.
if ( pYawBounds )
{
float flAngleDiff = AngleDiff( vecViewAngles.y, vehicleEyeAngles.y );
flAngleDiff = clamp( flAngleDiff, pYawBounds[0], pYawBounds[1] );
vecViewAngles.y = vehicleEyeAngles.y + flAngleDiff;
}
// Limit the pitch.
if ( pPitchBounds )
{
float flAngleDiff = AngleDiff( vecViewAngles.x, vehicleEyeAngles.x );
flAngleDiff = clamp( flAngleDiff, pPitchBounds[0], pPitchBounds[1] );
vecViewAngles.x = vehicleEyeAngles.x + flAngleDiff;
}
// Limit the roll.
if ( pRollBounds )
{
float flAngleDiff = AngleDiff( vecViewAngles.z, vehicleEyeAngles.z );
flAngleDiff = clamp( flAngleDiff, pRollBounds[0], pRollBounds[1] );
vecViewAngles.z = vehicleEyeAngles.z + flAngleDiff;
}
}
/*! angle_diff(a, b)
Returns the relative angle in (-180, 180) between
angle //a// and //b// such that (a + angle_diff(a, b)) = b (mod 360)
*/
int l_angle_diff(lua_State* L)
{
AngleDiff diff(AngleDiff(lua_tonumber(L, 1)).relative(AngleDiff(lua_tonumber(L, 2))));
lua_pushnumber(L, diff.toDeg());
return 1;
}
void vParticleOperator_WorldRotationForce::Simulate( vParticle *parent )
{
QAngle cur = MainViewAngles();
Vector2D force( AngleDiff( cur.y, last.y ), AngleDiff( last.x, cur.x ) );
force *= GetImpulse( parent ) * vParticle::GetRelativeScale() * str;
float rainHack = CFrameTimeHelper::GetFrameTime() - (1.0f/60.0f);
force += force * rainHack * -20.0f;
last = cur;
parent->vecVelocity += force;
}
//-----------------------------------------------------------------------------
// Purpose:
// Input : pLocalPlayer -
// pCmd -
//-----------------------------------------------------------------------------
void C_PropVehiclePrisonerPod::UpdateViewAngles( C_BasePlayer *pLocalPlayer, CUserCmd *pCmd )
{
int eyeAttachmentIndex = LookupAttachment( "vehicle_driver_eyes" );
Vector vehicleEyeOrigin;
QAngle vehicleEyeAngles;
GetAttachmentLocal( eyeAttachmentIndex, vehicleEyeOrigin, vehicleEyeAngles );
// Limit the yaw.
float flAngleDiff = AngleDiff( pCmd->viewangles.y, vehicleEyeAngles.y );
flAngleDiff = clamp( flAngleDiff, POD_VIEW_YAW_MIN, POD_VIEW_YAW_MAX );
pCmd->viewangles.y = vehicleEyeAngles.y + flAngleDiff;
// Limit the pitch -- don't let them look down into the empty pod!
flAngleDiff = AngleDiff( pCmd->viewangles.x, vehicleEyeAngles.x );
flAngleDiff = clamp( flAngleDiff, POD_VIEW_PITCH_MIN, POD_VIEW_PITCH_MAX );
pCmd->viewangles.x = vehicleEyeAngles.x + flAngleDiff;
}
//-----------------------------------------------------------------------------
// Purpose:
// Input : pLocalPlayer -
// pCmd -
//-----------------------------------------------------------------------------
void C_PropVehicleChoreoGeneric::UpdateViewAngles( C_BasePlayer *pLocalPlayer, CUserCmd *pCmd )
{
int eyeAttachmentIndex = LookupAttachment( "vehicle_driver_eyes" );
Vector vehicleEyeOrigin;
QAngle vehicleEyeAngles;
GetAttachmentLocal( eyeAttachmentIndex, vehicleEyeOrigin, vehicleEyeAngles );
// Limit the yaw.
float flAngleDiff = AngleDiff( pCmd->viewangles.y, vehicleEyeAngles.y );
flAngleDiff = clamp( flAngleDiff, m_vehicleView.flYawMin, m_vehicleView.flYawMax );
pCmd->viewangles.y = vehicleEyeAngles.y + flAngleDiff;
// Limit the pitch -- don't let them look down into the empty pod!
flAngleDiff = AngleDiff( pCmd->viewangles.x, vehicleEyeAngles.x );
flAngleDiff = clamp( flAngleDiff, m_vehicleView.flPitchMin, m_vehicleView.flPitchMax );
pCmd->viewangles.x = vehicleEyeAngles.x + flAngleDiff;
}
void C_QUA_Strider::RestrictView( float *pYawBounds, float *pPitchBounds,
float *pRollBounds, QAngle &vecViewAngles )
{
// Cambiado. Para que el cliente vea.
//int eyeAttachmentIndex = LookupAttachment( "vehicle_driver_eyes" );
// Encontrado el bug de los Bones... ahora debemos mirar que hacemos.
Vector vehicleEyeOrigin;
QAngle vehicleEyeAngles;
//GetAttachmentLocal( eyeAttachmentIndex, vehicleEyeOrigin, vehicleEyeAngles );
Vector up,forward;
vehicleEyeOrigin=this->GetAbsOrigin();
vehicleEyeAngles=this->GetAbsAngles();
this->GetVectors(&forward,NULL,&up);
vehicleEyeOrigin+=(forward*120)+(up*-10);
// NO limitamos el yaw en el Strider, porque si no no vemos nada.
// Limit the yaw.
if ( pYawBounds )
{
float flAngleDiff = AngleDiff( vecViewAngles.y, vehicleEyeAngles.y );
flAngleDiff = clamp( flAngleDiff, pYawBounds[0], pYawBounds[1] );
vecViewAngles.y = vehicleEyeAngles.y + flAngleDiff;
}
// Limit the pitch.
if ( pPitchBounds )
{
float flAngleDiff = AngleDiff( vecViewAngles.x, vehicleEyeAngles.x );
flAngleDiff = clamp( flAngleDiff, pPitchBounds[0], pPitchBounds[1] );
vecViewAngles.x = vehicleEyeAngles.x + flAngleDiff;
}
// Limit the roll.
if ( pRollBounds )
{
float flAngleDiff = AngleDiff( vecViewAngles.z, vehicleEyeAngles.z );
flAngleDiff = clamp( flAngleDiff, pRollBounds[0], pRollBounds[1] );
vecViewAngles.z = vehicleEyeAngles.z + flAngleDiff;
}
}
Real MultiRobot2DCSpace::Distance(const Config& x, const Config& y)
{
int stride = (allowRotation ? 3 : 2);
Assert(x.n==stride*(int)robots.size());
Assert(x.n==y.n);
int k=0;
Real d2=0;
for(size_t i=0;i<robots.size();i++,k+=stride) {
d2 += Vector2(x(k+0),x(k+1)).distanceSquared(Vector2(y(k+0),y(k+1)));
d2 += Sqr(angleDistanceWeight)*Sqr(AngleDiff(x(k+2),y(k+2)));
}
return Sqrt(d2);
}
//-----------------------------------------------------------------------------
// Clamps the view angles while manning the gun
//-----------------------------------------------------------------------------
void C_ObjectBaseMannedGun::UpdateViewAngles( C_BasePlayer *pLocalPlayer, CUserCmd *pCmd )
{
#if 0
// Confine the view to the appropriate yaw range...
float flAngleDiff = AngleDiff( pCmd->viewangles[YAW], flCenterYaw );
// Here, we must clamp to the cone...
if (flAngleDiff < m_Movement.GetMinYaw())
pCmd->viewangles[YAW] = anglemod(flCenterYaw + m_Movement.GetMinYaw());
else if (flAngleDiff > m_Movement.GetMaxYaw())
pCmd->viewangles[YAW] = anglemod(flCenterYaw + m_Movement.GetMaxYaw());
#endif
// Prevent too much downward looking
if ( pCmd->viewangles[PITCH] > m_Movement.GetMaxPitch())
{
pCmd->viewangles[PITCH] = m_Movement.GetMaxPitch();
}
}
bool Aircraft::Step(float FT, sf::Vector2f Wind)
{
bool Die = false;
Time += FT;
const sf::Vector2f &Me = Shape.getPosition();
TakeoffSound.setPosition(Me.x, Me.y, 0.f);
FlySound.setPosition(Me.x, Me.y, 0.f);
LandingSound.setPosition(Me.x, Me.y, 0.f);
Shape.update(FT);
float Turning = 0.f;
switch (State)
{
case FlyingIn:
{
sf::Vector2f To(400.f, 300.f);
Shape.setRotation(Angle(Me - To));
if (P.NumPoints() > 0)
{
State = FlyingPath;
}
else if (Me.x > 100 && Me.x < 700 && Me.y > 100 && Me.y < 500)
{
State = FlyingFree;
Turning = 0.2f;
}
break;
}
case FlyingOut:
{
sf::Vector2f From(400.f, 300.f);
Shape.rotate(AngleDiff(Shape.getRotation(), Angle(From - Me)) * FT);
float Scale = Map(Speed / Template.Speed, 0.f, 1.f, 1.f, 0.9f);
float ShadowRadius = Radius * Scale;
sf::Vector2f Shadow = sf::Transform().scale(sf::Vector2f(Scale, Scale), sf::Vector2f(800 / 2, 600 * 0.8f)).transformPoint(Me);
if ((Me.x < -Radius || Me.x > (800 + Radius) || Me.y < -Radius || Me.y > (600 + Radius)) &&
(Shadow.x < -ShadowRadius || Shadow.x > (800 + ShadowRadius) || Shadow.y < -ShadowRadius || Shadow.y > (600 + ShadowRadius)))
{
Die = true;
}
break;
}
case FlyingFree:
{
float Angle = AngleFix(Shape.getRotation()), AddAngle;
if ((Me.x < 100 && Angle > 180) ||
(Me.x > 700 && Angle < 180) ||
(Me.y < 100 && (Angle > 90 && Angle < 270)) ||
(Me.y > 500 && (Angle > 270 || Angle < 90)))
{
AddAngle = Turn;
Turning = Turn / 10;
}
else if ((Me.x < 100 && Angle <= 180) ||
(Me.x > 700 && Angle >= 180) ||
(Me.y < 100 && (Angle <= 90 || Angle >= 270)) ||
(Me.y > 500 && (Angle <= 270 && Angle >= 90)))
{
AddAngle = -Turn;
Turning = -Turn / 10;
}
else
{
AddAngle = Turning;
}
Angle += AddAngle;
Shape.setRotation(AngleFix(Angle));
if (P.NumPoints() > 0)
{
State = FlyingPath;
}
break;
}
case FlyingPath:
{
if (P.NumPoints() > 0)
{
const sf::Vector2f To = P[0]; // might crash
if (InRange(Me, To, 5))
P.RemovePoint(0);
float Target = Angle(Me - To);
Shape.setRotation(Target);
if (Land &&
P.NumPoints() == 0 &&
/*Land->OnMe(Me) &&*/
P.Highlight
/*abs(AngleDiff(GetAngle(), Land->GetAngle())) <= Land->GetTemplate().LandAngle*/)
{
FlySound.stop();
LandingSound.play();
State = Landing;
LandPoint = Me;
}
else if (P.NumPoints() == 0)
{
if (Direction == Out &&
((OutDirection == OutUp && To.y < 50) ||
(OutDirection == OutDown && To.y > 550) ||
(OutDirection == OutLeft && To.x < 50) ||
(OutDirection == OutRight && To.x > 750)))
{
State = FlyingOut;
}
else
{
State = FlyingFree;
}
}
}
break;
}
case Landing:
{
sf::Vector2f Runway = Land->GetPos() + PolarToRect(sf::Vector2f(Land->GetLength() * 1.5f, Land->GetAngle()));
float Dist = Distance(Me, LandPoint);
Speed = Map(Dist, 0.f, Land->GetLength() * 1.1f, Template.Speed, 0.f);
if (Land->GetTemplate().Directional)
{
Shape.rotate(AngleDiff(Shape.getRotation(), Angle(Me - Runway)) * 3 * FT);
}
float Scale = Map2(Dist, 0.f, Land->GetLength() * 1.1f, 1.f, 0.65f);
Shape.setScale(Scale, Scale);
Radius = Template.Radius * Scale;
if (Dist > Land->GetLength())
{
Die = true;
}
break;
}
case TakingOff:
{
sf::Vector2f Runway = Land->GetPos() + PolarToRect(sf::Vector2f(Land->GetLength() * 1.5f, Land->GetAngle()));
float Dist = Distance(Me, Land->GetPos());
Speed = Map(Dist, 0.f, Land->GetLength() * 1.1f, 10.f, Template.Speed);
if (Land->GetTemplate().Directional)
{
Shape.rotate(AngleDiff(Shape.getRotation(), Angle(Me - Runway)) * 3 * FT);
}
float Scale = Map2(Dist, 0.f, Land->GetLength() * 1.1f, 0.65f, 1.f);
Shape.setScale(Scale, Scale);
Radius = Template.Radius * Scale;
if (Dist > Land->GetLength())
{
FlySound.play();
State = FlyingFree;
Land = 0;
}
break;
}
}
Shape.move(PolarToRect(sf::Vector2f(Speed, Shape.getRotation())) * FT);
Shape.move(Wind * FT * Magnitude(Shape.getScale()) / sqrt(2.f));
return Die;
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CPointCommentaryNode::UpdateViewThink( void )
{
if ( !m_bActive )
return;
CBasePlayer *pPlayer = GetCommentaryPlayer();
if ( !pPlayer )
return;
// Swing the view towards the target
if ( m_hViewTarget )
{
QAngle angGoal;
QAngle angCurrent;
if ( m_hViewPositionMover )
{
angCurrent = m_hViewPositionMover->GetAbsAngles();
VectorAngles( m_hViewTarget->WorldSpaceCenter() - m_hViewPositionMover->GetAbsOrigin(), angGoal );
}
else
{
angCurrent = pPlayer->EyeAngles();
VectorAngles( m_hViewTarget->WorldSpaceCenter() - pPlayer->EyePosition(), angGoal );
}
// Accelerate towards the target goal angles
float dx = AngleDiff( angGoal.x, angCurrent.x );
float dy = AngleDiff( angGoal.y, angCurrent.y );
float mod = 1.0 - ExponentialDecay( 0.5, 0.3, gpGlobals->frametime );
float dxmod = dx * mod;
float dymod = dy * mod;
angCurrent.x = AngleNormalize( angCurrent.x + dxmod );
angCurrent.y = AngleNormalize( angCurrent.y + dymod );
if ( m_hViewPositionMover )
{
m_hViewPositionMover->SetAbsAngles( angCurrent );
}
else
{
pPlayer->SnapEyeAngles( angCurrent );
}
SetNextThink( gpGlobals->curtime, s_pCommentaryUpdateViewThink );
}
if ( m_hViewPosition.Get() )
{
if ( pPlayer->GetActiveWeapon() )
{
pPlayer->GetActiveWeapon()->Holster();
}
if ( !m_hViewPositionMover )
{
// Make an invisible info target entity for us to attach the view to,
// and move it to the desired view position.
m_hViewPositionMover = CreateEntityByName( "env_laserdot" );
m_hViewPositionMover->SetAbsAngles( pPlayer->EyeAngles() );
pPlayer->SetViewEntity( m_hViewPositionMover );
}
// Blend to the target position over time.
float flCurTime = (gpGlobals->curtime - m_flStartTime);
float flBlendPerc = clamp( flCurTime / 2.0, 0, 1 );
// Figure out the current view position
Vector vecCurEye;
VectorLerp( pPlayer->EyePosition(), m_hViewPosition.Get()->GetAbsOrigin(), flBlendPerc, vecCurEye );
m_hViewPositionMover->SetAbsOrigin( vecCurEye );
SetNextThink( gpGlobals->curtime, s_pCommentaryUpdateViewThink );
}
}
//------------------------------------------------------------------------------
// Purpose :
// Input :
// Output :
//------------------------------------------------------------------------------
void CNPC_CombineDropship::Flight( void )
{
// Only run pose params in some flight states
bool bRunPoseParams = ( m_iLandState == LANDING_NO ||
m_iLandState == LANDING_LEVEL_OUT ||
m_iLandState == LANDING_LIFTOFF ||
m_iLandState == LANDING_SWOOPING );
if ( bRunPoseParams )
{
if( GetFlags() & FL_ONGROUND )
{
//This would be really bad.
RemoveFlag( FL_ONGROUND );
}
// NDebugOverlay::Line(GetLocalOrigin(), GetDesiredPosition(), 0,0,255, true, 0.1);
Vector deltaPos = GetDesiredPosition() - GetLocalOrigin();
// calc desired acceleration
float dt = 1.0f;
Vector accel;
float accelRate = DROPSHIP_ACCEL_RATE;
float maxSpeed = m_flMaxSpeed;
if ( m_lifeState == LIFE_DYING )
{
accelRate *= 5.0;
maxSpeed *= 5.0;
}
float flDist = min( GetAbsVelocity().Length() + accelRate, maxSpeed );
// Only decelerate to our goal if we're going to hit it
if ( deltaPos.Length() > flDist * dt )
{
float scale = flDist * dt / deltaPos.Length();
deltaPos = deltaPos * scale;
}
// If we're swooping, floor it
if ( m_iLandState == LANDING_SWOOPING )
{
VectorNormalize( deltaPos );
deltaPos *= maxSpeed;
}
// calc goal linear accel to hit deltaPos in dt time.
accel.x = 2.0 * (deltaPos.x - GetAbsVelocity().x * dt) / (dt * dt);
accel.y = 2.0 * (deltaPos.y - GetAbsVelocity().y * dt) / (dt * dt);
accel.z = 2.0 * (deltaPos.z - GetAbsVelocity().z * dt + 0.5 * 384 * dt * dt) / (dt * dt);
//NDebugOverlay::Line(GetLocalOrigin(), GetLocalOrigin() + deltaPos, 255,0,0, true, 0.1);
//NDebugOverlay::Line(GetLocalOrigin(), GetLocalOrigin() + accel, 0,255,0, true, 0.1);
// don't fall faster than 0.2G or climb faster than 2G
if ( m_iLandState != LANDING_SWOOPING )
{
accel.z = clamp( accel.z, 384 * 0.2, 384 * 2.0 );
}
Vector forward, right, up;
GetVectors( &forward, &right, &up );
Vector goalUp = accel;
VectorNormalize( goalUp );
// calc goal orientation to hit linear accel forces
float goalPitch = RAD2DEG( asin( DotProduct( forward, goalUp ) ) );
float goalYaw = UTIL_VecToYaw( m_vecDesiredFaceDir );
float goalRoll = RAD2DEG( asin( DotProduct( right, goalUp ) ) );
// clamp goal orientations
goalPitch = clamp( goalPitch, -45, 60 );
goalRoll = clamp( goalRoll, -45, 45 );
// calc angular accel needed to hit goal pitch in dt time.
dt = 0.6;
QAngle goalAngAccel;
goalAngAccel.x = 2.0 * (AngleDiff( goalPitch, AngleNormalize( GetLocalAngles().x ) ) - GetLocalAngularVelocity().x * dt) / (dt * dt);
goalAngAccel.y = 2.0 * (AngleDiff( goalYaw, AngleNormalize( GetLocalAngles().y ) ) - GetLocalAngularVelocity().y * dt) / (dt * dt);
goalAngAccel.z = 2.0 * (AngleDiff( goalRoll, AngleNormalize( GetLocalAngles().z ) ) - GetLocalAngularVelocity().z * dt) / (dt * dt);
goalAngAccel.x = clamp( goalAngAccel.x, -300, 300 );
//goalAngAccel.y = clamp( goalAngAccel.y, -60, 60 );
goalAngAccel.y = clamp( goalAngAccel.y, -120, 120 );
goalAngAccel.z = clamp( goalAngAccel.z, -300, 300 );
// limit angular accel changes to simulate mechanical response times
dt = 0.1;
QAngle angAccelAccel;
angAccelAccel.x = (goalAngAccel.x - m_vecAngAcceleration.x) / dt;
angAccelAccel.y = (goalAngAccel.y - m_vecAngAcceleration.y) / dt;
angAccelAccel.z = (goalAngAccel.z - m_vecAngAcceleration.z) / dt;
angAccelAccel.x = clamp( angAccelAccel.x, -1000, 1000 );
angAccelAccel.y = clamp( angAccelAccel.y, -1000, 1000 );
angAccelAccel.z = clamp( angAccelAccel.z, -1000, 1000 );
m_vecAngAcceleration += angAccelAccel * 0.1;
// Msg( "pitch %6.1f (%6.1f:%6.1f) ", goalPitch, GetLocalAngles().x, m_vecAngVelocity.x );
// Msg( "roll %6.1f (%6.1f:%6.1f) : ", goalRoll, GetLocalAngles().z, m_vecAngVelocity.z );
// Msg( "%6.1f %6.1f %6.1f : ", goalAngAccel.x, goalAngAccel.y, goalAngAccel.z );
// Msg( "%6.0f %6.0f %6.0f\n", angAccelAccel.x, angAccelAccel.y, angAccelAccel.z );
ApplySidewaysDrag( right );
ApplyGeneralDrag();
QAngle angVel = GetLocalAngularVelocity();
angVel += m_vecAngAcceleration * 0.1;
//angVel.y = clamp( angVel.y, -60, 60 );
//angVel.y = clamp( angVel.y, -120, 120 );
angVel.y = clamp( angVel.y, -120, 120 );
SetLocalAngularVelocity( angVel );
m_flForce = m_flForce * 0.8 + (accel.z + fabs( accel.x ) * 0.1 + fabs( accel.y ) * 0.1) * 0.1 * 0.2;
Vector vecImpulse = m_flForce * up;
if ( m_lifeState == LIFE_DYING )
{
vecImpulse.z = -38.4; // 64ft/sec
}
else
{
vecImpulse.z -= 38.4; // 32ft/sec
}
// Find our acceleration direction
Vector vecAccelDir = vecImpulse;
VectorNormalize( vecAccelDir );
// Find our current velocity
Vector vecVelDir = GetAbsVelocity();
VectorNormalize( vecVelDir );
// Level out our plane of movement
vecAccelDir.z = 0.0f;
vecVelDir.z = 0.0f;
forward.z = 0.0f;
right.z = 0.0f;
// Find out how "fast" we're moving in relation to facing and acceleration
float speed = m_flForce * DotProduct( vecVelDir, vecAccelDir );// * DotProduct( forward, vecVelDir );
// Use the correct pose params
char *sBodyAccel;
char *sBodySway;
if ( m_hContainer || m_iLandState == LANDING_SWOOPING )
{
sBodyAccel = "cargo_body_accel";
sBodySway = "cargo_body_sway";
SetPoseParameter( "body_accel", 0 );
SetPoseParameter( "body_sway", 0 );
}
else
{
sBodyAccel = "body_accel";
sBodySway = "body_sway";
SetPoseParameter( "cargo_body_accel", 0 );
SetPoseParameter( "cargo_body_sway", 0 );
}
// Apply the acceleration blend to the fins
float finAccelBlend = SimpleSplineRemapVal( speed, -60, 60, -1, 1 );
float curFinAccel = GetPoseParameter( sBodyAccel );
curFinAccel = UTIL_Approach( finAccelBlend, curFinAccel, 0.5f );
SetPoseParameter( sBodyAccel, curFinAccel );
speed = m_flForce * DotProduct( vecVelDir, right );
// Apply the spin sway to the fins
float finSwayBlend = SimpleSplineRemapVal( speed, -60, 60, -1, 1 );
float curFinSway = GetPoseParameter( sBodySway );
curFinSway = UTIL_Approach( finSwayBlend, curFinSway, 0.5f );
SetPoseParameter( sBodySway, curFinSway );
// Add in our velocity pulse for this frame
ApplyAbsVelocityImpulse( vecImpulse );
//Msg("FinAccel: %f, Finsway: %f\n", curFinAccel, curFinSway );
}
else
{
SetPoseParameter( "body_accel", 0 );
SetPoseParameter( "body_sway", 0 );
SetPoseParameter( "cargo_body_accel", 0 );
SetPoseParameter( "cargo_body_sway", 0 );
}
}
void IntegratedStateEstimator::ReadSensors(RobotSensors& sensors)
{
JointPositionSensor* jp = sensors.GetTypedSensor<JointPositionSensor>();
JointVelocitySensor* jv = sensors.GetTypedSensor<JointVelocitySensor>();
if(jv) {
for(size_t i=0;i<robot.joints.size();i++) {
if(robot.joints[i].type == RobotJoint::Normal || robot.joints[i].type == RobotJoint::Spin) {
int link = robot.joints[i].linkIndex;
dq_predicted[link] = jv->dq[link];
}
}
}
else if(jp) {
if(last_dt == 0)
dq_predicted.setZero();
else {
//do finite differencing
for(size_t i=0;i<robot.joints.size();i++) {
if(robot.joints[i].type == RobotJoint::Normal) {
int link = robot.joints[i].linkIndex;
dq_predicted[link] = (jp->q[link] - q_predicted[link])/last_dt;
}
else if(robot.joints[i].type == RobotJoint::Spin) {
int link = robot.joints[i].linkIndex;
dq_predicted[link] = AngleDiff(jp->q[link],q_predicted[link])/last_dt;
}
}
}
}
//update joint positions for normal joints
if(jp) {
for(size_t i=0;i<robot.joints.size();i++) {
if(robot.joints[i].type == RobotJoint::Normal || robot.joints[i].type == RobotJoint::Spin) {
int link = robot.joints[i].linkIndex;
q_predicted[link] = jp->q[link];
}
}
}
//use gyro sensors to update q_predicted
vector<GyroSensor*> gyroSensors;
sensors.GetTypedSensors(gyroSensors);
if(!gyroSensors.empty()) {
robot.UpdateConfig(q_predicted);
for(size_t j=0;j<gyroSensors.size();j++) {
int glink = gyroSensors[j]->link;
int match = -1;
for(size_t i=0;i<robot.joints.size();i++) {
if(robot.joints[i].type == RobotJoint::Floating) {
if(robot.joints[i].linkIndex == glink) {
match = (int)i;
break;
}
}
}
if(match >= 0) {
robot.SetJointByOrientation(match,glink,gyroSensors[j]->rotation);
robot.SetJointVelocityByMoment(match,glink,gyroSensors[j]->angVel,Vector3(Zero));
vector<int> indices;
robot.GetJointIndices(match,indices);
assert(indices.size()==6);
q_predicted(indices[3]) = robot.q(indices[3]);
q_predicted(indices[4]) = robot.q(indices[4]);
q_predicted(indices[5]) = robot.q(indices[5]);
dq_predicted(indices[3]) = robot.dq(indices[3]);
dq_predicted(indices[4]) = robot.dq(indices[4]);
dq_predicted(indices[5]) = robot.dq(indices[5]);
}
else {
fprintf(stderr,"Warning: can't use gyro sensor on a non-floating link %d\n",glink);
}
}
}
vector<Accelerometer*> accelerometers;
sensors.GetTypedSensors(accelerometers);
if(accelerometerFrames.empty()) {
robot.UpdateConfig(q_predicted);
//initialize
accelerometerFrames.resize(accelerometers.size());
accelerometerVels.resize(accelerometers.size());
for(size_t i=0;i<accelerometers.size();i++) {
int alink = accelerometers[i]->link;
accelerometerFrames[i] = robot.links[alink].T_World*accelerometers[i]->Tsensor;
robot.GetWorldVelocity(accelerometers[i]->Tsensor.t,alink,robot.dq,accelerometerVels[i].v);
robot.GetWorldAngularVelocity(alink,robot.dq,accelerometerVels[i].w);
}
}
//advance accelerometers
if(last_dt > 0.0 && !accelerometers.empty()) {
fprintf(stderr,"TODO: integrate accelerometers\n");
}
}
void AngleInterval::setMinRange(Real a, Real b)
{
d = AngleDiff(a,b);
if(d<0) { c=a; d=-d; }
else c=b;
}
Real AngleInterp(Real a, Real b, Real u)
{
Real d = AngleDiff(b,a);
Assert(d >= -Pi && d <= Pi);
return AngleNormalize(a+u*d);
}
//-----------------------------------------------------------------------------
// Purpose: Vehicle dampening shared between server and client
//-----------------------------------------------------------------------------
void SharedVehicleViewSmoothing(CBasePlayer *pPlayer,
Vector *pAbsOrigin, QAngle *pAbsAngles,
bool bEnterAnimOn, bool bExitAnimOn,
const Vector &vecEyeExitEndpoint,
ViewSmoothingData_t *pData,
float *pFOV )
{
int eyeAttachmentIndex = pData->pVehicle->LookupAttachment( "vehicle_driver_eyes" );
matrix3x4_t vehicleEyePosToWorld;
Vector vehicleEyeOrigin;
QAngle vehicleEyeAngles;
pData->pVehicle->GetAttachment( eyeAttachmentIndex, vehicleEyeOrigin, vehicleEyeAngles );
AngleMatrix( vehicleEyeAngles, vehicleEyePosToWorld );
// Dampen the eye positional change as we drive around.
*pAbsAngles = pPlayer->EyeAngles();
if ( r_VehicleViewDampen.GetInt() && pData->bDampenEyePosition )
{
CPropVehicleDriveable *pDriveable = assert_cast<CPropVehicleDriveable*>(pData->pVehicle);
pDriveable->DampenEyePosition( vehicleEyeOrigin, vehicleEyeAngles );
}
// Started running an entry or exit anim?
bool bRunningAnim = ( bEnterAnimOn || bExitAnimOn );
if ( bRunningAnim && !pData->bWasRunningAnim )
{
pData->bRunningEnterExit = true;
pData->flEnterExitStartTime = gpGlobals->curtime;
pData->flEnterExitDuration = pData->pVehicle->SequenceDuration( pData->pVehicle->GetSequence() );
#ifdef CLIENT_DLL
pData->vecOriginSaved = PrevMainViewOrigin();
pData->vecAnglesSaved = PrevMainViewAngles();
#endif
// Save our initial angular error, which we will blend out over the length of the animation.
pData->vecAngleDiffSaved.x = AngleDiff( vehicleEyeAngles.x, pData->vecAnglesSaved.x );
pData->vecAngleDiffSaved.y = AngleDiff( vehicleEyeAngles.y, pData->vecAnglesSaved.y );
pData->vecAngleDiffSaved.z = AngleDiff( vehicleEyeAngles.z, pData->vecAnglesSaved.z );
pData->vecAngleDiffMin = pData->vecAngleDiffSaved;
}
pData->bWasRunningAnim = bRunningAnim;
float frac = 0;
float flFracFOV = 0;
// If we're in an enter/exit animation, blend the player's eye angles to the attachment's
if ( bRunningAnim || pData->bRunningEnterExit )
{
*pAbsAngles = vehicleEyeAngles;
// Forward integrate to determine the elapsed time in this entry/exit anim.
frac = ( gpGlobals->curtime - pData->flEnterExitStartTime ) / pData->flEnterExitDuration;
frac = clamp( frac, 0.0f, 1.0f );
flFracFOV = ( gpGlobals->curtime - pData->flEnterExitStartTime ) / ( pData->flEnterExitDuration * 0.85f );
flFracFOV = clamp( flFracFOV, 0.0f, 1.0f );
//Msg("Frac: %f\n", frac );
if ( frac < 1.0 )
{
// Blend to the desired vehicle eye origin
//Vector vecToView = (vehicleEyeOrigin - PrevMainViewOrigin());
//vehicleEyeOrigin = PrevMainViewOrigin() + (vecToView * SimpleSpline(frac));
//debugoverlay->AddBoxOverlay( vehicleEyeOrigin, -Vector(1,1,1), Vector(1,1,1), vec3_angle, 0,255,255, 64, 10 );
}
else
{
pData->bRunningEnterExit = false;
// Enter animation has finished, align view with the eye attachment point
// so they can start mouselooking around.
#if !defined ( HL2MP_DEV_DLL ) && !defined ( HL2MP_DEV_VEHICLE_FIX )
if ( !bExitAnimOn )
{
Vector localEyeOrigin;
QAngle localEyeAngles;
pData->pVehicle->GetAttachmentLocal( eyeAttachmentIndex, localEyeOrigin, localEyeAngles );
#ifdef CLIENT_DLL
engine->SetViewAngles( localEyeAngles );
#endif
}
#else
#ifdef CLIENT_DLL
pPlayer = C_BasePlayer::GetLocalPlayer();
#endif
if ( pPlayer )
{
if ( !bExitAnimOn )
{
Vector localEyeOrigin;
QAngle localEyeAngles;
pData->pVehicle->GetAttachmentLocal( eyeAttachmentIndex, localEyeOrigin, localEyeAngles );
#ifdef CLIENT_DLL
engine->SetViewAngles( localEyeAngles );
#endif
}
}
#endif
}
}
// Compute the relative rotation between the unperturbed eye attachment + the eye angles
matrix3x4_t cameraToWorld;
AngleMatrix( *pAbsAngles, cameraToWorld );
matrix3x4_t worldToEyePos;
MatrixInvert( vehicleEyePosToWorld, worldToEyePos );
matrix3x4_t vehicleCameraToEyePos;
ConcatTransforms( worldToEyePos, cameraToWorld, vehicleCameraToEyePos );
// Damp out some of the vehicle motion (neck/head would do this)
if ( pData->bClampEyeAngles )
{
RemapViewAngles( pData, vehicleEyeAngles );
}
AngleMatrix( vehicleEyeAngles, vehicleEyeOrigin, vehicleEyePosToWorld );
// Now treat the relative eye angles as being relative to this new, perturbed view position...
matrix3x4_t newCameraToWorld;
ConcatTransforms( vehicleEyePosToWorld, vehicleCameraToEyePos, newCameraToWorld );
// output new view abs angles
MatrixAngles( newCameraToWorld, *pAbsAngles );
// UNDONE: *pOrigin would already be correct in single player if the HandleView() on the server ran after vphysics
MatrixGetColumn( newCameraToWorld, 3, *pAbsOrigin );
float flDefaultFOV;
#ifdef CLIENT_DLL
flDefaultFOV = default_fov.GetFloat();
#else
flDefaultFOV = pPlayer->GetDefaultFOV();
#endif
// If we're playing an entry or exit animation...
if ( bRunningAnim || pData->bRunningEnterExit )
{
float flSplineFrac = clamp( SimpleSpline( frac ), 0.f, 1.f );
// Blend out the error between the player's initial eye angles and the animation's initial
// eye angles over the duration of the animation.
QAngle vecAngleDiffBlend = ( ( 1 - flSplineFrac ) * pData->vecAngleDiffSaved );
// If our current error is less than the error amount that we're blending
// out, use that. This lets the angles converge as quickly as possible.
QAngle vecAngleDiffCur;
vecAngleDiffCur.x = AngleDiff( vehicleEyeAngles.x, pData->vecAnglesSaved.x );
vecAngleDiffCur.y = AngleDiff( vehicleEyeAngles.y, pData->vecAnglesSaved.y );
vecAngleDiffCur.z = AngleDiff( vehicleEyeAngles.z, pData->vecAnglesSaved.z );
// In either case, never increase the error, so track the minimum error and clamp to that.
for (int i = 0; i < 3; i++)
{
if ( fabs(vecAngleDiffCur[i] ) < fabs( pData->vecAngleDiffMin[i] ) )
{
pData->vecAngleDiffMin[i] = vecAngleDiffCur[i];
}
if ( fabs(vecAngleDiffBlend[i] ) < fabs( pData->vecAngleDiffMin[i] ) )
{
pData->vecAngleDiffMin[i] = vecAngleDiffBlend[i];
}
}
// Add the error to the animation's eye angles.
*pAbsAngles -= pData->vecAngleDiffMin;
// Use this as the basis for the next error calculation.
pData->vecAnglesSaved = *pAbsAngles;
//if ( gpGlobals->frametime )
//{
// Msg("Angle : %.2f %.2f %.2f\n", target.x, target.y, target.z );
//}
//Msg("Prev: %.2f %.2f %.2f\n", pData->vecAnglesSaved.x, pData->vecAnglesSaved.y, pData->vecAnglesSaved.z );
Vector vecAbsOrigin = *pAbsOrigin;
// If we're exiting, our desired position is the server-sent exit position
if ( bExitAnimOn )
{
//debugoverlay->AddBoxOverlay( vecEyeExitEndpoint, -Vector(1,1,1), Vector(1,1,1), vec3_angle, 255,255,255, 64, 10 );
// Blend to the exit position
*pAbsOrigin = Lerp( flSplineFrac, vecAbsOrigin, vecEyeExitEndpoint );
if ( pFOV != NULL )
{
if ( pData->flFOV > flDefaultFOV )
{
*pFOV = Lerp( flFracFOV, pData->flFOV, flDefaultFOV );
}
}
}
else
{
// Blend from our starting position to the desired origin
*pAbsOrigin = Lerp( flSplineFrac, pData->vecOriginSaved, vecAbsOrigin );
if ( pFOV != NULL )
{
if ( pData->flFOV > flDefaultFOV )
{
*pFOV = Lerp( flFracFOV, flDefaultFOV, pData->flFOV );
}
}
}
}
else if ( pFOV != NULL )
{
if ( pData->flFOV > flDefaultFOV )
{
// Not running an entry/exit anim. Just use the vehicle's FOV.
*pFOV = pData->flFOV;
}
}
}
void C_ASW_Simple_Drone::UpdatePoseParams()
{
VPROF_BUDGET( "C_ASW_Simple_Drone::UpdatePoseParams", VPROF_BUDGETGROUP_ASW_CLIENT );
// update pose params based on velocity and our angles
// calculate the angle difference between our facing and our velocity
Vector v;
EstimateAbsVelocity(v);
float travel_yaw = anglemod(UTIL_VecToYaw(v));
float current_yaw = anglemod( GetLocalAngles().y );
// Draw a green triangle on the ground for the travel yaw
if (cl_asw_drone_travel_yaw.GetBool())
{
float flBaseSize = 10;
float flHeight = 80;
Vector vBasePos = GetAbsOrigin() + Vector( 0, 0, 5 );
QAngle angles( 0, 0, 0 );
Vector vForward, vRight, vUp;
angles[YAW] = travel_yaw;
AngleVectors( angles, &vForward, &vRight, &vUp );
debugoverlay->AddTriangleOverlay( vBasePos+vRight*flBaseSize/2, vBasePos-vRight*flBaseSize/2, vBasePos+vForward*flHeight, 0, 255, 0, 255, false, 0.01 );
}
// calculate our fraction of full anim velocity
float speed_fraction = 0;
float ground_speed = GetRunSpeed();
if (ground_speed > 0)
speed_fraction = clamp<float>(
(v.Length()) / ground_speed,
0.0f, 1.0f);
speed_fraction = 1.0f - speed_fraction;
// smooth out the travel yaw to prevent sudden changes in move_yaw pose parameter
if (m_flCurrentTravelYaw == -1)
m_flCurrentTravelYaw = travel_yaw;
else
{
float travel_diff = AngleDiff(m_flCurrentTravelYaw, travel_yaw);
if (travel_diff < 0)
travel_diff = -travel_diff;
travel_diff = clamp<float>(travel_diff, 32.0f, 256.0f); // alter the yaw by this amount - i.e. faster if the angle is bigger, but clamped
if (speed_fraction > 0.75f) // change the angle even quicker if we're moving very slowly
{
travel_diff *= (2.0f + ((speed_fraction - 0.75f) * 8.0f));
}
if (speed_fraction < 1.0f) // don't bother adjusting the yaw if we're standing still
m_flCurrentTravelYaw = AI_ClampYaw( travel_diff * cl_asw_drone_travel_yaw_rate.GetFloat(), m_flCurrentTravelYaw, travel_yaw, gpGlobals->frametime );
else
m_flCurrentTravelYaw = travel_yaw; // if we're standing still, immediately change
//Msg("travel=%.1f current_travel=%.1f current=%.1f t=%f\n",
//travel_yaw, m_flCurrentTravelYaw, current_yaw, gpGlobals->curtime);
travel_yaw = m_flCurrentTravelYaw;
}
// set the move_yaw pose parameter
float diff = AngleDiff(travel_yaw, current_yaw);
// Draw a green triangle on the ground for the move yaw
if (cl_asw_drone_travel_yaw.GetBool())
{
float flBaseSize = 10;
float flHeight = 80;
Vector vBasePos = GetAbsOrigin() + Vector( 0, 0, 5 );
QAngle angles( 0, 0, 0 );
Vector vForward, vRight, vUp;
angles[YAW] = travel_yaw;
AngleVectors( angles, &vForward, &vRight, &vUp );
debugoverlay->AddTriangleOverlay( vBasePos+vRight*flBaseSize/2, vBasePos-vRight*flBaseSize/2, vBasePos+vForward*flHeight, 0, 0, 255, 255, false, 0.01 );
angles[YAW] = diff;
AngleVectors( angles, &vForward, &vRight, &vUp );
debugoverlay->AddTriangleOverlay( vBasePos+vRight*flBaseSize/2, vBasePos-vRight*flBaseSize/2, vBasePos+vForward*flHeight, 255, 0, 0, 255, false, 0.01 );
}
diff = clamp<float>(diff, -180.0f, 180.0f);
int pose_index = LookupPoseParameter( "move_yaw" );
if (pose_index >= 0)
{
SetPoseParameter(pose_index, diff);
}
// smooth out our speed fraction to prevent sudden changes to idle_move pose parameter
//Msg("sf=%f cts=%f gs=%f vl=%f\n", speed_fraction, m_flCurrentTravelSpeed,
//ground_speed, v.Length());
if (m_flCurrentTravelSpeed == -1)
{
m_flCurrentTravelSpeed = speed_fraction;
}
else
{
if (m_flCurrentTravelSpeed < speed_fraction)
{
m_flCurrentTravelSpeed = clamp<float>(
m_flCurrentTravelSpeed + gpGlobals->frametime,
0.0f, speed_fraction);
}
else
{
m_flCurrentTravelSpeed = clamp<float>(
m_flCurrentTravelSpeed - gpGlobals->frametime * 3.0f,
speed_fraction, 1.0f);
}
speed_fraction = m_flCurrentTravelSpeed;
}
// set the idle_move pose parameter
pose_index = LookupPoseParameter( "idle_move" );
if (pose_index >= 0)
{
SetPoseParameter(pose_index, 100.0f * speed_fraction);
}
}
