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