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 *) &current.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( &current, 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