Modified source engine (2017) developed by valve and leaked in 2020. Not for commercial purporses
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

1079 lines
28 KiB

5 years ago
//========= 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, &currentRotAxis );
// 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<CBaseEntity *>(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 &currentSpeed, 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;
}