neo/d3xp/physics/Physics_RigidBody.cpp

   1 /*
   2 ===========================================================================
   3
   4 Doom 3 GPL Source Code
   5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
   6
   7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
   8
   9 Doom 3 Source Code is free software: you can redistribute it and/or modify
  10 it under the terms of the GNU General Public License as published by
  11 the Free Software Foundation, either version 3 of the License, or
  12 (at your option) any later version.
  13
  14 Doom 3 Source Code is distributed in the hope that it will be useful,
  15 but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 GNU General Public License for more details.
  18
  19 You should have received a copy of the GNU General Public License
  20 along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
  21
  22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
  23
  24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
  25
  26 ===========================================================================
  27 */
  28
  29 #include "../../idlib/precompiled.h"
  30 #pragma hdrstop
  31
  32 #include "../Game_local.h"
  33
  34 CLASS_DECLARATION( idPhysics_Base, idPhysics_RigidBody )
  35 END_CLASS
  36
  37 const float STOP_SPEED          = 10.0f;
  38
  39
  40 #undef RB_TIMINGS
  41
  42 #ifdef RB_TIMINGS
  43 static int lastTimerReset = 0;
  44 static int numRigidBodies = 0;
  45 static idTimer timer_total, timer_collision;
  46 #endif
  47
  48
  49 /*
  50 ================
  51 RigidBodyDerivatives
  52 ================
  53 */
  54 void RigidBodyDerivatives( const float t, const void *clientData, const float *state, float *derivatives ) {
  55         const idPhysics_RigidBody *p = (idPhysics_RigidBody *) clientData;
  56         rigidBodyIState_t *s = (rigidBodyIState_t *) state;
  57         // NOTE: this struct should be build conform rigidBodyIState_t
  58         struct rigidBodyDerivatives_s {
  59                 idVec3                          linearVelocity;
  60                 idMat3                          angularMatrix;
  61                 idVec3                          force;
  62                 idVec3                          torque;
  63         } *d = (struct rigidBodyDerivatives_s *) derivatives;
  64         idVec3 angularVelocity;
  65         idMat3 inverseWorldInertiaTensor;
  66
  67         inverseWorldInertiaTensor = s->orientation * p->inverseInertiaTensor * s->orientation.Transpose();
  68         angularVelocity = inverseWorldInertiaTensor * s->angularMomentum;
  69         // derivatives
  70         d->linearVelocity = p->inverseMass * s->linearMomentum;
  71         d->angularMatrix = SkewSymmetric( angularVelocity ) * s->orientation;
  72         d->force = - p->linearFriction * s->linearMomentum + p->current.externalForce;
  73         d->torque = - p->angularFriction * s->angularMomentum + p->current.externalTorque;
  74 }
  75
  76 /*
  77 ================
  78 idPhysics_RigidBody::Integrate
  79
  80   Calculate next state from the current state using an integrator.
  81 ================
  82 */
  83 void idPhysics_RigidBody::Integrate( float deltaTime, rigidBodyPState_t &next ) {
  84         idVec3 position;
  85
  86         position = current.i.position;
  87         current.i.position += centerOfMass * current.i.orientation;
  88
  89         current.i.orientation.TransposeSelf();
  90
  91         integrator->Evaluate( (float *) &current.i, (float *) &next.i, 0, deltaTime );
  92         next.i.orientation.OrthoNormalizeSelf();
  93
  94         // apply gravity
  95         next.i.linearMomentum += deltaTime * gravityVector * mass;
  96
  97         current.i.orientation.TransposeSelf();
  98         next.i.orientation.TransposeSelf();
  99
 100         current.i.position = position;
 101         next.i.position -= centerOfMass * next.i.orientation;
 102
 103         next.atRest = current.atRest;
 104 }
 105
 106 /*
 107 ================
 108 idPhysics_RigidBody::CollisionImpulse
 109
 110   Calculates the collision impulse using the velocity relative to the collision object.
 111   The current state should be set to the moment of impact.
 112 ================
 113 */
 114 bool idPhysics_RigidBody::CollisionImpulse( const trace_t &collision, idVec3 &impulse ) {
 115         idVec3 r, linearVelocity, angularVelocity, velocity;
 116         idMat3 inverseWorldInertiaTensor;
 117         float impulseNumerator, impulseDenominator, vel;
 118         impactInfo_t info;
 119         idEntity *ent;
 120
 121         // get info from other entity involved
 122         ent = gameLocal.entities[collision.c.entityNum];
 123         ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info );
 124
 125         // collision point relative to the body center of mass
 126         r = collision.c.point - ( current.i.position + centerOfMass * current.i.orientation );
 127         // the velocity at the collision point
 128         linearVelocity = inverseMass * current.i.linearMomentum;
 129         inverseWorldInertiaTensor = current.i.orientation.Transpose() * inverseInertiaTensor * current.i.orientation;
 130         angularVelocity = inverseWorldInertiaTensor * current.i.angularMomentum;
 131         velocity = linearVelocity + angularVelocity.Cross(r);
 132         // subtract velocity of other entity
 133         velocity -= info.velocity;
 134
 135         // velocity in normal direction
 136         vel = velocity * collision.c.normal;
 137
 138         if ( vel > -STOP_SPEED ) {
 139                 impulseNumerator = STOP_SPEED;
 140         }
 141         else {
 142                 impulseNumerator = -( 1.0f + bouncyness ) * vel;
 143         }
 144         impulseDenominator = inverseMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal );
 145         if ( info.invMass ) {
 146                 impulseDenominator += info.invMass + ( ( info.invInertiaTensor * info.position.Cross( collision.c.normal ) ).Cross( info.position ) * collision.c.normal );
 147         }
 148         impulse = (impulseNumerator / impulseDenominator) * collision.c.normal;
 149
 150         // update linear and angular momentum with impulse
 151         current.i.linearMomentum += impulse;
 152         current.i.angularMomentum += r.Cross(impulse);
 153
 154         // if no movement at all don't blow up
 155         if ( collision.fraction < 0.0001f ) {
 156                 current.i.linearMomentum *= 0.5f;
 157                 current.i.angularMomentum *= 0.5f;
 158         }
 159
 160         // callback to self to let the entity know about the collision
 161         return self->Collide( collision, velocity );
 162 }
 163
 164 /*
 165 ================
 166 idPhysics_RigidBody::CheckForCollisions
 167
 168   Check for collisions between the current and next state.
 169   If there is a collision the next state is set to the state at the moment of impact.
 170 ================
 171 */
 172 bool idPhysics_RigidBody::CheckForCollisions( const float deltaTime, rigidBodyPState_t &next, trace_t &collision ) {
 173 //#define TEST_COLLISION_DETECTION
 174         idMat3 axis;
 175         idRotation rotation;
 176         bool collided = false;
 177
 178 #ifdef TEST_COLLISION_DETECTION
 179         bool startsolid;
 180         if ( gameLocal.clip.Contents( current.i.position, clipModel, current.i.orientation, clipMask, self ) ) {
 181                 startsolid = true;
 182         }
 183 #endif
 184
 185         TransposeMultiply( current.i.orientation, next.i.orientation, axis );
 186         rotation = axis.ToRotation();
 187         rotation.SetOrigin( current.i.position );
 188
 189         // if there was a collision
 190         if ( gameLocal.clip.Motion( collision, current.i.position, next.i.position, rotation, clipModel, current.i.orientation, clipMask, self ) ) {
 191                 // set the next state to the state at the moment of impact
 192                 next.i.position = collision.endpos;
 193                 next.i.orientation = collision.endAxis;
 194                 next.i.linearMomentum = current.i.linearMomentum;
 195                 next.i.angularMomentum = current.i.angularMomentum;
 196                 collided = true;
 197         }
 198
 199 #ifdef TEST_COLLISION_DETECTION
 200         if ( gameLocal.clip.Contents( next.i.position, clipModel, next.i.orientation, clipMask, self ) ) {
 201                 if ( !startsolid ) {
 202                         int bah = 1;
 203                 }
 204         }
 205 #endif
 206         return collided;
 207 }
 208
 209 /*
 210 ================
 211 idPhysics_RigidBody::ContactFriction
 212
 213   Does not solve friction for multiple simultaneous contacts but applies contact friction in isolation.
 214   Uses absolute velocity at the contact points instead of the velocity relative to the contact object.
 215 ================
 216 */
 217 void idPhysics_RigidBody::ContactFriction( float deltaTime ) {
 218         int i;
 219         float magnitude, impulseNumerator, impulseDenominator;
 220         idMat3 inverseWorldInertiaTensor;
 221         idVec3 linearVelocity, angularVelocity;
 222         idVec3 massCenter, r, velocity, normal, impulse, normalVelocity;
 223
 224         inverseWorldInertiaTensor = current.i.orientation.Transpose() * inverseInertiaTensor * current.i.orientation;
 225
 226         massCenter = current.i.position + centerOfMass * current.i.orientation;
 227
 228         for ( i = 0; i < contacts.Num(); i++ ) {
 229
 230                 r = contacts[i].point - massCenter;
 231
 232                 // calculate velocity at contact point
 233                 linearVelocity = inverseMass * current.i.linearMomentum;
 234                 angularVelocity = inverseWorldInertiaTensor * current.i.angularMomentum;
 235                 velocity = linearVelocity + angularVelocity.Cross(r);
 236
 237                 // velocity along normal vector
 238                 normalVelocity = ( velocity * contacts[i].normal ) * contacts[i].normal;
 239
 240                 // calculate friction impulse
 241                 normal = -( velocity - normalVelocity );
 242                 magnitude = normal.Normalize();
 243                 impulseNumerator = contactFriction * magnitude;
 244                 impulseDenominator = inverseMass + ( ( inverseWorldInertiaTensor * r.Cross( normal ) ).Cross( r ) * normal );
 245                 impulse = (impulseNumerator / impulseDenominator) * normal;
 246
 247                 // apply friction impulse
 248                 current.i.linearMomentum += impulse;
 249                 current.i.angularMomentum += r.Cross(impulse);
 250
 251                 // if moving towards the surface at the contact point
 252                 if ( normalVelocity * contacts[i].normal < 0.0f ) {
 253                         // calculate impulse
 254                         normal = -normalVelocity;
 255                         impulseNumerator = normal.Normalize();
 256                         impulseDenominator = inverseMass + ( ( inverseWorldInertiaTensor * r.Cross( normal ) ).Cross( r ) * normal );
 257                         impulse = (impulseNumerator / impulseDenominator) * normal;
 258
 259                         // apply impulse
 260                         current.i.linearMomentum += impulse;
 261                         current.i.angularMomentum += r.Cross( impulse );
 262                 }
 263         }
 264 }
 265
 266 /*
 267 ================
 268 idPhysics_RigidBody::TestIfAtRest
 269
 270   Returns true if the body is considered at rest.
 271   Does not catch all cases where the body is at rest but is generally good enough.
 272 ================
 273 */
 274 bool idPhysics_RigidBody::TestIfAtRest( void ) const {
 275         int i;
 276         float gv;
 277         idVec3 v, av, normal, point;
 278         idMat3 inverseWorldInertiaTensor;
 279         idFixedWinding contactWinding;
 280
 281         if ( current.atRest >= 0 ) {
 282                 return true;
 283         }
 284
 285         // need at least 3 contact points to come to rest
 286         if ( contacts.Num() < 3 ) {
 287                 return false;
 288         }
 289
 290         // get average contact plane normal
 291         normal.Zero();
 292         for ( i = 0; i < contacts.Num(); i++ ) {
 293                 normal += contacts[i].normal;
 294         }
 295         normal /= (float) contacts.Num();
 296         normal.Normalize();
 297
 298         // if on a too steep surface
 299         if ( (normal * gravityNormal) > -0.7f ) {
 300                 return false;
 301         }
 302
 303         // create bounds for contact points
 304         contactWinding.Clear();
 305         for ( i = 0; i < contacts.Num(); i++ ) {
 306                 // project point onto plane through origin orthogonal to the gravity
 307                 point = contacts[i].point - (contacts[i].point * gravityNormal) * gravityNormal;
 308                 contactWinding.AddToConvexHull( point, gravityNormal );
 309         }
 310
 311         // need at least 3 contact points to come to rest
 312         if ( contactWinding.GetNumPoints() < 3 ) {
 313                 return false;
 314         }
 315
 316         // center of mass in world space
 317         point = current.i.position + centerOfMass * current.i.orientation;
 318         point -= (point * gravityNormal) * gravityNormal;
 319
 320         // if the point is not inside the winding
 321         if ( !contactWinding.PointInside( gravityNormal, point, 0 ) ) {
 322                 return false;
 323         }
 324
 325         // linear velocity of body
 326         v = inverseMass * current.i.linearMomentum;
 327         // linear velocity in gravity direction
 328         gv = v * gravityNormal;
 329         // linear velocity orthogonal to gravity direction
 330         v -= gv * gravityNormal;
 331
 332         // if too much velocity orthogonal to gravity direction
 333         if ( v.Length() > STOP_SPEED ) {
 334                 return false;
 335         }
 336         // if too much velocity in gravity direction
 337         if ( gv > 2.0f * STOP_SPEED || gv < -2.0f * STOP_SPEED ) {
 338                 return false;
 339         }
 340
 341         // calculate rotational velocity
 342         inverseWorldInertiaTensor = current.i.orientation * inverseInertiaTensor * current.i.orientation.Transpose();
 343         av = inverseWorldInertiaTensor * current.i.angularMomentum;
 344
 345         // if too much rotational velocity
 346         if ( av.LengthSqr() > STOP_SPEED ) {
 347                 return false;
 348         }
 349
 350         return true;
 351 }
 352
 353 /*
 354 ================
 355 idPhysics_RigidBody::DropToFloorAndRest
 356
 357   Drops the object straight down to the floor and verifies if the object is at rest on the floor.
 358 ================
 359 */
 360 void idPhysics_RigidBody::DropToFloorAndRest( void ) {
 361         idVec3 down;
 362         trace_t tr;
 363
 364         if ( testSolid ) {
 365
 366                 testSolid = false;
 367
 368                 if ( gameLocal.clip.Contents( current.i.position, clipModel, current.i.orientation, clipMask, self ) ) {
 369                         gameLocal.DWarning( "rigid body in solid for entity '%s' type '%s' at (%s)",
 370                                                                 self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) );
 371                         Rest();
 372                         dropToFloor = false;
 373                         return;
 374                 }
 375         }
 376
 377         // put the body on the floor
 378         down = current.i.position + gravityNormal * 128.0f;
 379         gameLocal.clip.Translation( tr, current.i.position, down, clipModel, current.i.orientation, clipMask, self );
 380         current.i.position = tr.endpos;
 381         clipModel->Link( gameLocal.clip, self, clipModel->GetId(), tr.endpos, current.i.orientation );
 382
 383         // if on the floor already
 384         if ( tr.fraction == 0.0f ) {
 385                 // test if we are really at rest
 386                 EvaluateContacts();
 387                 if ( !TestIfAtRest() ) {
 388                         gameLocal.DWarning( "rigid body not at rest for entity '%s' type '%s' at (%s)",
 389                                                                 self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) );
 390                 }
 391                 Rest();
 392                 dropToFloor = false;
 393         } else if ( IsOutsideWorld() ) {
 394                 gameLocal.Warning( "rigid body outside world bounds for entity '%s' type '%s' at (%s)",
 395                                                         self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) );
 396                 Rest();
 397                 dropToFloor = false;
 398         }
 399 }
 400
 401 /*
 402 ================
 403 idPhysics_RigidBody::DebugDraw
 404 ================
 405 */
 406 void idPhysics_RigidBody::DebugDraw( void ) {
 407
 408         if ( rb_showBodies.GetBool() || ( rb_showActive.GetBool() && current.atRest < 0 ) ) {
 409                 collisionModelManager->DrawModel( clipModel->Handle(), clipModel->GetOrigin(), clipModel->GetAxis(), vec3_origin, 0.0f );
 410         }
 411
 412         if ( rb_showMass.GetBool() ) {
 413                 gameRenderWorld->DrawText( va( "\n%1.2f", mass ), current.i.position, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
 414         }
 415
 416         if ( rb_showInertia.GetBool() ) {
 417                 idMat3 &I = inertiaTensor;
 418                 gameRenderWorld->DrawText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )",
 419                                                                         I[0].x, I[0].y, I[0].z,
 420                                                                         I[1].x, I[1].y, I[1].z,
 421                                                                         I[2].x, I[2].y, I[2].z ),
 422                                                                         current.i.position, 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
 423         }
 424
 425         if ( rb_showVelocity.GetBool() ) {
 426                 DrawVelocity( clipModel->GetId(), 0.1f, 4.0f );
 427         }
 428 }
 429
 430 /*
 431 ================
 432 idPhysics_RigidBody::idPhysics_RigidBody
 433 ================
 434 */
 435 idPhysics_RigidBody::idPhysics_RigidBody( void ) {
 436
 437         // set default rigid body properties
 438         SetClipMask( MASK_SOLID );
 439         SetBouncyness( 0.6f );
 440         SetFriction( 0.6f, 0.6f, 0.0f );
 441         clipModel = NULL;
 442
 443         memset( &current, 0, sizeof( current ) );
 444
 445         current.atRest = -1;
 446         current.lastTimeStep = USERCMD_MSEC;
 447
 448         current.i.position.Zero();
 449         current.i.orientation.Identity();
 450
 451         current.i.linearMomentum.Zero();
 452         current.i.angularMomentum.Zero();
 453
 454         saved = current;
 455
 456         mass = 1.0f;
 457         inverseMass = 1.0f;
 458         centerOfMass.Zero();
 459         inertiaTensor.Identity();
 460         inverseInertiaTensor.Identity();
 461
 462         // use the least expensive euler integrator
 463         integrator = new idODE_Euler( sizeof(rigidBodyIState_t) / sizeof(float), RigidBodyDerivatives, this );
 464
 465         dropToFloor = false;
 466         noImpact = false;
 467         noContact = false;
 468
 469         hasMaster = false;
 470         isOrientated = false;
 471
 472 #ifdef RB_TIMINGS
 473         lastTimerReset = 0;
 474 #endif
 475 }
 476
 477 /*
 478 ================
 479 idPhysics_RigidBody::~idPhysics_RigidBody
 480 ================
 481 */
 482 idPhysics_RigidBody::~idPhysics_RigidBody( void ) {
 483         if ( clipModel ) {
 484                 delete clipModel;
 485                 clipModel = NULL;
 486         }
 487         delete integrator;
 488 }
 489
 490 /*
 491 ================
 492 idPhysics_RigidBody_SavePState
 493 ================
 494 */
 495 void idPhysics_RigidBody_SavePState( idSaveGame *savefile, const rigidBodyPState_t &state ) {
 496         savefile->WriteInt( state.atRest );
 497         savefile->WriteFloat( state.lastTimeStep );
 498         savefile->WriteVec3( state.localOrigin );
 499         savefile->WriteMat3( state.localAxis );
 500         savefile->WriteVec6( state.pushVelocity );
 501         savefile->WriteVec3( state.externalForce );
 502         savefile->WriteVec3( state.externalTorque );
 503
 504         savefile->WriteVec3( state.i.position );
 505         savefile->WriteMat3( state.i.orientation );
 506         savefile->WriteVec3( state.i.linearMomentum );
 507         savefile->WriteVec3( state.i.angularMomentum );
 508 }
 509
 510 /*
 511 ================
 512 idPhysics_RigidBody_RestorePState
 513 ================
 514 */
 515 void idPhysics_RigidBody_RestorePState( idRestoreGame *savefile, rigidBodyPState_t &state ) {
 516         savefile->ReadInt( state.atRest );
 517         savefile->ReadFloat( state.lastTimeStep );
 518         savefile->ReadVec3( state.localOrigin );
 519         savefile->ReadMat3( state.localAxis );
 520         savefile->ReadVec6( state.pushVelocity );
 521         savefile->ReadVec3( state.externalForce );
 522         savefile->ReadVec3( state.externalTorque );
 523
 524         savefile->ReadVec3( state.i.position );
 525         savefile->ReadMat3( state.i.orientation );
 526         savefile->ReadVec3( state.i.linearMomentum );
 527         savefile->ReadVec3( state.i.angularMomentum );
 528 }
 529
 530 /*
 531 ================
 532 idPhysics_RigidBody::Save
 533 ================
 534 */
 535 void idPhysics_RigidBody::Save( idSaveGame *savefile ) const {
 536
 537         idPhysics_RigidBody_SavePState( savefile, current );
 538         idPhysics_RigidBody_SavePState( savefile, saved );
 539
 540         savefile->WriteFloat( linearFriction );
 541         savefile->WriteFloat( angularFriction );
 542         savefile->WriteFloat( contactFriction );
 543         savefile->WriteFloat( bouncyness );
 544         savefile->WriteClipModel( clipModel );
 545
 546         savefile->WriteFloat( mass );
 547         savefile->WriteFloat( inverseMass );
 548         savefile->WriteVec3( centerOfMass );
 549         savefile->WriteMat3( inertiaTensor );
 550         savefile->WriteMat3( inverseInertiaTensor );
 551
 552         savefile->WriteBool( dropToFloor );
 553         savefile->WriteBool( testSolid );
 554         savefile->WriteBool( noImpact );
 555         savefile->WriteBool( noContact );
 556
 557         savefile->WriteBool( hasMaster );
 558         savefile->WriteBool( isOrientated );
 559 }
 560
 561 /*
 562 ================
 563 idPhysics_RigidBody::Restore
 564 ================
 565 */
 566 void idPhysics_RigidBody::Restore( idRestoreGame *savefile ) {
 567
 568         idPhysics_RigidBody_RestorePState( savefile, current );
 569         idPhysics_RigidBody_RestorePState( savefile, saved );
 570
 571         savefile->ReadFloat( linearFriction );
 572         savefile->ReadFloat( angularFriction );
 573         savefile->ReadFloat( contactFriction );
 574         savefile->ReadFloat( bouncyness );
 575         savefile->ReadClipModel( clipModel );
 576
 577         savefile->ReadFloat( mass );
 578         savefile->ReadFloat( inverseMass );
 579         savefile->ReadVec3( centerOfMass );
 580         savefile->ReadMat3( inertiaTensor );
 581         savefile->ReadMat3( inverseInertiaTensor );
 582
 583         savefile->ReadBool( dropToFloor );
 584         savefile->ReadBool( testSolid );
 585         savefile->ReadBool( noImpact );
 586         savefile->ReadBool( noContact );
 587
 588         savefile->ReadBool( hasMaster );
 589         savefile->ReadBool( isOrientated );
 590 }
 591
 592 /*
 593 ================
 594 idPhysics_RigidBody::SetClipModel
 595 ================
 596 */
 597 #define MAX_INERTIA_SCALE               10.0f
 598
 599 void idPhysics_RigidBody::SetClipModel( idClipModel *model, const float density, int id, bool freeOld ) {
 600         int minIndex;
 601         idMat3 inertiaScale;
 602
 603         assert( self );
 604         assert( model );                                        // we need a clip model
 605         assert( model->IsTraceModel() );        // and it should be a trace model
 606         assert( density > 0.0f );                       // density should be valid
 607
 608         if ( clipModel && clipModel != model && freeOld ) {
 609                 delete clipModel;
 610         }
 611         clipModel = model;
 612         clipModel->Link( gameLocal.clip, self, 0, current.i.position, current.i.orientation );
 613
 614         // get mass properties from the trace model
 615         clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor );
 616
 617         // check whether or not the clip model has valid mass properties
 618         if ( mass <= 0.0f || FLOAT_IS_NAN( mass ) ) {
 619                 gameLocal.Warning( "idPhysics_RigidBody::SetClipModel: invalid mass for entity '%s' type '%s'",
 620                                                         self->name.c_str(), self->GetType()->classname );
 621                 mass = 1.0f;
 622                 centerOfMass.Zero();
 623                 inertiaTensor.Identity();
 624         }
 625
 626         // check whether or not the inertia tensor is balanced
 627         minIndex = Min3Index( inertiaTensor[0][0], inertiaTensor[1][1], inertiaTensor[2][2] );
 628         inertiaScale.Identity();
 629         inertiaScale[0][0] = inertiaTensor[0][0] / inertiaTensor[minIndex][minIndex];
 630         inertiaScale[1][1] = inertiaTensor[1][1] / inertiaTensor[minIndex][minIndex];
 631         inertiaScale[2][2] = inertiaTensor[2][2] / inertiaTensor[minIndex][minIndex];
 632
 633         if ( inertiaScale[0][0] > MAX_INERTIA_SCALE || inertiaScale[1][1] > MAX_INERTIA_SCALE || inertiaScale[2][2] > MAX_INERTIA_SCALE ) {
 634                 gameLocal.DWarning( "idPhysics_RigidBody::SetClipModel: unbalanced inertia tensor for entity '%s' type '%s'",
 635                                                         self->name.c_str(), self->GetType()->classname );
 636                 float min = inertiaTensor[minIndex][minIndex] * MAX_INERTIA_SCALE;
 637                 inertiaScale[(minIndex+1)%3][(minIndex+1)%3] = min / inertiaTensor[(minIndex+1)%3][(minIndex+1)%3];
 638                 inertiaScale[(minIndex+2)%3][(minIndex+2)%3] = min / inertiaTensor[(minIndex+2)%3][(minIndex+2)%3];
 639                 inertiaTensor *= inertiaScale;
 640         }
 641
 642         inverseMass = 1.0f / mass;
 643         inverseInertiaTensor = inertiaTensor.Inverse() * ( 1.0f / 6.0f );
 644
 645         current.i.linearMomentum.Zero();
 646         current.i.angularMomentum.Zero();
 647 }
 648
 649 /*
 650 ================
 651 idPhysics_RigidBody::GetClipModel
 652 ================
 653 */
 654 idClipModel *idPhysics_RigidBody::GetClipModel( int id ) const {
 655         return clipModel;
 656 }
 657
 658 /*
 659 ================
 660 idPhysics_RigidBody::GetNumClipModels
 661 ================
 662 */
 663 int idPhysics_RigidBody::GetNumClipModels( void ) const {
 664         return 1;
 665 }
 666
 667 /*
 668 ================
 669 idPhysics_RigidBody::SetMass
 670 ================
 671 */
 672 void idPhysics_RigidBody::SetMass( float mass, int id ) {
 673         assert( mass > 0.0f );
 674         inertiaTensor *= mass / this->mass;
 675         inverseInertiaTensor = inertiaTensor.Inverse() * (1.0f / 6.0f);
 676         this->mass = mass;
 677         inverseMass = 1.0f / mass;
 678 }
 679
 680 /*
 681 ================
 682 idPhysics_RigidBody::GetMass
 683 ================
 684 */
 685 float idPhysics_RigidBody::GetMass( int id ) const {
 686         return mass;
 687 }
 688
 689 /*
 690 ================
 691 idPhysics_RigidBody::SetFriction
 692 ================
 693 */
 694 void idPhysics_RigidBody::SetFriction( const float linear, const float angular, const float contact ) {
 695         if (    linear < 0.0f || linear > 1.0f ||
 696                         angular < 0.0f || angular > 1.0f ||
 697                         contact < 0.0f || contact > 1.0f ) {
 698                 return;
 699         }
 700         linearFriction = linear;
 701         angularFriction = angular;
 702         contactFriction = contact;
 703 }
 704
 705 /*
 706 ================
 707 idPhysics_RigidBody::SetBouncyness
 708 ================
 709 */
 710 void idPhysics_RigidBody::SetBouncyness( const float b ) {
 711         if ( b < 0.0f || b > 1.0f ) {
 712                 return;
 713         }
 714         bouncyness = b;
 715 }
 716
 717 /*
 718 ================
 719 idPhysics_RigidBody::Rest
 720 ================
 721 */
 722 void idPhysics_RigidBody::Rest( void ) {
 723         current.atRest = gameLocal.time;
 724         current.i.linearMomentum.Zero();
 725         current.i.angularMomentum.Zero();
 726         self->BecomeInactive( TH_PHYSICS );
 727 }
 728
 729 /*
 730 ================
 731 idPhysics_RigidBody::DropToFloor
 732 ================
 733 */
 734 void idPhysics_RigidBody::DropToFloor( void ) {
 735         dropToFloor = true;
 736         testSolid = true;
 737 }
 738
 739 /*
 740 ================
 741 idPhysics_RigidBody::NoContact
 742 ================
 743 */
 744 void idPhysics_RigidBody::NoContact( void ) {
 745         noContact = true;
 746 }
 747
 748 /*
 749 ================
 750 idPhysics_RigidBody::Activate
 751 ================
 752 */
 753 void idPhysics_RigidBody::Activate( void ) {
 754         current.atRest = -1;
 755         self->BecomeActive( TH_PHYSICS );
 756 }
 757
 758 /*
 759 ================
 760 idPhysics_RigidBody::PutToRest
 761
 762   put to rest untill something collides with this physics object
 763 ================
 764 */
 765 void idPhysics_RigidBody::PutToRest( void ) {
 766         Rest();
 767 }
 768
 769 /*
 770 ================
 771 idPhysics_RigidBody::EnableImpact
 772 ================
 773 */
 774 void idPhysics_RigidBody::EnableImpact( void ) {
 775         noImpact = false;
 776 }
 777
 778 /*
 779 ================
 780 idPhysics_RigidBody::DisableImpact
 781 ================
 782 */
 783 void idPhysics_RigidBody::DisableImpact( void ) {
 784         noImpact = true;
 785 }
 786
 787 /*
 788 ================
 789 idPhysics_RigidBody::SetContents
 790 ================
 791 */
 792 void idPhysics_RigidBody::SetContents( int contents, int id ) {
 793         clipModel->SetContents( contents );
 794 }
 795
 796 /*
 797 ================
 798 idPhysics_RigidBody::GetContents
 799 ================
 800 */
 801 int idPhysics_RigidBody::GetContents( int id ) const {
 802         return clipModel->GetContents();
 803 }
 804
 805 /*
 806 ================
 807 idPhysics_RigidBody::GetBounds
 808 ================
 809 */
 810 const idBounds &idPhysics_RigidBody::GetBounds( int id ) const {
 811         return clipModel->GetBounds();
 812 }
 813
 814 /*
 815 ================
 816 idPhysics_RigidBody::GetAbsBounds
 817 ================
 818 */
 819 const idBounds &idPhysics_RigidBody::GetAbsBounds( int id ) const {
 820         return clipModel->GetAbsBounds();
 821 }
 822
 823 /*
 824 ================
 825 idPhysics_RigidBody::Evaluate
 826
 827   Evaluate the impulse based rigid body physics.
 828   When a collision occurs an impulse is applied at the moment of impact but
 829   the remaining time after the collision is ignored.
 830 ================
 831 */
 832 bool idPhysics_RigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) {
 833         rigidBodyPState_t next;
 834         idAngles angles;
 835         trace_t collision;
 836         idVec3 impulse;
 837         idEntity *ent;
 838         idVec3 oldOrigin, masterOrigin;
 839         idMat3 oldAxis, masterAxis;
 840         float timeStep;
 841         bool collided, cameToRest = false;
 842
 843         timeStep = MS2SEC( timeStepMSec );
 844         current.lastTimeStep = timeStep;
 845
 846         if ( hasMaster ) {
 847                 oldOrigin = current.i.position;
 848                 oldAxis = current.i.orientation;
 849                 self->GetMasterPosition( masterOrigin, masterAxis );
 850                 current.i.position = masterOrigin + current.localOrigin * masterAxis;
 851                 if ( isOrientated ) {
 852                         current.i.orientation = current.localAxis * masterAxis;
 853                 }
 854                 else {
 855                         current.i.orientation = current.localAxis;
 856                 }
 857                 clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation );
 858                 current.i.linearMomentum = mass * ( ( current.i.position - oldOrigin ) / timeStep );
 859                 current.i.angularMomentum = inertiaTensor * ( ( current.i.orientation * oldAxis.Transpose() ).ToAngularVelocity() / timeStep );
 860                 current.externalForce.Zero();
 861                 current.externalTorque.Zero();
 862
 863                 return ( current.i.position != oldOrigin || current.i.orientation != oldAxis );
 864         }
 865
 866         // if the body is at rest
 867         if ( current.atRest >= 0 || timeStep <= 0.0f ) {
 868                 DebugDraw();
 869                 return false;
 870         }
 871
 872         // if putting the body to rest
 873         if ( dropToFloor ) {
 874                 DropToFloorAndRest();
 875                 current.externalForce.Zero();
 876                 current.externalTorque.Zero();
 877                 return true;
 878         }
 879
 880 #ifdef RB_TIMINGS
 881         timer_total.Start();
 882 #endif
 883
 884         // move the rigid body velocity into the frame of a pusher
 885 //      current.i.linearMomentum -= current.pushVelocity.SubVec3( 0 ) * mass;
 886 //      current.i.angularMomentum -= current.pushVelocity.SubVec3( 1 ) * inertiaTensor;
 887
 888         clipModel->Unlink();
 889
 890         next = current;
 891
 892         // calculate next position and orientation
 893         Integrate( timeStep, next );
 894
 895 #ifdef RB_TIMINGS
 896         timer_collision.Start();
 897 #endif
 898
 899         // check for collisions from the current to the next state
 900         collided = CheckForCollisions( timeStep, next, collision );
 901
 902 #ifdef RB_TIMINGS
 903         timer_collision.Stop();
 904 #endif
 905
 906         // set the new state
 907         current = next;
 908
 909         if ( collided ) {
 910                 // apply collision impulse
 911                 if ( CollisionImpulse( collision, impulse ) ) {
 912                         current.atRest = gameLocal.time;
 913                 }
 914         }
 915
 916         // update the position of the clip model
 917         clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation );
 918
 919         DebugDraw();
 920
 921         if ( !noContact ) {
 922
 923 #ifdef RB_TIMINGS
 924                 timer_collision.Start();
 925 #endif
 926                 // get contacts
 927                 EvaluateContacts();
 928
 929 #ifdef RB_TIMINGS
 930                 timer_collision.Stop();
 931 #endif
 932
 933                 // check if the body has come to rest
 934                 if ( TestIfAtRest() ) {
 935                         // put to rest
 936                         Rest();
 937                         cameToRest = true;
 938                 }  else {
 939                         // apply contact friction
 940                         ContactFriction( timeStep );
 941                 }
 942         }
 943
 944         if ( current.atRest < 0 ) {
 945                 ActivateContactEntities();
 946         }
 947
 948         if ( collided ) {
 949                 // if the rigid body didn't come to rest or the other entity is not at rest
 950                 ent = gameLocal.entities[collision.c.entityNum];
 951                 if ( ent && ( !cameToRest || !ent->IsAtRest() ) ) {
 952                         // apply impact to other entity
 953                         ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse );
 954                 }
 955         }
 956
 957         // move the rigid body velocity back into the world frame
 958 //      current.i.linearMomentum += current.pushVelocity.SubVec3( 0 ) * mass;
 959 //      current.i.angularMomentum += current.pushVelocity.SubVec3( 1 ) * inertiaTensor;
 960         current.pushVelocity.Zero();
 961
 962         current.lastTimeStep = timeStep;
 963         current.externalForce.Zero();
 964         current.externalTorque.Zero();
 965
 966         if ( IsOutsideWorld() ) {
 967                 gameLocal.Warning( "rigid body moved outside world bounds for entity '%s' type '%s' at (%s)",
 968                                         self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) );
 969                 Rest();
 970         }
 971
 972 #ifdef RB_TIMINGS
 973         timer_total.Stop();
 974
 975         if ( rb_showTimings->integer == 1 ) {
 976                 gameLocal.Printf( "%12s: t %1.4f cd %1.4f\n",
 977                                                 self->name.c_str(),
 978                                                 timer_total.Milliseconds(), timer_collision.Milliseconds() );
 979                 lastTimerReset = 0;
 980         }
 981         else if ( rb_showTimings->integer == 2 ) {
 982                 numRigidBodies++;
 983                 if ( endTimeMSec > lastTimerReset ) {
 984                         gameLocal.Printf( "rb %d: t %1.4f cd %1.4f\n",
 985                                                         numRigidBodies,
 986                                                         timer_total.Milliseconds(), timer_collision.Milliseconds() );
 987                 }
 988         }
 989         if ( endTimeMSec > lastTimerReset ) {
 990                 lastTimerReset = endTimeMSec;
 991                 numRigidBodies = 0;
 992                 timer_total.Clear();
 993                 timer_collision.Clear();
 994         }
 995 #endif
 996
 997         return true;
 998 }
 999
1000 /*
1001 ================
1002 idPhysics_RigidBody::UpdateTime
1003 ================
1004 */
1005 void idPhysics_RigidBody::UpdateTime( int endTimeMSec ) {
1006 }
1007
1008 /*
1009 ================
1010 idPhysics_RigidBody::GetTime
1011 ================
1012 */
1013 int idPhysics_RigidBody::GetTime( void ) const {
1014         return gameLocal.time;
1015 }
1016
1017 /*
1018 ================
1019 idPhysics_RigidBody::GetImpactInfo
1020 ================
1021 */
1022 void idPhysics_RigidBody::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const {
1023         idVec3 linearVelocity, angularVelocity;
1024         idMat3 inverseWorldInertiaTensor;
1025
1026         linearVelocity = inverseMass * current.i.linearMomentum;
1027         inverseWorldInertiaTensor = current.i.orientation.Transpose() * inverseInertiaTensor * current.i.orientation;
1028         angularVelocity = inverseWorldInertiaTensor * current.i.angularMomentum;
1029
1030         info->invMass = inverseMass;
1031         info->invInertiaTensor = inverseWorldInertiaTensor;
1032         info->position = point - ( current.i.position + centerOfMass * current.i.orientation );
1033         info->velocity = linearVelocity + angularVelocity.Cross( info->position );
1034 }
1035
1036 /*
1037 ================
1038 idPhysics_RigidBody::ApplyImpulse
1039 ================
1040 */
1041 void idPhysics_RigidBody::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) {
1042         if ( noImpact ) {
1043                 return;
1044         }
1045         current.i.linearMomentum += impulse;
1046         current.i.angularMomentum += ( point - ( current.i.position + centerOfMass * current.i.orientation ) ).Cross( impulse );
1047         Activate();
1048 }
1049
1050 /*
1051 ================
1052 idPhysics_RigidBody::AddForce
1053 ================
1054 */
1055 void idPhysics_RigidBody::AddForce( const int id, const idVec3 &point, const idVec3 &force ) {
1056         if ( noImpact ) {
1057                 return;
1058         }
1059         current.externalForce += force;
1060         current.externalTorque += ( point - ( current.i.position + centerOfMass * current.i.orientation ) ).Cross( force );
1061         Activate();
1062 }
1063
1064 /*
1065 ================
1066 idPhysics_RigidBody::IsAtRest
1067 ================
1068 */
1069 bool idPhysics_RigidBody::IsAtRest( void ) const {
1070         return current.atRest >= 0;
1071 }
1072
1073 /*
1074 ================
1075 idPhysics_RigidBody::GetRestStartTime
1076 ================
1077 */
1078 int idPhysics_RigidBody::GetRestStartTime( void ) const {
1079         return current.atRest;
1080 }
1081
1082 /*
1083 ================
1084 idPhysics_RigidBody::IsPushable
1085 ================
1086 */
1087 bool idPhysics_RigidBody::IsPushable( void ) const {
1088         return ( !noImpact && !hasMaster );
1089 }
1090
1091 /*
1092 ================
1093 idPhysics_RigidBody::SaveState
1094 ================
1095 */
1096 void idPhysics_RigidBody::SaveState( void ) {
1097         saved = current;
1098 }
1099
1100 /*
1101 ================
1102 idPhysics_RigidBody::RestoreState
1103 ================
1104 */
1105 void idPhysics_RigidBody::RestoreState( void ) {
1106         current = saved;
1107
1108         clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation );
1109
1110         EvaluateContacts();
1111 }
1112
1113 /*
1114 ================
1115 idPhysics::SetOrigin
1116 ================
1117 */
1118 void idPhysics_RigidBody::SetOrigin( const idVec3 &newOrigin, int id ) {
1119         idVec3 masterOrigin;
1120         idMat3 masterAxis;
1121
1122         current.localOrigin = newOrigin;
1123         if ( hasMaster ) {
1124                 self->GetMasterPosition( masterOrigin, masterAxis );
1125                 current.i.position = masterOrigin + newOrigin * masterAxis;
1126         }
1127         else {
1128                 current.i.position = newOrigin;
1129         }
1130
1131         clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, clipModel->GetAxis() );
1132
1133         Activate();
1134 }
1135
1136 /*
1137 ================
1138 idPhysics::SetAxis
1139 ================
1140 */
1141 void idPhysics_RigidBody::SetAxis( const idMat3 &newAxis, int id ) {
1142         idVec3 masterOrigin;
1143         idMat3 masterAxis;
1144
1145         current.localAxis = newAxis;
1146         if ( hasMaster && isOrientated ) {
1147                 self->GetMasterPosition( masterOrigin, masterAxis );
1148                 current.i.orientation = newAxis * masterAxis;
1149         }
1150         else {
1151                 current.i.orientation = newAxis;
1152         }
1153
1154         clipModel->Link( gameLocal.clip, self, clipModel->GetId(), clipModel->GetOrigin(), current.i.orientation );
1155
1156         Activate();
1157 }
1158
1159 /*
1160 ================
1161 idPhysics::Move
1162 ================
1163 */
1164 void idPhysics_RigidBody::Translate( const idVec3 &translation, int id ) {
1165
1166         current.localOrigin += translation;
1167         current.i.position += translation;
1168
1169         clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, clipModel->GetAxis() );
1170
1171         Activate();
1172 }
1173
1174 /*
1175 ================
1176 idPhysics::Rotate
1177 ================
1178 */
1179 void idPhysics_RigidBody::Rotate( const idRotation &rotation, int id ) {
1180         idVec3 masterOrigin;
1181         idMat3 masterAxis;
1182
1183         current.i.orientation *= rotation.ToMat3();
1184         current.i.position *= rotation;
1185
1186         if ( hasMaster ) {
1187                 self->GetMasterPosition( masterOrigin, masterAxis );
1188                 current.localAxis *= rotation.ToMat3();
1189                 current.localOrigin = ( current.i.position - masterOrigin ) * masterAxis.Transpose();
1190         }
1191         else {
1192                 current.localAxis = current.i.orientation;
1193                 current.localOrigin = current.i.position;
1194         }
1195
1196         clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation );
1197
1198         Activate();
1199 }
1200
1201 /*
1202 ================
1203 idPhysics_RigidBody::GetOrigin
1204 ================
1205 */
1206 const idVec3 &idPhysics_RigidBody::GetOrigin( int id ) const {
1207         return current.i.position;
1208 }
1209
1210 /*
1211 ================
1212 idPhysics_RigidBody::GetAxis
1213 ================
1214 */
1215 const idMat3 &idPhysics_RigidBody::GetAxis( int id ) const {
1216         return current.i.orientation;
1217 }
1218
1219 /*
1220 ================
1221 idPhysics_RigidBody::SetLinearVelocity
1222 ================
1223 */
1224 void idPhysics_RigidBody::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) {
1225         current.i.linearMomentum = newLinearVelocity * mass;
1226         Activate();
1227 }
1228
1229 /*
1230 ================
1231 idPhysics_RigidBody::SetAngularVelocity
1232 ================
1233 */
1234 void idPhysics_RigidBody::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) {
1235         current.i.angularMomentum = newAngularVelocity * inertiaTensor;
1236         Activate();
1237 }
1238
1239 /*
1240 ================
1241 idPhysics_RigidBody::GetLinearVelocity
1242 ================
1243 */
1244 const idVec3 &idPhysics_RigidBody::GetLinearVelocity( int id ) const {
1245         static idVec3 curLinearVelocity;
1246         curLinearVelocity = current.i.linearMomentum * inverseMass;
1247         return curLinearVelocity;
1248 }
1249
1250 /*
1251 ================
1252 idPhysics_RigidBody::GetAngularVelocity
1253 ================
1254 */
1255 const idVec3 &idPhysics_RigidBody::GetAngularVelocity( int id ) const {
1256         static idVec3 curAngularVelocity;
1257         idMat3 inverseWorldInertiaTensor;
1258
1259         inverseWorldInertiaTensor = current.i.orientation.Transpose() * inverseInertiaTensor * current.i.orientation;
1260         curAngularVelocity = inverseWorldInertiaTensor * current.i.angularMomentum;
1261         return curAngularVelocity;
1262 }
1263
1264 /*
1265 ================
1266 idPhysics_RigidBody::ClipTranslation
1267 ================
1268 */
1269 void idPhysics_RigidBody::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const {
1270         if ( model ) {
1271                 gameLocal.clip.TranslationModel( results, clipModel->GetOrigin(), clipModel->GetOrigin() + translation,
1272                                                                                         clipModel, clipModel->GetAxis(), clipMask,
1273                                                                                         model->Handle(), model->GetOrigin(), model->GetAxis() );
1274         }
1275         else {
1276                 gameLocal.clip.Translation( results, clipModel->GetOrigin(), clipModel->GetOrigin() + translation,
1277                                                                                         clipModel, clipModel->GetAxis(), clipMask, self );
1278         }
1279 }
1280
1281 /*
1282 ================
1283 idPhysics_RigidBody::ClipRotation
1284 ================
1285 */
1286 void idPhysics_RigidBody::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const {
1287         if ( model ) {
1288                 gameLocal.clip.RotationModel( results, clipModel->GetOrigin(), rotation,
1289                                                                                         clipModel, clipModel->GetAxis(), clipMask,
1290                                                                                         model->Handle(), model->GetOrigin(), model->GetAxis() );
1291         }
1292         else {
1293                 gameLocal.clip.Rotation( results, clipModel->GetOrigin(), rotation,
1294                                                                                         clipModel, clipModel->GetAxis(), clipMask, self );
1295         }
1296 }
1297
1298 /*
1299 ================
1300 idPhysics_RigidBody::ClipContents
1301 ================
1302 */
1303 int idPhysics_RigidBody::ClipContents( const idClipModel *model ) const {
1304         if ( model ) {
1305                 return gameLocal.clip.ContentsModel( clipModel->GetOrigin(), clipModel, clipModel->GetAxis(), -1,
1306                                                                         model->Handle(), model->GetOrigin(), model->GetAxis() );
1307         }
1308         else {
1309                 return gameLocal.clip.Contents( clipModel->GetOrigin(), clipModel, clipModel->GetAxis(), -1, NULL );
1310         }
1311 }
1312
1313 /*
1314 ================
1315 idPhysics_RigidBody::DisableClip
1316 ================
1317 */
1318 void idPhysics_RigidBody::DisableClip( void ) {
1319         clipModel->Disable();
1320 }
1321
1322 /*
1323 ================
1324 idPhysics_RigidBody::EnableClip
1325 ================
1326 */
1327 void idPhysics_RigidBody::EnableClip( void ) {
1328         clipModel->Enable();
1329 }
1330
1331 /*
1332 ================
1333 idPhysics_RigidBody::UnlinkClip
1334 ================
1335 */
1336 void idPhysics_RigidBody::UnlinkClip( void ) {
1337         clipModel->Unlink();
1338 }
1339
1340 /*
1341 ================
1342 idPhysics_RigidBody::LinkClip
1343 ================
1344 */
1345 void idPhysics_RigidBody::LinkClip( void ) {
1346         clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation );
1347 }
1348
1349 /*
1350 ================
1351 idPhysics_RigidBody::EvaluateContacts
1352 ================
1353 */
1354 bool idPhysics_RigidBody::EvaluateContacts( void ) {
1355         idVec6 dir;
1356         int num;
1357
1358         ClearContacts();
1359
1360         contacts.SetNum( 10, false );
1361
1362         dir.SubVec3(0) = current.i.linearMomentum + current.lastTimeStep * gravityVector * mass;
1363         dir.SubVec3(1) = current.i.angularMomentum;
1364         dir.SubVec3(0).Normalize();
1365         dir.SubVec3(1).Normalize();
1366         num = gameLocal.clip.Contacts( &contacts[0], 10, clipModel->GetOrigin(),
1367                                         dir, CONTACT_EPSILON, clipModel, clipModel->GetAxis(), clipMask, self );
1368         contacts.SetNum( num, false );
1369
1370         AddContactEntitiesForContacts();
1371
1372         return ( contacts.Num() != 0 );
1373 }
1374
1375 /*
1376 ================
1377 idPhysics_RigidBody::SetPushed
1378 ================
1379 */
1380 void idPhysics_RigidBody::SetPushed( int deltaTime ) {
1381         idRotation rotation;
1382
1383         rotation = ( saved.i.orientation * current.i.orientation ).ToRotation();
1384
1385         // velocity with which the af is pushed
1386         current.pushVelocity.SubVec3(0) += ( current.i.position - saved.i.position ) / ( deltaTime * idMath::M_MS2SEC );
1387         current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( deltaTime * idMath::M_MS2SEC );
1388 }
1389
1390 /*
1391 ================
1392 idPhysics_RigidBody::GetPushedLinearVelocity
1393 ================
1394 */
1395 const idVec3 &idPhysics_RigidBody::GetPushedLinearVelocity( const int id ) const {
1396         return current.pushVelocity.SubVec3(0);
1397 }
1398
1399 /*
1400 ================
1401 idPhysics_RigidBody::GetPushedAngularVelocity
1402 ================
1403 */
1404 const idVec3 &idPhysics_RigidBody::GetPushedAngularVelocity( const int id ) const {
1405         return current.pushVelocity.SubVec3(1);
1406 }
1407
1408 /*
1409 ================
1410 idPhysics_RigidBody::SetMaster
1411 ================
1412 */
1413 void idPhysics_RigidBody::SetMaster( idEntity *master, const bool orientated ) {
1414         idVec3 masterOrigin;
1415         idMat3 masterAxis;
1416
1417         if ( master ) {
1418                 if ( !hasMaster ) {
1419                         // transform from world space to master space
1420                         self->GetMasterPosition( masterOrigin, masterAxis );
1421                         current.localOrigin = ( current.i.position - masterOrigin ) * masterAxis.Transpose();
1422                         if ( orientated ) {
1423                                 current.localAxis = current.i.orientation * masterAxis.Transpose();
1424                         }
1425                         else {
1426                                 current.localAxis = current.i.orientation;
1427                         }
1428                         hasMaster = true;
1429                         isOrientated = orientated;
1430                         ClearContacts();
1431                 }
1432         }
1433         else {
1434                 if ( hasMaster ) {
1435                         hasMaster = false;
1436                         Activate();
1437                 }
1438         }
1439 }
1440
1441 const float     RB_VELOCITY_MAX                         = 16000;
1442 const int       RB_VELOCITY_TOTAL_BITS          = 16;
1443 const int       RB_VELOCITY_EXPONENT_BITS       = idMath::BitsForInteger( idMath::BitsForFloat( RB_VELOCITY_MAX ) ) + 1;
1444 const int       RB_VELOCITY_MANTISSA_BITS       = RB_VELOCITY_TOTAL_BITS - 1 - RB_VELOCITY_EXPONENT_BITS;
1445 const float     RB_MOMENTUM_MAX                         = 1e20f;
1446 const int       RB_MOMENTUM_TOTAL_BITS          = 16;
1447 const int       RB_MOMENTUM_EXPONENT_BITS       = idMath::BitsForInteger( idMath::BitsForFloat( RB_MOMENTUM_MAX ) ) + 1;
1448 const int       RB_MOMENTUM_MANTISSA_BITS       = RB_MOMENTUM_TOTAL_BITS - 1 - RB_MOMENTUM_EXPONENT_BITS;
1449 const float     RB_FORCE_MAX                            = 1e20f;
1450 const int       RB_FORCE_TOTAL_BITS                     = 16;
1451 const int       RB_FORCE_EXPONENT_BITS          = idMath::BitsForInteger( idMath::BitsForFloat( RB_FORCE_MAX ) ) + 1;
1452 const int       RB_FORCE_MANTISSA_BITS          = RB_FORCE_TOTAL_BITS - 1 - RB_FORCE_EXPONENT_BITS;
1453
1454 /*
1455 ================
1456 idPhysics_RigidBody::WriteToSnapshot
1457 ================
1458 */
1459 void idPhysics_RigidBody::WriteToSnapshot( idBitMsgDelta &msg ) const {
1460         idCQuat quat, localQuat;
1461
1462         quat = current.i.orientation.ToCQuat();
1463         localQuat = current.localAxis.ToCQuat();
1464
1465         msg.WriteLong( current.atRest );
1466         msg.WriteFloat( current.i.position[0] );
1467         msg.WriteFloat( current.i.position[1] );
1468         msg.WriteFloat( current.i.position[2] );
1469         msg.WriteFloat( quat.x );
1470         msg.WriteFloat( quat.y );
1471         msg.WriteFloat( quat.z );
1472         msg.WriteFloat( current.i.linearMomentum[0], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1473         msg.WriteFloat( current.i.linearMomentum[1], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1474         msg.WriteFloat( current.i.linearMomentum[2], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1475         msg.WriteFloat( current.i.angularMomentum[0], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1476         msg.WriteFloat( current.i.angularMomentum[1], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1477         msg.WriteFloat( current.i.angularMomentum[2], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1478         msg.WriteDeltaFloat( current.i.position[0], current.localOrigin[0] );
1479         msg.WriteDeltaFloat( current.i.position[1], current.localOrigin[1] );
1480         msg.WriteDeltaFloat( current.i.position[2], current.localOrigin[2] );
1481         msg.WriteDeltaFloat( quat.x, localQuat.x );
1482         msg.WriteDeltaFloat( quat.y, localQuat.y );
1483         msg.WriteDeltaFloat( quat.z, localQuat.z );
1484         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS );
1485         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS );
1486         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS );
1487         msg.WriteDeltaFloat( 0.0f, current.externalForce[0], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1488         msg.WriteDeltaFloat( 0.0f, current.externalForce[1], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1489         msg.WriteDeltaFloat( 0.0f, current.externalForce[2], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1490         msg.WriteDeltaFloat( 0.0f, current.externalTorque[0], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1491         msg.WriteDeltaFloat( 0.0f, current.externalTorque[1], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1492         msg.WriteDeltaFloat( 0.0f, current.externalTorque[2], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1493 }
1494
1495 /*
1496 ================
1497 idPhysics_RigidBody::ReadFromSnapshot
1498 ================
1499 */
1500 void idPhysics_RigidBody::ReadFromSnapshot( const idBitMsgDelta &msg ) {
1501         idCQuat quat, localQuat;
1502
1503         current.atRest = msg.ReadLong();
1504         current.i.position[0] = msg.ReadFloat();
1505         current.i.position[1] = msg.ReadFloat();
1506         current.i.position[2] = msg.ReadFloat();
1507         quat.x = msg.ReadFloat();
1508         quat.y = msg.ReadFloat();
1509         quat.z = msg.ReadFloat();
1510         current.i.linearMomentum[0] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1511         current.i.linearMomentum[1] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1512         current.i.linearMomentum[2] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1513         current.i.angularMomentum[0] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1514         current.i.angularMomentum[1] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1515         current.i.angularMomentum[2] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS );
1516         current.localOrigin[0] = msg.ReadDeltaFloat( current.i.position[0] );
1517         current.localOrigin[1] = msg.ReadDeltaFloat( current.i.position[1] );
1518         current.localOrigin[2] = msg.ReadDeltaFloat( current.i.position[2] );
1519         localQuat.x = msg.ReadDeltaFloat( quat.x );
1520         localQuat.y = msg.ReadDeltaFloat( quat.y );
1521         localQuat.z = msg.ReadDeltaFloat( quat.z );
1522         current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS );
1523         current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS );
1524         current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS );
1525         current.externalForce[0] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1526         current.externalForce[1] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1527         current.externalForce[2] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1528         current.externalTorque[0] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1529         current.externalTorque[1] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1530         current.externalTorque[2] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS );
1531
1532         current.i.orientation = quat.ToMat3();
1533         current.localAxis = localQuat.ToMat3();
1534
1535         if ( clipModel ) {
1536                 clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation );
1537         }
1538 }