//========= Copyright Valve Corporation, All rights reserved. ============// // // Purpose: // // $NoKeywords: $ //=============================================================================// #include "cbase.h" #include "entitylist.h" #include "physics.h" #include "vphysics/constraints.h" #include "physics_saverestore.h" #include "phys_controller.h" // memdbgon must be the last include file in a .cpp file!!! #include "tier0/memdbgon.h" #define SF_THRUST_STARTACTIVE 0x0001 #define SF_THRUST_FORCE 0x0002 #define SF_THRUST_TORQUE 0x0004 #define SF_THRUST_LOCAL_ORIENTATION 0x0008 #define SF_THRUST_MASS_INDEPENDENT 0x0010 #define SF_THRUST_IGNORE_POS 0x0020 class CPhysThruster; //----------------------------------------------------------------------------- // Purpose: This class only implements the IMotionEvent-specific behavior // It keeps track of the forces so they can be integrated //----------------------------------------------------------------------------- class CConstantForceController : public IMotionEvent { DECLARE_SIMPLE_DATADESC(); public: void Init( IMotionEvent::simresult_e controlType ) { m_controlType = controlType; } void SetConstantForce( const Vector &linear, const AngularImpulse &angular ); void ScaleConstantForce( float scale ); IMotionEvent::simresult_e Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular ); IMotionEvent::simresult_e m_controlType; Vector m_linear; AngularImpulse m_angular; Vector m_linearSave; AngularImpulse m_angularSave; }; BEGIN_SIMPLE_DATADESC( CConstantForceController ) DEFINE_FIELD( m_controlType, FIELD_INTEGER ), DEFINE_FIELD( m_linear, FIELD_VECTOR ), DEFINE_FIELD( m_angular, FIELD_VECTOR ), DEFINE_FIELD( m_linearSave, FIELD_VECTOR ), DEFINE_FIELD( m_angularSave, FIELD_VECTOR ), END_DATADESC() void CConstantForceController::SetConstantForce( const Vector &linear, const AngularImpulse &angular ) { m_linear = linear; m_angular = angular; // cache these for scaling later m_linearSave = linear; m_angularSave = angular; } void CConstantForceController::ScaleConstantForce( float scale ) { m_linear = m_linearSave * scale; m_angular = m_angularSave * scale; } IMotionEvent::simresult_e CConstantForceController::Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular ) { linear = m_linear; angular = m_angular; return m_controlType; } // UNDONE: Make these logical entities //----------------------------------------------------------------------------- // Purpose: This is a general entity that has a force/motion controller that // simply integrates a constant linear/angular acceleration //----------------------------------------------------------------------------- abstract_class CPhysForce : public CPointEntity { public: DECLARE_CLASS( CPhysForce, CPointEntity ); CPhysForce(); ~CPhysForce(); DECLARE_DATADESC(); virtual void OnRestore( ); void Spawn( void ); void Activate( void ); void ForceOn( void ); void ForceOff( void ); void ActivateForce( void ); // Input handlers void InputActivate( inputdata_t &inputdata ); void InputDeactivate( inputdata_t &inputdata ); void InputForceScale( inputdata_t &inputdata ); void SaveForce( void ); void ScaleForce( float scale ); // MUST IMPLEMENT THIS IN DERIVED CLASS virtual void SetupForces( IPhysicsObject *pPhys, Vector &linear, AngularImpulse &angular ) = 0; // optional virtual void OnActivate( void ) {} protected: IPhysicsMotionController *m_pController; string_t m_nameAttach; float m_force; float m_forceTime; EHANDLE m_attachedObject; bool m_wasRestored; CConstantForceController m_integrator; }; BEGIN_DATADESC( CPhysForce ) DEFINE_PHYSPTR( m_pController ), DEFINE_KEYFIELD( m_nameAttach, FIELD_STRING, "attach1" ), DEFINE_KEYFIELD( m_force, FIELD_FLOAT, "force" ), DEFINE_KEYFIELD( m_forceTime, FIELD_FLOAT, "forcetime" ), DEFINE_FIELD( m_attachedObject, FIELD_EHANDLE ), //DEFINE_FIELD( m_wasRestored, FIELD_BOOLEAN ), // NOTE: DO NOT save/load this - it's used to detect loads DEFINE_EMBEDDED( m_integrator ), DEFINE_INPUTFUNC( FIELD_VOID, "Activate", InputActivate ), DEFINE_INPUTFUNC( FIELD_VOID, "Deactivate", InputDeactivate ), DEFINE_INPUTFUNC( FIELD_FLOAT, "scale", InputForceScale ), // Function Pointers DEFINE_FUNCTION( ForceOff ), END_DATADESC() CPhysForce::CPhysForce( void ) { m_pController = NULL; m_wasRestored = false; } CPhysForce::~CPhysForce() { if ( m_pController ) { physenv->DestroyMotionController( m_pController ); } } void CPhysForce::Spawn( void ) { if ( m_spawnflags & SF_THRUST_LOCAL_ORIENTATION ) { m_integrator.Init( IMotionEvent::SIM_LOCAL_ACCELERATION ); } else { m_integrator.Init( IMotionEvent::SIM_GLOBAL_ACCELERATION ); } } void CPhysForce::OnRestore( ) { BaseClass::OnRestore(); if ( m_pController ) { m_pController->SetEventHandler( &m_integrator ); } m_wasRestored = true; } void CPhysForce::Activate( void ) { BaseClass::Activate(); if ( m_pController ) { m_pController->WakeObjects(); } if ( m_wasRestored ) return; if ( m_attachedObject == NULL ) { m_attachedObject = gEntList.FindEntityByName( NULL, m_nameAttach ); } // Let the derived class set up before we throw the switch OnActivate(); if ( m_spawnflags & SF_THRUST_STARTACTIVE ) { ForceOn(); } } //----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- void CPhysForce::InputActivate( inputdata_t &inputdata ) { ForceOn(); } //----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- void CPhysForce::InputDeactivate( inputdata_t &inputdata ) { ForceOff(); } //----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- void CPhysForce::InputForceScale( inputdata_t &inputdata ) { ScaleForce( inputdata.value.Float() ); } //----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- void CPhysForce::ForceOn( void ) { if ( m_pController ) return; ActivateForce(); if ( m_forceTime ) { SetNextThink( gpGlobals->curtime + m_forceTime ); SetThink( &CPhysForce::ForceOff ); } } void CPhysForce::ActivateForce( void ) { IPhysicsObject *pPhys = NULL; if ( m_attachedObject ) { pPhys = m_attachedObject->VPhysicsGetObject(); } if ( !pPhys ) return; Vector linear; AngularImpulse angular; SetupForces( pPhys, linear, angular ); m_integrator.SetConstantForce( linear, angular ); m_pController = physenv->CreateMotionController( &m_integrator ); m_pController->AttachObject( pPhys, true ); // Make sure the object is simulated pPhys->Wake(); } void CPhysForce::ForceOff( void ) { if ( !m_pController ) return; physenv->DestroyMotionController( m_pController ); m_pController = NULL; SetThink( NULL ); SetNextThink( TICK_NEVER_THINK ); IPhysicsObject *pPhys = NULL; if ( m_attachedObject ) { pPhys = m_attachedObject->VPhysicsGetObject(); if ( pPhys ) { pPhys->Wake(); } } } void CPhysForce::ScaleForce( float scale ) { if ( !m_pController ) ForceOn(); m_integrator.ScaleConstantForce( scale ); m_pController->WakeObjects(); } //----------------------------------------------------------------------------- // Purpose: A rocket-engine/thruster based on the force controller above // Calculate the force (and optional torque) that the engine would create //----------------------------------------------------------------------------- class CPhysThruster : public CPhysForce { DECLARE_CLASS( CPhysThruster, CPhysForce ); public: DECLARE_DATADESC(); virtual void OnActivate( void ); virtual void SetupForces( IPhysicsObject *pPhys, Vector &linear, AngularImpulse &angular ); private: Vector m_localOrigin; }; LINK_ENTITY_TO_CLASS( phys_thruster, CPhysThruster ); BEGIN_DATADESC( CPhysThruster ) DEFINE_FIELD( m_localOrigin, FIELD_VECTOR ), END_DATADESC() void CPhysThruster::OnActivate( void ) { if ( m_attachedObject != NULL ) { matrix3x4_t worldToAttached, thrusterToAttached; MatrixInvert( m_attachedObject->EntityToWorldTransform(), worldToAttached ); ConcatTransforms( worldToAttached, EntityToWorldTransform(), thrusterToAttached ); MatrixGetColumn( thrusterToAttached, 3, m_localOrigin ); if ( HasSpawnFlags( SF_THRUST_LOCAL_ORIENTATION ) ) { QAngle angles; MatrixAngles( thrusterToAttached, angles ); SetLocalAngles( angles ); } // maintain the local relationship with this entity // it may move before the thruster is activated if ( HasSpawnFlags( SF_THRUST_IGNORE_POS ) ) { m_localOrigin.Init(); } } } // utility function to duplicate this call in local space void CalculateVelocityOffsetLocal( IPhysicsObject *pPhys, const Vector &forceLocal, const Vector &positionLocal, Vector &outVelLocal, AngularImpulse &outAngular ) { Vector posWorld, forceWorld; pPhys->LocalToWorld( &posWorld, positionLocal ); pPhys->LocalToWorldVector( &forceWorld, forceLocal ); Vector velWorld; pPhys->CalculateVelocityOffset( forceWorld, posWorld, &velWorld, &outAngular ); pPhys->WorldToLocalVector( &outVelLocal, velWorld ); } void CPhysThruster::SetupForces( IPhysicsObject *pPhys, Vector &linear, AngularImpulse &angular ) { Vector thrustVector; AngleVectors( GetLocalAngles(), &thrustVector ); thrustVector *= m_force; // multiply the force by mass (it's actually just an acceleration) if ( m_spawnflags & SF_THRUST_MASS_INDEPENDENT ) { thrustVector *= pPhys->GetMass(); } if ( m_spawnflags & SF_THRUST_LOCAL_ORIENTATION ) { CalculateVelocityOffsetLocal( pPhys, thrustVector, m_localOrigin, linear, angular ); } else { Vector position; VectorTransform( m_localOrigin, m_attachedObject->EntityToWorldTransform(), position ); pPhys->CalculateVelocityOffset( thrustVector, position, &linear, &angular ); } if ( !(m_spawnflags & SF_THRUST_FORCE) ) { // clear out force linear.Init(); } if ( !(m_spawnflags & SF_THRUST_TORQUE) ) { // clear out torque angular.Init(); } } //----------------------------------------------------------------------------- // Purpose: A controllable motor - exerts torque //----------------------------------------------------------------------------- class CPhysTorque : public CPhysForce { DECLARE_CLASS( CPhysTorque, CPhysForce ); public: DECLARE_DATADESC(); void Spawn( void ); virtual void SetupForces( IPhysicsObject *pPhys, Vector &linear, AngularImpulse &angular ); private: Vector m_axis; }; BEGIN_DATADESC( CPhysTorque ) DEFINE_KEYFIELD( m_axis, FIELD_VECTOR, "axis" ), END_DATADESC() LINK_ENTITY_TO_CLASS( phys_torque, CPhysTorque ); void CPhysTorque::Spawn( void ) { // force spawnflags to agree with implementation of this class m_spawnflags |= SF_THRUST_TORQUE | SF_THRUST_MASS_INDEPENDENT; m_spawnflags &= ~SF_THRUST_FORCE; m_axis -= GetAbsOrigin(); VectorNormalize(m_axis); UTIL_SnapDirectionToAxis( m_axis ); BaseClass::Spawn(); } void CPhysTorque::SetupForces( IPhysicsObject *pPhys, Vector &linear, AngularImpulse &angular ) { // clear out force linear.Init(); matrix3x4_t matrix; pPhys->GetPositionMatrix( &matrix ); // transform motor axis to local space Vector axis_ls; VectorIRotate( m_axis, matrix, axis_ls ); // Set torque to be around selected axis angular = axis_ls * m_force; } //----------------------------------------------------------------------------- // Purpose: This class only implements the IMotionEvent-specific behavior // It keeps track of the forces so they can be integrated //----------------------------------------------------------------------------- class CMotorController : public IMotionEvent { DECLARE_SIMPLE_DATADESC(); public: IMotionEvent::simresult_e Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular ); float m_speed; float m_maxTorque; Vector m_axis; float m_inertiaFactor; float m_lastSpeed; float m_lastAcceleration; float m_lastForce; float m_restistanceDamping; }; BEGIN_SIMPLE_DATADESC( CMotorController ) DEFINE_FIELD( m_speed, FIELD_FLOAT ), DEFINE_FIELD( m_maxTorque, FIELD_FLOAT ), DEFINE_KEYFIELD( m_axis, FIELD_VECTOR, "axis" ), DEFINE_KEYFIELD( m_inertiaFactor, FIELD_FLOAT, "inertiafactor" ), DEFINE_FIELD( m_lastSpeed, FIELD_FLOAT ), DEFINE_FIELD( m_lastAcceleration, FIELD_FLOAT ), DEFINE_FIELD( m_lastForce, FIELD_FLOAT ), DEFINE_FIELD( m_restistanceDamping, FIELD_FLOAT ), END_DATADESC() IMotionEvent::simresult_e CMotorController::Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular ) { linear = vec3_origin; angular = vec3_origin; if ( m_speed == 0 ) return SIM_NOTHING; matrix3x4_t matrix; pObject->GetPositionMatrix( &matrix ); AngularImpulse currentRotAxis; // currentRotAxis is in local space pObject->GetVelocity( NULL, ¤tRotAxis ); // transform motor axis to local space Vector motorAxis_ls; VectorIRotate( m_axis, matrix, motorAxis_ls ); float currentSpeed = DotProduct( currentRotAxis, motorAxis_ls ); float inertia = DotProductAbs( pObject->GetInertia(), motorAxis_ls ); // compute absolute acceleration, don't integrate over the timestep float accel = m_speed - currentSpeed; float rotForce = accel * inertia * m_inertiaFactor; // BUGBUG: This heuristic is a little flaky // UNDONE: Make a better heuristic for speed control if ( fabsf(m_lastAcceleration) > 0 ) { float deltaSpeed = currentSpeed - m_lastSpeed; // make sure they are going the same way if ( deltaSpeed * accel > 0 ) { float factor = deltaSpeed / m_lastAcceleration; factor = 1 - clamp( factor, 0.f, 1.f ); rotForce += m_lastForce * factor * m_restistanceDamping; } else { if ( currentSpeed != 0 ) { // have we reached a steady state that isn't our target? float increase = deltaSpeed / m_lastAcceleration; if ( fabsf(increase) < 0.05 ) { rotForce += m_lastForce * m_restistanceDamping; } } } } // ------------------------------------------------------- if ( m_maxTorque != 0 ) { if ( rotForce > m_maxTorque ) { rotForce = m_maxTorque; } else if ( rotForce < -m_maxTorque ) { rotForce = -m_maxTorque; } } m_lastForce = rotForce; m_lastAcceleration = (rotForce / inertia); m_lastSpeed = currentSpeed; // this is in local space angular = motorAxis_ls * rotForce; return SIM_LOCAL_FORCE; } #define SF_MOTOR_START_ON 0x0001 // starts on by default #define SF_MOTOR_NOCOLLIDE 0x0002 // don't collide with world geometry #define SF_MOTOR_HINGE 0x0004 // motor also acts as a hinge constraining the object to this axis // NOTE: THIS DOESN'T WORK YET #define SF_MOTOR_LOCAL 0x0008 // Maintain local relationship with the attached object class CPhysMotor : public CLogicalEntity { DECLARE_CLASS( CPhysMotor, CLogicalEntity ); public: ~CPhysMotor(); DECLARE_DATADESC(); void Spawn( void ); void Activate( void ); void Think( void ); void TurnOn( void ); void TargetSpeedChanged( void ); void OnRestore(); void InputSetTargetSpeed( inputdata_t &inputdata ); void InputTurnOn( inputdata_t &inputdata ); void InputTurnOff( inputdata_t &inputdata ); void CalculateAcceleration(); string_t m_nameAttach; EHANDLE m_attachedObject; float m_spinUp; float m_additionalAcceleration; float m_angularAcceleration; float m_lastTime; // FIXME: can we remove m_flSpeed from CBaseEntity? //float m_flSpeed; IPhysicsConstraint *m_pHinge; IPhysicsMotionController *m_pController; CMotorController m_motor; }; BEGIN_DATADESC( CPhysMotor ) DEFINE_KEYFIELD( m_nameAttach, FIELD_STRING, "attach1" ), DEFINE_FIELD( m_attachedObject, FIELD_EHANDLE ), DEFINE_KEYFIELD( m_spinUp, FIELD_FLOAT, "spinup" ), DEFINE_KEYFIELD( m_additionalAcceleration, FIELD_FLOAT, "addangaccel" ), DEFINE_FIELD( m_angularAcceleration, FIELD_FLOAT ), DEFINE_FIELD( m_lastTime, FIELD_TIME ), DEFINE_PHYSPTR( m_pHinge ), DEFINE_PHYSPTR( m_pController ), DEFINE_INPUTFUNC( FIELD_FLOAT, "SetSpeed", InputSetTargetSpeed ), DEFINE_INPUTFUNC( FIELD_VOID, "TurnOn", InputTurnOn ), DEFINE_INPUTFUNC( FIELD_VOID, "TurnOff", InputTurnOff ), DEFINE_EMBEDDED( m_motor ), END_DATADESC() LINK_ENTITY_TO_CLASS( phys_motor, CPhysMotor ); void CPhysMotor::CalculateAcceleration() { if ( m_spinUp ) { m_angularAcceleration = fabsf(m_flSpeed / m_spinUp); } else { m_angularAcceleration = fabsf(m_flSpeed); } } //----------------------------------------------------------------------------- // Purpose: Input handler that sets a speed to spin up or down to. //----------------------------------------------------------------------------- void CPhysMotor::InputSetTargetSpeed( inputdata_t &inputdata ) { if ( m_flSpeed == inputdata.value.Float() ) return; m_flSpeed = inputdata.value.Float(); TargetSpeedChanged(); CalculateAcceleration(); } void CPhysMotor::TargetSpeedChanged( void ) { SetNextThink( gpGlobals->curtime ); m_lastTime = gpGlobals->curtime; m_pController->WakeObjects(); } //------------------------------------------------------------------------------ // Purpose: Input handler that turns the motor on. //------------------------------------------------------------------------------ void CPhysMotor::InputTurnOn( inputdata_t &inputdata ) { TurnOn(); } //------------------------------------------------------------------------------ // Purpose: Input handler that turns the motor off. //------------------------------------------------------------------------------ void CPhysMotor::InputTurnOff( inputdata_t &inputdata ) { m_motor.m_speed = 0; SetNextThink( TICK_NEVER_THINK ); } CPhysMotor::~CPhysMotor() { if ( m_attachedObject && m_pHinge ) { IPhysicsObject *pPhys = m_attachedObject->VPhysicsGetObject(); if ( pPhys ) { PhysClearGameFlags(pPhys, FVPHYSICS_NO_PLAYER_PICKUP); } } physenv->DestroyConstraint( m_pHinge ); physenv->DestroyMotionController( m_pController ); } void CPhysMotor::Spawn( void ) { m_motor.m_axis -= GetLocalOrigin(); float axisLength = VectorNormalize(m_motor.m_axis); // double check that the axis is at least a unit long. If not, warn and self-destruct. if ( axisLength > 1.0f ) { UTIL_SnapDirectionToAxis( m_motor.m_axis ); } else { Warning("phys_motor %s does not have a valid axis helper, and self-destructed!\n", GetDebugName()); m_motor.m_speed = 0; SetNextThink( TICK_NEVER_THINK ); UTIL_Remove(this); } } void CPhysMotor::TurnOn( void ) { CBaseEntity *pAttached = m_attachedObject; if ( !pAttached ) return; IPhysicsObject *pPhys = pAttached->VPhysicsGetObject(); if ( pPhys ) { m_pController->WakeObjects(); // If the current speed is zero, the objects can run a tick without getting torque'd and go back to sleep // so force a think now and have some acceleration happen before the controller gets called. m_lastTime = gpGlobals->curtime - TICK_INTERVAL; Think(); } } void CPhysMotor::Activate( void ) { BaseClass::Activate(); // This gets called after all objects spawn and after all objects restore if ( m_attachedObject == NULL ) { CBaseEntity *pAttach = gEntList.FindEntityByName( NULL, m_nameAttach ); if ( pAttach && pAttach->GetMoveType() == MOVETYPE_VPHYSICS ) { m_attachedObject = pAttach; IPhysicsObject *pPhys = m_attachedObject->VPhysicsGetObject(); CalculateAcceleration(); matrix3x4_t matrix; pPhys->GetPositionMatrix( &matrix ); Vector motorAxis_ls; VectorIRotate( m_motor.m_axis, matrix, motorAxis_ls ); float inertia = DotProductAbs( pPhys->GetInertia(), motorAxis_ls ); m_motor.m_maxTorque = inertia * m_motor.m_inertiaFactor * (m_angularAcceleration + m_additionalAcceleration); m_motor.m_restistanceDamping = 1.0f; } } if ( m_attachedObject ) { IPhysicsObject *pPhys = m_attachedObject->VPhysicsGetObject(); // create a hinge constraint for this object? if ( m_spawnflags & SF_MOTOR_HINGE ) { // UNDONE: Don't do this on restore? if ( !m_pHinge ) { constraint_hingeparams_t hingeParams; hingeParams.Defaults(); hingeParams.worldAxisDirection = m_motor.m_axis; hingeParams.worldPosition = GetLocalOrigin(); m_pHinge = physenv->CreateHingeConstraint( g_PhysWorldObject, pPhys, NULL, hingeParams ); m_pHinge->SetGameData( (void *)this ); // can't grab this object PhysSetGameFlags(pPhys, FVPHYSICS_NO_PLAYER_PICKUP); } if ( m_spawnflags & SF_MOTOR_NOCOLLIDE ) { PhysDisableEntityCollisions( g_PhysWorldObject, pPhys ); } } else { m_pHinge = NULL; } // NOTE: On restore, this path isn't run because m_pController will not be NULL if ( !m_pController ) { m_pController = physenv->CreateMotionController( &m_motor ); m_pController->AttachObject( m_attachedObject->VPhysicsGetObject(), false ); if ( m_spawnflags & SF_MOTOR_START_ON ) { TurnOn(); } } } } void CPhysMotor::OnRestore() { BaseClass::OnRestore(); // Need to do this on restore since there's no good way to save this if ( m_pController ) { m_pController->SetEventHandler( &m_motor ); } } void CPhysMotor::Think( void ) { // angular acceleration is always positive - it should be treated as a magnitude - the controller // will apply it in the proper direction Assert(m_angularAcceleration>=0); m_motor.m_speed = UTIL_Approach( m_flSpeed, m_motor.m_speed, m_angularAcceleration*(gpGlobals->curtime-m_lastTime) ); m_lastTime = gpGlobals->curtime; if ( m_motor.m_speed != m_flSpeed ) { SetNextThink( gpGlobals->curtime ); } } //====================================================================================== // KEEPUPRIGHT CONTROLLER //====================================================================================== class CKeepUpright : public CPointEntity, public IMotionEvent { DECLARE_CLASS( CKeepUpright, CPointEntity ); public: DECLARE_DATADESC(); CKeepUpright(); ~CKeepUpright(); void Spawn(); void Activate(); // IMotionEvent virtual simresult_e Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular ); // Inputs void InputTurnOn( inputdata_t &inputdata ) { m_bActive = true; } void InputTurnOff( inputdata_t &inputdata ) { m_bActive = false; } void InputSetAngularLimit( inputdata_t &inputdata ) { m_angularLimit = inputdata.value.Float(); } private: friend CBaseEntity *CreateKeepUpright( const Vector &vecOrigin, const QAngle &vecAngles, CBaseEntity *pOwner, float flAngularLimit, bool bActive ); Vector m_worldGoalAxis; Vector m_localTestAxis; IPhysicsMotionController *m_pController; string_t m_nameAttach; EHANDLE m_attachedObject; float m_angularLimit; bool m_bActive; bool m_bDampAllRotation; }; #define SF_KEEPUPRIGHT_START_INACTIVE 0x0001 LINK_ENTITY_TO_CLASS( phys_keepupright, CKeepUpright ); BEGIN_DATADESC( CKeepUpright ) DEFINE_FIELD( m_worldGoalAxis, FIELD_VECTOR ), DEFINE_FIELD( m_localTestAxis, FIELD_VECTOR ), DEFINE_PHYSPTR( m_pController ), DEFINE_KEYFIELD( m_nameAttach, FIELD_STRING, "attach1" ), DEFINE_FIELD( m_attachedObject, FIELD_EHANDLE ), DEFINE_KEYFIELD( m_angularLimit, FIELD_FLOAT, "angularlimit" ), DEFINE_FIELD( m_bActive, FIELD_BOOLEAN ), DEFINE_FIELD( m_bDampAllRotation, FIELD_BOOLEAN ), DEFINE_INPUTFUNC( FIELD_VOID, "TurnOn", InputTurnOn ), DEFINE_INPUTFUNC( FIELD_VOID, "TurnOff", InputTurnOff ), DEFINE_INPUTFUNC( FIELD_FLOAT, "SetAngularLimit", InputSetAngularLimit ), END_DATADESC() CKeepUpright::CKeepUpright() { // by default, recover from up to 15 degrees / sec angular velocity m_angularLimit = 15; m_attachedObject = NULL; m_bDampAllRotation = false; } CKeepUpright::~CKeepUpright() { if ( m_pController ) { physenv->DestroyMotionController( m_pController ); m_pController = NULL; } } void CKeepUpright::Spawn() { // align the object's local Z axis m_localTestAxis.Init( 0, 0, 1 ); // Use our Up axis so mapmakers can orient us arbitrarily GetVectors( NULL, NULL, &m_worldGoalAxis ); SetMoveType( MOVETYPE_NONE ); if ( m_spawnflags & SF_KEEPUPRIGHT_START_INACTIVE ) { m_bActive = false; } else { m_bActive = true; } } void CKeepUpright::Activate() { BaseClass::Activate(); if ( !m_pController ) { // This case occurs when spawning IPhysicsObject *pPhys; if ( m_attachedObject ) { pPhys = m_attachedObject->VPhysicsGetObject(); } else { pPhys = FindPhysicsObjectByName( STRING(m_nameAttach), this ); } if ( !pPhys ) { UTIL_Remove(this); return; } // HACKHACK: Due to changes in the vehicle simulator the keepupright controller used in coast_01 is unstable // force it to have perfect damping to compensate. // detect it using the hack of angular limit == 150, attached to a vehicle // Fixing it in the code is the simplest course of action presently #ifdef HL2_DLL if ( m_angularLimit == 150.0f ) { CBaseEntity *pEntity = static_cast(pPhys->GetGameData()); if ( pEntity && pEntity->GetServerVehicle() && Q_stristr( gpGlobals->mapname.ToCStr(), "d2_coast_01" ) ) { m_bDampAllRotation = true; } } #endif m_pController = physenv->CreateMotionController( (IMotionEvent *)this ); m_pController->AttachObject( pPhys, false ); } else { // This case occurs when restoring m_pController->SetEventHandler( this ); } } //----------------------------------------------------------------------------- // Purpose: Use this to spawn a keepupright controller via code instead of map-placed //----------------------------------------------------------------------------- CBaseEntity *CreateKeepUpright( const Vector &vecOrigin, const QAngle &vecAngles, CBaseEntity *pOwner, float flAngularLimit, bool bActive ) { CKeepUpright *pKeepUpright = (CKeepUpright*)CBaseEntity::Create( "phys_keepupright", vecOrigin, vecAngles, pOwner ); if ( pKeepUpright ) { pKeepUpright->m_attachedObject = pOwner; pKeepUpright->m_angularLimit = flAngularLimit; if ( !bActive ) { pKeepUpright->AddSpawnFlags( SF_KEEPUPRIGHT_START_INACTIVE ); } pKeepUpright->Spawn(); pKeepUpright->Activate(); } return pKeepUpright; } IMotionEvent::simresult_e CKeepUpright::Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular ) { if ( !m_bActive ) return SIM_NOTHING; linear.Init(); AngularImpulse angVel; pObject->GetVelocity( NULL, &angVel ); matrix3x4_t matrix; // get the object's local to world transform pObject->GetPositionMatrix( &matrix ); // Get the alignment axis in object space Vector currentLocalTargetAxis; VectorIRotate( m_worldGoalAxis, matrix, currentLocalTargetAxis ); float invDeltaTime = (1/deltaTime); if ( m_bDampAllRotation ) { angular = ComputeRotSpeedToAlignAxes( m_localTestAxis, currentLocalTargetAxis, angVel, 0, invDeltaTime, m_angularLimit ); angular -= angVel; angular *= invDeltaTime; return SIM_LOCAL_ACCELERATION; } angular = ComputeRotSpeedToAlignAxes( m_localTestAxis, currentLocalTargetAxis, angVel, 1.0, invDeltaTime, m_angularLimit ); angular *= invDeltaTime; #if 0 Vector position, out, worldAxis; MatrixGetColumn( matrix, 3, position ); out = angular * 0.1; VectorRotate( m_localTestAxis, matrix, worldAxis ); NDebugOverlay::Line( position, position + worldAxis * 100, 255, 0, 0, 0, 0 ); NDebugOverlay::Line( position, position + m_worldGoalAxis * 100, 255, 0, 0, 0, 0 ); NDebugOverlay::Line( position, position + out, 255, 255, 0, 0, 0 ); #endif return SIM_LOCAL_ACCELERATION; } // computes the torque necessary to align testAxis with alignAxis AngularImpulse ComputeRotSpeedToAlignAxes( const Vector &testAxis, const Vector &alignAxis, const AngularImpulse ¤tSpeed, float damping, float scale, float maxSpeed ) { Vector rotationAxis = CrossProduct( testAxis, alignAxis ); // atan2() is well defined, so do a Dot & Cross instead of asin(Cross) float cosine = DotProduct( testAxis, alignAxis ); float sine = VectorNormalize( rotationAxis ); float angle = atan2( sine, cosine ); angle = RAD2DEG(angle); AngularImpulse angular = rotationAxis * scale * angle; angular -= rotationAxis * damping * DotProduct( currentSpeed, rotationAxis ); float len = VectorNormalize( angular ); if ( len > maxSpeed ) { len = maxSpeed; } return angular * len; }