1486 lines
40 KiB
C++
1486 lines
40 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_SimpleRigidBody.h"
|
||
|
|
#include "../Entity.h"
|
||
|
|
#include "../Player.h"
|
||
|
|
#include "../ContentMask.h"
|
||
|
|
#include "../script/Script_Helper.h"
|
||
|
|
#include "../script/Script_ScriptObject.h"
|
||
|
|
|
||
|
|
CLASS_DECLARATION( idPhysics_Base, sdPhysics_SimpleRigidBody )
|
||
|
|
END_CLASS
|
||
|
|
|
||
|
|
const float STOP_SPEED = 10.0f;
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdSimpleRigidBodyNetworkState::MakeDefault
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdSimpleRigidBodyNetworkState::MakeDefault( void ) {
|
||
|
|
position = vec3_origin;
|
||
|
|
orientation.x = 0.f;
|
||
|
|
orientation.y = 0.f;
|
||
|
|
orientation.z = 0.f;
|
||
|
|
linearVelocity = vec3_zero;
|
||
|
|
angularVelocity = vec3_zero;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdSimpleRigidBodyNetworkState::Write
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdSimpleRigidBodyNetworkState::Write( idFile* file ) const {
|
||
|
|
file->WriteVec3( position );
|
||
|
|
file->WriteCQuat( orientation );
|
||
|
|
file->WriteVec3( linearVelocity );
|
||
|
|
file->WriteVec3( angularVelocity );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdSimpleRigidBodyNetworkState::Read
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdSimpleRigidBodyNetworkState::Read( idFile* file ) {
|
||
|
|
file->ReadVec3( position );
|
||
|
|
file->ReadCQuat( orientation );
|
||
|
|
file->ReadVec3( linearVelocity );
|
||
|
|
file->ReadVec3( angularVelocity );
|
||
|
|
|
||
|
|
position.FixDenormals();
|
||
|
|
orientation.FixDenormals();
|
||
|
|
angularVelocity.FixDenormals();
|
||
|
|
linearVelocity.FixDenormals();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdSimpleRigidBodyBroadcastState::MakeDefault
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdSimpleRigidBodyBroadcastState::MakeDefault( void ) {
|
||
|
|
localPosition = vec3_origin;
|
||
|
|
localOrientation.x = 0.f;
|
||
|
|
localOrientation.y = 0.f;
|
||
|
|
localOrientation.z = 0.f;
|
||
|
|
atRest = -1;
|
||
|
|
orientedClip = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdSimpleRigidBodyBroadcastState::Write
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdSimpleRigidBodyBroadcastState::Write( idFile* file ) const {
|
||
|
|
file->WriteVec3( localPosition );
|
||
|
|
file->WriteCQuat( localOrientation );
|
||
|
|
file->WriteInt( atRest );
|
||
|
|
file->WriteBool( orientedClip );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdSimpleRigidBodyBroadcastState::Read
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdSimpleRigidBodyBroadcastState::Read( idFile* file ) {
|
||
|
|
file->ReadVec3( localPosition );
|
||
|
|
file->ReadCQuat( localOrientation );
|
||
|
|
file->ReadInt( atRest );
|
||
|
|
file->ReadBool( orientedClip );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::Integrate
|
||
|
|
|
||
|
|
Very coarse & naive
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::Integrate( float deltaTime, simpleRigidBodyPState_t &next ) {
|
||
|
|
|
||
|
|
next = current;
|
||
|
|
|
||
|
|
// apply gravity
|
||
|
|
next.linearVelocity += deltaTime * gravityVector;
|
||
|
|
|
||
|
|
// remove any contribution towards the last normal
|
||
|
|
float velocityTowardsLast = -lastCollideNormal * next.linearVelocity;
|
||
|
|
if ( velocityTowardsLast > 0.0f ) {
|
||
|
|
next.linearVelocity += lastCollideNormal * velocityTowardsLast;
|
||
|
|
}
|
||
|
|
|
||
|
|
next.position = next.position + deltaTime * next.linearVelocity;
|
||
|
|
|
||
|
|
// rotation
|
||
|
|
idVec3 rotAxis = current.angularVelocity;
|
||
|
|
float angle = RAD2DEG( rotAxis.Normalize() );
|
||
|
|
if ( angle > 0.00001f ) {
|
||
|
|
idRotation rotation( vec3_origin, rotAxis, -angle * deltaTime );
|
||
|
|
next.orientation = current.orientation * rotation.ToMat3();
|
||
|
|
next.orientation.OrthoNormalizeSelf();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::CollisionResponse
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
bool sdPhysics_SimpleRigidBody::CollisionResponse( const trace_t &collision, idVec3 &impulse ) {
|
||
|
|
|
||
|
|
idVec3& velocity = current.linearVelocity;
|
||
|
|
idVec3 hitVelocity = velocity;
|
||
|
|
|
||
|
|
float normalVel = velocity * collision.c.normal;
|
||
|
|
idVec3 tangent = velocity - normalVel * collision.c.normal;
|
||
|
|
float tangentVel = tangent.Normalize();
|
||
|
|
if ( tangentVel < idMath::FLT_EPSILON ) {
|
||
|
|
tangent.Set( 0.0f, 0.0f, 1.0f );
|
||
|
|
tangentVel = 0.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
// HACK - scale the bouncyness so that things can't stop on steep walls
|
||
|
|
float bounceScale = 1.0f;
|
||
|
|
if ( idMath::Fabs( collision.c.normal.z ) < 0.7071f ) {
|
||
|
|
float bounceNeeded = Min( 1.0f, -90.0f / normalVel );
|
||
|
|
if ( normalBouncyness > idMath::FLT_EPSILON ) {
|
||
|
|
bounceScale = Max( 1.0f, bounceNeeded / normalBouncyness );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// bounce the velocity, always bounce off of noplant surfaces.
|
||
|
|
if ( collision.c.material != NULL && ( collision.c.material->GetSurfaceFlags() & SURF_NOPLANT ) ) {
|
||
|
|
normalVel = -normalVel;
|
||
|
|
} else {
|
||
|
|
normalVel *= -normalBouncyness * bounceScale;
|
||
|
|
tangentVel *= tangentialBouncyness * bounceScale;
|
||
|
|
}
|
||
|
|
velocity = normalVel*collision.c.normal + tangentVel*tangent;
|
||
|
|
|
||
|
|
// check for stopping
|
||
|
|
if ( collision.c.normal.z > 0.2f && velocity.LengthSqr() < Square( stopSpeed ) ) {
|
||
|
|
velocity.Zero();
|
||
|
|
current.angularVelocity.Zero();
|
||
|
|
} else {
|
||
|
|
if ( collision.c.normal.z > 0.2f ) {
|
||
|
|
// small velocity -> zero
|
||
|
|
velocity.FixDenormals( 0.1f );
|
||
|
|
} else {
|
||
|
|
velocity.FixDenormals( 0.000001f );
|
||
|
|
}
|
||
|
|
|
||
|
|
// push out from collision surface
|
||
|
|
current.position += collision.c.normal * 0.01f;
|
||
|
|
|
||
|
|
// reflect the angular velocity about the normal too
|
||
|
|
current.angularVelocity -= ( ( 1.0f - angularBouncyness ) * current.angularVelocity*collision.c.normal )*collision.c.normal;
|
||
|
|
current.angularVelocity.FixDenormals( 0.000001f );
|
||
|
|
}
|
||
|
|
|
||
|
|
idEntity* ent = gameLocal.entities[ collision.c.entityNum ];
|
||
|
|
|
||
|
|
ent->Hit( collision, hitVelocity, self );
|
||
|
|
return self->Collide( collision, hitVelocity, -1 );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::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_SimpleRigidBody::CheckForCollisions( const float deltaTime, simpleRigidBodyPState_t &next, trace_t &collision ) {
|
||
|
|
collision.fraction = 1.0f;
|
||
|
|
|
||
|
|
// calculate the position of the center of mass at current and next
|
||
|
|
idVec3 CoMstart = current.position + centerOfMass;
|
||
|
|
idVec3 CoMend = next.position + centerOfMass;
|
||
|
|
|
||
|
|
// if there was a collision
|
||
|
|
if ( gameLocal.clip.Translation( CLIP_DEBUG_PARMS_ENTINFO( self ) collision, CoMstart, CoMend, centeredClipModel, mat3_identity, clipMask, self ) ) {
|
||
|
|
// set the next state to the state at the moment of impact
|
||
|
|
next.position = collision.endpos - centerOfMass;
|
||
|
|
next.orientation = current.orientation;
|
||
|
|
next.linearVelocity = current.linearVelocity;
|
||
|
|
next.angularVelocity = current.angularVelocity;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::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_SimpleRigidBody::TestIfAtRest( void ) const {
|
||
|
|
if ( current.atRest >= 0 ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
// when its finished bouncing the collision impulse code manually sets the
|
||
|
|
// velocities to zero
|
||
|
|
if ( current.linearVelocity != vec3_origin || current.angularVelocity != vec3_origin ) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// needs to be touching the ground
|
||
|
|
if ( contacts.Num() < 1 ) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::DropToFloorAndRest
|
||
|
|
|
||
|
|
Drops the object straight down to the floor and verifies if the object is at rest on the floor.
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::DropToFloorAndRest( void ) {
|
||
|
|
idVec3 down;
|
||
|
|
trace_t tr;
|
||
|
|
|
||
|
|
if ( testSolid ) {
|
||
|
|
|
||
|
|
testSolid = false;
|
||
|
|
|
||
|
|
if ( gameLocal.clip.Contents( CLIP_DEBUG_PARMS_ENTINFO( self ) current.position, clipModel, mat3_identity, clipMask, self ) ) {
|
||
|
|
gameLocal.DWarning( "rigid body in solid for entity '%s' type '%s' at (%s)",
|
||
|
|
self->name.c_str(), self->GetType()->classname, current.position.ToString(0) );
|
||
|
|
Rest();
|
||
|
|
dropToFloor = false;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// put the body on the floor
|
||
|
|
down = current.position + gravityNormal * 128.0f;
|
||
|
|
gameLocal.clip.Translation( CLIP_DEBUG_PARMS_ENTINFO( self ) tr, current.position, down, clipModel, mat3_identity, clipMask, self );
|
||
|
|
current.position = tr.endpos;
|
||
|
|
if ( !orientedClip ) {
|
||
|
|
clipModel->Link( gameLocal.clip, self, clipModel->GetId(), tr.endpos, mat3_identity );
|
||
|
|
} else {
|
||
|
|
clipModel->Link( gameLocal.clip, self, clipModel->GetId(), tr.endpos, current.orientation );
|
||
|
|
}
|
||
|
|
|
||
|
|
// if on the floor already
|
||
|
|
if ( tr.fraction == 0.0f ) {
|
||
|
|
// test if we are really at rest
|
||
|
|
EvaluateContacts( CLIP_DEBUG_PARMS_ENTINFO_ONLY( self ) );
|
||
|
|
if ( !TestIfAtRest() ) {
|
||
|
|
gameLocal.DWarning( "rigid body not at rest for entity '%s' type '%s' at (%s)",
|
||
|
|
self->name.c_str(), self->GetType()->classname, current.position.ToString(0) );
|
||
|
|
}
|
||
|
|
Rest();
|
||
|
|
dropToFloor = false;
|
||
|
|
} else if ( IsOutsideWorld() ) {
|
||
|
|
// gameLocal.Warning( "rigid body outside world bounds for entity '%s' type '%s' at (%s)",
|
||
|
|
// self->name.c_str(), self->GetType()->classname, current.position.ToString(0) );
|
||
|
|
Rest();
|
||
|
|
dropToFloor = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::DebugDraw
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::DebugDraw( void ) {
|
||
|
|
|
||
|
|
if ( rb_showBodies.GetBool() || ( rb_showActive.GetBool() && current.atRest < 0 ) ) {
|
||
|
|
clipModel->Draw();
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( rb_showMass.GetBool() ) {
|
||
|
|
gameRenderWorld->DrawText( va( "\n%1.2f", mass ), current.position, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( rb_showVelocity.GetBool() ) {
|
||
|
|
DrawVelocity( clipModel->GetId(), 0.1f, 4.0f );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::sdPhysics_SimpleRigidBody
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
sdPhysics_SimpleRigidBody::sdPhysics_SimpleRigidBody( void ) {
|
||
|
|
|
||
|
|
// set default rigid body properties
|
||
|
|
SetClipMask( MASK_SOLID | CONTENTS_BODY | CONTENTS_SLIDEMOVER );
|
||
|
|
SetBouncyness( 0.6f );
|
||
|
|
SetStopSpeed( 40.0f );
|
||
|
|
SetFriction( 0.6f, 0.6f, 0.0f );
|
||
|
|
SetWaterFriction( 1.f, 1.f );
|
||
|
|
clipModel = NULL;
|
||
|
|
centeredClipModel = new idClipModel();
|
||
|
|
|
||
|
|
memset( ¤t, 0, sizeof( current ) );
|
||
|
|
|
||
|
|
current.atRest = -1;
|
||
|
|
current.lastTimeStep = MS2SEC( gameLocal.msec );
|
||
|
|
|
||
|
|
current.position.Zero();
|
||
|
|
current.orientation.Identity();
|
||
|
|
|
||
|
|
current.linearVelocity.Zero();
|
||
|
|
current.angularVelocity.Zero();
|
||
|
|
lastCollideNormal.Zero();
|
||
|
|
|
||
|
|
saved = current;
|
||
|
|
|
||
|
|
mass = 1.0f;
|
||
|
|
inverseMass = 1.0f;
|
||
|
|
centerOfMass.Zero();
|
||
|
|
inertiaTensor.Identity();
|
||
|
|
inverseInertiaTensor.Identity();
|
||
|
|
SetBuoyancy( 1.f );
|
||
|
|
waterLevel = 0.0f;
|
||
|
|
|
||
|
|
dropToFloor = false;
|
||
|
|
noImpact = false;
|
||
|
|
noContact = false;
|
||
|
|
|
||
|
|
hasMaster = false;
|
||
|
|
isOrientated = false;
|
||
|
|
|
||
|
|
orientedClip = false;
|
||
|
|
|
||
|
|
restFunc = NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::~sdPhysics_SimpleRigidBody
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
sdPhysics_SimpleRigidBody::~sdPhysics_SimpleRigidBody( void ) {
|
||
|
|
gameLocal.clip.DeleteClipModel( clipModel );
|
||
|
|
gameLocal.clip.DeleteClipModel( centeredClipModel );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetClipModel
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
#define MAX_INERTIA_SCALE 10.0f
|
||
|
|
|
||
|
|
void sdPhysics_SimpleRigidBody::SetClipModel( idClipModel *model, const float density, int id, bool freeOld ) {
|
||
|
|
idMat3 inertiaScale;
|
||
|
|
|
||
|
|
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
|
||
|
|
|
||
|
|
if ( clipModel != NULL && clipModel != model && freeOld ) {
|
||
|
|
gameLocal.clip.DeleteClipModel( clipModel );
|
||
|
|
}
|
||
|
|
clipModel = model;
|
||
|
|
if ( !orientedClip ) {
|
||
|
|
clipModel->Link( gameLocal.clip, self, 0, current.position, mat3_identity );
|
||
|
|
} else {
|
||
|
|
clipModel->Link( gameLocal.clip, self, 0, current.position, current.orientation );
|
||
|
|
}
|
||
|
|
|
||
|
|
// get mass properties from the trace model
|
||
|
|
clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor );
|
||
|
|
|
||
|
|
// check whether or not the clip model has valid mass properties
|
||
|
|
if ( mass < idMath::FLT_EPSILON || FLOAT_IS_NAN( mass ) ) {
|
||
|
|
gameLocal.Warning( "sdPhysics_SimpleRigidBody::SetClipModel: invalid mass for entity '%s' type '%s'",
|
||
|
|
self->name.c_str(), self->GetType()->classname );
|
||
|
|
mass = 1.0f;
|
||
|
|
centerOfMass.Zero();
|
||
|
|
}
|
||
|
|
|
||
|
|
// check whether or not the inertia tensor is balanced
|
||
|
|
int minIndex = Min3Index( inertiaTensor[0][0], inertiaTensor[1][1], inertiaTensor[2][2] );
|
||
|
|
inertiaScale.Identity();
|
||
|
|
inertiaScale[0][0] = inertiaTensor[0][0] / inertiaTensor[minIndex][minIndex];
|
||
|
|
inertiaScale[1][1] = inertiaTensor[1][1] / inertiaTensor[minIndex][minIndex];
|
||
|
|
inertiaScale[2][2] = inertiaTensor[2][2] / inertiaTensor[minIndex][minIndex];
|
||
|
|
|
||
|
|
if ( inertiaScale[0][0] > MAX_INERTIA_SCALE || inertiaScale[1][1] > MAX_INERTIA_SCALE || inertiaScale[2][2] > MAX_INERTIA_SCALE ) {
|
||
|
|
gameLocal.Warning( "idPhysics_RigidBody::SetClipModel: unbalanced inertia tensor for entity '%s' type '%s'",
|
||
|
|
self->name.c_str(), self->GetType()->classname );
|
||
|
|
|
||
|
|
// correct the inertia tensor by replacing it with that of a box of the same bounds as this
|
||
|
|
idTraceModel trm( clipModel->GetBounds() );
|
||
|
|
trm.GetMassProperties( density, mass, centerOfMass, inertiaTensor );
|
||
|
|
}
|
||
|
|
|
||
|
|
inverseMass = 1.0f / mass;
|
||
|
|
inverseInertiaTensor = inertiaTensor.Inverse() * ( 1.0f / 6.0f );
|
||
|
|
|
||
|
|
current.linearVelocity.Zero();
|
||
|
|
current.angularVelocity.Zero();
|
||
|
|
|
||
|
|
// set up the centered clip model
|
||
|
|
idTraceModel tempTrace = *clipModel->GetTraceModel();
|
||
|
|
tempTrace.Translate( -centerOfMass );
|
||
|
|
centeredClipModel->LoadTraceModel( tempTrace, false );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetClipModel
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
idClipModel *sdPhysics_SimpleRigidBody::GetClipModel( int id ) const {
|
||
|
|
return clipModel;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetNumClipModels
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
int sdPhysics_SimpleRigidBody::GetNumClipModels( void ) const {
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetMass
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetMass( float mass, int id ) {
|
||
|
|
assert( mass > 0.0f );
|
||
|
|
this->mass = mass;
|
||
|
|
inverseMass = 1.0f / mass;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetMass
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
float sdPhysics_SimpleRigidBody::GetMass( int id ) const {
|
||
|
|
return mass;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetWaterFriction
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::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_SimpleRigidBody::SetBuoyancy
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetBuoyancy( float b ) {
|
||
|
|
buoyancy = b;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetBouncyness
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetBouncyness( const float b ) {
|
||
|
|
if ( b < 0.0f || b > 1.0f ) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
normalBouncyness = b;
|
||
|
|
tangentialBouncyness = b;
|
||
|
|
angularBouncyness = -1.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetBouncyness
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetBouncyness( float normal, float tangential, float angular ) {
|
||
|
|
normalBouncyness = normal;
|
||
|
|
tangentialBouncyness = tangential;
|
||
|
|
angularBouncyness = angular;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetStopSpeed
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetStopSpeed( float _stopSpeed ) {
|
||
|
|
stopSpeed = _stopSpeed;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::Rest
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::Rest( void ) {
|
||
|
|
int restTime = current.atRest;
|
||
|
|
|
||
|
|
current.atRest = gameLocal.time;
|
||
|
|
current.linearVelocity.Zero();
|
||
|
|
current.angularVelocity.Zero();
|
||
|
|
self->BecomeInactive( TH_PHYSICS );
|
||
|
|
|
||
|
|
if ( current.atRest != restTime ) {
|
||
|
|
sdScriptHelper h1;
|
||
|
|
self->GetScriptObject()->CallNonBlockingScriptEvent( self->GetScriptObject()->GetFunction( "OnRest" ), h1 );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::DropToFloor
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::DropToFloor( void ) {
|
||
|
|
dropToFloor = true;
|
||
|
|
testSolid = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::NoContact
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::NoContact( void ) {
|
||
|
|
noContact = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::Activate
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::Activate( void ) {
|
||
|
|
current.atRest = -1;
|
||
|
|
self->BecomeActive( TH_PHYSICS );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::PutToRest
|
||
|
|
|
||
|
|
put to rest untill something collides with this physics object
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::PutToRest( void ) {
|
||
|
|
Rest();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::EnableImpact
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::EnableImpact( void ) {
|
||
|
|
noImpact = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::DisableImpact
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::DisableImpact( void ) {
|
||
|
|
noImpact = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetContents
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetContents( int contents, int id ) {
|
||
|
|
if ( clipModel ) {
|
||
|
|
clipModel->SetContents( contents );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetContents
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
int sdPhysics_SimpleRigidBody::GetContents( int id ) const {
|
||
|
|
return clipModel->GetContents();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetBounds
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
const idBounds &sdPhysics_SimpleRigidBody::GetBounds( int id ) const {
|
||
|
|
return clipModel->GetBounds();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetAbsBounds
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
const idBounds &sdPhysics_SimpleRigidBody::GetAbsBounds( int id ) const {
|
||
|
|
return clipModel->GetAbsBounds();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::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_SimpleRigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) {
|
||
|
|
simpleRigidBodyPState_t next;
|
||
|
|
idAngles angles;
|
||
|
|
trace_t collision;
|
||
|
|
idVec3 impulse;
|
||
|
|
idVec3 oldOrigin, masterOrigin;
|
||
|
|
idMat3 oldAxis, masterAxis;
|
||
|
|
float timeStep, minTimeStep;
|
||
|
|
bool collided, cameToRest = false;
|
||
|
|
|
||
|
|
current.angularVelocity.FixDenormals();
|
||
|
|
current.linearVelocity.FixDenormals();
|
||
|
|
current.orientation.FixDenormals();
|
||
|
|
current.position.FixDenormals();
|
||
|
|
|
||
|
|
timeStep = MS2SEC( timeStepMSec );
|
||
|
|
minTimeStep = MS2SEC( 1 );
|
||
|
|
current.lastTimeStep = timeStep;
|
||
|
|
|
||
|
|
if ( hasMaster ) {
|
||
|
|
if ( timeStepMSec <= 0 ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
oldOrigin = current.position;
|
||
|
|
oldAxis = current.orientation;
|
||
|
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
|
current.position = masterOrigin + localOrigin * masterAxis;
|
||
|
|
if ( isOrientated ) {
|
||
|
|
current.orientation = localAxis * masterAxis;
|
||
|
|
} else {
|
||
|
|
current.orientation = localAxis;
|
||
|
|
}
|
||
|
|
LinkClip();
|
||
|
|
current.linearVelocity = ( current.position - oldOrigin ) / timeStep;
|
||
|
|
current.angularVelocity = ( current.orientation * oldAxis.Transpose() ).ToAngularVelocity() / timeStep;
|
||
|
|
|
||
|
|
return ( current.position != oldOrigin || current.orientation != oldAxis );
|
||
|
|
}
|
||
|
|
|
||
|
|
// if the body is at rest
|
||
|
|
if ( current.atRest >= 0 || timeStep <= 0.0f ) {
|
||
|
|
DebugDraw();
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// move the rigid body velocity into the frame of a pusher
|
||
|
|
current.linearVelocity -= current.pushVelocity.SubVec3( 0 );
|
||
|
|
current.angularVelocity -= current.pushVelocity.SubVec3( 1 );
|
||
|
|
|
||
|
|
clipModel->Unlink( gameLocal.clip );
|
||
|
|
|
||
|
|
int count = 0;
|
||
|
|
int maxRepetitions = 2;
|
||
|
|
|
||
|
|
do {
|
||
|
|
|
||
|
|
CheckWater();
|
||
|
|
|
||
|
|
// calculate next position and orientation
|
||
|
|
Integrate( timeStep, next );
|
||
|
|
|
||
|
|
// check for collisions from the current to the next state
|
||
|
|
collided = CheckForCollisions( timeStep, next, collision );
|
||
|
|
float timeStepUsed = collision.fraction * timeStep;
|
||
|
|
|
||
|
|
// set the new state
|
||
|
|
current = next;
|
||
|
|
|
||
|
|
if ( collided ) {
|
||
|
|
// apply collision impulse
|
||
|
|
if ( CollisionResponse( collision, impulse ) ) {
|
||
|
|
current.atRest = gameLocal.time;
|
||
|
|
}
|
||
|
|
lastCollideNormal = collision.c.normal;
|
||
|
|
} else {
|
||
|
|
lastCollideNormal.Zero();
|
||
|
|
}
|
||
|
|
|
||
|
|
// update the position of the clip model
|
||
|
|
LinkClip();
|
||
|
|
|
||
|
|
DebugDraw();
|
||
|
|
|
||
|
|
if ( !noContact ) {
|
||
|
|
// get contacts
|
||
|
|
EvaluateContacts( CLIP_DEBUG_PARMS_ENTINFO_ONLY( self ) );
|
||
|
|
|
||
|
|
// check if the body has come to rest
|
||
|
|
if ( TestIfAtRest() ) {
|
||
|
|
// put to rest
|
||
|
|
Rest();
|
||
|
|
cameToRest = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( current.atRest < 0 ) {
|
||
|
|
ActivateContactEntities();
|
||
|
|
}
|
||
|
|
|
||
|
|
timeStep -= timeStepUsed;
|
||
|
|
count++;
|
||
|
|
} while( count < maxRepetitions && timeStep >= minTimeStep );
|
||
|
|
|
||
|
|
// move the rigid body velocity back into the world frame
|
||
|
|
current.linearVelocity += current.pushVelocity.SubVec3( 0 );
|
||
|
|
current.angularVelocity += current.pushVelocity.SubVec3( 1 );
|
||
|
|
current.pushVelocity.Zero();
|
||
|
|
|
||
|
|
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.position.ToString(0) );
|
||
|
|
Rest();
|
||
|
|
}
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::UpdateTime
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::UpdateTime( int endTimeMSec ) {
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetTime
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
int sdPhysics_SimpleRigidBody::GetTime( void ) const {
|
||
|
|
return gameLocal.time;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetImpactInfo
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const {
|
||
|
|
idVec3 linearVelocity, angularVelocity;
|
||
|
|
idMat3 inverseWorldInertiaTensor;
|
||
|
|
|
||
|
|
linearVelocity = current.linearVelocity;
|
||
|
|
angularVelocity = current.angularVelocity;
|
||
|
|
|
||
|
|
info->invMass = inverseMass;
|
||
|
|
info->invInertiaTensor.Zero();
|
||
|
|
info->position = point - ( current.position + centerOfMass * current.orientation );
|
||
|
|
info->velocity = linearVelocity + angularVelocity.Cross( info->position );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::ApplyImpulse
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) {
|
||
|
|
if ( noImpact ) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
if ( hasMaster ) {
|
||
|
|
self->GetMaster()->GetPhysics()->ApplyImpulse( 0, point, impulse );
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
current.linearVelocity += impulse * inverseMass;
|
||
|
|
const idVec3 momentumDelta = ( point - ( current.position + centerOfMass * current.orientation ) ).Cross( impulse );
|
||
|
|
const idMat3 inverseWorldInertiaTensor = current.orientation.Transpose() * inverseInertiaTensor * current.orientation;
|
||
|
|
current.angularVelocity += RAD2DEG( inverseWorldInertiaTensor * momentumDelta );
|
||
|
|
|
||
|
|
Activate();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::AddForce
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::AddForce( const int id, const idVec3 &point, const idVec3 &force ) {
|
||
|
|
if ( noImpact ) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
if ( hasMaster ) {
|
||
|
|
self->GetMaster()->GetPhysics()->AddForce( 0, point, force );
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
idVec3 impulse = force * current.lastTimeStep;
|
||
|
|
ApplyImpulse( id, point, impulse );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::IsAtRest
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
bool sdPhysics_SimpleRigidBody::IsAtRest( void ) const {
|
||
|
|
return current.atRest >= 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetRestStartTime
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
int sdPhysics_SimpleRigidBody::GetRestStartTime( void ) const {
|
||
|
|
return current.atRest;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::IsPushable
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
bool sdPhysics_SimpleRigidBody::IsPushable( void ) const {
|
||
|
|
return ( !noImpact && !hasMaster );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SaveState
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SaveState( void ) {
|
||
|
|
saved = current;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::RestoreState
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::RestoreState( void ) {
|
||
|
|
current = saved;
|
||
|
|
|
||
|
|
LinkClip();
|
||
|
|
|
||
|
|
EvaluateContacts( CLIP_DEBUG_PARMS_ENTINFO_ONLY( self ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
idPhysics::SetOrigin
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetOrigin( const idVec3 &newOrigin, int id ) {
|
||
|
|
idVec3 masterOrigin;
|
||
|
|
idMat3 masterAxis;
|
||
|
|
|
||
|
|
if ( hasMaster ) {
|
||
|
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
|
current.position = masterOrigin + newOrigin * masterAxis;
|
||
|
|
localOrigin = newOrigin;
|
||
|
|
} else {
|
||
|
|
current.position = newOrigin;
|
||
|
|
}
|
||
|
|
|
||
|
|
LinkClip();
|
||
|
|
|
||
|
|
Activate();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
idPhysics::SetAxis
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetAxis( const idMat3 &newAxis, int id ) {
|
||
|
|
idVec3 masterOrigin;
|
||
|
|
idMat3 masterAxis;
|
||
|
|
|
||
|
|
if ( hasMaster ) {
|
||
|
|
if ( isOrientated ) {
|
||
|
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
|
current.orientation = newAxis * masterAxis;
|
||
|
|
}
|
||
|
|
localAxis = newAxis;
|
||
|
|
} else {
|
||
|
|
current.orientation = newAxis;
|
||
|
|
}
|
||
|
|
|
||
|
|
LinkClip();
|
||
|
|
|
||
|
|
Activate();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
idPhysics::Move
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::Translate( const idVec3 &translation, int id ) {
|
||
|
|
|
||
|
|
if ( hasMaster ) {
|
||
|
|
localOrigin += translation;
|
||
|
|
}
|
||
|
|
current.position += translation;
|
||
|
|
|
||
|
|
LinkClip();
|
||
|
|
|
||
|
|
Activate();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
idPhysics::Rotate
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::Rotate( const idRotation &rotation, int id ) {
|
||
|
|
idVec3 masterOrigin;
|
||
|
|
idMat3 masterAxis;
|
||
|
|
|
||
|
|
current.orientation *= rotation.ToMat3();
|
||
|
|
current.position *= rotation;
|
||
|
|
|
||
|
|
if ( hasMaster ) {
|
||
|
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
|
localAxis *= rotation.ToMat3();
|
||
|
|
localOrigin = ( current.position - masterOrigin ) * masterAxis.Transpose();
|
||
|
|
}
|
||
|
|
|
||
|
|
LinkClip();
|
||
|
|
|
||
|
|
Activate();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetOrigin
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
const idVec3 &sdPhysics_SimpleRigidBody::GetOrigin( int id ) const {
|
||
|
|
return current.position;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetAxis
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
const idMat3 &sdPhysics_SimpleRigidBody::GetAxis( int id ) const {
|
||
|
|
return current.orientation;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetLinearVelocity
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) {
|
||
|
|
current.linearVelocity = newLinearVelocity;
|
||
|
|
Activate();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetAngularVelocity
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) {
|
||
|
|
current.angularVelocity = newAngularVelocity;
|
||
|
|
Activate();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetLinearVelocity
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
const idVec3 &sdPhysics_SimpleRigidBody::GetLinearVelocity( int id ) const {
|
||
|
|
return current.linearVelocity;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetAngularVelocity
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
const idVec3 &sdPhysics_SimpleRigidBody::GetAngularVelocity( int id ) const {
|
||
|
|
return current.angularVelocity;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::ClipTranslation
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const {
|
||
|
|
if ( model ) {
|
||
|
|
gameLocal.clip.TranslationModel( CLIP_DEBUG_PARMS_ENTINFO( self ) results, clipModel->GetOrigin(), clipModel->GetOrigin() + translation,
|
||
|
|
clipModel, mat3_identity, clipMask,
|
||
|
|
model, model->GetOrigin(), model->GetAxis() );
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
gameLocal.clip.Translation( CLIP_DEBUG_PARMS_ENTINFO( self ) results, clipModel->GetOrigin(), clipModel->GetOrigin() + translation,
|
||
|
|
clipModel, mat3_identity, clipMask, self );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::ClipRotation
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const {
|
||
|
|
// physical presence not affected by rotation
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::ClipContents
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
int sdPhysics_SimpleRigidBody::ClipContents( const idClipModel *model ) const {
|
||
|
|
if ( model ) {
|
||
|
|
return gameLocal.clip.ContentsModel( CLIP_DEBUG_PARMS_ENTINFO( self ) clipModel->GetOrigin(), clipModel, mat3_identity, -1,
|
||
|
|
model, model->GetOrigin(), model->GetAxis() );
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
return gameLocal.clip.Contents( CLIP_DEBUG_PARMS_ENTINFO( self ) clipModel->GetOrigin(), clipModel, mat3_identity, -1, NULL );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::UnlinkClip
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::UnlinkClip( void ) {
|
||
|
|
clipModel->Unlink( gameLocal.clip );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::LinkClip
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::LinkClip( void ) {
|
||
|
|
if ( !orientedClip ) {
|
||
|
|
clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.position, mat3_identity );
|
||
|
|
} else {
|
||
|
|
clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.position, current.orientation );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::DisableClip
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::DisableClip( bool activateContacting ) {
|
||
|
|
if ( activateContacting ) {
|
||
|
|
WakeEntitiesContacting( self, clipModel );
|
||
|
|
}
|
||
|
|
clipModel->Disable();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::EnableClip
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::EnableClip( void ) {
|
||
|
|
clipModel->Enable();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::EvaluateContacts
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
bool sdPhysics_SimpleRigidBody::EvaluateContacts( CLIP_DEBUG_PARMS_DECLARATION_ONLY ) {
|
||
|
|
idVec3 dir;
|
||
|
|
int num;
|
||
|
|
|
||
|
|
ClearContacts();
|
||
|
|
|
||
|
|
contacts.SetNum( 10, false );
|
||
|
|
|
||
|
|
dir = current.linearVelocity + current.lastTimeStep * gravityVector;
|
||
|
|
dir.Normalize();
|
||
|
|
num = gameLocal.clip.Contacts( CLIP_DEBUG_PARMS_ENTINFO( self ) &contacts[0], 10, clipModel->GetOrigin(),
|
||
|
|
&dir, CONTACT_EPSILON, clipModel, mat3_identity, clipMask, self );
|
||
|
|
contacts.SetNum( num, false );
|
||
|
|
|
||
|
|
AddContactEntitiesForContacts();
|
||
|
|
|
||
|
|
return ( contacts.Num() != 0 );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetPushed
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetPushed( int deltaTime ) {
|
||
|
|
idRotation rotation;
|
||
|
|
|
||
|
|
rotation = ( saved.orientation * current.orientation ).ToRotation();
|
||
|
|
|
||
|
|
// velocity with which the af is pushed
|
||
|
|
current.pushVelocity.SubVec3(0) += ( current.position - saved.position ) / ( deltaTime * idMath::M_MS2SEC );
|
||
|
|
current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( deltaTime * idMath::M_MS2SEC );
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetPushedLinearVelocity
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
const idVec3 &sdPhysics_SimpleRigidBody::GetPushedLinearVelocity( const int id ) const {
|
||
|
|
return current.pushVelocity.SubVec3(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::GetPushedAngularVelocity
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
const idVec3 &sdPhysics_SimpleRigidBody::GetPushedAngularVelocity( const int id ) const {
|
||
|
|
return current.pushVelocity.SubVec3(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::SetMaster
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::SetMaster( idEntity *master, const bool orientated ) {
|
||
|
|
idVec3 masterOrigin;
|
||
|
|
idMat3 masterAxis;
|
||
|
|
|
||
|
|
if ( master ) {
|
||
|
|
if ( !hasMaster ) {
|
||
|
|
// transform from world space to master space
|
||
|
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
||
|
|
localOrigin = ( current.position - masterOrigin ) * masterAxis.Transpose();
|
||
|
|
if ( orientated ) {
|
||
|
|
localAxis = current.orientation * masterAxis.Transpose();
|
||
|
|
} else {
|
||
|
|
localAxis = current.orientation;
|
||
|
|
}
|
||
|
|
hasMaster = true;
|
||
|
|
isOrientated = orientated;
|
||
|
|
ClearContacts();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
localOrigin = vec3_origin;
|
||
|
|
localAxis = mat3_identity;
|
||
|
|
|
||
|
|
if ( hasMaster ) {
|
||
|
|
hasMaster = false;
|
||
|
|
Activate();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::CheckWater
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::CheckWater( void ) {
|
||
|
|
waterLevel = WATERLEVEL_NONE;
|
||
|
|
|
||
|
|
const idBounds& absBounds = GetAbsBounds( -1 );
|
||
|
|
|
||
|
|
const idClipModel* waterModel;
|
||
|
|
idCollisionModel* model;
|
||
|
|
int count = gameLocal.clip.ClipModelsTouchingBounds( CLIP_DEBUG_PARMS_ENTINFO( self ) absBounds, CONTENTS_WATER, &waterModel, 1, NULL );
|
||
|
|
if ( !count ) {
|
||
|
|
self->CheckWaterEffectsOnly();
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( !waterModel->GetNumCollisionModels() ) {
|
||
|
|
self->CheckWaterEffectsOnly();
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
model = waterModel->GetCollisionModel( 0 );
|
||
|
|
int numPlanes = model->GetNumBrushPlanes();
|
||
|
|
if ( !numPlanes ) {
|
||
|
|
self->CheckWaterEffectsOnly();
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
const idBounds& modelBounds = model->GetBounds();
|
||
|
|
|
||
|
|
idBounds worldbb;
|
||
|
|
worldbb.FromTransformedBounds( GetBounds(), GetOrigin(), GetAxis() );
|
||
|
|
bool submerged = worldbb.GetMaxs()[2] < (modelBounds.GetMaxs()[2] + waterModel->GetOrigin().z);
|
||
|
|
|
||
|
|
if ( submerged ) {
|
||
|
|
waterLevel = WATERLEVEL_HEAD;
|
||
|
|
}
|
||
|
|
|
||
|
|
self->CheckWater( waterModel->GetOrigin(), waterModel->GetAxis(), model );
|
||
|
|
}
|
||
|
|
|
||
|
|
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_SimpleRigidBody::CheckNetworkStateChanges
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
bool sdPhysics_SimpleRigidBody::CheckNetworkStateChanges( networkStateMode_t mode, const sdEntityStateNetworkData& baseState ) const {
|
||
|
|
if ( mode == NSM_VISIBLE ) {
|
||
|
|
NET_GET_BASE( sdSimpleRigidBodyNetworkState );
|
||
|
|
|
||
|
|
if ( baseData.angularVelocity != GetAngularVelocity() ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( baseData.linearVelocity != GetLinearVelocity() ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( baseData.orientation != current.orientation.ToCQuat() ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( baseData.position != current.position ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( mode == NSM_BROADCAST ) {
|
||
|
|
NET_GET_BASE( sdSimpleRigidBodyBroadcastState );
|
||
|
|
|
||
|
|
if ( baseData.localPosition != localOrigin ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( baseData.localOrientation != localAxis.ToCQuat() ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( baseData.atRest != current.atRest ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( baseData.orientedClip != orientedClip ) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::WriteNetworkState
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::WriteNetworkState( networkStateMode_t mode, const sdEntityStateNetworkData& baseState, sdEntityStateNetworkData& newState, idBitMsg& msg ) const {
|
||
|
|
if ( mode == NSM_VISIBLE ) {
|
||
|
|
NET_GET_STATES( sdSimpleRigidBodyNetworkState );
|
||
|
|
|
||
|
|
// update state
|
||
|
|
newData.position = current.position;
|
||
|
|
newData.orientation = current.orientation.ToCQuat();
|
||
|
|
newData.linearVelocity = GetLinearVelocity();
|
||
|
|
newData.angularVelocity = GetAngularVelocity();
|
||
|
|
|
||
|
|
// write state
|
||
|
|
msg.WriteDeltaVector( baseData.position, newData.position, RB_ORIGIN_EXPONENT_BITS, RB_ORIGIN_MANTISSA_BITS );
|
||
|
|
msg.WriteDeltaCQuat( baseData.orientation, newData.orientation );
|
||
|
|
msg.WriteDeltaVector( baseData.linearVelocity, newData.linearVelocity, RB_LINEAR_VELOCITY_EXPONENT_BITS, RB_LINEAR_VELOCITY_MANTISSA_BITS );
|
||
|
|
msg.WriteDeltaVector( baseData.angularVelocity, newData.angularVelocity, RB_ANGULAR_VELOCITY_EXPONENT_BITS, RB_ANGULAR_VELOCITY_MANTISSA_BITS );
|
||
|
|
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( mode == NSM_BROADCAST ) {
|
||
|
|
NET_GET_STATES( sdSimpleRigidBodyBroadcastState );
|
||
|
|
|
||
|
|
// update state
|
||
|
|
newData.localPosition = localOrigin;
|
||
|
|
newData.localOrientation = localAxis.ToCQuat();
|
||
|
|
newData.atRest = current.atRest;
|
||
|
|
newData.orientedClip = orientedClip;
|
||
|
|
|
||
|
|
// write state
|
||
|
|
msg.WriteDeltaVector( baseData.localPosition, newData.localPosition );
|
||
|
|
msg.WriteDeltaCQuat( baseData.localOrientation, newData.localOrientation );
|
||
|
|
msg.WriteDeltaLong( baseData.atRest, newData.atRest );
|
||
|
|
msg.WriteBool( newData.orientedClip );
|
||
|
|
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::ApplyNetworkState
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::ApplyNetworkState( networkStateMode_t mode, const sdEntityStateNetworkData& newState ) {
|
||
|
|
traceCollection.ForceNextUpdate();
|
||
|
|
if ( mode == NSM_VISIBLE ) {
|
||
|
|
NET_GET_NEW( sdSimpleRigidBodyNetworkState );
|
||
|
|
|
||
|
|
// update state
|
||
|
|
current.position = newData.position;
|
||
|
|
current.orientation = newData.orientation.ToMat3();
|
||
|
|
if ( !newData.linearVelocity.Compare( current.linearVelocity, idMath::FLT_EPSILON ) ) {
|
||
|
|
SetLinearVelocity( newData.linearVelocity );
|
||
|
|
}
|
||
|
|
if ( !newData.angularVelocity.Compare( current.angularVelocity, idMath::FLT_EPSILON ) ) {
|
||
|
|
SetAngularVelocity( newData.angularVelocity );
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( clipModel ) {
|
||
|
|
LinkClip();
|
||
|
|
}
|
||
|
|
self->UpdateVisuals();
|
||
|
|
CheckWater();
|
||
|
|
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( mode == NSM_BROADCAST ) {
|
||
|
|
NET_GET_NEW( sdSimpleRigidBodyBroadcastState );
|
||
|
|
|
||
|
|
// update state
|
||
|
|
localOrigin = newData.localPosition;
|
||
|
|
localAxis = newData.localOrientation.ToMat3();
|
||
|
|
current.atRest = newData.atRest;
|
||
|
|
|
||
|
|
if ( orientedClip != newData.orientedClip ) {
|
||
|
|
orientedClip = newData.orientedClip;
|
||
|
|
LinkClip();
|
||
|
|
}
|
||
|
|
|
||
|
|
self->UpdateVisuals();
|
||
|
|
CheckWater();
|
||
|
|
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::ReadNetworkState
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
void sdPhysics_SimpleRigidBody::ReadNetworkState( networkStateMode_t mode, const sdEntityStateNetworkData& baseState, sdEntityStateNetworkData& newState, const idBitMsg& msg ) const {
|
||
|
|
if ( mode == NSM_VISIBLE ) {
|
||
|
|
NET_GET_STATES( sdSimpleRigidBodyNetworkState );
|
||
|
|
|
||
|
|
// read state
|
||
|
|
newData.position = msg.ReadDeltaVector( baseData.position, RB_ORIGIN_EXPONENT_BITS, RB_ORIGIN_MANTISSA_BITS );
|
||
|
|
newData.orientation = msg.ReadDeltaCQuat( baseData.orientation );
|
||
|
|
newData.linearVelocity = msg.ReadDeltaVector( baseData.linearVelocity, RB_LINEAR_VELOCITY_EXPONENT_BITS, RB_LINEAR_VELOCITY_MANTISSA_BITS );
|
||
|
|
newData.angularVelocity = msg.ReadDeltaVector( baseData.angularVelocity, RB_ANGULAR_VELOCITY_EXPONENT_BITS, RB_ANGULAR_VELOCITY_MANTISSA_BITS );
|
||
|
|
|
||
|
|
newData.position.FixDenormals();
|
||
|
|
newData.orientation.FixDenormals();
|
||
|
|
newData.angularVelocity.FixDenormals();
|
||
|
|
newData.linearVelocity.FixDenormals();
|
||
|
|
|
||
|
|
self->OnNewOriginRead( newData.position );
|
||
|
|
self->OnNewAxesRead( newData.orientation.ToMat3() );
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( mode == NSM_BROADCAST ) {
|
||
|
|
NET_GET_STATES( sdSimpleRigidBodyBroadcastState );
|
||
|
|
|
||
|
|
// read state
|
||
|
|
newData.localPosition = msg.ReadDeltaVector( baseData.localPosition );
|
||
|
|
newData.localOrientation = msg.ReadDeltaCQuat( baseData.localOrientation );
|
||
|
|
newData.atRest = msg.ReadDeltaLong( baseData.atRest );
|
||
|
|
newData.orientedClip = msg.ReadBool();
|
||
|
|
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
================
|
||
|
|
sdPhysics_SimpleRigidBody::CreateNetworkStructure
|
||
|
|
================
|
||
|
|
*/
|
||
|
|
sdEntityStateNetworkData* sdPhysics_SimpleRigidBody::CreateNetworkStructure( networkStateMode_t mode ) const {
|
||
|
|
if ( mode == NSM_VISIBLE ) {
|
||
|
|
return new sdSimpleRigidBodyNetworkState();
|
||
|
|
}
|
||
|
|
if ( mode == NSM_BROADCAST ) {
|
||
|
|
return new sdSimpleRigidBodyBroadcastState();
|
||
|
|
}
|
||
|
|
return NULL;
|
||
|
|
}
|