3418 lines
96 KiB
C++
3418 lines
96 KiB
C++
|
// Copyright (C) 2007 Id Software, Inc.
|
||
|
//
|
||
|
|
||
|
|
||
|
#include "../precompiled.h"
|
||
|
#pragma hdrstop
|
||
|
|
||
|
#if defined( _DEBUG ) && !defined( ID_REDIRECT_NEWDELETE )
|
||
|
#define new DEBUG_NEW
|
||
|
#undef THIS_FILE
|
||
|
static char THIS_FILE[] = __FILE__;
|
||
|
#endif
|
||
|
|
||
|
#include "Physics_RigidBodyMultiple.h"
|
||
|
#include "Clip.h"
|
||
|
#include "../Player.h"
|
||
|
#include "../../decllib/DeclSurfaceType.h"
|
||
|
#include "../vehicles/Transport.h"
|
||
|
#include "../Game_local.h"
|
||
|
#include "../vehicles/VehicleControl.h"
|
||
|
#include "../ContentMask.h"
|
||
|
|
||
|
#include "../misc/ProfileHelper.h"
|
||
|
|
||
|
const float GRAVITY_STOP_SPEED = 20.0f;
|
||
|
const float STOP_SPEED = 0.0f;
|
||
|
const float CONTACT_LCP_EPSILON = 1e-8f;
|
||
|
const float IMPULSE_WAKE_STRENGTH = 15000.0f; // this is the minimum impulse size that will wake this up
|
||
|
|
||
|
//#define RB_TIMINGS
|
||
|
|
||
|
#ifdef RB_TIMINGS
|
||
|
static int lastTimerReset = 0;
|
||
|
static int numRigidBodies = 0;
|
||
|
static idTimer timer_total, timer_collision, timer_findcontacts, timer_contacts;
|
||
|
#endif
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRBMultipleNetworkState::MakeDefault
|
||
|
================
|
||
|
*/
|
||
|
void sdRBMultipleNetworkState::MakeDefault( void ) {
|
||
|
origin = vec3_origin;
|
||
|
linearVelocity = vec3_zero;
|
||
|
angularVelocity = vec3_zero;
|
||
|
orientation.x = 0.f;
|
||
|
orientation.y = 0.f;
|
||
|
orientation.z = 0.f;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRBMultipleNetworkState::Write
|
||
|
================
|
||
|
*/
|
||
|
void sdRBMultipleNetworkState::Write( idFile* file ) const {
|
||
|
file->WriteVec3( origin );
|
||
|
file->WriteVec3( linearVelocity );
|
||
|
file->WriteVec3( angularVelocity );
|
||
|
file->WriteCQuat( orientation );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRBMultipleNetworkState::Read
|
||
|
================
|
||
|
*/
|
||
|
void sdRBMultipleNetworkState::Read( idFile* file ) {
|
||
|
file->ReadVec3( origin );
|
||
|
file->ReadVec3( linearVelocity );
|
||
|
file->ReadVec3( angularVelocity );
|
||
|
file->ReadCQuat( orientation );
|
||
|
|
||
|
// denormals are bad!
|
||
|
origin.FixDenormals();
|
||
|
linearVelocity.FixDenormals();
|
||
|
angularVelocity.FixDenormals();
|
||
|
orientation.FixDenormals();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRBMultipleBroadcastState::MakeDefault
|
||
|
================
|
||
|
*/
|
||
|
void sdRBMultipleBroadcastState::MakeDefault( void ) {
|
||
|
localOrigin = vec3_origin;
|
||
|
localOrientation.x = 0.f;
|
||
|
localOrientation.y = 0.f;
|
||
|
localOrientation.z = 0.f;
|
||
|
atRest = 0;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRBMultipleBroadcastState::Write
|
||
|
================
|
||
|
*/
|
||
|
void sdRBMultipleBroadcastState::Write( idFile* file ) const {
|
||
|
file->WriteVec3( localOrigin );
|
||
|
file->WriteCQuat( localOrientation );
|
||
|
file->WriteInt( atRest );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRBMultipleBroadcastState::Read
|
||
|
================
|
||
|
*/
|
||
|
void sdRBMultipleBroadcastState::Read( idFile* file ) {
|
||
|
file->ReadVec3( localOrigin );
|
||
|
file->ReadCQuat( localOrientation );
|
||
|
file->ReadInt( atRest );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
===============================================================
|
||
|
|
||
|
sdRigidBodyMulti_Body
|
||
|
|
||
|
===============================================================
|
||
|
*/
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::sdRigidBodyMulti_Body
|
||
|
================
|
||
|
*/
|
||
|
sdRigidBodyMulti_Body::sdRigidBodyMulti_Body( void ) {
|
||
|
clipModel = NULL;
|
||
|
centeredClipModel = new idClipModel();
|
||
|
|
||
|
Init();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::sdRigidBodyMulti_Body
|
||
|
================
|
||
|
*/
|
||
|
sdRigidBodyMulti_Body::~sdRigidBodyMulti_Body( void ) {
|
||
|
gameLocal.clip.DeleteClipModel( clipModel );
|
||
|
gameLocal.clip.DeleteClipModel( centeredClipModel );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::DebugDrawMass
|
||
|
================
|
||
|
*/
|
||
|
void sdRigidBodyMulti_Body::DebugDrawMass( void ) {
|
||
|
if( clipModel == NULL ) {
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
gameRenderWorld->DrawText( va( "\n%1.2f", mass ), clipModel->GetOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::Init
|
||
|
================
|
||
|
*/
|
||
|
void sdRigidBodyMulti_Body::Init( void ) {
|
||
|
centerOfMass.Zero();
|
||
|
inertiaTensor.Identity();
|
||
|
localOrigin.Zero();
|
||
|
|
||
|
contactFriction.Set( 0.001f, 0.05f, 0.05f );
|
||
|
mass = 1.0f;
|
||
|
inverseMass = 1.0f;
|
||
|
inverseInertiaTensor = inertiaTensor.Inverse() * ( 1.0f / 6.0f );
|
||
|
buoyancy = 1.0f;
|
||
|
waterDrag = 0.0f;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::SetClipModel
|
||
|
================
|
||
|
*/
|
||
|
void sdRigidBodyMulti_Body::SetClipModel( idClipModel* _clipModel, float density, int id, bool freeOld ) {
|
||
|
if ( freeOld && clipModel != NULL && ( clipModel != _clipModel ) ) {
|
||
|
gameLocal.clip.DeleteClipModel( clipModel );
|
||
|
}
|
||
|
clipModel = _clipModel;
|
||
|
|
||
|
if ( clipModel ) {
|
||
|
clipModel->SetId( id );
|
||
|
|
||
|
// get mass properties from the trace model
|
||
|
clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor );
|
||
|
if ( mass < idMath::FLT_EPSILON || FLOAT_IS_NAN( mass ) ) {
|
||
|
gameLocal.Warning( "sdRigidBodyMulti_Body::SetClipModel: invalid mass" );
|
||
|
Init();
|
||
|
}
|
||
|
|
||
|
inverseMass = 1.0f / mass;
|
||
|
inverseInertiaTensor = inertiaTensor.Inverse() * ( 1.0f / 6.0f );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::SetMainCenterOfMass
|
||
|
================
|
||
|
*/
|
||
|
void sdRigidBodyMulti_Body::SetMainCenterOfMass( const idVec3& com ) {
|
||
|
if ( clipModel ) {
|
||
|
assert( clipModel->GetTraceModel() != NULL );
|
||
|
|
||
|
idTraceModel tempTrace = *clipModel->GetTraceModel();
|
||
|
tempTrace.Translate( localOrigin - com );
|
||
|
centeredClipModel->LoadTraceModel( tempTrace, false );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::Link
|
||
|
================
|
||
|
*/
|
||
|
void sdRigidBodyMulti_Body::Link( idEntity* self, const rigidBodyPState_t& current ) {
|
||
|
if( !clipModel ) {
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
idVec3 org = current.i.position + ( localOrigin * current.i.orientation );
|
||
|
clipModel->Link( gameLocal.clip, self, clipModel->GetId(), org, current.i.orientation );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::UnLink
|
||
|
================
|
||
|
*/
|
||
|
void sdRigidBodyMulti_Body::UnLink( void ) {
|
||
|
if( clipModel != 0) {
|
||
|
clipModel->Unlink( gameLocal.clip );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdRigidBodyMulti_Body::SetMass
|
||
|
================
|
||
|
*/
|
||
|
void sdRigidBodyMulti_Body::SetMass( float _mass ) {
|
||
|
assert( _mass > 0.0f );
|
||
|
inertiaTensor *= _mass / mass;
|
||
|
inverseInertiaTensor = inertiaTensor.Inverse() * ( 1.0f / 6.0f );
|
||
|
mass = _mass;
|
||
|
inverseMass = 1.0f / mass;
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
RigidBodyDerivatives_Multi
|
||
|
================
|
||
|
*/
|
||
|
void RigidBodyDerivatives_Multi( const float t, const void *clientData, const float *state, float *derivatives ) {
|
||
|
const sdPhysics_RigidBodyMultiple* p = reinterpret_cast< const sdPhysics_RigidBodyMultiple* >( clientData );
|
||
|
const rigidBodyIState_t *s = reinterpret_cast< const rigidBodyIState_t* >( state );
|
||
|
|
||
|
struct rigidBodyDerivatives_s {
|
||
|
idVec3 linearVelocity;
|
||
|
idMat3 angularMatrix;
|
||
|
idVec3 force;
|
||
|
idVec3 torque;
|
||
|
} *d = ( struct rigidBodyDerivatives_s* ) derivatives;
|
||
|
|
||
|
idVec3 angularVelocity;
|
||
|
idMat3 inverseWorldInertiaTensor;
|
||
|
|
||
|
inverseWorldInertiaTensor = s->orientation * p->GetMainInverseInertiaTensor() * s->orientation.Transpose();
|
||
|
angularVelocity = inverseWorldInertiaTensor * s->angularMomentum;
|
||
|
|
||
|
// derivatives
|
||
|
d->linearVelocity = p->GetMainInverseMass() * s->linearMomentum;
|
||
|
d->angularMatrix = SkewSymmetric( angularVelocity ) * s->orientation;
|
||
|
|
||
|
const rigidBodyPState_t& current = p->GetCurrentState();
|
||
|
|
||
|
float linearFriction = p->InWater() ? p->GetLinearWaterFriction() : p->GetLinearFriction();
|
||
|
float angularFriction = p->InWater() ? p->GetAngularWaterFriction() : p->GetAngularFriction();
|
||
|
|
||
|
d->force = -linearFriction * s->linearMomentum + ( current.externalForce );
|
||
|
d->torque = -angularFriction * s->angularMomentum + ( current.externalTorque );
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
/*
|
||
|
===============================================================
|
||
|
|
||
|
sdPhysics_RigidBodyMultiple
|
||
|
|
||
|
===============================================================
|
||
|
*/
|
||
|
|
||
|
CLASS_DECLARATION( idPhysics_Base, sdPhysics_RigidBodyMultiple )
|
||
|
END_CLASS
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::sdPhysics_RigidBodyMultiple
|
||
|
================
|
||
|
*/
|
||
|
sdPhysics_RigidBodyMultiple::sdPhysics_RigidBodyMultiple( void ) {
|
||
|
// set default rigid body properties
|
||
|
SetBouncyness( 0.6f );
|
||
|
SetFriction( 0.6f, 0.6f );
|
||
|
SetWaterRestThreshold( 1.f );
|
||
|
|
||
|
// initialize state
|
||
|
current = &state[ 0 ];
|
||
|
next = &state[ 1 ];
|
||
|
|
||
|
memset( current, 0, sizeof( *current ) );
|
||
|
|
||
|
current->atRest = -1;
|
||
|
current->lastTimeStep = gameLocal.msec;
|
||
|
|
||
|
current->i.position.Zero();
|
||
|
current->i.orientation.Identity();
|
||
|
|
||
|
current->i.linearMomentum.Zero();
|
||
|
current->i.angularMomentum.Zero();
|
||
|
|
||
|
*next = *current;
|
||
|
saved = *current;
|
||
|
|
||
|
mainMass = 1.0f;
|
||
|
mainInverseMass = 1.0f;
|
||
|
mainCenterOfMass.Zero();
|
||
|
mainInertiaTensor.Identity();
|
||
|
mainInverseInertiaTensor.Identity();
|
||
|
customInertiaTensor = false;
|
||
|
|
||
|
waterLevel = 0.0f;
|
||
|
|
||
|
activateEndTime = 0;
|
||
|
|
||
|
// use the least expensive euler integrator
|
||
|
integrator = new idODE_Euler( sizeof( rigidBodyIState_t ) / sizeof( float ), RigidBodyDerivatives_Multi, this );
|
||
|
|
||
|
lcp = idLCP::AllocSymmetric();
|
||
|
|
||
|
masterEntity = NULL;
|
||
|
mainClipModel = new idClipModel();
|
||
|
centeredMainClipModel = new idClipModel();
|
||
|
|
||
|
flags.noImpact = false;
|
||
|
flags.noContact = false;
|
||
|
flags.isOrientated = false;
|
||
|
flags.useFastPath = false;
|
||
|
flags.comeToRest = true;
|
||
|
flags.noGravity = false;
|
||
|
flags.frozen = false;
|
||
|
|
||
|
memset( &lastCollision, 0, sizeof( lastCollision ) );
|
||
|
lastCollision.trace.fraction = 1.0f;
|
||
|
|
||
|
blockedTime = 0;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::~sdPhysics_RigidBodyMultiple
|
||
|
================
|
||
|
*/
|
||
|
sdPhysics_RigidBodyMultiple::~sdPhysics_RigidBodyMultiple( void ) {
|
||
|
delete integrator;
|
||
|
delete lcp;
|
||
|
gameLocal.clip.DeleteClipModel( mainClipModel );
|
||
|
gameLocal.clip.DeleteClipModel( centeredMainClipModel );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CalculateMassProperties
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::CalculateMassProperties( void ) {
|
||
|
int i;
|
||
|
|
||
|
mainMass = 0;
|
||
|
|
||
|
idVec3 moments;
|
||
|
moments.Zero();
|
||
|
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ i ];
|
||
|
if( !body.GetClipModel() ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
mainMass += body.GetMass();
|
||
|
|
||
|
moments += ( body.GetCenterOfMass() + body.GetOffset() ) * body.GetMass();
|
||
|
}
|
||
|
|
||
|
if ( mainMass ) {
|
||
|
mainInverseMass = 1.f / mainMass;
|
||
|
mainCenterOfMass = moments * mainInverseMass;
|
||
|
} else {
|
||
|
mainInverseMass = 0.f;
|
||
|
mainCenterOfMass.Zero();
|
||
|
}
|
||
|
|
||
|
idMat3 oldMainInertiaTensor = mainInertiaTensor;
|
||
|
|
||
|
mainInertiaTensor.Zero();
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ i ];
|
||
|
|
||
|
idMat3 localInertiaTensor = body.GetInertiaTensor();
|
||
|
localInertiaTensor.InertiaTranslateSelf( body.GetMass(), body.GetCenterOfMass() + body.GetOffset(), mainCenterOfMass - ( body.GetCenterOfMass() + body.GetOffset() ) );
|
||
|
mainInertiaTensor += localInertiaTensor;
|
||
|
|
||
|
body.SetMainCenterOfMass( mainCenterOfMass );
|
||
|
}
|
||
|
|
||
|
if ( customInertiaTensor ) {
|
||
|
mainInertiaTensor = oldMainInertiaTensor;
|
||
|
}
|
||
|
|
||
|
if ( bodies.Num() ) {
|
||
|
mainInverseInertiaTensor = mainInertiaTensor.Inverse() * ( 1.f / 6.f );
|
||
|
} else {
|
||
|
mainInverseInertiaTensor.Zero();
|
||
|
}
|
||
|
|
||
|
idMat3 axis = mat3_identity;
|
||
|
|
||
|
mainBounds.Clear();
|
||
|
totalBounds.Clear();
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
idClipModel* clipModel = bodies[ i ].GetClipModel();
|
||
|
if ( !clipModel ) {
|
||
|
continue;
|
||
|
}
|
||
|
idBounds bounds = clipModel->GetBounds();
|
||
|
bounds.TranslateSelf( bodies[ i ].GetOffset() );
|
||
|
|
||
|
totalBounds += bounds;
|
||
|
|
||
|
if ( bodies[ i ].GetClipMask() && clipModel->GetContents() != MASK_HURTZONE ) {
|
||
|
mainBounds += bounds;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
idTraceModel trm( mainBounds );
|
||
|
if ( !mainBounds.IsCleared() ) {
|
||
|
mainClipModel->LoadTraceModel( trm, false );
|
||
|
|
||
|
trm.Translate( -mainCenterOfMass );
|
||
|
centeredMainClipModel->LoadTraceModel( trm, false );
|
||
|
}
|
||
|
|
||
|
mainClipMask = 0;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
mainClipMask |= bodies[ i ].GetClipMask();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetBodyOffset
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetBodyOffset( int id, const idVec3& offset ) {
|
||
|
assert( id >= 0 && id < bodies.Num() );
|
||
|
|
||
|
sdRigidBodyMulti_Body& body = bodies[ id ];
|
||
|
body.SetOffset( offset );
|
||
|
body.Link( self, *current );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetBodyBuoyancy
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetBodyBuoyancy( int id, float buoyancy ) {
|
||
|
assert( id >= 0 && id < bodies.Num() );
|
||
|
|
||
|
sdRigidBodyMulti_Body& body = bodies[ id ];
|
||
|
body.SetBuoyancy( buoyancy );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetBodyWaterDrag
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetBodyWaterDrag( int id, float drag ) {
|
||
|
assert( id >= 0 && id < bodies.Num() );
|
||
|
|
||
|
sdRigidBodyMulti_Body& body = bodies[ id ];
|
||
|
body.SetWaterDrag( drag );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ClearClipModels
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::ClearClipModels( void ) {
|
||
|
for( int i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].SetClipModel( NULL, 1.f, i, true );
|
||
|
}
|
||
|
bodies.Clear();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetClipModel
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetClipModel( idClipModel *model, float density, int id, bool freeOld ) {
|
||
|
|
||
|
assert( self );
|
||
|
assert( model ); // we need a clip model
|
||
|
assert( model->IsTraceModel() ); // and it should be a trace model
|
||
|
assert( density > 0.0f ); // density should be valid
|
||
|
assert( id >= 0 );
|
||
|
|
||
|
int max = Max( id + 1, bodies.Num() );
|
||
|
if ( max > bodies.Num() ) {
|
||
|
bodies.AssureSize( max );
|
||
|
}
|
||
|
|
||
|
sdRigidBodyMulti_Body& body = bodies[ id ];
|
||
|
body.SetClipModel( model, density, id, freeOld );
|
||
|
body.Link( self, *current );
|
||
|
|
||
|
current->i.linearMomentum.Zero();
|
||
|
current->i.angularMomentum.Zero();
|
||
|
|
||
|
*next = *current;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetClipModel
|
||
|
================
|
||
|
*/
|
||
|
idClipModel* sdPhysics_RigidBodyMultiple::GetClipModel( int id ) const {
|
||
|
if( id < 0 || id >= bodies.Num() ) {
|
||
|
return NULL;
|
||
|
}
|
||
|
return bodies[ id ].GetClipModel();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetNumClipModels
|
||
|
================
|
||
|
*/
|
||
|
int sdPhysics_RigidBodyMultiple::GetNumClipModels( void ) const {
|
||
|
return bodies.Num();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetBouncyness
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetBouncyness( const float b ) {
|
||
|
if ( b < 0.0f || b > 1.0f ) {
|
||
|
return;
|
||
|
}
|
||
|
bouncyness = b;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetWaterRestThreshold
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetWaterRestThreshold( float threshold ) {
|
||
|
waterRestThreshold = threshold;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetFriction
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetFriction( const float linear, const float angular ) {
|
||
|
if ( linear < 0.0f || linear > 1.0f || angular < 0.0f || angular > 1.0f ) {
|
||
|
gameLocal.Warning( "sdPhysics_RigidBodyMultiple::SetFriction: friction out of range, linear = %.1f, angular = %.1f", linear, angular );
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
linearFriction = linear;
|
||
|
angularFriction = angular;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetWaterFriction
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetWaterFriction( const float linear, const float angular ) {
|
||
|
if ( linear < 0.0f || linear > 1.0f || angular < 0.0f || angular > 1.0f ) {
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
linearFrictionWater = linear;
|
||
|
angularFrictionWater = angular;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetContactFriction
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetContactFriction( const int id, const idVec3& contact ) {
|
||
|
assert( id >= 0 && id < bodies.Num() );
|
||
|
if ( !( id >= 0 && id < bodies.Num() ) ) {
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
bodies[ id ].SetContactFriction( contact );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::Freeze
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetFrozen( bool freeze ) {
|
||
|
flags.frozen = freeze;
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::Integrate
|
||
|
|
||
|
Calculate next state from the current state using an integrator.
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::Integrate( float deltaTime ) {
|
||
|
idVec3 position( current->i.position );
|
||
|
current->i.position += mainCenterOfMass * current->i.orientation;
|
||
|
|
||
|
current->i.orientation.TransposeSelf();
|
||
|
|
||
|
integrator->Evaluate( (float *) ¤t->i, (float *) &next->i, 0, deltaTime );
|
||
|
next->i.orientation.OrthoNormalizeSelf();
|
||
|
|
||
|
if ( !flags.noGravity ) {
|
||
|
next->i.linearMomentum += deltaTime * gravityVector * mainMass;
|
||
|
}
|
||
|
|
||
|
current->i.orientation.TransposeSelf();
|
||
|
next->i.orientation.TransposeSelf();
|
||
|
|
||
|
current->i.position = position;
|
||
|
next->i.position -= mainCenterOfMass * next->i.orientation;
|
||
|
|
||
|
next->atRest = current->atRest;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CollisionImpulse
|
||
|
|
||
|
Calculates the collision impulse using the velocity relative to the collision object.
|
||
|
The current state should be set to the moment of impact.
|
||
|
================
|
||
|
*/
|
||
|
#define DEBOUNCE_FACTOR 1.0f
|
||
|
|
||
|
bool sdPhysics_RigidBodyMultiple::CollisionImpulse( const trace_t& collision, idVec3& impulse, idVec3& relativeVelocity, bool noCollisionDamage ) {
|
||
|
|
||
|
// get info from other entity involved
|
||
|
impactInfo_t info;
|
||
|
idEntity* ent = gameLocal.entities[collision.c.entityNum];
|
||
|
idPhysics* phys = ent->GetPhysics();
|
||
|
ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info );
|
||
|
|
||
|
|
||
|
// gather information
|
||
|
float e = bouncyness;
|
||
|
idVec3 normal = collision.c.normal;
|
||
|
|
||
|
idVec3 v1A = mainInverseMass * current->i.linearMomentum;
|
||
|
idMat3 invIA = current->i.orientation.Transpose() * mainInverseInertiaTensor * current->i.orientation;
|
||
|
idVec3 w1A = invIA * current->i.angularMomentum;
|
||
|
idVec3 rAP = collision.c.point - ( current->i.position + mainCenterOfMass * current->i.orientation );
|
||
|
v1A = v1A + w1A.Cross( rAP );
|
||
|
|
||
|
float normalVel = v1A * normal;
|
||
|
|
||
|
idMat3 invIB = info.invInertiaTensor;
|
||
|
idVec3 rBP = info.position;
|
||
|
idVec3 v1B = info.velocity;
|
||
|
|
||
|
idVec3 v1AB = v1A - v1B;
|
||
|
relativeVelocity = v1AB;
|
||
|
|
||
|
float j = STOP_SPEED;
|
||
|
|
||
|
|
||
|
if ( normalVel > -STOP_SPEED ) {
|
||
|
j = -normalVel;
|
||
|
} else {
|
||
|
// evaluate
|
||
|
float vNorm = v1AB * normal;
|
||
|
|
||
|
float invMassA = mainInverseMass;
|
||
|
float invMassB = info.invMass;
|
||
|
|
||
|
float invMassSum = invMassA + invMassB;
|
||
|
|
||
|
idVec3 radiusFactorA = ( invIA * rAP.Cross( normal ) ).Cross( rAP );
|
||
|
idVec3 radiusFactorB = ( invIB * rBP.Cross( normal ) ).Cross( rBP );
|
||
|
float radiusFactors = ( radiusFactorA + radiusFactorB ) * normal;
|
||
|
|
||
|
float numerator = -( 1.0f + e ) * vNorm;
|
||
|
float denominator = invMassSum + radiusFactors;
|
||
|
|
||
|
j = numerator / denominator;
|
||
|
|
||
|
if ( j < -normalVel ) {
|
||
|
j = -normalVel;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
if ( collision.c.separation >= 0.0f && collision.c.separation < 0.25f ) {
|
||
|
// squidge impulse bounce scaling
|
||
|
float separationFactor = collision.c.separation * 4.0f;
|
||
|
float factor = ( 1.0f - separationFactor*separationFactor ) * DEBOUNCE_FACTOR;
|
||
|
j *= ( 1.0f + factor );
|
||
|
}
|
||
|
|
||
|
impulse = j * normal;
|
||
|
|
||
|
// if this collision isn't going to result in this body getting pushed,
|
||
|
// double the impulse and leave it all for the thing we're colliding with
|
||
|
|
||
|
// update linear and angular momentum with impulse
|
||
|
if ( self->IsCollisionPushable() || info.invMass == 0.0f || !phys->IsPushable() ) {
|
||
|
current->i.linearMomentum += impulse;
|
||
|
current->i.angularMomentum += rAP.Cross(impulse);
|
||
|
|
||
|
// if no movement at all don't blow up
|
||
|
if ( collision.fraction < 0.0001f ) {
|
||
|
|
||
|
float normalMomentum = current->i.linearMomentum * normal;
|
||
|
if ( normalMomentum < 0.0f ) {
|
||
|
// only scale the component of linear momentum that is towards the normal!!
|
||
|
current->i.linearMomentum -= 0.75f * normalMomentum * normal;
|
||
|
normalMomentum *= 0.25f;
|
||
|
}
|
||
|
current->i.angularMomentum *= 0.5f;
|
||
|
}
|
||
|
} else {
|
||
|
impulse *= 2.0f;
|
||
|
}
|
||
|
|
||
|
// decide which entity hit which - otherwise it is entity update order dependant :(
|
||
|
idVec3 rdA = rAP; rdA.NormalizeFast();
|
||
|
idVec3 rdB = rBP; rdB.NormalizeFast();
|
||
|
float inVel = rdA * v1AB;
|
||
|
float AVel = rdA * v1A;
|
||
|
float BVel = rdB * v1B;
|
||
|
if ( idMath::Fabs( AVel ) > idMath::Fabs( BVel ) ) {
|
||
|
// this entity hit the other
|
||
|
if ( noCollisionDamage ) {
|
||
|
// HACK: Zero the velocity to make no collision damage be done
|
||
|
v1A.Zero();
|
||
|
}
|
||
|
|
||
|
ent->Hit( collision, v1A, self );
|
||
|
return self->Collide( collision, v1A, -1 );
|
||
|
} else {
|
||
|
// the other entity hit this
|
||
|
// set up a trace structure so that the collision code handles things happily
|
||
|
if ( noCollisionDamage ) {
|
||
|
// HACK: Zero the velocity to make no collision damage be done
|
||
|
v1B.Zero();
|
||
|
}
|
||
|
|
||
|
trace_t fakeCollision = collision;
|
||
|
fakeCollision.c.normal = -collision.c.normal;
|
||
|
fakeCollision.c.id = collision.c.selfId;
|
||
|
fakeCollision.c.selfId = collision.c.id;
|
||
|
fakeCollision.c.entityNum = self->entityNumber;
|
||
|
|
||
|
self->Hit( fakeCollision, v1B, ent );
|
||
|
return ent->Collide( fakeCollision, v1B, -1 );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CheckForCollisions
|
||
|
|
||
|
Check for collisions between the current and next state.
|
||
|
If there is a collision the next state is set to the state at the moment of impact.
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::CheckForCollisions( trace_t &collision ) {
|
||
|
collision.fraction = 1.0f;
|
||
|
|
||
|
// don't do anything if the position & orientation aren't changing!
|
||
|
if ( current->i.position.Compare( next->i.position, idMath::FLT_EPSILON ) ) {
|
||
|
if ( current->i.orientation.Compare( next->i.orientation, idMath::FLT_EPSILON ) ) {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
idMat3 axis;
|
||
|
idRotation rotation;
|
||
|
bool collided = false;
|
||
|
int i;
|
||
|
|
||
|
if ( flags.useFastPath && !mainBounds.IsCleared() && bodies.Num() > 1 ) {
|
||
|
if ( !GetTraceCollection().Contents( CLIP_DEBUG_PARMS_ENTINFO( self ) current->i.position, mainClipModel, current->i.orientation, mainClipMask ) ) {
|
||
|
// calculate the position of the center of mass at current and next
|
||
|
idVec3 CoMstart = current->i.position + mainCenterOfMass * current->i.orientation;
|
||
|
idVec3 CoMend = next->i.position + mainCenterOfMass * next->i.orientation;
|
||
|
|
||
|
TransposeMultiply( current->i.orientation, next->i.orientation, axis );
|
||
|
rotation = axis.ToRotation();
|
||
|
rotation.SetOrigin( CoMstart );
|
||
|
|
||
|
trace_t tr;
|
||
|
if( !gameLocal.clip.Motion( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, CoMstart, CoMend, rotation, centeredMainClipModel, current->i.orientation, mainClipMask, self ) ) {
|
||
|
|
||
|
// no collision for the combined bounds, so we can early out
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ i ];
|
||
|
if ( body.GetClipMask() == 0 || body.GetClipMask() == MASK_HURTZONE ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idClipModel* clipModel = body.GetClipModel();
|
||
|
if( !clipModel ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
const idVec3 worldBodyOffset = body.GetOffset() * current->i.orientation;
|
||
|
|
||
|
idVec3 start = current->i.position + worldBodyOffset;
|
||
|
idVec3 end = next->i.position + worldBodyOffset;
|
||
|
|
||
|
trace_t tr;
|
||
|
if ( GetTraceCollection().Translation( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, start, end, clipModel, current->i.orientation, body.GetClipMask() ) ) {
|
||
|
next->i.position = tr.endpos - worldBodyOffset;
|
||
|
next->i.linearMomentum = Lerp( current->i.linearMomentum, next->i.linearMomentum, tr.fraction );
|
||
|
next->i.angularMomentum = current->i.angularMomentum;
|
||
|
collided = true;
|
||
|
|
||
|
// collision is potentially a fraction of a fraction of the full move
|
||
|
float oldFraction = collision.fraction;
|
||
|
collision = tr;
|
||
|
collision.fraction *= oldFraction;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ i ];
|
||
|
if ( body.GetClipMask() == 0 || body.GetClipMask() == MASK_HURTZONE ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
const idClipModel* clipModel = body.GetCenteredClipModel();
|
||
|
if( !clipModel ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idVec3 end = next->i.position + mainCenterOfMass * current->i.orientation;
|
||
|
|
||
|
TransposeMultiply( current->i.orientation, next->i.orientation, axis );
|
||
|
rotation = axis.ToRotation();
|
||
|
if ( idMath::Fabs( rotation.GetAngle() ) < idMath::FLT_EPSILON ) {
|
||
|
continue;
|
||
|
}
|
||
|
rotation.SetOrigin( next->i.position );
|
||
|
|
||
|
trace_t tr;
|
||
|
if ( GetTraceCollection().Rotation( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, end, rotation, clipModel, current->i.orientation, body.GetClipMask() ) ) {
|
||
|
next->i.orientation = tr.endAxis;
|
||
|
next->i.linearMomentum = current->i.linearMomentum;
|
||
|
next->i.angularMomentum = current->i.angularMomentum;
|
||
|
collided = true;
|
||
|
|
||
|
// collision is potentially a fraction of a fraction of the full move
|
||
|
float oldFraction = collision.fraction;
|
||
|
collision = tr;
|
||
|
collision.fraction *= oldFraction;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return collided;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CheckForCollisions_Simple
|
||
|
|
||
|
Rough version of CheckForCollisions that only uses the bounds for tracing
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::CheckForCollisions_Simple( trace_t &collision ) {
|
||
|
|
||
|
collision.fraction = 1.0f;
|
||
|
// don't do anything if the position & orientation aren't changing!
|
||
|
if ( current->i.position.Compare( next->i.position, idMath::FLT_EPSILON ) ) {
|
||
|
if ( current->i.orientation.Compare( next->i.orientation, idMath::FLT_EPSILON ) ) {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( !mainBounds.IsCleared() ) {
|
||
|
int tempClipMask = mainClipMask & ( ~GetVPushClipMask() );
|
||
|
|
||
|
// calculate the position of the center of mass at current and next
|
||
|
idVec3 CoMstart = current->i.position + mainCenterOfMass * current->i.orientation;
|
||
|
idVec3 CoMend = next->i.position + mainCenterOfMass * next->i.orientation;
|
||
|
|
||
|
idMat3 axis;
|
||
|
TransposeMultiply( current->i.orientation, next->i.orientation, axis );
|
||
|
|
||
|
idRotation rotation;
|
||
|
rotation = axis.ToRotation();
|
||
|
rotation.SetOrigin( CoMstart );
|
||
|
|
||
|
trace_t tr;
|
||
|
if( gameLocal.clip.Motion( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, CoMstart, CoMend, rotation, centeredMainClipModel, current->i.orientation, tempClipMask, self ) ) {
|
||
|
next->i.position = tr.endpos - ( mainCenterOfMass * tr.endAxis );
|
||
|
next->i.orientation = tr.endAxis;
|
||
|
next->i.linearMomentum = current->i.linearMomentum;
|
||
|
next->i.angularMomentum = current->i.angularMomentum;
|
||
|
|
||
|
collision = tr;
|
||
|
return true;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CheckForPlayerCollisions_Simple
|
||
|
|
||
|
Rough version of CheckForPlayerCollisions that only uses the bounds for tracing
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::CheckForPlayerCollisions_Simple( float timeDelta, trace_t &collision, bool& noCollisionDamage ) {
|
||
|
// TODO: Implement me!
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetupVPushCollection
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetupVPushCollection( void ) {
|
||
|
GetTraceCollection();
|
||
|
|
||
|
// find which ones to ignore for various reasons
|
||
|
vpushCollection.Clear();
|
||
|
vpushCollection.SetSelf( self );
|
||
|
idList< const idClipModel* >& collection = traceCollection.GetCollection();
|
||
|
int vpushClipMask = GetVPushClipMask();
|
||
|
|
||
|
for ( int i = 0; i < collection.Num(); i++ ) {
|
||
|
const idClipModel* otherModel = collection[ i ];
|
||
|
if ( !( otherModel->GetContents() & vpushClipMask ) ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idEntity* other = otherModel->GetEntity();
|
||
|
if ( other == NULL ) {
|
||
|
continue;
|
||
|
}
|
||
|
if ( !other->IsCollisionPushable() ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idPhysics* otherPhysics = other->GetPhysics();
|
||
|
idPhysics_Actor* actorPhysics = otherPhysics->Cast< idPhysics_Actor >();
|
||
|
|
||
|
if ( actorPhysics == NULL ) {
|
||
|
// only actor-derived things may be vpushed
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idPlayer* playerOther = other->Cast< idPlayer >();
|
||
|
if ( playerOther != NULL ) {
|
||
|
// can't run over players that are our passenger, or the leg model of proned players
|
||
|
if ( playerOther->GetProxyEntity()->Cast< sdTransport >() != NULL || otherModel->GetId() != 0 ) {
|
||
|
// don't want these to touch the normal collision path either
|
||
|
traceCollection.RemoveClipModel( otherModel );
|
||
|
i--;
|
||
|
continue;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// add it to the collection, remove it from the main collection
|
||
|
vpushCollection.AddClipModel( otherModel );
|
||
|
traceCollection.RemoveClipModel( otherModel );
|
||
|
i--;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CheckForPlayerCollisions
|
||
|
|
||
|
Check for collisions against players only between the current and next state, by
|
||
|
hurtzone bodies. These then immediately have the callbacks executed so the players
|
||
|
are caused damage, but no impulse is imparted on player or vehicle.
|
||
|
================
|
||
|
*/
|
||
|
#define RBM_MAX_CLIP_MODELS_COLLECTED 128
|
||
|
bool sdPhysics_RigidBodyMultiple::CheckForPlayerCollisions( float timeDelta, trace_t& collision, bool& noCollisionDamage ) {
|
||
|
|
||
|
// don't do anything if the position & orientation aren't changing!
|
||
|
if ( current->i.position.Compare( next->i.position, idMath::FLT_EPSILON ) ) {
|
||
|
if ( current->i.orientation.Compare( next->i.orientation, idMath::FLT_EPSILON ) ) {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
int vpushClipMask = GetVPushClipMask();
|
||
|
|
||
|
idVec3 currentPosition = current->i.position;
|
||
|
idVec3 nextPosition = next->i.position;
|
||
|
idMat3 currentOrientation = current->i.orientation;
|
||
|
idMat3 nextOrientation = next->i.orientation;
|
||
|
|
||
|
idList< const idClipModel* >& clipModelList = vpushCollection.GetCollection();
|
||
|
int numClipModels = vpushCollection.GetCollection().Num();
|
||
|
|
||
|
if ( numClipModels == 0 ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
bool isSquisher = false;
|
||
|
sdTransport* transportSelf = self->Cast< sdTransport >();
|
||
|
sdTransportPositionManager* positionManager = NULL;
|
||
|
if ( transportSelf != NULL ) {
|
||
|
isSquisher = transportSelf->GetVehicleControl()->IsSquisher();
|
||
|
positionManager = &transportSelf->GetPositionManager();
|
||
|
}
|
||
|
|
||
|
trace_t blockingTrace;
|
||
|
blockingTrace = collision;
|
||
|
// treat the final location of the true collision as the full distance travelled
|
||
|
blockingTrace.fraction = 1.0f;
|
||
|
|
||
|
idVec3 velocity = mainInverseMass * current->i.linearMomentum;
|
||
|
idMat3 invIA = currentOrientation.Transpose() * mainInverseInertiaTensor * currentOrientation;
|
||
|
idVec3 angVelocity = invIA * current->i.angularMomentum;
|
||
|
idVec3 currentCoM = currentPosition + mainCenterOfMass * currentOrientation;
|
||
|
idVec3 nextCoM = nextPosition + mainCenterOfMass * nextOrientation;
|
||
|
idVec3 CoMdelta = nextCoM - currentCoM;
|
||
|
|
||
|
// check each of our hurtzone bodies against each of the clip models that belong to players
|
||
|
// (ie, don't care about rigidbodymultiple->rigidbodymultiple collisions)
|
||
|
for( int bodyIndex = 0; bodyIndex < bodies.Num(); bodyIndex++ ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ bodyIndex ];
|
||
|
|
||
|
idClipModel* clipModel = body.GetClipModel();
|
||
|
if( clipModel == NULL ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idVec3 start = currentPosition + ( body.GetOffset() * currentOrientation );
|
||
|
idVec3 end = nextPosition + ( body.GetOffset() * currentOrientation );
|
||
|
idVec3 delta = end - start;
|
||
|
idVec3 direction = delta;
|
||
|
float distance = direction.Normalize();
|
||
|
|
||
|
for ( int otherIndex = 0; otherIndex < numClipModels; otherIndex++ ) {
|
||
|
int result = idPhysics_Actor::VPUSH_OK;
|
||
|
|
||
|
const idClipModel* otherModel = clipModelList[ otherIndex ];
|
||
|
assert( otherModel );
|
||
|
idEntity* other = otherModel->GetEntity();
|
||
|
|
||
|
// paranoid check a bunch of assumptions
|
||
|
if ( other == NULL || !other->IsCollisionPushable() || other->GetPhysics() == NULL || self == NULL ) {
|
||
|
gameLocal.Warning( "sdPhysics_RigidBodyMultiple::CheckForPlayerCollisions NULL entity" );
|
||
|
assert( false );
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idPlayer* playerOther = other->Cast< idPlayer >();
|
||
|
// purely PLAYERCLIP bodies only check against players
|
||
|
if ( body.GetClipMask() == MASK_HURTZONE ) {
|
||
|
if ( playerOther == NULL ) {
|
||
|
continue;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
bool noCollide = false;
|
||
|
if ( playerOther != NULL ) {
|
||
|
if ( gameLocal.time - positionManager->GetPlayerExitTime( playerOther ) < 1000 ) {
|
||
|
noCollide = true;
|
||
|
}
|
||
|
}
|
||
|
if ( other->GetPhysics()->IsGroundEntity( self->entityNumber ) ) {
|
||
|
noCollide = true;
|
||
|
}
|
||
|
|
||
|
trace_t tr;
|
||
|
gameLocal.clip.TranslationModel( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, start, end, clipModel, currentOrientation, vpushClipMask, otherModel,
|
||
|
otherModel->GetOrigin(), otherModel->GetAxis() );
|
||
|
if ( tr.fraction < 1.0f ) {
|
||
|
// do collision damage and stuff
|
||
|
idVec3 radius = tr.c.point - ( currentCoM + CoMdelta * tr.fraction + mainCenterOfMass * currentOrientation );
|
||
|
idVec3 hitVelocity = velocity + angVelocity.Cross( radius );
|
||
|
|
||
|
tr.c.selfId = bodyIndex;
|
||
|
tr.c.id = 0;
|
||
|
|
||
|
if ( !noCollide ) {
|
||
|
other->Hit( tr, hitVelocity, self );
|
||
|
self->Collide( tr, hitVelocity, -1 );
|
||
|
}
|
||
|
|
||
|
if ( body.GetClipMask() != MASK_HURTZONE ) {
|
||
|
idPhysics* otherPhysics = other->GetPhysics();
|
||
|
idPhysics_Actor* actorPhysics = otherPhysics->Cast< idPhysics_Actor >();
|
||
|
assert( actorPhysics != NULL );
|
||
|
|
||
|
if ( actorPhysics != NULL ) {
|
||
|
if ( tr.fraction > 0.0f ) {
|
||
|
// try to push the player
|
||
|
// increase the remaining delta a bit, just to provide a bit of an epsilon
|
||
|
float fractionLeft = 1.0f - tr.fraction;
|
||
|
idVec3 remainingDelta = ( fractionLeft * ( distance + 1.0f ) ) * direction;
|
||
|
result = actorPhysics->VehiclePush( false, timeDelta * fractionLeft, remainingDelta, clipModel, 0 );
|
||
|
} else if ( tr.fraction == 0.0f ) {
|
||
|
// the player is *inside* this clip model
|
||
|
idVec3 remainingDelta = delta;
|
||
|
result = actorPhysics->VehiclePush( true, timeDelta, remainingDelta, clipModel, 0 );
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( result != idPhysics_Actor::VPUSH_OK ) {
|
||
|
if ( isSquisher && other->fl.takedamage && ( !playerOther || !playerOther->GetGodMode() ) ) {
|
||
|
self->CollideFatal( other );
|
||
|
} else {
|
||
|
float trueFraction = tr.fraction * blockingTrace.fraction;
|
||
|
blockingTrace = tr;
|
||
|
blockingTrace.fraction = trueFraction;
|
||
|
nextPosition = tr.endpos - ( body.GetOffset() * currentOrientation );
|
||
|
|
||
|
end = nextPosition + ( body.GetOffset() * currentOrientation );
|
||
|
delta = end - start;
|
||
|
direction = delta;
|
||
|
distance = direction.Normalize();
|
||
|
|
||
|
noCollisionDamage = noCollide;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// Now do rotation
|
||
|
for( int bodyIndex = 0; bodyIndex < bodies.Num(); bodyIndex++ ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ bodyIndex ];
|
||
|
|
||
|
idClipModel* clipModel = body.GetCenteredClipModel();
|
||
|
idClipModel* normalClipModel = body.GetClipModel();
|
||
|
if( !clipModel || !normalClipModel ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
bool doPush = body.GetClipMask() != MASK_HURTZONE;
|
||
|
|
||
|
idVec3 end = nextPosition + mainCenterOfMass * currentOrientation;
|
||
|
|
||
|
idMat3 axis;
|
||
|
TransposeMultiply( currentOrientation, nextOrientation, axis );
|
||
|
idRotation rotation = axis.ToRotation();
|
||
|
rotation.SetOrigin( nextPosition );
|
||
|
|
||
|
for ( int otherIndex = 0; otherIndex < numClipModels; otherIndex++ ) {
|
||
|
int result = idPhysics_Actor::VPUSH_OK;
|
||
|
|
||
|
const idClipModel* otherModel = clipModelList[ otherIndex ];
|
||
|
assert( otherModel );
|
||
|
idEntity* other = otherModel->GetEntity();
|
||
|
|
||
|
// paranoid check a bunch of assumptions
|
||
|
if ( other == NULL || !other->IsCollisionPushable() || other->GetPhysics() == NULL || self == NULL ) {
|
||
|
gameLocal.Warning( "sdPhysics_RigidBodyMultiple::CheckForPlayerCollisions NULL entity" );
|
||
|
assert( false );
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idPlayer* playerOther = other->Cast< idPlayer >();
|
||
|
// purely PLAYERCLIP bodies only check against players
|
||
|
if ( body.GetClipMask() == MASK_HURTZONE ) {
|
||
|
if ( playerOther == NULL ) {
|
||
|
continue;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
bool noCollide = false;
|
||
|
if ( playerOther != NULL ) {
|
||
|
if ( gameLocal.time - positionManager->GetPlayerExitTime( playerOther ) < 1000 ) {
|
||
|
noCollide = true;
|
||
|
}
|
||
|
}
|
||
|
if ( other->GetPhysics()->IsGroundEntity( self->entityNumber ) ) {
|
||
|
noCollide = true;
|
||
|
}
|
||
|
|
||
|
if ( gameLocal.clip.ContentsModel( CLIP_DEBUG_PARMS_ENTINFO( self ) normalClipModel->GetOrigin(), normalClipModel, currentOrientation, -1, otherModel, otherModel->GetOrigin(), otherModel->GetAxis() ) ) {
|
||
|
// stuck inside it
|
||
|
idVec3 radius = otherModel->GetOrigin() - ( currentCoM + mainCenterOfMass * currentOrientation );
|
||
|
idVec3 hitVelocity = velocity + angVelocity.Cross( radius );
|
||
|
|
||
|
idPhysics* otherPhysics = other->GetPhysics();
|
||
|
idPhysics_Actor* actorPhysics = otherPhysics->Cast< idPhysics_Actor >();
|
||
|
assert( actorPhysics != NULL );
|
||
|
|
||
|
result = idPhysics_Actor::VPUSH_BLOCKED;
|
||
|
if ( doPush && actorPhysics != NULL ) {
|
||
|
idVec3 delta = hitVelocity * timeDelta;
|
||
|
// the player is *inside* this clip model
|
||
|
idVec3 remainingDelta = delta;
|
||
|
result = actorPhysics->VehiclePush( true, timeDelta, remainingDelta, normalClipModel, 0 );
|
||
|
|
||
|
// check if its STILL inside
|
||
|
if ( gameLocal.clip.ContentsModel( CLIP_DEBUG_PARMS_ENTINFO( self ) normalClipModel->GetOrigin(), normalClipModel, currentOrientation, -1, otherModel, otherModel->GetOrigin(), otherModel->GetAxis() ) ) {
|
||
|
result = idPhysics_Actor::VPUSH_BLOCKED;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( result != idPhysics_Actor::VPUSH_OK ) {
|
||
|
if ( isSquisher && other->fl.takedamage && ( !playerOther || !playerOther->GetGodMode() ) ) {
|
||
|
self->CollideFatal( other );
|
||
|
}
|
||
|
}
|
||
|
} else if ( doPush ) {
|
||
|
if ( other->GetPhysics()->IsGroundClipModel( normalClipModel->GetEntity()->entityNumber, normalClipModel->GetId() ) ) {
|
||
|
// car-surfing
|
||
|
// try moving the entity
|
||
|
if ( body.GetClipMask() != MASK_HURTZONE ) {
|
||
|
idPhysics* otherPhysics = other->GetPhysics();
|
||
|
idPhysics_Actor* actorPhysics = otherPhysics->Cast< idPhysics_Actor >();
|
||
|
assert( actorPhysics != NULL );
|
||
|
|
||
|
if ( actorPhysics != NULL ) {
|
||
|
idVec3 remainingDelta;
|
||
|
GetPointVelocity( otherPhysics->GetOrigin(), remainingDelta );
|
||
|
remainingDelta *= timeDelta;
|
||
|
result = actorPhysics->VehiclePush( false, timeDelta, remainingDelta, clipModel, 0 );
|
||
|
}
|
||
|
}
|
||
|
if ( result != idPhysics_Actor::VPUSH_OK ) {
|
||
|
if ( isSquisher && other->fl.takedamage && ( !playerOther || !playerOther->GetGodMode() ) ) {
|
||
|
self->CollideFatal( other );
|
||
|
}
|
||
|
}
|
||
|
} else {
|
||
|
trace_t tr;
|
||
|
gameLocal.clip.Rotation( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, end, rotation, clipModel, currentOrientation, vpushClipMask, self );
|
||
|
if ( tr.fraction < 1.0f ) {
|
||
|
// do collision damage and stuff
|
||
|
idVec3 radius = tr.c.point - ( currentCoM + CoMdelta * tr.fraction + mainCenterOfMass * tr.endAxis );
|
||
|
idVec3 hitVelocity = velocity + angVelocity.Cross( radius );
|
||
|
|
||
|
tr.c.selfId = bodyIndex;
|
||
|
tr.c.id = 0;
|
||
|
|
||
|
if ( !noCollide ) {
|
||
|
other->Hit( tr, hitVelocity, self );
|
||
|
self->Collide( tr, hitVelocity, -1 );
|
||
|
}
|
||
|
|
||
|
// pushing
|
||
|
idPhysics* otherPhysics = other->GetPhysics();
|
||
|
idPhysics_Actor* actorPhysics = otherPhysics->Cast< idPhysics_Actor >();
|
||
|
assert( actorPhysics != NULL );
|
||
|
|
||
|
if ( actorPhysics != NULL ) {
|
||
|
idVec3 delta = hitVelocity * timeDelta;
|
||
|
if ( tr.fraction > 0.0f ) {
|
||
|
// try to push the player
|
||
|
// increase the remaining delta a bit, just to provide a bit of an epsilon
|
||
|
idVec3 direction = delta;
|
||
|
float distance = direction.Normalize();
|
||
|
float fractionLeft = 1.0f - tr.fraction;
|
||
|
idVec3 remainingDelta = ( fractionLeft * ( distance + 1.0f ) ) * direction;
|
||
|
result = actorPhysics->VehiclePush( false, timeDelta * fractionLeft, remainingDelta, normalClipModel, 0 );
|
||
|
} else if ( tr.fraction == 0.0f ) {
|
||
|
// the player is *inside* this clip model
|
||
|
idVec3 remainingDelta = delta;
|
||
|
result = actorPhysics->VehiclePush( true, timeDelta, remainingDelta, normalClipModel, 0 );
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( result != idPhysics_Actor::VPUSH_OK ) {
|
||
|
if ( isSquisher && other->fl.takedamage && ( !playerOther || !playerOther->GetGodMode() ) ) {
|
||
|
self->CollideFatal( other );
|
||
|
} else {
|
||
|
float trueFraction = tr.fraction * blockingTrace.fraction;
|
||
|
blockingTrace = tr;
|
||
|
blockingTrace.fraction = trueFraction;
|
||
|
nextOrientation = tr.endAxis;
|
||
|
|
||
|
end = nextPosition + mainCenterOfMass * currentOrientation;
|
||
|
noCollisionDamage = noCollide;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( blockingTrace.fraction < 1.0f ) {
|
||
|
float trueFraction = blockingTrace.fraction * collision.fraction;
|
||
|
collision = blockingTrace;
|
||
|
collision.fraction = trueFraction;
|
||
|
next->i.position = nextPosition;
|
||
|
next->i.orientation = nextOrientation;
|
||
|
next->i.linearMomentum = current->i.linearMomentum;
|
||
|
next->i.angularMomentum = current->i.angularMomentum;
|
||
|
return true;
|
||
|
}
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SolveLCPConstraints
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::SolveLCPConstraints( constraintInfo_t* constraints, int numConstraints, float deltaTime ) const {
|
||
|
idMatX jmk;
|
||
|
idVecX rhs, w, lm, lo, hi;
|
||
|
int* boxIndex;
|
||
|
|
||
|
assert( constraints && numConstraints );
|
||
|
|
||
|
jmk.SetData( numConstraints, ( ( numConstraints + 3 ) & ~3 ), MATX_ALLOCA( numConstraints * ( ( numConstraints + 3 ) & ~3 ) ) );
|
||
|
|
||
|
idMat3 inverseWorldInertiaTensor;
|
||
|
|
||
|
inverseWorldInertiaTensor = current->i.orientation.Transpose() * mainInverseInertiaTensor * current->i.orientation;
|
||
|
|
||
|
int i;
|
||
|
for ( i = 0; i < numConstraints; i++ ) {
|
||
|
constraintInfo_t& constraint = constraints[ i ];
|
||
|
|
||
|
idVec6 t;
|
||
|
t.SubVec3( 0 ) = mainInverseMass * constraint.j.SubVec3( 0 );
|
||
|
t.SubVec3( 1 ) = inverseWorldInertiaTensor * constraint.j.SubVec3( 1 );
|
||
|
|
||
|
float* dstPtr = jmk[ i ];
|
||
|
|
||
|
int n;
|
||
|
for ( n = 0; n < numConstraints; n++ ) {
|
||
|
constraintInfo_t& otherConstraint = constraints[ n ];
|
||
|
|
||
|
dstPtr[ n ] = ( t.SubVec3( 0 ) * otherConstraint.j.SubVec3( 0 ) ) + ( t.SubVec3( 1 ) * otherConstraint.j.SubVec3( 1 ) );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
float invDelta = 1.f / deltaTime;
|
||
|
|
||
|
idVec6 acc;
|
||
|
acc.SubVec3( 0 ) = ( mainInverseMass * current->i.linearMomentum * invDelta );
|
||
|
acc.SubVec3( 1 ) = ( ( inverseWorldInertiaTensor * current->i.angularMomentum ) * invDelta );
|
||
|
|
||
|
// HACK: Don't take the external force/torque into account here as it can go funny
|
||
|
// acc.SubVec3( 0 ) += mainInverseMass * current->externalForce;
|
||
|
// acc.SubVec3( 1 ) += inverseWorldInertiaTensor * current->externalTorque;;
|
||
|
|
||
|
rhs.SetData( numConstraints, VECX_ALLOCA( numConstraints ) );
|
||
|
lo.SetData( numConstraints, VECX_ALLOCA( numConstraints ) );
|
||
|
hi.SetData( numConstraints, VECX_ALLOCA( numConstraints ) );
|
||
|
lm.SetData( numConstraints, VECX_ALLOCA( numConstraints ) );
|
||
|
boxIndex = ( int* ) _alloca16( numConstraints * sizeof( int ) );
|
||
|
|
||
|
for ( i = 0; i < numConstraints; i++ ) {
|
||
|
constraintInfo_t& constraint = constraints[ i ];
|
||
|
|
||
|
float* ptr = acc.ToFloatPtr();
|
||
|
float* j1 = constraint.j.ToFloatPtr();
|
||
|
|
||
|
rhs[ i ] = j1[ 0 ] * ptr[ 0 ] + j1[ 1 ] * ptr[ 1 ] + j1[ 2 ] * ptr[ 2 ] + j1[ 3 ] * ptr[ 3 ] + j1[ 4 ] * ptr[ 4 ] + j1[ 5 ] * ptr[ 5 ];
|
||
|
rhs[ i ] += constraint.c * invDelta;
|
||
|
|
||
|
rhs[ i ] = -rhs[ i ];
|
||
|
lo[ i ] = constraint.lo;
|
||
|
hi[ i ] = constraint.hi;
|
||
|
|
||
|
boxIndex[ i ] = constraint.boxIndex;
|
||
|
|
||
|
jmk[ i ][ i ] += constraint.error * invDelta;
|
||
|
}
|
||
|
|
||
|
if ( !lcp->Solve( jmk, lm, rhs, lo, hi, boxIndex ) ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
for ( i = 0; i < numConstraints; i++ ) {
|
||
|
constraintInfo_t& constraint = constraints[ i ];
|
||
|
constraint.lm = lm[ i ];
|
||
|
}
|
||
|
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ContactFriction
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::ContactFriction( float deltaTime, bool addEntityConstraints ) {
|
||
|
int i;
|
||
|
idVec3 massCenter, r;
|
||
|
|
||
|
massCenter = current->i.position + ( mainCenterOfMass * current->i.orientation );
|
||
|
|
||
|
constraintList_t constraintInfo;
|
||
|
constraintInfo.SetNum( MAX_CONSTRAINTS );
|
||
|
int numConstraints = 0;
|
||
|
|
||
|
for ( i = 0; i < contacts.Num(); i++ ) {
|
||
|
int startIndex = numConstraints;
|
||
|
|
||
|
contactInfo_t& contact = contacts[ i ];
|
||
|
contactInfoExt_t& contactExt = contactInfoExt[ i ];
|
||
|
|
||
|
constraintInfo_t& info = constraintInfo[ numConstraints++ ];
|
||
|
|
||
|
r = contact.point - massCenter;
|
||
|
|
||
|
info.j.SubVec3( 0 ) = contact.normal;
|
||
|
info.j.SubVec3( 1 ) = r.Cross( contact.normal );
|
||
|
info.boxIndex = -1;
|
||
|
info.lo = 0;
|
||
|
info.error = CONTACT_LCP_EPSILON;
|
||
|
info.pos = contact.point;
|
||
|
if ( contact.selfId >= 0 ) {
|
||
|
info.hi = idMath::INFINITY;
|
||
|
info.c = 0;
|
||
|
} else {
|
||
|
info.hi = contactExt.contactForceMax;
|
||
|
info.c = contactExt.contactForceVelocity;
|
||
|
}
|
||
|
info.lm = 0;
|
||
|
|
||
|
if ( contact.selfId >= 0 ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ contact.selfId ];
|
||
|
const idMat3& axes = current->i.orientation;
|
||
|
|
||
|
int j;
|
||
|
for ( j = 0; j < 3; j++ ) {
|
||
|
idVec3 dir1 = axes[ j ];
|
||
|
float strength = body.GetContactFriction()[ j ];
|
||
|
if( contact.surfaceType ) {
|
||
|
strength *= idGameLocal::GetSurfaceTypeForIndex( contact.surfaceType->Index() ).friction;
|
||
|
}
|
||
|
|
||
|
if ( strength > 0.0f ) {
|
||
|
constraintInfo_t& otherInfo = constraintInfo[ numConstraints++ ];
|
||
|
|
||
|
otherInfo.hi = strength;
|
||
|
otherInfo.lo = -strength;
|
||
|
otherInfo.j.SubVec3( 0 ) = dir1;
|
||
|
otherInfo.j.SubVec3( 1 ) = r.Cross( dir1 );
|
||
|
otherInfo.error = CONTACT_LCP_EPSILON;
|
||
|
otherInfo.boxIndex = startIndex;
|
||
|
otherInfo.c = 0.0f;
|
||
|
otherInfo.lm = 0.0f;
|
||
|
otherInfo.pos = contact.point;
|
||
|
}
|
||
|
}
|
||
|
} else {
|
||
|
idMat3 axes = contactExt.frictionAxes * current->i.orientation;
|
||
|
|
||
|
int j;
|
||
|
for ( j = 0; j < 3; j++ ) {
|
||
|
const idVec3& dir1 = axes[ j ];
|
||
|
|
||
|
float strength = contactExt.contactFriction[ j ];
|
||
|
if ( contact.surfaceType ) {
|
||
|
strength *= idGameLocal::GetSurfaceTypeForIndex( contact.surfaceType->Index() ).friction;
|
||
|
}
|
||
|
|
||
|
if ( strength > 0.0f ) {
|
||
|
constraintInfo_t& otherInfo = constraintInfo[ numConstraints++ ];
|
||
|
|
||
|
otherInfo.hi = strength;
|
||
|
otherInfo.lo = -strength;
|
||
|
otherInfo.j.SubVec3( 0 ) = dir1;
|
||
|
otherInfo.j.SubVec3( 1 ) = r.Cross( dir1 );
|
||
|
otherInfo.error = CONTACT_LCP_EPSILON;
|
||
|
otherInfo.boxIndex = startIndex;
|
||
|
otherInfo.c = 0.0f;
|
||
|
otherInfo.lm = 0.0f;
|
||
|
otherInfo.pos = contact.point;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( contactExt.motorForce ) {
|
||
|
idVec3 dir1 = contactExt.motorDirection * current->i.orientation;
|
||
|
dir1 = dir1 - ( ( dir1 * contact.normal ) * contact.normal );
|
||
|
|
||
|
dir1.Normalize();
|
||
|
float strength = contactExt.motorForce;
|
||
|
|
||
|
if ( strength > 0.0f ) {
|
||
|
constraintInfo_t& otherInfo = constraintInfo[ numConstraints++ ];
|
||
|
|
||
|
otherInfo.hi = strength;
|
||
|
otherInfo.lo = -strength;
|
||
|
otherInfo.j.SubVec3( 0 ) = -dir1;
|
||
|
otherInfo.j.SubVec3( 1 ) = r.Cross( -dir1 );
|
||
|
otherInfo.error = CONTACT_LCP_EPSILON;
|
||
|
otherInfo.boxIndex = -1;
|
||
|
otherInfo.c = contactExt.motorSpeed;
|
||
|
otherInfo.lm = 0.0f;
|
||
|
otherInfo.pos = contact.point;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( addEntityConstraints ) {
|
||
|
// add custom constraints
|
||
|
numConstraints += self->AddCustomConstraints( &constraintInfo[ numConstraints ], MAX_CONSTRAINTS - numConstraints );
|
||
|
}
|
||
|
|
||
|
if ( numConstraints == 0 ) {
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
if ( !SolveLCPConstraints( constraintInfo.Begin(), numConstraints, deltaTime ) ) {
|
||
|
// burp?
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
for ( i = 0; i < numConstraints; i++ ) {
|
||
|
constraintInfo_t& constraint = constraintInfo[ i ];
|
||
|
|
||
|
current->i.linearMomentum += constraint.j.SubVec3( 0 ) * constraint.lm * deltaTime;
|
||
|
current->i.angularMomentum += constraint.j.SubVec3( 1 ) * constraint.lm * deltaTime;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::TestIfAtRest
|
||
|
|
||
|
Returns true if the body is considered at rest.
|
||
|
Does not catch all cases where the body is at rest but is generally good enough.
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::TestIfAtRest( void ) const {
|
||
|
if ( gameLocal.time < activateEndTime ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
if ( current->atRest >= 0 ) {
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
float gv;
|
||
|
idVec3 normal, point;
|
||
|
|
||
|
idFixedWinding contactWinding;
|
||
|
|
||
|
bool inWater = waterLevel > waterRestThreshold;
|
||
|
bool needsGroundToRest = true;
|
||
|
sdTransport* transportSelf = self->Cast< sdTransport >();
|
||
|
if ( transportSelf != NULL ) {
|
||
|
const sdVehicleControlBase* control = transportSelf->GetVehicleControl();
|
||
|
if ( control != NULL ) {
|
||
|
needsGroundToRest = control->RestNeedsGround();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// need at least 3 contact points to come to rest
|
||
|
if ( !inWater && needsGroundToRest ) {
|
||
|
if ( contacts.Num() < 3 ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
// get average contact plane normal
|
||
|
normal.Zero();
|
||
|
for ( int i = 0; i < contacts.Num(); i++ ) {
|
||
|
normal += contacts[i].normal;
|
||
|
}
|
||
|
normal.Normalize();
|
||
|
|
||
|
// if on a too steep surface
|
||
|
if ( (normal * gravityNormal) > -0.7f ) {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// create bounds for contact points
|
||
|
contactWinding.Clear();
|
||
|
|
||
|
// linear velocity of body
|
||
|
idVec3 v = GetLinearVelocity();
|
||
|
|
||
|
// linear velocity in gravity direction
|
||
|
gv = v * gravityNormal;
|
||
|
// linear velocity orthogonal to gravity direction
|
||
|
v -= gv * gravityNormal;
|
||
|
|
||
|
float gs = ( gravityVector * gravityNormal ) * MS2SEC( gameLocal.msec );
|
||
|
gv -= gs;
|
||
|
|
||
|
// if too much velocity in gravity direction
|
||
|
if ( gv > 2.0f * GRAVITY_STOP_SPEED || gv < -2.0f * GRAVITY_STOP_SPEED ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
float speed = v.Length();
|
||
|
|
||
|
// if too much velocity orthogonal to gravity direction
|
||
|
if ( speed > 2.f ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
// calculate rotational velocity
|
||
|
float avs = GetAngularVelocity().Length();
|
||
|
// if too much rotational velocity
|
||
|
if ( avs > 0.25f ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
speed = speed + avs;
|
||
|
|
||
|
bool moving = speed > Square( 0.1 );
|
||
|
|
||
|
for ( int i = 0; i < contacts.Num(); i++ ) {
|
||
|
if ( contacts[ i ].selfId == -1 ) {
|
||
|
if ( ( moving && !contactInfoExt[ i ].rested ) || ( contactInfoExt[ i ].motorForce && contactInfoExt[ i ].motorSpeed != 0.0f ) ) {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// project point onto plane through origin orthogonal to the gravity
|
||
|
point = contacts[i].point - (contacts[i].point * gravityNormal) * gravityNormal;
|
||
|
contactWinding.AddToConvexHull( point, gravityNormal );
|
||
|
}
|
||
|
|
||
|
if ( !inWater && needsGroundToRest ) {
|
||
|
// need at least 3 contact points to come to rest
|
||
|
if ( contactWinding.GetNumPoints() < 3 ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
// center of mass in world space
|
||
|
point = current->i.position + ( mainCenterOfMass * current->i.orientation );
|
||
|
point -= (point * gravityNormal) * gravityNormal;
|
||
|
|
||
|
// if the point is not inside the winding
|
||
|
if ( !contactWinding.PointInside( gravityNormal, point, 0 ) ) {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if( current->externalForce.LengthSqr() > Square( 10000.f ) ) {
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::DebugDraw
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::DebugDraw( void ) {
|
||
|
|
||
|
for( int i = 0; i < bodies.Num(); i++ ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ i ];
|
||
|
idClipModel* clipModel = body.GetClipModel();
|
||
|
if( !clipModel ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idPlayer* localPlayer = gameLocal.GetLocalPlayer();
|
||
|
if ( localPlayer ) {
|
||
|
if ( rb_showBodies.GetBool() || ( rb_showActive.GetBool() && current->atRest < 0 ) ) {
|
||
|
clipModel->Draw();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
if ( rb_showVelocity.GetBool() ) {
|
||
|
DrawVelocity( clipModel->GetId(), 0.1f, 4.0f );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( rb_showBodies.GetBool() ) {
|
||
|
mainClipModel->Draw( current->i.position, current->i.orientation );
|
||
|
}
|
||
|
|
||
|
if( rb_showContacts.GetBool() ) {
|
||
|
for( int i = 0; i < contacts.Num(); i++ ) {
|
||
|
contactInfo_t& contact = contacts[ i ];
|
||
|
|
||
|
idVec3 x, y;
|
||
|
contact.normal.NormalVectors( x, y );
|
||
|
gameRenderWorld->DebugLine( colorBlue, contact.point, contact.point + 12.0f * contact.normal );
|
||
|
gameRenderWorld->DebugLine( colorBlue, contact.point - 4.0f * x, contact.point + 4.0f * x );
|
||
|
gameRenderWorld->DebugLine( colorBlue, contact.point - 4.0f * y, contact.point + 4.0f * y );
|
||
|
if( contact.surfaceType ) {
|
||
|
gameRenderWorld->DrawText( contact.surfaceType->GetName(), contact.point, 0.2f, colorWhite, gameLocal.GetLocalPlayer()->GetRenderView()->viewaxis );
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( rb_showMass.GetBool() ) {
|
||
|
gameRenderWorld->DrawText( va( "\n%1.2f", mainMass ), current->i.position, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
|
||
|
|
||
|
gameRenderWorld->DebugCircle( colorYellow, current->i.position + ( mainCenterOfMass * current->i.orientation ), idVec3( 0.f, 0.f, 1.f ), 16, 16 );
|
||
|
gameRenderWorld->DebugCircle( colorYellow, current->i.position + ( mainCenterOfMass * current->i.orientation ), idVec3( 0.f, 1.f, 0.f ), 16, 16 );
|
||
|
gameRenderWorld->DebugCircle( colorYellow, current->i.position + ( mainCenterOfMass * current->i.orientation ), idVec3( 1.f, 0.f, 0.f ), 16, 16 );
|
||
|
|
||
|
for( int i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].DebugDrawMass();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( rb_showInertia.GetBool() ) {
|
||
|
idMat3 &I = mainInertiaTensor;
|
||
|
gameRenderWorld->DrawText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )",
|
||
|
I[0].x, I[0].y, I[0].z,
|
||
|
I[1].x, I[1].y, I[1].z,
|
||
|
I[2].x, I[2].y, I[2].z ),
|
||
|
current->i.position, 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetMass
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetMass( float mass, int id ) {
|
||
|
assert( id >= 0 && id < bodies.Num() );
|
||
|
|
||
|
if( id < 0 || id >= bodies.Num() ) {
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
bodies[ id ].SetMass( mass );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetMass
|
||
|
================
|
||
|
*/
|
||
|
float sdPhysics_RigidBodyMultiple::GetMass( int id ) const {
|
||
|
if( id < 0 || id >= bodies.Num() ) {
|
||
|
return mainMass;
|
||
|
}
|
||
|
|
||
|
return bodies[ id ].GetMass();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetInertiaTensor
|
||
|
================
|
||
|
*/
|
||
|
const idMat3& sdPhysics_RigidBodyMultiple::GetInertiaTensor( int id ) const {
|
||
|
if( id < 0 || id >= bodies.Num() ) {
|
||
|
return mainInertiaTensor;
|
||
|
}
|
||
|
|
||
|
return bodies[ id ].GetInertiaTensor();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetInertiaTensor
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetInertiaTensor( const idMat3& itt ) {
|
||
|
mainInertiaTensor = itt;
|
||
|
mainInverseInertiaTensor = mainInertiaTensor.Inverse() * ( 1.f / 6.f );
|
||
|
customInertiaTensor = true;
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::Rest
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::Rest( int time ) {
|
||
|
current->atRest = time;
|
||
|
current->i.linearMomentum.Zero();
|
||
|
current->i.angularMomentum.Zero();
|
||
|
self->OnPhysicsRested();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::PutToRest
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::PutToRest( void ) {
|
||
|
Rest( gameLocal.time );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::NoContact
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::NoContact( void ) {
|
||
|
flags.noContact = true;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::Activate
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::Activate( void ) {
|
||
|
if ( !IsAtRest() ) {
|
||
|
return;
|
||
|
}
|
||
|
current->atRest = -1;
|
||
|
activateEndTime = gameLocal.time + SEC2MS( 2.f );
|
||
|
self->BecomeActive( TH_PHYSICS );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::EnableImpact
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::EnableImpact( void ) {
|
||
|
flags.noImpact = false;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::DisableImpact
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::DisableImpact( void ) {
|
||
|
flags.noImpact = true;
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetContents
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetContents( int contents, int id ) {
|
||
|
assert( id >=0 && id < bodies.Num() );
|
||
|
|
||
|
if( id < 0 || id > bodies.Num() ) {
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
idClipModel* clipModel = bodies[ id ].GetClipModel();
|
||
|
if( clipModel ) {
|
||
|
clipModel->SetContents( contents );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetContents
|
||
|
================
|
||
|
*/
|
||
|
int sdPhysics_RigidBodyMultiple::GetContents( int id ) const {
|
||
|
// assert( id >=0 && id < bodies.Num() );
|
||
|
|
||
|
if( id < 0 || id > bodies.Num() ) {
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
idClipModel* clipModel = bodies[ id ].GetClipModel();
|
||
|
if( !clipModel ) {
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
return clipModel->GetContents();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetBounds
|
||
|
================
|
||
|
*/
|
||
|
const idBounds& sdPhysics_RigidBodyMultiple::GetBounds( int id ) const {
|
||
|
if ( id >= 0 && id < bodies.Num() ) {
|
||
|
return bodies[ id ].GetClipModel()->GetBounds();
|
||
|
}
|
||
|
|
||
|
return mainBounds;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetAbsBounds
|
||
|
================
|
||
|
*/
|
||
|
const idBounds &sdPhysics_RigidBodyMultiple::GetAbsBounds( int id ) const {
|
||
|
static idBounds absBounds;
|
||
|
|
||
|
if( id >= 0 && id < bodies.Num() ) {
|
||
|
return bodies[ id ].GetClipModel()->GetAbsBounds();
|
||
|
}
|
||
|
|
||
|
absBounds.FromTransformedBounds( totalBounds, current->i.position, current->i.orientation );
|
||
|
return absBounds;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::Evaluate
|
||
|
|
||
|
Evaluate the impulse based rigid body physics.
|
||
|
When a collision occurs an impulse is applied at the moment of impact but
|
||
|
the remaining time after the collision is ignored.
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::Evaluate( int timeStepMSec, int endTimeMSec ) {
|
||
|
|
||
|
// eliminate any values that are too small - little bit of a hack, but prevents denormals
|
||
|
current->i.angularMomentum.FixDenormals();
|
||
|
current->i.linearMomentum.FixDenormals();
|
||
|
current->i.orientation.FixDenormals();
|
||
|
current->i.position.FixDenormals();
|
||
|
|
||
|
if ( masterEntity != NULL ) {
|
||
|
if ( timeStepMSec <= 0 ) {
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
float timeStep = MS2SEC( timeStepMSec );
|
||
|
|
||
|
idVec3 masterOrigin;
|
||
|
idMat3 masterAxis;
|
||
|
idVec3 oldOrigin = current->i.position;
|
||
|
idMat3 oldAxis = current->i.orientation;
|
||
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
current->i.position = masterOrigin + localOrigin * masterAxis;
|
||
|
if ( flags.isOrientated ) {
|
||
|
current->i.orientation = localAxis * masterAxis;
|
||
|
} else {
|
||
|
current->i.orientation = localAxis;
|
||
|
}
|
||
|
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
|
||
|
current->i.linearMomentum = mainMass * ( ( current->i.position - oldOrigin ) / timeStep );
|
||
|
current->i.angularMomentum = mainInertiaTensor * ( ( current->i.orientation * oldAxis.Transpose() ).ToAngularVelocity() / timeStep );
|
||
|
current->externalForce.Zero();
|
||
|
current->externalTorque.Zero();
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
if ( pm_pausePhysics.GetBool() || flags.frozen ) {
|
||
|
current->externalForce.Zero();
|
||
|
current->externalTorque.Zero();
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
trace_t collision;
|
||
|
|
||
|
float timeStep = MS2SEC( timeStepMSec );
|
||
|
float fullTimeStep = timeStep;
|
||
|
float timeStepUsed = timeStep;
|
||
|
const float minTimeStep = MS2SEC( 1 );
|
||
|
|
||
|
int count = 0;
|
||
|
int maxRepetitions = 2;
|
||
|
if ( !self->IsCollisionPushable() ) {
|
||
|
maxRepetitions = 5;
|
||
|
}
|
||
|
|
||
|
// HACK - increase the maximum number of repetitions for a vehicle that has people in it
|
||
|
// and is not at rest
|
||
|
if ( current->atRest < 0 ) {
|
||
|
sdTransport* transportSelf = self->Cast< sdTransport >();
|
||
|
if ( transportSelf != NULL ) {
|
||
|
if ( !transportSelf->GetPositionManager().IsEmpty() ) {
|
||
|
maxRepetitions = 5;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// cut back the number of repetitions allowed based on AOR
|
||
|
if ( self->GetAORPhysicsLOD() == 1 ) {
|
||
|
maxRepetitions = 2;
|
||
|
} else if ( self->GetAORPhysicsLOD() >= 2 ) {
|
||
|
maxRepetitions = 1;
|
||
|
}
|
||
|
|
||
|
idVec3 initialPosition = current->i.position;
|
||
|
|
||
|
// HACK - add back in the velocity from gravity that we took off after our last evaluation
|
||
|
idVec3 gravityMomentum = mainMass * GetGravity() * timeStep;
|
||
|
current->i.linearMomentum += gravityMomentum;
|
||
|
|
||
|
const idClipModel *water = NULL;
|
||
|
do {
|
||
|
idVec3 impulse;
|
||
|
idEntity *ent;
|
||
|
bool collided = false;
|
||
|
bool cameToRest = false;
|
||
|
int i;
|
||
|
|
||
|
current->lastTimeStep = timeStep;
|
||
|
|
||
|
// if the body is at rest
|
||
|
if ( current->atRest >= 0 || timeStep <= 0.0f ) {
|
||
|
DebugDraw();
|
||
|
self->OnPhysicsRested();
|
||
|
|
||
|
// HACK - remove the gravity contribution from momentum outside of evaluation
|
||
|
current->i.linearMomentum -= gravityMomentum;
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
//
|
||
|
// update the clip model collection
|
||
|
//
|
||
|
SetupVPushCollection();
|
||
|
|
||
|
const idClipModel * possWater = CheckWater();
|
||
|
if ( possWater != NULL ) {
|
||
|
water = possWater;
|
||
|
}
|
||
|
|
||
|
#ifdef RB_TIMINGS
|
||
|
timer_total.Start();
|
||
|
#endif
|
||
|
|
||
|
if ( !flags.noContact ) {
|
||
|
|
||
|
#ifdef RB_TIMINGS
|
||
|
timer_findcontacts.Start();
|
||
|
#endif
|
||
|
// get contacts
|
||
|
EvaluateContacts( count == 0 );
|
||
|
|
||
|
#ifdef RB_TIMINGS
|
||
|
timer_findcontacts.Stop();
|
||
|
timer_contacts.Start();
|
||
|
#endif
|
||
|
|
||
|
// check if the body has come to rest
|
||
|
if ( flags.comeToRest && TestIfAtRest() ) {
|
||
|
// put to rest
|
||
|
Rest( gameLocal.time );
|
||
|
cameToRest = true;
|
||
|
} else {
|
||
|
// apply contact friction
|
||
|
ContactFriction( timeStep, count == 0 );
|
||
|
}
|
||
|
|
||
|
#ifdef RB_TIMINGS
|
||
|
timer_contacts.Stop();
|
||
|
#endif // RB_TIMINGS
|
||
|
}
|
||
|
|
||
|
if ( current->atRest < 0 ) {
|
||
|
ActivateContactEntities();
|
||
|
}
|
||
|
|
||
|
if ( !cameToRest ) {
|
||
|
// calculate next position and orientation
|
||
|
Integrate( timeStep );
|
||
|
|
||
|
#ifdef RB_TIMINGS
|
||
|
timer_collision.Start();
|
||
|
#endif
|
||
|
|
||
|
// check for collisions from the current to the next state
|
||
|
int aorLOD = self->GetAORPhysicsLOD();
|
||
|
if ( aorLOD < 2 ) {
|
||
|
collided = CheckForCollisions( collision );
|
||
|
} else if ( aorLOD < 3 ) {
|
||
|
collided = CheckForCollisions_Simple( collision );
|
||
|
}
|
||
|
|
||
|
current->i.linearMomentum -= gravityMomentum;
|
||
|
// check for collisions with players during this move
|
||
|
bool noCollisionDamage = false;
|
||
|
if ( collision.fraction > 0.0f ) {
|
||
|
if ( aorLOD < 2 ) {
|
||
|
collided |= CheckForPlayerCollisions( collision.fraction * timeStep, collision, noCollisionDamage );
|
||
|
} else if ( aorLOD < 3 ) {
|
||
|
collided |= CheckForPlayerCollisions_Simple( collision.fraction * timeStep, collision, noCollisionDamage );
|
||
|
}
|
||
|
}
|
||
|
current->i.linearMomentum += gravityMomentum;
|
||
|
|
||
|
timeStepUsed = collision.fraction * timeStep;
|
||
|
|
||
|
|
||
|
#ifdef RB_TIMINGS
|
||
|
timer_collision.Stop();
|
||
|
#endif
|
||
|
|
||
|
// swap states for next simulation step
|
||
|
|
||
|
Swap( current, next );
|
||
|
|
||
|
if ( collided ) {
|
||
|
// apply collision impulse
|
||
|
lastCollision.trace = collision;
|
||
|
lastCollision.time = gameLocal.time;
|
||
|
if ( CollisionImpulse( collision, impulse, lastCollision.velocity, noCollisionDamage ) ) {
|
||
|
current->atRest = gameLocal.time;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// update the position of the clip models
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
|
||
|
current->externalForce.Zero();
|
||
|
current->externalTorque.Zero();
|
||
|
|
||
|
if ( current->atRest < 0 ) {
|
||
|
ActivateContactEntities();
|
||
|
}
|
||
|
|
||
|
if ( collided ) {
|
||
|
// if the rigid body didn't come to rest or the other entity is not at rest
|
||
|
ent = gameLocal.entities[collision.c.entityNum];
|
||
|
if ( ent && ( !cameToRest || !ent->IsAtRest() ) && ent->IsCollisionPushable() ) {
|
||
|
// apply impact to other entity
|
||
|
ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse );
|
||
|
}
|
||
|
|
||
|
// ensure that the collection will be reset next frame
|
||
|
traceCollection.ForceNextUpdate();
|
||
|
}
|
||
|
|
||
|
current->lastTimeStep = timeStep;
|
||
|
|
||
|
if ( IsOutsideWorld() ) {
|
||
|
// gameLocal.Warning( "rigid body moved outside world bounds for entity '%s' type '%s' at (%s)",
|
||
|
// self->name.c_str(), self->GetType()->classname, current->i.position.ToString(0) );
|
||
|
Rest( gameLocal.time );
|
||
|
}
|
||
|
|
||
|
#ifdef RB_TIMINGS
|
||
|
timer_total.Stop();
|
||
|
|
||
|
if ( rb_showTimings.GetInteger() == 1 ) {
|
||
|
gameLocal.Printf( "%12s: t %1.4f cd %1.4f cnt %1.4f\n",
|
||
|
self->name.c_str(),
|
||
|
timer_total.Milliseconds(), timer_collision.Milliseconds(), timer_findcontacts.Milliseconds(), timer_contacts.Milliseconds() );
|
||
|
lastTimerReset = 0;
|
||
|
}
|
||
|
else if ( rb_showTimings.GetInteger() == 2 ) {
|
||
|
numRigidBodies++;
|
||
|
if ( endTimeMSec > lastTimerReset ) {
|
||
|
gameLocal.Printf( "rb %d: t %1.4f fc %1.4f cd %1.4f cnt %1.4f\n",
|
||
|
numRigidBodies,
|
||
|
timer_total.Milliseconds(), timer_collision.Milliseconds(), timer_findcontacts.Milliseconds(), timer_contacts.Milliseconds() );
|
||
|
}
|
||
|
}
|
||
|
if ( endTimeMSec > lastTimerReset ) {
|
||
|
lastTimerReset = endTimeMSec;
|
||
|
numRigidBodies = 0;
|
||
|
timer_total.Clear();
|
||
|
timer_collision.Clear();
|
||
|
timer_contacts.Clear();
|
||
|
timer_findcontacts.Clear();
|
||
|
}
|
||
|
#endif
|
||
|
|
||
|
timeStep -= timeStepUsed;
|
||
|
count++;
|
||
|
} while ( count < maxRepetitions && timeStep >= minTimeStep ); //&& collision.fraction > 0.0001f );
|
||
|
|
||
|
// HACK - remove the gravity contribution from momentum outside of evaluation
|
||
|
current->i.linearMomentum -= gravityMomentum;
|
||
|
|
||
|
idVec3 finalPosition = current->i.position;
|
||
|
|
||
|
if ( !gameLocal.isClient ) {
|
||
|
// HACK: see if a pusher is blocked somehow
|
||
|
bool blocked = false;
|
||
|
if ( !self->IsCollisionPushable() && lastCollision.time == gameLocal.time ) {
|
||
|
idEntity* lastCollideEnt = gameLocal.entities[ lastCollision.trace.c.entityNum ];
|
||
|
if ( lastCollideEnt != NULL && lastCollideEnt->fl.takedamage ) {
|
||
|
idVec3 effectiveVelocity = ( finalPosition - initialPosition ) / MS2SEC( gameLocal.msec );
|
||
|
float desiredSpeed = GetLinearVelocity().LengthSqr();
|
||
|
float apparentSpeed = effectiveVelocity.LengthSqr();
|
||
|
if ( desiredSpeed > idMath::FLT_EPSILON ) {
|
||
|
float speedFraction = apparentSpeed / desiredSpeed;
|
||
|
if ( speedFraction < 0.01f ) {
|
||
|
if ( blockedTime > 200 ) {
|
||
|
self->CollideFatal( lastCollideEnt );
|
||
|
} else {
|
||
|
blocked = true;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( blocked ) {
|
||
|
blockedTime += gameLocal.msec;
|
||
|
} else {
|
||
|
blockedTime = 0;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
DebugDraw();
|
||
|
|
||
|
if ( water ) {
|
||
|
idCollisionModel* model = water->GetCollisionModel( 0 );
|
||
|
int numPlanes = model->GetNumBrushPlanes();
|
||
|
if ( numPlanes ) {
|
||
|
self->CheckWater( water->GetOrigin(), water->GetAxis(), model );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::UpdateTime
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::UpdateTime( int endTimeMSec ) {
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetImpactInfo
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const {
|
||
|
idVec3 linearVelocity, angularVelocity;
|
||
|
idMat3 inverseWorldInertiaTensor;
|
||
|
|
||
|
linearVelocity = mainInverseMass * current->i.linearMomentum;
|
||
|
inverseWorldInertiaTensor = current->i.orientation.Transpose() * mainInverseInertiaTensor * current->i.orientation;
|
||
|
angularVelocity = inverseWorldInertiaTensor * current->i.angularMomentum;
|
||
|
|
||
|
info->invMass = mainInverseMass;
|
||
|
info->invInertiaTensor = inverseWorldInertiaTensor;
|
||
|
info->position = point - ( current->i.position + mainCenterOfMass * current->i.orientation );
|
||
|
info->velocity = linearVelocity + angularVelocity.Cross( info->position );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ApplyImpulse
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) {
|
||
|
if ( flags.noImpact ) {
|
||
|
return;
|
||
|
}
|
||
|
if ( masterEntity ) {
|
||
|
masterEntity->GetPhysics()->ApplyImpulse( 0, point, impulse );
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
if ( IsAtRest() ) {
|
||
|
// don't wake up for small impulses (magic numbers FTW)
|
||
|
if ( impulse.LengthFast() < IMPULSE_WAKE_STRENGTH ) {
|
||
|
return;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
current->i.linearMomentum += impulse;
|
||
|
current->i.angularMomentum += ( point - ( current->i.position + mainCenterOfMass * current->i.orientation ) ).Cross( impulse );
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::AddForce
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::AddForce( const int id, const idVec3 &point, const idVec3 &force ) {
|
||
|
if ( flags.noImpact ) {
|
||
|
return;
|
||
|
}
|
||
|
if ( masterEntity ) {
|
||
|
masterEntity->GetPhysics()->AddForce( 0, point, force );
|
||
|
return;
|
||
|
}
|
||
|
current->externalForce += force;
|
||
|
|
||
|
idVec3 comWorld = current->i.position + ( mainCenterOfMass * current->i.orientation );
|
||
|
current->externalTorque += ( point - comWorld ).Cross( force );
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::AddForce
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::AddForce( const idVec3& force ) {
|
||
|
if ( flags.noImpact ) {
|
||
|
return;
|
||
|
}
|
||
|
if ( masterEntity ) {
|
||
|
masterEntity->GetPhysics()->AddForce( force );
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
current->externalForce += force;
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::AddLocalForce
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::AddLocalForce( const int id, const idVec3 &point, const idVec3 &force ) {
|
||
|
if ( flags.noImpact ) {
|
||
|
return;
|
||
|
}
|
||
|
if ( masterEntity ) {
|
||
|
masterEntity->GetPhysics()->AddForce( 0, ( point + mainCenterOfMass ) * current->i.orientation, force );
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
current->externalForce += force*current->i.orientation;
|
||
|
current->externalTorque += point.Cross( force )*current->i.orientation;
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::AddTorque
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::AddTorque( const idVec3& torque ) {
|
||
|
current->externalTorque += torque;
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::IsAtRest
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::IsAtRest( void ) const {
|
||
|
return current->atRest >= 0;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetRestStartTime
|
||
|
================
|
||
|
*/
|
||
|
int sdPhysics_RigidBodyMultiple::GetRestStartTime( void ) const {
|
||
|
return current->atRest;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::Pushable
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::IsPushable( void ) const {
|
||
|
return ( !flags.noImpact && !masterEntity );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetVPushClipMask
|
||
|
================
|
||
|
*/
|
||
|
int sdPhysics_RigidBodyMultiple::GetVPushClipMask() const {
|
||
|
int vpushClipMask = CONTENTS_SLIDEMOVER;
|
||
|
if ( !self->IsCollisionPushable() ) {
|
||
|
vpushClipMask |= CONTENTS_MONSTER;
|
||
|
}
|
||
|
|
||
|
return vpushClipMask;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::EvaluateContacts
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::EvaluateContacts( bool addEntityContacts ) {
|
||
|
idVec3 dir;
|
||
|
int num;
|
||
|
|
||
|
ClearContacts();
|
||
|
|
||
|
contacts.SetNum( RBM_MAX_CONTACTS, false );
|
||
|
|
||
|
dir = current->i.linearMomentum + current->lastTimeStep * gravityVector * mainMass;
|
||
|
dir.Normalize();
|
||
|
|
||
|
/* dir = ( gravityVector * mainMass * current->lastTimeStep ); // + v;
|
||
|
dir.Normalize();
|
||
|
*/
|
||
|
|
||
|
int count = 0;
|
||
|
|
||
|
idEntity* collisionEnt = NULL;
|
||
|
if ( lastCollision.time >= gameLocal.time - gameLocal.msec ) {
|
||
|
collisionEnt = gameLocal.entities[ lastCollision.trace.c.entityNum ];
|
||
|
}
|
||
|
|
||
|
int vpushClipMask = GetVPushClipMask();
|
||
|
|
||
|
for( int i = 0; i < bodies.Num() && count < RBM_MAX_CONTACTS; i++ ) {
|
||
|
sdRigidBodyMulti_Body& body = bodies[ i ];
|
||
|
idClipModel* clipModel = body.GetClipModel();
|
||
|
|
||
|
if ( body.GetClipMask() == MASK_HURTZONE ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
num = GetTraceCollection().Contacts( CLIP_DEBUG_PARMS_ENTINFO( self ) &contacts[ count ], RBM_MAX_CONTACTS - count, clipModel->GetOrigin(), NULL, /*CONTACT_EPSILON*/ 1.0f, clipModel, clipModel->GetAxis(), body.GetClipMask() );
|
||
|
|
||
|
count += num;
|
||
|
|
||
|
// add contacts from any slidemovers we may be blocked by
|
||
|
if ( collisionEnt != NULL ) {
|
||
|
int contents = collisionEnt->GetPhysics()->GetContents();
|
||
|
if ( contents & vpushClipMask ) {
|
||
|
idClipModel* otherClip = collisionEnt->GetPhysics()->GetClipModel();
|
||
|
if ( otherClip != NULL ) {
|
||
|
num = gameLocal.clip.ContactsModel( CLIP_DEBUG_PARMS_ENTINFO( self ) &contacts[ count ], RBM_MAX_CONTACTS - count,
|
||
|
clipModel->GetOrigin(), &dir, CONTACT_EPSILON, clipModel,
|
||
|
clipModel->GetAxis(), body.GetClipMask(), otherClip,
|
||
|
otherClip->GetOrigin(), otherClip->GetAxis() );
|
||
|
|
||
|
for ( int j = count; j < count + num; j++ ) {
|
||
|
contacts[ j ].entityNum = collisionEnt->entityNumber;
|
||
|
contacts[ j ].id = clipModel->GetId();
|
||
|
contacts[ j ].selfId = i;
|
||
|
}
|
||
|
count += num;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
// remove any contacts from entities if we're not to be pushed by them
|
||
|
// otherwise the LCP solver will push us away from the entity a bit, slowing us down :(
|
||
|
if ( !self->IsCollisionPushable() ) {
|
||
|
for ( int i = 0; i < count; i++ ) {
|
||
|
idEntity* ent = gameLocal.entities[ contacts[ i ].entityNum ];
|
||
|
if ( ent->GetPhysics()->IsPushable() && ent->IsCollisionPushable() ) {
|
||
|
contacts.RemoveIndex( i );
|
||
|
i--;
|
||
|
count--;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( !flags.noGravity && count < RBM_MAX_CONTACTS && addEntityContacts ) {
|
||
|
count += self->EvaluateContacts( &contacts[ count ], &contactInfoExt[ count ], RBM_MAX_CONTACTS - count );
|
||
|
}
|
||
|
|
||
|
contacts.SetNum( count, false );
|
||
|
|
||
|
AddContactEntitiesForContacts();
|
||
|
|
||
|
return contacts.Num() != 0;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SaveState
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SaveState( void ) {
|
||
|
saved = *current;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::RestoreState
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::RestoreState( void ) {
|
||
|
*current = saved;
|
||
|
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
|
||
|
EvaluateContacts( true );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetOrigin
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetOrigin( const idVec3 &newOrigin, int id ) {
|
||
|
idVec3 masterOrigin;
|
||
|
idMat3 masterAxis;
|
||
|
|
||
|
if ( masterEntity != NULL ) {
|
||
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
current->i.position = masterOrigin + newOrigin * masterAxis;
|
||
|
localOrigin = newOrigin;
|
||
|
} else {
|
||
|
current->i.position = newOrigin;
|
||
|
}
|
||
|
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetAxis
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetAxis( const idMat3 &newAxis, int id ) {
|
||
|
idVec3 masterOrigin;
|
||
|
idMat3 masterAxis;
|
||
|
|
||
|
if ( masterEntity != NULL ) {
|
||
|
if ( flags.isOrientated ) {
|
||
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
current->i.orientation = newAxis * masterAxis;
|
||
|
}
|
||
|
localAxis = newAxis;
|
||
|
}
|
||
|
else {
|
||
|
current->i.orientation = newAxis;
|
||
|
}
|
||
|
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::Move
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::Translate( const idVec3 &translation, int id ) {
|
||
|
|
||
|
if ( masterEntity != NULL ) {
|
||
|
localOrigin += translation;
|
||
|
}
|
||
|
current->i.position += translation;
|
||
|
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::Rotate
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::Rotate( const idRotation &rotation, int id ) {
|
||
|
idVec3 masterOrigin;
|
||
|
idMat3 masterAxis;
|
||
|
|
||
|
current->i.orientation *= rotation.ToMat3();
|
||
|
current->i.position *= rotation;
|
||
|
|
||
|
if ( masterEntity != NULL ) {
|
||
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
localAxis *= rotation.ToMat3();
|
||
|
localOrigin = ( current->i.position - masterOrigin ) * masterAxis.Transpose();
|
||
|
}
|
||
|
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetBodyOffset
|
||
|
================
|
||
|
*/
|
||
|
const idVec3& sdPhysics_RigidBodyMultiple::GetBodyOffset( int id ) const {
|
||
|
assert( id >= 0 && id < bodies.Num() );
|
||
|
|
||
|
if( id < 0 || id >= bodies.Num() ) {
|
||
|
return vec3_origin;
|
||
|
}
|
||
|
|
||
|
return bodies[ id ].GetOffset();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetOrigin
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::GetBodyOrigin( idVec3& org, int id ) const {
|
||
|
assert( id >= 0 && id < bodies.Num() );
|
||
|
|
||
|
if( id < 0 || id >= bodies.Num() ) {
|
||
|
org.Zero();
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
org = current->i.position + ( bodies[ id ].GetOffset() * current->i.orientation );
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetOrigin
|
||
|
================
|
||
|
*/
|
||
|
const idVec3& sdPhysics_RigidBodyMultiple::GetOrigin( int id ) const {
|
||
|
return current->i.position;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetAxis
|
||
|
================
|
||
|
*/
|
||
|
const idMat3& sdPhysics_RigidBodyMultiple::GetAxis( int id ) const {
|
||
|
return current->i.orientation;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetLinearVelocity
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) {
|
||
|
current->i.linearMomentum = newLinearVelocity * mainMass;
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetAngularVelocity
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) {
|
||
|
idMat3 worldInertiaTensor = 6.0f * ( current->i.orientation.Transpose() * mainInertiaTensor * current->i.orientation );
|
||
|
current->i.angularMomentum = worldInertiaTensor * newAngularVelocity;
|
||
|
Activate();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetLinearVelocity
|
||
|
================
|
||
|
*/
|
||
|
const idVec3& sdPhysics_RigidBodyMultiple::GetLinearVelocity( int id ) const {
|
||
|
static idVec3 curLinearVelocity;
|
||
|
curLinearVelocity = current->i.linearMomentum * mainInverseMass;
|
||
|
return curLinearVelocity;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetPointVelocity
|
||
|
================
|
||
|
*/
|
||
|
const idVec3& sdPhysics_RigidBodyMultiple::GetPointVelocity( const idVec3& point, idVec3& velocity ) const {
|
||
|
idVec3 comWorld = current->i.position + ( mainCenterOfMass * current->i.orientation );
|
||
|
velocity = ( current->i.linearMomentum * mainInverseMass );
|
||
|
velocity += GetAngularVelocity().Cross( point - comWorld );
|
||
|
return velocity;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetAngularVelocity
|
||
|
================
|
||
|
*/
|
||
|
const idVec3& sdPhysics_RigidBodyMultiple::GetAngularVelocity( int id ) const {
|
||
|
static idVec3 curAngularVelocity;
|
||
|
idMat3 inverseWorldInertiaTensor;
|
||
|
|
||
|
inverseWorldInertiaTensor = current->i.orientation.Transpose() * mainInverseInertiaTensor * current->i.orientation;
|
||
|
curAngularVelocity = inverseWorldInertiaTensor * current->i.angularMomentum;
|
||
|
return curAngularVelocity;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ClipTranslation
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const {
|
||
|
int i;
|
||
|
trace_t tr;
|
||
|
|
||
|
memset( &results, 0, sizeof( results ) );
|
||
|
results.fraction = 1.f;
|
||
|
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
idClipModel* clipModel = bodies[ i ].GetClipModel();
|
||
|
if( !clipModel ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
if( model ) {
|
||
|
gameLocal.clip.TranslationModel( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, clipModel->GetOrigin(), clipModel->GetOrigin() + translation, clipModel, clipModel->GetAxis(), bodies[ i ].GetClipMask(), model, model->GetOrigin(), model->GetAxis() );
|
||
|
} else {
|
||
|
gameLocal.clip.Translation( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, clipModel->GetOrigin(), clipModel->GetOrigin() + translation, clipModel, clipModel->GetAxis(), bodies[ i ].GetClipMask(), self );
|
||
|
}
|
||
|
|
||
|
if( tr.fraction < results.fraction ) {
|
||
|
results = tr;
|
||
|
}
|
||
|
|
||
|
}
|
||
|
|
||
|
results.endpos = current->i.position + results.fraction * translation;
|
||
|
results.endAxis = current->i.orientation;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ClipRotation
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const {
|
||
|
int i;
|
||
|
trace_t tr;
|
||
|
idRotation partialRotation;
|
||
|
|
||
|
memset( &results, 0, sizeof( results ) );
|
||
|
results.fraction = 1.f;
|
||
|
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
idClipModel* clipModel = bodies[ i ].GetClipModel();
|
||
|
if( !clipModel ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
if ( model ) {
|
||
|
gameLocal.clip.RotationModel( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, clipModel->GetOrigin(), rotation, clipModel, clipModel->GetAxis(), bodies[ i ].GetClipMask(), model, model->GetOrigin(), model->GetAxis() );
|
||
|
} else {
|
||
|
gameLocal.clip.Rotation( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, clipModel->GetOrigin(), rotation, clipModel, clipModel->GetAxis(), bodies[ i ].GetClipMask(), self );
|
||
|
}
|
||
|
|
||
|
if( tr.fraction < results.fraction ) {
|
||
|
results = tr;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
partialRotation = rotation * results.fraction;
|
||
|
results.endpos = current->i.position * partialRotation;
|
||
|
results.endAxis = current->i.orientation * partialRotation.ToMat3();
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ClipContents
|
||
|
================
|
||
|
*/
|
||
|
int sdPhysics_RigidBodyMultiple::ClipContents( const idClipModel *model ) const {
|
||
|
int contents = 0;
|
||
|
int i;
|
||
|
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
idClipModel* clipModel = bodies[ i ].GetClipModel();
|
||
|
if( !clipModel ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
if ( model ) {
|
||
|
contents |= gameLocal.clip.ContentsModel( CLIP_DEBUG_PARMS_ENTINFO( self ) clipModel->GetOrigin(), clipModel, clipModel->GetAxis(), -1, model, model->GetOrigin(), model->GetAxis() );
|
||
|
} else {
|
||
|
contents |= gameLocal.clip.Contents( CLIP_DEBUG_PARMS_ENTINFO( self ) clipModel->GetOrigin(), clipModel, clipModel->GetAxis(), -1, NULL );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return contents;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetMaster
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetMaster( idEntity *master, const bool orientated ) {
|
||
|
idVec3 masterOrigin;
|
||
|
idMat3 masterAxis;
|
||
|
|
||
|
if ( master ) {
|
||
|
if ( !masterEntity ) {
|
||
|
// transform from world space to master space
|
||
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
localOrigin = ( current->i.position - masterOrigin ) * masterAxis.Transpose();
|
||
|
if ( orientated ) {
|
||
|
localAxis = current->i.orientation * masterAxis.Transpose();
|
||
|
} else {
|
||
|
localAxis = current->i.orientation;
|
||
|
}
|
||
|
masterEntity = master;
|
||
|
flags.isOrientated = orientated;
|
||
|
ClearContacts();
|
||
|
}
|
||
|
} else {
|
||
|
localOrigin = vec3_origin;
|
||
|
localAxis = mat3_identity;
|
||
|
|
||
|
if ( masterEntity ) {
|
||
|
masterEntity = NULL;
|
||
|
Activate();
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetPushed
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetPushed( int deltaTime ) {
|
||
|
idRotation rotation;
|
||
|
|
||
|
rotation = ( saved.i.orientation * current->i.orientation ).ToRotation();
|
||
|
|
||
|
// velocity with which the af is pushed
|
||
|
current->pushVelocity.SubVec3(0) += ( current->i.position - saved.i.position ) / ( MS2SEC( deltaTime ) );
|
||
|
current->pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( MS2SEC( deltaTime ) );
|
||
|
next->pushVelocity = current->pushVelocity;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetPushedLinearVelocity
|
||
|
================
|
||
|
*/
|
||
|
const idVec3 &sdPhysics_RigidBodyMultiple::GetPushedLinearVelocity( const int id ) const {
|
||
|
return current->pushVelocity.SubVec3(0);
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetPushedAngularVelocity
|
||
|
================
|
||
|
*/
|
||
|
const idVec3 &sdPhysics_RigidBodyMultiple::GetPushedAngularVelocity( const int id ) const {
|
||
|
return current->pushVelocity.SubVec3(1);
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::SetClipMask
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::SetClipMask( int mask, int id ) {
|
||
|
if( id >= 0 && id < bodies.Num() ) {
|
||
|
bodies[ id ].SetClipMask( mask );
|
||
|
} else {
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].SetClipMask( mask );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
mainClipMask = 0;
|
||
|
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
mainClipMask |= bodies[ i ].GetClipMask();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetClipMask
|
||
|
================
|
||
|
*/
|
||
|
int sdPhysics_RigidBodyMultiple::GetClipMask( int id ) const {
|
||
|
int contents = 0;
|
||
|
|
||
|
if( id >= 0 && id < bodies.Num() ) {
|
||
|
contents = bodies[ id ].GetClipMask();
|
||
|
} else {
|
||
|
int i;
|
||
|
for( i = 0; i < bodies.Num(); i++ ) {
|
||
|
contents |= bodies[ i ].GetClipMask();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return contents;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetBodyContacts
|
||
|
================
|
||
|
*/
|
||
|
int sdPhysics_RigidBodyMultiple::GetBodyContacts( const int id, const contactInfo_t** _contacts, int maxContacts ) const {
|
||
|
int i;
|
||
|
int num = 0;
|
||
|
|
||
|
assert( maxContacts >= 1 );
|
||
|
if ( !( maxContacts >= 1 ) ) {
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
for ( i = 0; i < contacts.Num(); i++ ) {
|
||
|
if ( id == -1 || contacts[ i ].selfId == id ) {
|
||
|
_contacts[ num ] = &contacts[ i ];
|
||
|
num++;
|
||
|
if ( num >= maxContacts ) {
|
||
|
return num;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return num;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::GetBodyGroundContacts
|
||
|
================
|
||
|
*/
|
||
|
int sdPhysics_RigidBodyMultiple::GetBodyGroundContacts( const int id, const contactInfo_t** _contacts, int maxContacts ) const {
|
||
|
int i;
|
||
|
int num = 0;
|
||
|
|
||
|
assert( maxContacts >= 1 );
|
||
|
if ( !( maxContacts >= 1 ) ) {
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
for ( i = 0; i < contacts.Num(); i++ ) {
|
||
|
if ( id == -1 || contacts[ i ].selfId == id ) {
|
||
|
if ( contacts[ i ].normal * -gravityNormal <= 0.0f ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
_contacts[ num ] = &contacts[ i ];
|
||
|
num++;
|
||
|
if ( num >= maxContacts ) {
|
||
|
return num;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return num;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ApplyImpulse
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::ApplyImpulse( const idVec3& linearImpulse, const idVec3& angularImpulse ) {
|
||
|
current->i.linearMomentum += linearImpulse;
|
||
|
current->i.angularMomentum += angularImpulse;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::UnlinkClip
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::UnlinkClip( void ) {
|
||
|
int i;
|
||
|
for ( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].UnLink();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::LinkClip
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::LinkClip( void ) {
|
||
|
int i;
|
||
|
for ( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::EnableClip
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::EnableClip( void ) {
|
||
|
int i;
|
||
|
for ( i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].GetClipModel()->Enable();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::DisableClip
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::DisableClip( bool activateContacting ) {
|
||
|
int i;
|
||
|
for ( i = 0; i < bodies.Num(); i++ ) {
|
||
|
if ( activateContacting ) {
|
||
|
WakeEntitiesContacting( self, bodies[ i ].GetClipModel() );
|
||
|
}
|
||
|
bodies[ i ].GetClipModel()->Disable();
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CheckWater
|
||
|
================
|
||
|
*/
|
||
|
|
||
|
const idClipModel* sdPhysics_RigidBodyMultiple::CheckWater( void ) {
|
||
|
waterLevel = 0.0f;
|
||
|
|
||
|
idBounds absBounds;
|
||
|
absBounds.FromTransformedBounds( totalBounds, current->i.position, current->i.orientation );
|
||
|
|
||
|
const idClipModel* clipModel;
|
||
|
int count = gameLocal.clip.ClipModelsTouchingBounds( CLIP_DEBUG_PARMS_ENTINFO( self ) absBounds, CONTENTS_WATER, &clipModel, 1, NULL );
|
||
|
if ( !count ) {
|
||
|
return NULL;
|
||
|
}
|
||
|
|
||
|
if ( !clipModel->GetNumCollisionModels() ) {
|
||
|
return NULL;
|
||
|
}
|
||
|
|
||
|
idVec3 waterCurrent;
|
||
|
clipModel->GetEntity()->GetWaterCurrent( waterCurrent );
|
||
|
|
||
|
idCollisionModel* model = clipModel->GetCollisionModel( 0 );
|
||
|
int numPlanes = model->GetNumBrushPlanes();
|
||
|
if ( !numPlanes ) {
|
||
|
return NULL;
|
||
|
}
|
||
|
const idBounds& modelBounds = model->GetBounds();
|
||
|
|
||
|
//self->CheckWater( clipModel->GetOrigin(), clipModel->GetAxis(), model );
|
||
|
|
||
|
{
|
||
|
|
||
|
float scratch[ MAX_TRACEMODEL_WATER_POINTS ];
|
||
|
float velocityScratch[ MAX_TRACEMODEL_WATER_POINTS ];
|
||
|
float waterVolume = 0.0f;
|
||
|
float totalVolume = 0.0f;
|
||
|
idVec3 comWorld = mainCenterOfMass * current->i.orientation + current->i.position;
|
||
|
|
||
|
for ( int j = 0; j < bodies.Num(); j++ ) {
|
||
|
bool doBuoyancy = bodies[ j ].GetBuoyancy() > 0.0f;
|
||
|
bool doDrag = bodies[ j ].GetWaterDrag() > 0.0f;
|
||
|
if ( !doBuoyancy && !doDrag ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idClipModel* bodyClip = bodies[ j ].GetClipModel();
|
||
|
const traceModelWater_t* waterPoints = bodyClip->GetWaterPoints();
|
||
|
float volume = bodyClip->GetTraceModelVolume();
|
||
|
totalVolume += volume;
|
||
|
|
||
|
const idMat3 transpose = bodyClip->GetAxis().Transpose();
|
||
|
const idVec3 localAngularVelocity = GetAngularVelocity() * transpose;
|
||
|
const idVec3 localLinearVelocity = GetLinearVelocity() * transpose;
|
||
|
const idVec3 localGravity = gravityNormal * transpose;
|
||
|
const idVec3 localCenterOfMass = ( comWorld - bodyClip->GetOrigin() ) * transpose;
|
||
|
|
||
|
float bodyDrag = bodies[ j ].GetWaterDrag();
|
||
|
|
||
|
for ( int i = 0; i < MAX_TRACEMODEL_WATER_POINTS; i++ ) {
|
||
|
scratch[ i ] = waterPoints[ i ].weight * volume;
|
||
|
}
|
||
|
|
||
|
for ( int l = 0; l < numPlanes; l++ ) {
|
||
|
idPlane plane = model->GetBrushPlane( l );
|
||
|
plane.TranslateSelf( clipModel->GetOrigin() - bodyClip->GetOrigin() );
|
||
|
plane.Normal() *= clipModel->GetAxis();
|
||
|
plane.Normal() *= transpose;
|
||
|
|
||
|
for ( int i = 0; i < MAX_TRACEMODEL_WATER_POINTS; i++ ) {
|
||
|
if ( plane.Distance( waterPoints[ i ].xyz ) > 0.0f ) {
|
||
|
scratch[ i ] = 0.0f;
|
||
|
velocityScratch[ i ] = 0.0f;
|
||
|
} else if ( doDrag ) {
|
||
|
// this is calculated here as it is fairly expensive
|
||
|
velocityScratch[ i ] = localGravity * ( localAngularVelocity.Cross( waterPoints[ i ].xyz - localCenterOfMass ) + localLinearVelocity );
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
float height = clipModel->GetOrigin().z - bodyClip->GetOrigin().z + modelBounds.GetMaxs().z;
|
||
|
idPlane plane( transpose[ 2 ], height );
|
||
|
|
||
|
for ( int i = 0; i < MAX_TRACEMODEL_WATER_POINTS; i++ ) {
|
||
|
if ( scratch[ i ] == 0.0f || velocityScratch[ i ] == 0.0f ) {
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
idVec3 org = bodyClip->GetOrigin() + ( bodyClip->GetAxis() * waterPoints[ i ].xyz );
|
||
|
idVec3 mainSpaceOrg = org - (current->i.position + mainCenterOfMass * current->i.orientation );
|
||
|
|
||
|
if ( doBuoyancy ) {
|
||
|
scratch[ i ] *= Min( -plane.Distance( waterPoints[ i ].xyz ) / 16.0f, 1.0f );
|
||
|
|
||
|
idVec3 impulse = scratch[ i ] * ( ( -gravityNormal * bodies[ j ].GetBuoyancy() ) + ( waterCurrent * mainInverseMass ) );
|
||
|
|
||
|
current->i.linearMomentum += impulse;
|
||
|
current->i.angularMomentum += ( mainSpaceOrg ).Cross( impulse );
|
||
|
|
||
|
waterVolume += scratch[ i ];
|
||
|
}
|
||
|
|
||
|
if ( doDrag ) {
|
||
|
float drag = -waterPoints[ i ].weight * velocityScratch[ i ];
|
||
|
idVec3 impulse = bodyDrag * drag * gravityNormal * mainMass;
|
||
|
|
||
|
current->i.linearMomentum += impulse;
|
||
|
current->i.angularMomentum += ( mainSpaceOrg ).Cross( impulse );
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if ( totalVolume > idMath::FLT_EPSILON ) {
|
||
|
waterLevel = waterVolume / totalVolume;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return clipModel;
|
||
|
}
|
||
|
|
||
|
const float RB_ORIGIN_MAX = 32767;
|
||
|
const int RB_ORIGIN_TOTAL_BITS = 24;
|
||
|
const int RB_ORIGIN_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( RB_ORIGIN_MAX ) ) + 1;
|
||
|
const int RB_ORIGIN_MANTISSA_BITS = RB_ORIGIN_TOTAL_BITS - 1 - RB_ORIGIN_EXPONENT_BITS;
|
||
|
|
||
|
const float RB_LINEAR_VELOCITY_MIN = 0.05f;
|
||
|
const float RB_LINEAR_VELOCITY_MAX = 8192.0f;
|
||
|
const int RB_LINEAR_VELOCITY_TOTAL_BITS = 20;
|
||
|
const int RB_LINEAR_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( RB_LINEAR_VELOCITY_MAX, RB_LINEAR_VELOCITY_MIN ) ) + 1;
|
||
|
const int RB_LINEAR_VELOCITY_MANTISSA_BITS = RB_LINEAR_VELOCITY_TOTAL_BITS - 1 - RB_LINEAR_VELOCITY_EXPONENT_BITS;
|
||
|
|
||
|
const float RB_ANGULAR_VELOCITY_MIN = 0.00001f;
|
||
|
const float RB_ANGULAR_VELOCITY_MAX = idMath::PI * 8.0f;
|
||
|
const int RB_ANGULAR_VELOCITY_TOTAL_BITS = 20;
|
||
|
const int RB_ANGULAR_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( RB_ANGULAR_VELOCITY_MAX, RB_ANGULAR_VELOCITY_MIN ) ) + 1;
|
||
|
const int RB_ANGULAR_VELOCITY_MANTISSA_BITS = RB_ANGULAR_VELOCITY_TOTAL_BITS - 1 - RB_ANGULAR_VELOCITY_EXPONENT_BITS;
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CheckNetworkStateChanges
|
||
|
================
|
||
|
*/
|
||
|
bool sdPhysics_RigidBodyMultiple::CheckNetworkStateChanges( networkStateMode_t mode, const sdEntityStateNetworkData& baseState ) const {
|
||
|
if ( mode == NSM_VISIBLE ) {
|
||
|
NET_GET_BASE( sdRBMultipleNetworkState );
|
||
|
|
||
|
idVec3 angularVelocity = GetAngularVelocity();
|
||
|
idVec3 linearVelocity = GetLinearVelocity();
|
||
|
|
||
|
NET_CHECK_FIELD( angularVelocity, angularVelocity );
|
||
|
NET_CHECK_FIELD( linearVelocity, linearVelocity );
|
||
|
NET_CHECK_FIELD( orientation, current->i.orientation.ToCQuat() );
|
||
|
NET_CHECK_FIELD( origin, current->i.position );
|
||
|
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
if ( mode == NSM_BROADCAST ) {
|
||
|
NET_GET_BASE( sdRBMultipleBroadcastState );
|
||
|
|
||
|
NET_CHECK_FIELD( localOrigin, localOrigin );
|
||
|
NET_CHECK_FIELD( localOrientation, localAxis.ToCQuat() );
|
||
|
NET_CHECK_FIELD( atRest, current->atRest );
|
||
|
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::WriteNetworkState
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::WriteNetworkState( networkStateMode_t mode, const sdEntityStateNetworkData& baseState, sdEntityStateNetworkData& newState, idBitMsg& msg ) const {
|
||
|
if ( mode == NSM_VISIBLE ) {
|
||
|
NET_GET_STATES( sdRBMultipleNetworkState );
|
||
|
|
||
|
newData.origin = current->i.position;
|
||
|
newData.orientation = current->i.orientation.ToCQuat();
|
||
|
newData.angularVelocity = GetAngularVelocity();
|
||
|
newData.linearVelocity = GetLinearVelocity();
|
||
|
|
||
|
msg.WriteDeltaVector( baseData.origin, newData.origin, RB_ORIGIN_EXPONENT_BITS, RB_ORIGIN_MANTISSA_BITS );
|
||
|
msg.WriteDeltaCQuat( baseData.orientation, newData.orientation );
|
||
|
msg.WriteDeltaVector( baseData.angularVelocity, newData.angularVelocity, RB_ANGULAR_VELOCITY_EXPONENT_BITS, RB_ANGULAR_VELOCITY_MANTISSA_BITS );
|
||
|
msg.WriteDeltaVector( baseData.linearVelocity, newData.linearVelocity, RB_LINEAR_VELOCITY_EXPONENT_BITS, RB_LINEAR_VELOCITY_MANTISSA_BITS );
|
||
|
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
if ( mode == NSM_BROADCAST ) {
|
||
|
NET_GET_STATES( sdRBMultipleBroadcastState );
|
||
|
|
||
|
newData.localOrigin = localOrigin;
|
||
|
newData.localOrientation = localAxis.ToCQuat();
|
||
|
newData.atRest = current->atRest;
|
||
|
|
||
|
msg.WriteDeltaVector( baseData.localOrigin, newData.localOrigin, RB_ORIGIN_EXPONENT_BITS, RB_ORIGIN_MANTISSA_BITS );
|
||
|
msg.WriteDeltaCQuat( baseData.localOrientation, newData.localOrientation );
|
||
|
msg.WriteDeltaLong( baseData.atRest, newData.atRest );
|
||
|
|
||
|
return;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ApplyNetworkState
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::ApplyNetworkState( networkStateMode_t mode, const sdEntityStateNetworkData& newState ) {
|
||
|
traceCollection.ForceNextUpdate();
|
||
|
if ( mode == NSM_VISIBLE ) {
|
||
|
NET_GET_NEW( sdRBMultipleNetworkState );
|
||
|
|
||
|
current->i.position = newData.origin;
|
||
|
current->i.orientation = newData.orientation.ToMat3();
|
||
|
SetAngularVelocity( newData.angularVelocity );
|
||
|
SetLinearVelocity( newData.linearVelocity );
|
||
|
|
||
|
self->UpdateVisuals();
|
||
|
for( int i = 0; i < bodies.Num(); i++ ) {
|
||
|
bodies[ i ].Link( self, *current );
|
||
|
}
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
if ( mode == NSM_BROADCAST ) {
|
||
|
NET_GET_NEW( sdRBMultipleBroadcastState );
|
||
|
|
||
|
localOrigin = newData.localOrigin;
|
||
|
localAxis = newData.localOrientation.ToMat3();
|
||
|
|
||
|
if ( current->atRest != newData.atRest ) {
|
||
|
if ( newData.atRest == -1 ) {
|
||
|
Activate();
|
||
|
} else {
|
||
|
Rest( newData.atRest );
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::ReadNetworkState
|
||
|
================
|
||
|
*/
|
||
|
void sdPhysics_RigidBodyMultiple::ReadNetworkState( networkStateMode_t mode, const sdEntityStateNetworkData& baseState, sdEntityStateNetworkData& newState, const idBitMsg& msg ) const {
|
||
|
if ( mode == NSM_VISIBLE ) {
|
||
|
NET_GET_STATES( sdRBMultipleNetworkState );
|
||
|
|
||
|
newData.origin = msg.ReadDeltaVector( baseData.origin, RB_ORIGIN_EXPONENT_BITS, RB_ORIGIN_MANTISSA_BITS );
|
||
|
newData.orientation = msg.ReadDeltaCQuat( baseData.orientation );
|
||
|
newData.angularVelocity = msg.ReadDeltaVector( baseData.angularVelocity, RB_ANGULAR_VELOCITY_EXPONENT_BITS, RB_ANGULAR_VELOCITY_MANTISSA_BITS );
|
||
|
newData.linearVelocity = msg.ReadDeltaVector( baseData.linearVelocity, RB_LINEAR_VELOCITY_EXPONENT_BITS, RB_LINEAR_VELOCITY_MANTISSA_BITS );
|
||
|
|
||
|
// denormals are bad!
|
||
|
newData.origin.FixDenormals();
|
||
|
newData.orientation.FixDenormals();
|
||
|
newData.angularVelocity.FixDenormals();
|
||
|
newData.linearVelocity.FixDenormals();
|
||
|
|
||
|
self->OnNewOriginRead( newData.origin );
|
||
|
self->OnNewAxesRead( newData.orientation.ToMat3() );
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
if ( mode == NSM_BROADCAST ) {
|
||
|
NET_GET_STATES( sdRBMultipleBroadcastState );
|
||
|
|
||
|
newData.localOrigin = msg.ReadDeltaVector( baseData.localOrigin, RB_ORIGIN_EXPONENT_BITS, RB_ORIGIN_MANTISSA_BITS );
|
||
|
newData.localOrientation = msg.ReadDeltaCQuat( baseData.localOrientation );
|
||
|
newData.atRest = msg.ReadDeltaLong( baseData.atRest );
|
||
|
|
||
|
return;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
================
|
||
|
sdPhysics_RigidBodyMultiple::CreateNetworkStructure
|
||
|
================
|
||
|
*/
|
||
|
sdEntityStateNetworkData* sdPhysics_RigidBodyMultiple::CreateNetworkStructure( networkStateMode_t mode ) const {
|
||
|
if ( mode == NSM_VISIBLE ) {
|
||
|
return new sdRBMultipleNetworkState();
|
||
|
}
|
||
|
if ( mode == NSM_BROADCAST ) {
|
||
|
return new sdRBMultipleBroadcastState();
|
||
|
}
|
||
|
return NULL;
|
||
|
}
|