doom3-gpl
Doom 3 GPL source release
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Physics_AF.cpp
Go to the documentation of this file.
1 /*
2 ===========================================================================
3 
4 Doom 3 GPL Source Code
5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
6 
7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
8 
9 Doom 3 Source Code is free software: you can redistribute it and/or modify
10 it under the terms of the GNU General Public License as published by
11 the Free Software Foundation, either version 3 of the License, or
12 (at your option) any later version.
13 
14 Doom 3 Source Code is distributed in the hope that it will be useful,
15 but WITHOUT ANY WARRANTY; without even the implied warranty of
16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 GNU General Public License for more details.
18 
19 You should have received a copy of the GNU General Public License
20 along with Doom 3 Source Code. If not, see <http://www.gnu.org/licenses/>.
21 
22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code. If not, please request a copy in writing from id Software at the address below.
23 
24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
25 
26 ===========================================================================
27 */
28 
29 #include "../../idlib/precompiled.h"
30 #pragma hdrstop
31 
32 #include "../Game_local.h"
33 
34 CLASS_DECLARATION( idPhysics_Base, idPhysics_AF )
35 END_CLASS
36 
37 const float ERROR_REDUCTION = 0.5f;
38 const float ERROR_REDUCTION_MAX = 256.0f;
39 const float LIMIT_ERROR_REDUCTION = 0.3f;
40 const float LCP_EPSILON = 1e-7f;
41 const float LIMIT_LCP_EPSILON = 1e-4f;
42 const float CONTACT_LCP_EPSILON = 1e-6f;
43 const float CENTER_OF_MASS_EPSILON = 1e-4f;
44 const float NO_MOVE_TIME = 1.0f;
45 const float NO_MOVE_TRANSLATION_TOLERANCE = 10.0f;
46 const float NO_MOVE_ROTATION_TOLERANCE = 10.0f;
47 const float MIN_MOVE_TIME = -1.0f;
48 const float MAX_MOVE_TIME = -1.0f;
49 const float IMPULSE_THRESHOLD = 500.0f;
50 const float SUSPEND_LINEAR_VELOCITY = 10.0f;
51 const float SUSPEND_ANGULAR_VELOCITY = 15.0f;
52 const float SUSPEND_LINEAR_ACCELERATION = 20.0f;
53 const float SUSPEND_ANGULAR_ACCELERATION = 30.0f;
54 const idVec6 vec6_lcp_epsilon = idVec6( LCP_EPSILON, LCP_EPSILON, LCP_EPSILON,
55  LCP_EPSILON, LCP_EPSILON, LCP_EPSILON );
56 
57 #define AF_TIMINGS
58 
59 #ifdef AF_TIMINGS
60 static int lastTimerReset = 0;
61 static int numArticulatedFigures = 0;
62 static idTimer timer_total, timer_pc, timer_ac, timer_collision, timer_lcp;
63 #endif
64 
65 
66 
67 //===============================================================
68 //
69 // idAFConstraint
70 //
71 //===============================================================
72 
73 /*
74 ================
75 idAFConstraint::idAFConstraint
76 ================
77 */
78 idAFConstraint::idAFConstraint( void ) {
79  type = CONSTRAINT_INVALID;
80  name = "noname";
81  body1 = NULL;
82  body2 = NULL;
83  physics = NULL;
84 
85  lo.Zero( 6 );
86  lo.SubVec6(0) = -vec6_infinity;
87  hi.Zero( 6 );
88  hi.SubVec6(0) = vec6_infinity;
89  e.SetSize( 6 );
90  e.SubVec6(0) = vec6_lcp_epsilon;
91 
92  boxConstraint = NULL;
93  boxIndex[0] = -1;
94  boxIndex[1] = -1;
95  boxIndex[2] = -1;
96  boxIndex[3] = -1;
97  boxIndex[4] = -1;
98  boxIndex[5] = -1;
99 
100  firstIndex = 0;
101 
102  memset( &fl, 0, sizeof( fl ) );
103 }
104 
105 /*
106 ================
107 idAFConstraint::~idAFConstraint
108 ================
109 */
110 idAFConstraint::~idAFConstraint( void ) {
111 }
112 
113 /*
114 ================
115 idAFConstraint::SetBody1
116 ================
117 */
118 void idAFConstraint::SetBody1( idAFBody *body ) {
119  if ( body1 != body) {
120  body1 = body;
121  if ( physics ) {
122  physics->SetChanged();
123  }
124  }
125 }
126 
127 /*
128 ================
129 idAFConstraint::SetBody2
130 ================
131 */
132 void idAFConstraint::SetBody2( idAFBody *body ) {
133  if ( body2 != body ) {
134  body2 = body;
135  if ( physics ) {
136  physics->SetChanged();
137  }
138  }
139 }
140 
141 /*
142 ================
143 idAFConstraint::GetMultiplier
144 ================
145 */
146 const idVecX &idAFConstraint::GetMultiplier( void ) {
147  return lm;
148 }
149 
150 /*
151 ================
152 idAFConstraint::Evaluate
153 ================
154 */
155 void idAFConstraint::Evaluate( float invTimeStep ) {
156  assert( 0 );
157 }
158 
159 /*
160 ================
161 idAFConstraint::ApplyFriction
162 ================
163 */
164 void idAFConstraint::ApplyFriction( float invTimeStep ) {
165 }
166 
167 /*
168 ================
169 idAFConstraint::GetForce
170 ================
171 */
172 void idAFConstraint::GetForce( idAFBody *body, idVec6 &force ) {
173  idVecX v;
174 
175  v.SetData( 6, VECX_ALLOCA( 6 ) );
176  if ( body == body1 ) {
177  J1.TransposeMultiply( v, lm );
178  }
179  else if ( body == body2 ) {
180  J2.TransposeMultiply( v, lm );
181  }
182  else {
183  v.Zero();
184  }
185  force[0] = v[0]; force[1] = v[1]; force[2] = v[2]; force[3] = v[3]; force[4] = v[4]; force[5] = v[5];
186 }
187 
188 /*
189 ================
190 idAFConstraint::Translate
191 ================
192 */
193 void idAFConstraint::Translate( const idVec3 &translation ) {
194  assert( 0 );
195 }
196 
197 /*
198 ================
199 idAFConstraint::Rotate
200 ================
201 */
202 void idAFConstraint::Rotate( const idRotation &rotation ) {
203  assert( 0 );
204 }
205 
206 /*
207 ================
208 idAFConstraint::GetCenter
209 ================
210 */
211 void idAFConstraint::GetCenter( idVec3 &center ) {
212  center.Zero();
213 }
214 
215 /*
216 ================
217 idAFConstraint::DebugDraw
218 ================
219 */
220 void idAFConstraint::DebugDraw( void ) {
221 }
222 
223 /*
224 ================
225 idAFConstraint::InitSize
226 ================
227 */
228 void idAFConstraint::InitSize( int size ) {
229  J1.Zero( size, 6 );
230  J2.Zero( size, 6 );
231  c1.Zero( size );
232  c2.Zero( size );
233  s.Zero( size );
234  lm.Zero( size );
235 }
236 
237 /*
238 ================
239 idAFConstraint::Save
240 ================
241 */
242 void idAFConstraint::Save( idSaveGame *saveFile ) const {
243  saveFile->WriteInt( type );
244 }
245 
246 /*
247 ================
248 idAFConstraint::Restore
249 ================
250 */
251 void idAFConstraint::Restore( idRestoreGame *saveFile ) {
252  constraintType_t t;
253  saveFile->ReadInt( (int &)t );
254  assert( t == type );
255 }
256 
257 
258 //===============================================================
259 //
260 // idAFConstraint_Fixed
261 //
262 //===============================================================
263 
264 /*
265 ================
266 idAFConstraint_Fixed::idAFConstraint_Fixed
267 ================
268 */
269 idAFConstraint_Fixed::idAFConstraint_Fixed( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
270  assert( body1 );
271  type = CONSTRAINT_FIXED;
272  this->name = name;
273  this->body1 = body1;
274  this->body2 = body2;
275  InitSize( 6 );
276  fl.allowPrimary = true;
277  fl.noCollision = true;
278 
279  InitOffset();
280 }
281 
282 /*
283 ================
284 idAFConstraint_Fixed::InitOffset
285 ================
286 */
287 void idAFConstraint_Fixed::InitOffset( void ) {
288  if ( body2 ) {
289  offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
290  relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose();
291  }
292  else {
293  offset = body1->GetWorldOrigin();
294  relAxis = body1->GetWorldAxis();
295  }
296 }
297 
298 /*
299 ================
300 idAFConstraint_Fixed::SetBody1
301 ================
302 */
303 void idAFConstraint_Fixed::SetBody1( idAFBody *body ) {
304  if ( body1 != body) {
305  body1 = body;
306  InitOffset();
307  if ( physics ) {
308  physics->SetChanged();
309  }
310  }
311 }
312 
313 /*
314 ================
315 idAFConstraint_Fixed::SetBody2
316 ================
317 */
318 void idAFConstraint_Fixed::SetBody2( idAFBody *body ) {
319  if ( body2 != body ) {
320  body2 = body;
321  InitOffset();
322  if ( physics ) {
323  physics->SetChanged();
324  }
325  }
326 }
327 
328 /*
329 ================
330 idAFConstraint_Fixed::Evaluate
331 ================
332 */
333 void idAFConstraint_Fixed::Evaluate( float invTimeStep ) {
334  idVec3 ofs, a2;
335  idMat3 ax;
336  idRotation r;
337  idAFBody *master;
338 
339  master = body2 ? body2 : physics->GetMasterBody();
340 
341  if ( master ) {
342  a2 = offset * master->GetWorldAxis();
343  ofs = a2 + master->GetWorldOrigin();
344  ax = relAxis * master->GetWorldAxis();
345  }
346  else {
347  a2.Zero();
348  ofs = offset;
349  ax = relAxis;
350  }
351 
352  J1.Set( mat3_identity, mat3_zero,
353  mat3_zero, mat3_identity );
354 
355  if ( body2 ) {
356  J2.Set( -mat3_identity, SkewSymmetric( a2 ),
357  mat3_zero, -mat3_identity );
358  }
359  else {
360  J2.Zero( 6, 6 );
361  }
362 
363  c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( ofs - body1->GetWorldOrigin() );
364  r = ( body1->GetWorldAxis().Transpose() * ax ).ToRotation();
365  c1.SubVec3(1) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * -(float) DEG2RAD( r.GetAngle() ) );
366 
367  c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
368 }
369 
370 /*
371 ================
372 idAFConstraint_Fixed::ApplyFriction
373 ================
374 */
375 void idAFConstraint_Fixed::ApplyFriction( float invTimeStep ) {
376  // no friction
377 }
378 
379 /*
380 ================
381 idAFConstraint_Fixed::Translate
382 ================
383 */
384 void idAFConstraint_Fixed::Translate( const idVec3 &translation ) {
385  if ( !body2 ) {
386  offset += translation;
387  }
388 }
389 
390 /*
391 ================
392 idAFConstraint_Fixed::Rotate
393 ================
394 */
395 void idAFConstraint_Fixed::Rotate( const idRotation &rotation ) {
396  if ( !body2 ) {
397  offset *= rotation;
398  relAxis *= rotation.ToMat3();
399  }
400 }
401 
402 /*
403 ================
404 idAFConstraint_Fixed::GetCenter
405 ================
406 */
407 void idAFConstraint_Fixed::GetCenter( idVec3 &center ) {
408  center = body1->GetWorldOrigin();
409 }
410 
411 /*
412 ================
413 idAFConstraint_Fixed::DebugDraw
414 ================
415 */
416 void idAFConstraint_Fixed::DebugDraw( void ) {
417  idAFBody *master;
418 
419  master = body2 ? body2 : physics->GetMasterBody();
420  if ( master ) {
421  gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), master->GetWorldOrigin() );
422  }
423  else {
424  gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), vec3_origin );
425  }
426 }
427 
428 /*
429 ================
430 idAFConstraint_Fixed::Save
431 ================
432 */
433 void idAFConstraint_Fixed::Save( idSaveGame *saveFile ) const {
434  idAFConstraint::Save( saveFile );
435  saveFile->WriteVec3( offset );
436  saveFile->WriteMat3( relAxis );
437 }
438 
439 /*
440 ================
441 idAFConstraint_Fixed::Restore
442 ================
443 */
444 void idAFConstraint_Fixed::Restore( idRestoreGame *saveFile ) {
445  idAFConstraint::Restore( saveFile );
446  saveFile->ReadVec3( offset );
447  saveFile->ReadMat3( relAxis );
448 }
449 
450 
451 //===============================================================
452 //
453 // idAFConstraint_BallAndSocketJoint
454 //
455 //===============================================================
456 
457 /*
458 ================
459 idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint
460 ================
461 */
462 idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
463  assert( body1 );
464  type = CONSTRAINT_BALLANDSOCKETJOINT;
465  this->name = name;
466  this->body1 = body1;
467  this->body2 = body2;
468  InitSize( 3 );
469  coneLimit = NULL;
470  pyramidLimit = NULL;
471  friction = 0.0f;
472  fc = NULL;
473  fl.allowPrimary = true;
474  fl.noCollision = true;
475 }
476 
477 /*
478 ================
479 idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint
480 ================
481 */
482 idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint( void ) {
483  if ( coneLimit ) {
484  delete coneLimit;
485  }
486  if ( pyramidLimit ) {
487  delete pyramidLimit;
488  }
489 }
490 
491 /*
492 ================
493 idAFConstraint_BallAndSocketJoint::SetAnchor
494 ================
495 */
496 void idAFConstraint_BallAndSocketJoint::SetAnchor( const idVec3 &worldPosition ) {
497 
498  // get anchor relative to center of mass of body1
499  anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
500  if ( body2 ) {
501  // get anchor relative to center of mass of body2
502  anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
503  }
504  else {
505  anchor2 = worldPosition;
506  }
507 
508  if ( coneLimit ) {
509  coneLimit->SetAnchor( anchor2 );
510  }
511  if ( pyramidLimit ) {
512  pyramidLimit->SetAnchor( anchor2 );
513  }
514 }
515 
516 /*
517 ================
518 idAFConstraint_BallAndSocketJoint::GetAnchor
519 ================
520 */
521 idVec3 idAFConstraint_BallAndSocketJoint::GetAnchor( void ) const {
522  if ( body2 ) {
523  return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
524  }
525  return anchor2;
526 }
527 
528 /*
529 ================
530 idAFConstraint_BallAndSocketJoint::SetNoLimit
531 ================
532 */
533 void idAFConstraint_BallAndSocketJoint::SetNoLimit( void ) {
534  if ( coneLimit ) {
535  delete coneLimit;
536  coneLimit = NULL;
537  }
538  if ( pyramidLimit ) {
539  delete pyramidLimit;
540  pyramidLimit = NULL;
541  }
542 }
543 
544 /*
545 ================
546 idAFConstraint_BallAndSocketJoint::SetConeLimit
547 ================
548 */
549 void idAFConstraint_BallAndSocketJoint::SetConeLimit( const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) {
550  if ( pyramidLimit ) {
551  delete pyramidLimit;
552  pyramidLimit = NULL;
553  }
554  if ( !coneLimit ) {
555  coneLimit = new idAFConstraint_ConeLimit;
556  coneLimit->SetPhysics( physics );
557  }
558  if ( body2 ) {
559  coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, body1Axis * body1->GetWorldAxis().Transpose() );
560  }
561  else {
562  coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, body1Axis * body1->GetWorldAxis().Transpose() );
563  }
564 }
565 
566 /*
567 ================
568 idAFConstraint_BallAndSocketJoint::SetPyramidLimit
569 ================
570 */
571 void idAFConstraint_BallAndSocketJoint::SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis,
572  const float angle1, const float angle2, const idVec3 &body1Axis ) {
573  if ( coneLimit ) {
574  delete coneLimit;
575  coneLimit = NULL;
576  }
577  if ( !pyramidLimit ) {
578  pyramidLimit = new idAFConstraint_PyramidLimit;
579  pyramidLimit->SetPhysics( physics );
580  }
581  if ( body2 ) {
582  pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(),
583  baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2,
584  body1Axis * body1->GetWorldAxis().Transpose() );
585  }
586  else {
587  pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2,
588  body1Axis * body1->GetWorldAxis().Transpose() );
589  }
590 }
591 
592 /*
593 ================
594 idAFConstraint_BallAndSocketJoint::SetLimitEpsilon
595 ================
596 */
597 void idAFConstraint_BallAndSocketJoint::SetLimitEpsilon( const float e ) {
598  if ( coneLimit ) {
599  coneLimit->SetEpsilon( e );
600  }
601  if ( pyramidLimit ) {
602  pyramidLimit->SetEpsilon( e );
603  }
604 }
605 
606 /*
607 ================
608 idAFConstraint_BallAndSocketJoint::GetFriction
609 ================
610 */
611 float idAFConstraint_BallAndSocketJoint::GetFriction( void ) const {
612  if ( af_forceFriction.GetFloat() > 0.0f ) {
613  return af_forceFriction.GetFloat();
614  }
615  return friction * physics->GetJointFrictionScale();
616 }
617 
618 /*
619 ================
620 idAFConstraint_BallAndSocketJoint::Evaluate
621 ================
622 */
623 void idAFConstraint_BallAndSocketJoint::Evaluate( float invTimeStep ) {
624  idVec3 a1, a2;
625  idAFBody *master;
626 
627  master = body2 ? body2 : physics->GetMasterBody();
628 
629  a1 = anchor1 * body1->GetWorldAxis();
630 
631  if ( master ) {
632  a2 = anchor2 * master->GetWorldAxis();
633  c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
634  }
635  else {
636  c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( anchor2 - ( a1 + body1->GetWorldOrigin() ) );
637  }
638 
639  c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
640 
641  J1.Set( mat3_identity, -SkewSymmetric( a1 ) );
642 
643  if ( body2 ) {
644  J2.Set( -mat3_identity, SkewSymmetric( a2 ) );
645  }
646  else {
647  J2.Zero( 3, 6 );
648  }
649 
650  if ( coneLimit ) {
651  coneLimit->Add( physics, invTimeStep );
652  }
653  else if ( pyramidLimit ) {
654  pyramidLimit->Add( physics, invTimeStep );
655  }
656 }
657 
658 /*
659 ================
660 idAFConstraint_BallAndSocketJoint::ApplyFriction
661 ================
662 */
663 void idAFConstraint_BallAndSocketJoint::ApplyFriction( float invTimeStep ) {
664  idVec3 angular;
665  float invMass, currentFriction;
666 
667  currentFriction = GetFriction();
668 
669  if ( currentFriction <= 0.0f ) {
670  return;
671  }
672 
673  if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) {
674 
675  angular = body1->GetAngularVelocity();
676  invMass = body1->GetInverseMass();
677  if ( body2 ) {
678  angular -= body2->GetAngularVelocity();
679  invMass += body2->GetInverseMass();
680  }
681 
682  angular *= currentFriction / invMass;
683 
684  body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
685  if ( body2 ) {
686  body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
687  }
688  }
689  else {
690  if ( !fc ) {
691  fc = new idAFConstraint_BallAndSocketJointFriction;
692  fc->Setup( this );
693  }
694 
695  fc->Add( physics, invTimeStep );
696  }
697 }
698 
699 /*
700 ================
701 idAFConstraint_BallAndSocketJoint::GetForce
702 ================
703 */
704 void idAFConstraint_BallAndSocketJoint::GetForce( idAFBody *body, idVec6 &force ) {
705  idAFConstraint::GetForce( body, force );
706  // FIXME: add limit force
707 }
708 
709 /*
710 ================
711 idAFConstraint_BallAndSocketJoint::Translate
712 ================
713 */
714 void idAFConstraint_BallAndSocketJoint::Translate( const idVec3 &translation ) {
715  if ( !body2 ) {
716  anchor2 += translation;
717  }
718  if ( coneLimit ) {
719  coneLimit->Translate( translation );
720  }
721  else if ( pyramidLimit ) {
722  pyramidLimit->Translate( translation );
723  }
724 }
725 
726 /*
727 ================
728 idAFConstraint_BallAndSocketJoint::Rotate
729 ================
730 */
731 void idAFConstraint_BallAndSocketJoint::Rotate( const idRotation &rotation ) {
732  if ( !body2 ) {
733  anchor2 *= rotation;
734  }
735  if ( coneLimit ) {
736  coneLimit->Rotate( rotation );
737  }
738  else if ( pyramidLimit ) {
739  pyramidLimit->Rotate( rotation );
740  }
741 }
742 
743 /*
744 ================
745 idAFConstraint_BallAndSocketJoint::GetCenter
746 ================
747 */
748 void idAFConstraint_BallAndSocketJoint::GetCenter( idVec3 &center ) {
749  center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
750 }
751 
752 /*
753 ================
754 idAFConstraint_BallAndSocketJoint::DebugDraw
755 ================
756 */
757 void idAFConstraint_BallAndSocketJoint::DebugDraw( void ) {
758  idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
759  gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 5, 0, 0 ), a1 + idVec3( 5, 0, 0 ) );
760  gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 0, 5, 0 ), a1 + idVec3( 0, 5, 0 ) );
761  gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 0, 0, 5 ), a1 + idVec3( 0, 0, 5 ) );
762 
763  if ( af_showLimits.GetBool() ) {
764  if ( coneLimit ) {
765  coneLimit->DebugDraw();
766  }
767  if ( pyramidLimit ) {
768  pyramidLimit->DebugDraw();
769  }
770  }
771 }
772 
773 /*
774 ================
775 idAFConstraint_BallAndSocketJoint::Save
776 ================
777 */
778 void idAFConstraint_BallAndSocketJoint::Save( idSaveGame *saveFile ) const {
779  idAFConstraint::Save( saveFile );
780  saveFile->WriteVec3( anchor1 );
781  saveFile->WriteVec3( anchor2 );
782  saveFile->WriteFloat( friction );
783  if ( coneLimit ) {
784  coneLimit->Save( saveFile );
785  }
786  if ( pyramidLimit ) {
787  pyramidLimit->Save( saveFile );
788  }
789 }
790 
791 /*
792 ================
793 idAFConstraint_BallAndSocketJoint::Restore
794 ================
795 */
796 void idAFConstraint_BallAndSocketJoint::Restore( idRestoreGame *saveFile ) {
797  idAFConstraint::Restore( saveFile );
798  saveFile->ReadVec3( anchor1 );
799  saveFile->ReadVec3( anchor2 );
800  saveFile->ReadFloat( friction );
801  if ( coneLimit ) {
802  coneLimit->Restore( saveFile );
803  }
804  if ( pyramidLimit ) {
805  pyramidLimit->Restore( saveFile );
806  }
807 }
808 
809 
810 //===============================================================
811 //
812 // idAFConstraint_BallAndSocketJointFriction
813 //
814 //===============================================================
815 
816 /*
817 ================
818 idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction
819 ================
820 */
821 idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction( void ) {
822  type = CONSTRAINT_FRICTION;
823  name = "ballAndSocketJointFriction";
824  InitSize( 3 );
825  joint = NULL;
826  fl.allowPrimary = false;
827  fl.frameConstraint = true;
828 }
829 
830 /*
831 ================
832 idAFConstraint_BallAndSocketJointFriction::Setup
833 ================
834 */
835 void idAFConstraint_BallAndSocketJointFriction::Setup( idAFConstraint_BallAndSocketJoint *bsj ) {
836  this->joint = bsj;
837  body1 = bsj->GetBody1();
838  body2 = bsj->GetBody2();
839 }
840 
841 /*
842 ================
843 idAFConstraint_BallAndSocketJointFriction::Evaluate
844 ================
845 */
846 void idAFConstraint_BallAndSocketJointFriction::Evaluate( float invTimeStep ) {
847  // do nothing
848 }
849 
850 /*
851 ================
852 idAFConstraint_BallAndSocketJointFriction::ApplyFriction
853 ================
854 */
855 void idAFConstraint_BallAndSocketJointFriction::ApplyFriction( float invTimeStep ) {
856  // do nothing
857 }
858 
859 /*
860 ================
861 idAFConstraint_BallAndSocketJointFriction::Add
862 ================
863 */
864 bool idAFConstraint_BallAndSocketJointFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
865  float f;
866 
867  physics = phys;
868 
869  f = joint->GetFriction() * joint->GetMultiplier().Length();
870  if ( f == 0.0f ) {
871  return false;
872  }
873 
874  lo[0] = lo[1] = lo[2] = -f;
875  hi[0] = hi[1] = hi[2] = f;
876 
877  J1.Zero( 3, 6 );
878  J1[0][3] = J1[1][4] = J1[2][5] = 1.0f;
879 
880  if ( body2 ) {
881 
882  J2.Zero( 3, 6 );
883  J2[0][3] = J2[1][4] = J2[2][5] = 1.0f;
884  }
885 
886  physics->AddFrameConstraint( this );
887 
888  return true;
889 }
890 
891 /*
892 ================
893 idAFConstraint_BallAndSocketJointFriction::Translate
894 ================
895 */
896 void idAFConstraint_BallAndSocketJointFriction::Translate( const idVec3 &translation ) {
897 }
898 
899 /*
900 ================
901 idAFConstraint_BallAndSocketJointFriction::Rotate
902 ================
903 */
904 void idAFConstraint_BallAndSocketJointFriction::Rotate( const idRotation &rotation ) {
905 }
906 
907 
908 //===============================================================
909 //
910 // idAFConstraint_UniversalJoint
911 //
912 //===============================================================
913 
914 /*
915 ================
916 idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint
917 ================
918 */
919 idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
920  assert( body1 );
921  type = CONSTRAINT_UNIVERSALJOINT;
922  this->name = name;
923  this->body1 = body1;
924  this->body2 = body2;
925  InitSize( 4 );
926  coneLimit = NULL;
927  pyramidLimit = NULL;
928  friction = 0.0f;
929  fc = NULL;
930  fl.allowPrimary = true;
931  fl.noCollision = true;
932 }
933 
934 /*
935 ================
936 idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint
937 ================
938 */
939 idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint( void ) {
940  if ( coneLimit ) {
941  delete coneLimit;
942  }
943  if ( pyramidLimit ) {
944  delete pyramidLimit;
945  }
946  if ( fc ) {
947  delete fc;
948  }
949 }
950 
951 /*
952 ================
953 idAFConstraint_UniversalJoint::SetAnchor
954 ================
955 */
956 void idAFConstraint_UniversalJoint::SetAnchor( const idVec3 &worldPosition ) {
957 
958  // get anchor relative to center of mass of body1
959  anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
960  if ( body2 ) {
961  // get anchor relative to center of mass of body2
962  anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
963  }
964  else {
965  anchor2 = worldPosition;
966  }
967 
968  if ( coneLimit ) {
969  coneLimit->SetAnchor( anchor2 );
970  }
971  if ( pyramidLimit ) {
972  pyramidLimit->SetAnchor( anchor2 );
973  }
974 }
975 
976 /*
977 ================
978 idAFConstraint_UniversalJoint::GetAnchor
979 ================
980 */
981 idVec3 idAFConstraint_UniversalJoint::GetAnchor( void ) const {
982  if ( body2 ) {
983  return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
984  }
985  return anchor2;
986 }
987 
988 /*
989 ================
990 idAFConstraint_UniversalJoint::SetShafts
991 ================
992 */
993 void idAFConstraint_UniversalJoint::SetShafts( const idVec3 &cardanShaft1, const idVec3 &cardanShaft2 ) {
994  idVec3 cardanAxis;
995  float l;
996 
997  shaft1 = cardanShaft1;
998  l = shaft1.Normalize();
999  assert( l != 0.0f );
1000  shaft2 = cardanShaft2;
1001  l = shaft2.Normalize();
1002  assert( l != 0.0f );
1003 
1004  // the cardan axis is a vector orthogonal to both cardan shafts
1005  cardanAxis = shaft1.Cross( shaft2 );
1006  if ( cardanAxis.Normalize() == 0.0f ) {
1007  idVec3 vecY;
1008  shaft1.OrthogonalBasis( cardanAxis, vecY );
1009  cardanAxis.Normalize();
1010  }
1011 
1012  shaft1 *= body1->GetWorldAxis().Transpose();
1013  axis1 = cardanAxis * body1->GetWorldAxis().Transpose();
1014  if ( body2 ) {
1015  shaft2 *= body2->GetWorldAxis().Transpose();
1016  axis2 = cardanAxis * body2->GetWorldAxis().Transpose();
1017  }
1018  else {
1019  axis2 = cardanAxis;
1020  }
1021 
1022  if ( coneLimit ) {
1023  coneLimit->SetBody1Axis( shaft1 );
1024  }
1025  if ( pyramidLimit ) {
1026  pyramidLimit->SetBody1Axis( shaft1 );
1027  }
1028 }
1029 
1030 /*
1031 ================
1032 idAFConstraint_UniversalJoint::SetNoLimit
1033 ================
1034 */
1035 void idAFConstraint_UniversalJoint::SetNoLimit( void ) {
1036  if ( coneLimit ) {
1037  delete coneLimit;
1038  coneLimit = NULL;
1039  }
1040  if ( pyramidLimit ) {
1041  delete pyramidLimit;
1042  pyramidLimit = NULL;
1043  }
1044 }
1045 
1046 /*
1047 ================
1048 idAFConstraint_UniversalJoint::SetConeLimit
1049 ================
1050 */
1051 void idAFConstraint_UniversalJoint::SetConeLimit( const idVec3 &coneAxis, const float coneAngle ) {
1052  if ( pyramidLimit ) {
1053  delete pyramidLimit;
1054  pyramidLimit = NULL;
1055  }
1056  if ( !coneLimit ) {
1057  coneLimit = new idAFConstraint_ConeLimit;
1058  coneLimit->SetPhysics( physics );
1059  }
1060  if ( body2 ) {
1061  coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, shaft1 );
1062  }
1063  else {
1064  coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, shaft1 );
1065  }
1066 }
1067 
1068 /*
1069 ================
1070 idAFConstraint_UniversalJoint::SetPyramidLimit
1071 ================
1072 */
1073 void idAFConstraint_UniversalJoint::SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis,
1074  const float angle1, const float angle2 ) {
1075  if ( coneLimit ) {
1076  delete coneLimit;
1077  coneLimit = NULL;
1078  }
1079  if ( !pyramidLimit ) {
1080  pyramidLimit = new idAFConstraint_PyramidLimit;
1081  pyramidLimit->SetPhysics( physics );
1082  }
1083  if ( body2 ) {
1084  pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(),
1085  baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, shaft1 );
1086  }
1087  else {
1088  pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, shaft1 );
1089  }
1090 }
1091 
1092 /*
1093 ================
1094 idAFConstraint_UniversalJoint::SetLimitEpsilon
1095 ================
1096 */
1097 void idAFConstraint_UniversalJoint::SetLimitEpsilon( const float e ) {
1098  if ( coneLimit ) {
1099  coneLimit->SetEpsilon( e );
1100  }
1101  if ( pyramidLimit ) {
1102  pyramidLimit->SetEpsilon( e );
1103  }
1104 }
1105 
1106 /*
1107 ================
1108 idAFConstraint_UniversalJoint::GetFriction
1109 ================
1110 */
1111 float idAFConstraint_UniversalJoint::GetFriction( void ) const {
1112  if ( af_forceFriction.GetFloat() > 0.0f ) {
1113  return af_forceFriction.GetFloat();
1114  }
1115  return friction * physics->GetJointFrictionScale();
1116 }
1117 
1118 /*
1119 ================
1120 idAFConstraint_UniversalJoint::Evaluate
1121 
1122  NOTE: this joint is homokinetic
1123 ================
1124 */
1125 void idAFConstraint_UniversalJoint::Evaluate( float invTimeStep ) {
1126  idVec3 a1, a2, s1, s2, d1, d2, v;
1127  idAFBody *master;
1128 
1129  master = body2 ? body2 : physics->GetMasterBody();
1130 
1131  a1 = anchor1 * body1->GetWorldAxis();
1132  s1 = shaft1 * body1->GetWorldAxis();
1133  d1 = s1.Cross( axis1 * body1->GetWorldAxis() );
1134 
1135  if ( master ) {
1136  a2 = anchor2 * master->GetWorldAxis();
1137  s2 = shaft2 * master->GetWorldAxis();
1138  d2 = axis2 * master->GetWorldAxis();
1139  c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
1140  }
1141  else {
1142  a2 = anchor2;
1143  s2 = shaft2;
1144  d2 = axis2;
1145  c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) );
1146  }
1147 
1148  J1.Set( mat3_identity, -SkewSymmetric( a1 ),
1149  mat3_zero, idMat3( s1[0], s1[1], s1[2],
1150  0.0f, 0.0f, 0.0f,
1151  0.0f, 0.0f, 0.0f ) );
1152  J1.SetSize( 4, 6 );
1153 
1154  if ( body2 ) {
1155  J2.Set( -mat3_identity, SkewSymmetric( a2 ),
1156  mat3_zero, idMat3( s2[0], s2[1], s2[2],
1157  0.0f, 0.0f, 0.0f,
1158  0.0f, 0.0f, 0.0f ) );
1159  J2.SetSize( 4, 6 );
1160  }
1161  else {
1162  J2.Zero( 4, 6 );
1163  }
1164 
1165  v = s1.Cross( s2 );
1166  if ( v.Normalize() != 0.0f ) {
1167  idMat3 m1, m2;
1168 
1169  m1[0] = s1;
1170  m1[1] = v;
1171  m1[2] = v.Cross( m1[0] );
1172 
1173  m2[0] = -s2;
1174  m2[1] = v;
1175  m2[2] = v.Cross( m2[0] );
1176 
1177  d2 *= m2.Transpose() * m1;
1178  }
1179 
1180  c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( d1 * d2 );
1181 
1182  c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
1183 
1184  if ( coneLimit ) {
1185  coneLimit->Add( physics, invTimeStep );
1186  }
1187  else if ( pyramidLimit ) {
1188  pyramidLimit->Add( physics, invTimeStep );
1189  }
1190 }
1191 
1192 /*
1193 ================
1194 idAFConstraint_UniversalJoint::ApplyFriction
1195 ================
1196 */
1197 void idAFConstraint_UniversalJoint::ApplyFriction( float invTimeStep ) {
1198  idVec3 angular;
1199  float invMass, currentFriction;
1200 
1201  currentFriction = GetFriction();
1202 
1203  if ( currentFriction <= 0.0f ) {
1204  return;
1205  }
1206 
1207  if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) {
1208 
1209  angular = body1->GetAngularVelocity();
1210  invMass = body1->GetInverseMass();
1211  if ( body2 ) {
1212  angular -= body2->GetAngularVelocity();
1213  invMass += body2->GetInverseMass();
1214  }
1215 
1216  angular *= currentFriction / invMass;
1217 
1218  body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
1219  if ( body2 ) {
1220  body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
1221  }
1222  }
1223  else {
1224  if ( !fc ) {
1225  fc = new idAFConstraint_UniversalJointFriction;
1226  fc->Setup( this );
1227  }
1228 
1229  fc->Add( physics, invTimeStep );
1230  }
1231 }
1232 
1233 /*
1234 ================
1235 idAFConstraint_UniversalJoint::GetForce
1236 ================
1237 */
1238 void idAFConstraint_UniversalJoint::GetForce( idAFBody *body, idVec6 &force ) {
1239  idAFConstraint::GetForce( body, force );
1240  // FIXME: add limit force
1241 }
1242 
1243 /*
1244 ================
1245 idAFConstraint_UniversalJoint::Translate
1246 ================
1247 */
1248 void idAFConstraint_UniversalJoint::Translate( const idVec3 &translation ) {
1249  if ( !body2 ) {
1250  anchor2 += translation;
1251  }
1252  if ( coneLimit ) {
1253  coneLimit->Translate( translation );
1254  }
1255  else if ( pyramidLimit ) {
1256  pyramidLimit->Translate( translation );
1257  }
1258 }
1259 
1260 /*
1261 ================
1262 idAFConstraint_UniversalJoint::Rotate
1263 ================
1264 */
1265 void idAFConstraint_UniversalJoint::Rotate( const idRotation &rotation ) {
1266  if ( !body2 ) {
1267  anchor2 *= rotation;
1268  shaft2 *= rotation.ToMat3();
1269  axis2 *= rotation.ToMat3();
1270  }
1271  if ( coneLimit ) {
1272  coneLimit->Rotate( rotation );
1273  }
1274  else if ( pyramidLimit ) {
1275  pyramidLimit->Rotate( rotation );
1276  }
1277 }
1278 
1279 /*
1280 ================
1281 idAFConstraint_UniversalJoint::GetCenter
1282 ================
1283 */
1284 void idAFConstraint_UniversalJoint::GetCenter( idVec3 &center ) {
1285  center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
1286 }
1287 
1288 /*
1289 ================
1290 idAFConstraint_UniversalJoint::DebugDraw
1291 ================
1292 */
1293 void idAFConstraint_UniversalJoint::DebugDraw( void ) {
1294  idVec3 a1, a2, s1, s2, d1, d2, v;
1295  idAFBody *master;
1296 
1297  master = body2 ? body2 : physics->GetMasterBody();
1298 
1299  a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
1300  s1 = shaft1 * body1->GetWorldAxis();
1301  d1 = axis1 * body1->GetWorldAxis();
1302 
1303  if ( master ) {
1304  a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
1305  s2 = shaft2 * master->GetWorldAxis();
1306  d2 = axis2 * master->GetWorldAxis();
1307  }
1308  else {
1309  a2 = anchor2;
1310  s2 = shaft2;
1311  d2 = axis2;
1312  }
1313 
1314  v = s1.Cross( s2 );
1315  if ( v.Normalize() != 0.0f ) {
1316  idMat3 m1, m2;
1317 
1318  m1[0] = s1;
1319  m1[1] = v;
1320  m1[2] = v.Cross( m1[0] );
1321 
1322  m2[0] = -s2;
1323  m2[1] = v;
1324  m2[2] = v.Cross( m2[0] );
1325 
1326  d2 *= m2.Transpose() * m1;
1327  }
1328 
1329  gameRenderWorld->DebugArrow( colorCyan, a1, a1 + s1 * 5.0f, 1.0f );
1330  gameRenderWorld->DebugArrow( colorBlue, a2, a2 + s2 * 5.0f, 1.0f );
1331  gameRenderWorld->DebugLine( colorGreen, a1, a1 + d1 * 5.0f );
1332  gameRenderWorld->DebugLine( colorGreen, a2, a2 + d2 * 5.0f );
1333 
1334  if ( af_showLimits.GetBool() ) {
1335  if ( coneLimit ) {
1336  coneLimit->DebugDraw();
1337  }
1338  if ( pyramidLimit ) {
1339  pyramidLimit->DebugDraw();
1340  }
1341  }
1342 }
1343 
1344 /*
1345 ================
1346 idAFConstraint_UniversalJoint::Save
1347 ================
1348 */
1349 void idAFConstraint_UniversalJoint::Save( idSaveGame *saveFile ) const {
1350  idAFConstraint::Save( saveFile );
1351  saveFile->WriteVec3( anchor1 );
1352  saveFile->WriteVec3( anchor2 );
1353  saveFile->WriteVec3( shaft1 );
1354  saveFile->WriteVec3( shaft2 );
1355  saveFile->WriteVec3( axis1 );
1356  saveFile->WriteVec3( axis2 );
1357  saveFile->WriteFloat( friction );
1358  if ( coneLimit ) {
1359  coneLimit->Save( saveFile );
1360  }
1361  if ( pyramidLimit ) {
1362  pyramidLimit->Save( saveFile );
1363  }
1364 }
1365 
1366 /*
1367 ================
1368 idAFConstraint_UniversalJoint::Restore
1369 ================
1370 */
1371 void idAFConstraint_UniversalJoint::Restore( idRestoreGame *saveFile ) {
1372  idAFConstraint::Restore( saveFile );
1373  saveFile->ReadVec3( anchor1 );
1374  saveFile->ReadVec3( anchor2 );
1375  saveFile->ReadVec3( shaft1 );
1376  saveFile->ReadVec3( shaft2 );
1377  saveFile->ReadVec3( axis1 );
1378  saveFile->ReadVec3( axis2 );
1379  saveFile->ReadFloat( friction );
1380  if ( coneLimit ) {
1381  coneLimit->Restore( saveFile );
1382  }
1383  if ( pyramidLimit ) {
1384  pyramidLimit->Restore( saveFile );
1385  }
1386 }
1387 
1388 
1389 //===============================================================
1390 //
1391 // idAFConstraint_UniversalJointFriction
1392 //
1393 //===============================================================
1394 
1395 /*
1396 ================
1397 idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction
1398 ================
1399 */
1400 idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction( void ) {
1401  type = CONSTRAINT_FRICTION;
1402  name = "universalJointFriction";
1403  InitSize( 2 );
1404  joint = NULL;
1405  fl.allowPrimary = false;
1406  fl.frameConstraint = true;
1407 }
1408 
1409 /*
1410 ================
1411 idAFConstraint_UniversalJointFriction::Setup
1412 ================
1413 */
1414 void idAFConstraint_UniversalJointFriction::Setup( idAFConstraint_UniversalJoint *uj ) {
1415  this->joint = uj;
1416  body1 = uj->GetBody1();
1417  body2 = uj->GetBody2();
1418 }
1419 
1420 /*
1421 ================
1422 idAFConstraint_UniversalJointFriction::Evaluate
1423 ================
1424 */
1425 void idAFConstraint_UniversalJointFriction::Evaluate( float invTimeStep ) {
1426  // do nothing
1427 }
1428 
1429 /*
1430 ================
1431 idAFConstraint_UniversalJointFriction::ApplyFriction
1432 ================
1433 */
1434 void idAFConstraint_UniversalJointFriction::ApplyFriction( float invTimeStep ) {
1435  // do nothing
1436 }
1437 
1438 /*
1439 ================
1440 idAFConstraint_UniversalJointFriction::Add
1441 ================
1442 */
1443 bool idAFConstraint_UniversalJointFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
1444  idVec3 s1, s2, dir1, dir2;
1445  float f;
1446 
1447  physics = phys;
1448 
1449  f = joint->GetFriction() * joint->GetMultiplier().Length();
1450  if ( f == 0.0f ) {
1451  return false;
1452  }
1453 
1454  lo[0] = lo[1] = -f;
1455  hi[0] = hi[1] = f;
1456 
1457  joint->GetShafts( s1, s2 );
1458 
1459  s1 *= body1->GetWorldAxis();
1460  s1.NormalVectors( dir1, dir2 );
1461 
1462  J1.SetSize( 2, 6 );
1463  J1.SubVec6(0).SubVec3(0).Zero();
1464  J1.SubVec6(0).SubVec3(1) = dir1;
1465  J1.SubVec6(1).SubVec3(0).Zero();
1466  J1.SubVec6(1).SubVec3(1) = dir2;
1467 
1468  if ( body2 ) {
1469 
1470  J2.SetSize( 2, 6 );
1471  J2.SubVec6(0).SubVec3(0).Zero();
1472  J2.SubVec6(0).SubVec3(1) = -dir1;
1473  J2.SubVec6(1).SubVec3(0).Zero();
1474  J2.SubVec6(1).SubVec3(1) = -dir2;
1475  }
1476 
1477  physics->AddFrameConstraint( this );
1478 
1479  return true;
1480 }
1481 
1482 /*
1483 ================
1484 idAFConstraint_UniversalJointFriction::Translate
1485 ================
1486 */
1487 void idAFConstraint_UniversalJointFriction::Translate( const idVec3 &translation ) {
1488 }
1489 
1490 /*
1491 ================
1492 idAFConstraint_UniversalJointFriction::Rotate
1493 ================
1494 */
1495 void idAFConstraint_UniversalJointFriction::Rotate( const idRotation &rotation ) {
1496 }
1497 
1498 
1499 //===============================================================
1500 //
1501 // idAFConstraint_CylindricalJoint
1502 //
1503 //===============================================================
1504 
1505 /*
1506 ================
1507 idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint
1508 ================
1509 */
1510 idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
1511  assert( 0 ); // FIXME: implement
1512 }
1513 
1514 /*
1515 ================
1516 idAFConstraint_CylindricalJoint::Evaluate
1517 ================
1518 */
1519 void idAFConstraint_CylindricalJoint::Evaluate( float invTimeStep ) {
1520  assert( 0 ); // FIXME: implement
1521 }
1522 
1523 /*
1524 ================
1525 idAFConstraint_CylindricalJoint::ApplyFriction
1526 ================
1527 */
1528 void idAFConstraint_CylindricalJoint::ApplyFriction( float invTimeStep ) {
1529  assert( 0 ); // FIXME: implement
1530 }
1531 
1532 /*
1533 ================
1534 idAFConstraint_CylindricalJoint::Translate
1535 ================
1536 */
1537 void idAFConstraint_CylindricalJoint::Translate( const idVec3 &translation ) {
1538  assert( 0 ); // FIXME: implement
1539 }
1540 
1541 /*
1542 ================
1543 idAFConstraint_CylindricalJoint::Rotate
1544 ================
1545 */
1546 void idAFConstraint_CylindricalJoint::Rotate( const idRotation &rotation ) {
1547  assert( 0 ); // FIXME: implement
1548 }
1549 
1550 /*
1551 ================
1552 idAFConstraint_CylindricalJoint::DebugDraw
1553 ================
1554 */
1555 void idAFConstraint_CylindricalJoint::DebugDraw( void ) {
1556  assert( 0 ); // FIXME: implement
1557 }
1558 
1559 
1560 //===============================================================
1561 //
1562 // idAFConstraint_Hinge
1563 //
1564 //===============================================================
1565 
1566 /*
1567 ================
1568 idAFConstraint_Hinge::idAFConstraint_Hinge
1569 ================
1570 */
1571 idAFConstraint_Hinge::idAFConstraint_Hinge( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
1572  assert( body1 );
1573  type = CONSTRAINT_HINGE;
1574  this->name = name;
1575  this->body1 = body1;
1576  this->body2 = body2;
1577  InitSize( 5 );
1578  coneLimit = NULL;
1579  steering = NULL;
1580  friction = 0.0f;
1581  fc = NULL;
1582  fl.allowPrimary = true;
1583  fl.noCollision = true;
1584  initialAxis = body1->GetWorldAxis();
1585  if ( body2 ) {
1586  initialAxis *= body2->GetWorldAxis().Transpose();
1587  }
1588 }
1589 
1590 /*
1591 ================
1592 idAFConstraint_Hinge::~idAFConstraint_Hinge
1593 ================
1594 */
1595 idAFConstraint_Hinge::~idAFConstraint_Hinge( void ) {
1596  if ( coneLimit ) {
1597  delete coneLimit;
1598  }
1599  if ( fc ) {
1600  delete fc;
1601  }
1602  if ( steering ) {
1603  delete steering;
1604  }
1605 }
1606 
1607 /*
1608 ================
1609 idAFConstraint_Hinge::SetAnchor
1610 ================
1611 */
1612 void idAFConstraint_Hinge::SetAnchor( const idVec3 &worldPosition ) {
1613  // get anchor relative to center of mass of body1
1614  anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
1615  if ( body2 ) {
1616  // get anchor relative to center of mass of body2
1617  anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
1618  }
1619  else {
1620  anchor2 = worldPosition;
1621  }
1622 
1623  if ( coneLimit ) {
1624  coneLimit->SetAnchor( anchor2 );
1625  }
1626 }
1627 
1628 /*
1629 ================
1630 idAFConstraint_Hinge::GetAnchor
1631 ================
1632 */
1633 idVec3 idAFConstraint_Hinge::GetAnchor( void ) const {
1634  if ( body2 ) {
1635  return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
1636  }
1637  return anchor2;
1638 }
1639 
1640 /*
1641 ================
1642 idAFConstraint_Hinge::SetAxis
1643 ================
1644 */
1645 void idAFConstraint_Hinge::SetAxis( const idVec3 &axis ) {
1646  idVec3 normAxis;
1647 
1648  normAxis = axis;
1649  normAxis.Normalize();
1650 
1651  // get axis relative to body1
1652  axis1 = normAxis * body1->GetWorldAxis().Transpose();
1653  if ( body2 ) {
1654  // get axis relative to body2
1655  axis2 = normAxis * body2->GetWorldAxis().Transpose();
1656  }
1657  else {
1658  axis2 = normAxis;
1659  }
1660 }
1661 
1662 /*
1663 ================
1664 idAFConstraint_Hinge::GetAxis
1665 ================
1666 */
1667 idVec3 idAFConstraint_Hinge::GetAxis( void ) const {
1668  if ( body2 ) {
1669  return axis2 * body2->GetWorldAxis();
1670  }
1671  return axis2;
1672 }
1673 
1674 /*
1675 ================
1676 idAFConstraint_Hinge::SetNoLimit
1677 ================
1678 */
1679 void idAFConstraint_Hinge::SetNoLimit( void ) {
1680  if ( coneLimit ) {
1681  delete coneLimit;
1682  coneLimit = NULL;
1683  }
1684 }
1685 
1686 /*
1687 ================
1688 idAFConstraint_Hinge::SetLimit
1689 ================
1690 */
1691 void idAFConstraint_Hinge::SetLimit( const idVec3 &axis, const float angle, const idVec3 &body1Axis ) {
1692  if ( !coneLimit ) {
1693  coneLimit = new idAFConstraint_ConeLimit;
1694  coneLimit->SetPhysics( physics );
1695  }
1696  if ( body2 ) {
1697  coneLimit->Setup( body1, body2, anchor2, axis * body2->GetWorldAxis().Transpose(), angle, body1Axis * body1->GetWorldAxis().Transpose() );
1698  }
1699  else {
1700  coneLimit->Setup( body1, body2, anchor2, axis, angle, body1Axis * body1->GetWorldAxis().Transpose() );
1701  }
1702 }
1703 
1704 /*
1705 ================
1706 idAFConstraint_Hinge::SetLimitEpsilon
1707 ================
1708 */
1709 void idAFConstraint_Hinge::SetLimitEpsilon( const float e ) {
1710  if ( coneLimit ) {
1711  coneLimit->SetEpsilon( e );
1712  }
1713 }
1714 
1715 /*
1716 ================
1717 idAFConstraint_Hinge::GetFriction
1718 ================
1719 */
1720 float idAFConstraint_Hinge::GetFriction( void ) const {
1721  if ( af_forceFriction.GetFloat() > 0.0f ) {
1722  return af_forceFriction.GetFloat();
1723  }
1724  return friction * physics->GetJointFrictionScale();
1725 }
1726 
1727 /*
1728 ================
1729 idAFConstraint_Hinge::GetAngle
1730 ================
1731 */
1732 float idAFConstraint_Hinge::GetAngle( void ) const {
1733  idMat3 axis;
1734  idRotation rotation;
1735  float angle;
1736 
1737  axis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose() * initialAxis.Transpose();
1738  rotation = axis.ToRotation();
1739  angle = rotation.GetAngle();
1740  if ( rotation.GetVec() * axis1 < 0.0f ) {
1741  return -angle;
1742  }
1743  return angle;
1744 }
1745 
1746 /*
1747 ================
1748 idAFConstraint_Hinge::SetSteerAngle
1749 ================
1750 */
1751 void idAFConstraint_Hinge::SetSteerAngle( const float degrees ) {
1752  if ( coneLimit ) {
1753  delete coneLimit;
1754  coneLimit = NULL;
1755  }
1756  if ( !steering ) {
1757  steering = new idAFConstraint_HingeSteering();
1758  steering->Setup( this );
1759  }
1760  steering->SetSteerAngle( degrees );
1761 }
1762 
1763 /*
1764 ================
1765 idAFConstraint_Hinge::SetSteerSpeed
1766 ================
1767 */
1768 void idAFConstraint_Hinge::SetSteerSpeed( const float speed ) {
1769  if ( steering ) {
1770  steering->SetSteerSpeed( speed );
1771  }
1772 }
1773 
1774 /*
1775 ================
1776 idAFConstraint_Hinge::Evaluate
1777 ================
1778 */
1779 void idAFConstraint_Hinge::Evaluate( float invTimeStep ) {
1780  idVec3 a1, a2;
1781  idVec3 x1, x2, cross;
1782  idVec3 vecX, vecY;
1783  idAFBody *master;
1784 
1785  master = body2 ? body2 : physics->GetMasterBody();
1786 
1787  x1 = axis1 * body1->GetWorldAxis(); // axis in body1 space
1788  x1.OrthogonalBasis( vecX, vecY ); // basis for axis in body1 space
1789 
1790  a1 = anchor1 * body1->GetWorldAxis(); // anchor in body1 space
1791 
1792  if ( master ) {
1793  a2 = anchor2 * master->GetWorldAxis(); // anchor in master space
1794  x2 = axis2 * master->GetWorldAxis();
1795  c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
1796  }
1797  else {
1798  a2 = anchor2;
1799  x2 = axis2;
1800  c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) );
1801  }
1802 
1803  J1.Set( mat3_identity, -SkewSymmetric( a1 ),
1804  mat3_zero, idMat3( vecX[0], vecX[1], vecX[2],
1805  vecY[0], vecY[1], vecY[2],
1806  0.0f, 0.0f, 0.0f ) );
1807  J1.SetSize( 5, 6 );
1808 
1809  if ( body2 ) {
1810  J2.Set( -mat3_identity, SkewSymmetric( a2 ),
1811  mat3_zero, idMat3( -vecX[0], -vecX[1], -vecX[2],
1812  -vecY[0], -vecY[1], -vecY[2],
1813  0.0f, 0.0f, 0.0f ) );
1814  J2.SetSize( 5, 6 );
1815  }
1816  else {
1817  J2.Zero( 5, 6 );
1818  }
1819 
1820  cross = x1.Cross( x2 );
1821 
1822  c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecX );
1823  c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecY );
1824 
1825  c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
1826 
1827  if ( steering ) {
1828  steering->Add( physics, invTimeStep );
1829  }
1830  else if ( coneLimit ) {
1831  coneLimit->Add( physics, invTimeStep );
1832  }
1833 }
1834 
1835 /*
1836 ================
1837 idAFConstraint_Hinge::ApplyFriction
1838 ================
1839 */
1840 void idAFConstraint_Hinge::ApplyFriction( float invTimeStep ) {
1841  idVec3 angular;
1842  float invMass, currentFriction;
1843 
1844  currentFriction = GetFriction();
1845 
1846  if ( currentFriction <= 0.0f ) {
1847  return;
1848  }
1849 
1850  if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) {
1851 
1852  angular = body1->GetAngularVelocity();
1853  invMass = body1->GetInverseMass();
1854  if ( body2 ) {
1855  angular -= body2->GetAngularVelocity();
1856  invMass += body2->GetInverseMass();
1857  }
1858 
1859  angular *= currentFriction / invMass;
1860 
1861  body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
1862  if ( body2 ) {
1863  body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
1864  }
1865  }
1866  else {
1867  if ( !fc ) {
1868  fc = new idAFConstraint_HingeFriction;
1869  fc->Setup( this );
1870  }
1871 
1872  fc->Add( physics, invTimeStep );
1873  }
1874 }
1875 
1876 /*
1877 ================
1878 idAFConstraint_Hinge::GetForce
1879 ================
1880 */
1881 void idAFConstraint_Hinge::GetForce( idAFBody *body, idVec6 &force ) {
1882  idAFConstraint::GetForce( body, force );
1883  // FIXME: add limit force
1884 }
1885 
1886 /*
1887 ================
1888 idAFConstraint_Hinge::Translate
1889 ================
1890 */
1891 void idAFConstraint_Hinge::Translate( const idVec3 &translation ) {
1892  if ( !body2 ) {
1893  anchor2 += translation;
1894  }
1895  if ( coneLimit ) {
1896  coneLimit->Translate( translation );
1897  }
1898 }
1899 
1900 /*
1901 ================
1902 idAFConstraint_Hinge::Rotate
1903 ================
1904 */
1905 void idAFConstraint_Hinge::Rotate( const idRotation &rotation ) {
1906  if ( !body2 ) {
1907  anchor2 *= rotation;
1908  axis2 *= rotation.ToMat3();
1909  }
1910  if ( coneLimit ) {
1911  coneLimit->Rotate( rotation );
1912  }
1913 }
1914 
1915 /*
1916 ================
1917 idAFConstraint_Hinge::GetCenter
1918 ================
1919 */
1920 void idAFConstraint_Hinge::GetCenter( idVec3 &center ) {
1921  center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
1922 }
1923 
1924 /*
1925 ================
1926 idAFConstraint_Hinge::DebugDraw
1927 ================
1928 */
1929 void idAFConstraint_Hinge::DebugDraw( void ) {
1930  idVec3 vecX, vecY;
1931  idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
1932  idVec3 x1 = axis1 * body1->GetWorldAxis();
1933  x1.OrthogonalBasis( vecX, vecY );
1934 
1935  gameRenderWorld->DebugArrow( colorBlue, a1 - 4.0f * x1, a1 + 4.0f * x1, 1 );
1936  gameRenderWorld->DebugLine( colorBlue, a1 - 2.0f * vecX, a1 + 2.0f * vecX );
1937  gameRenderWorld->DebugLine( colorBlue, a1 - 2.0f * vecY, a1 + 2.0f * vecY );
1938 
1939  if ( af_showLimits.GetBool() ) {
1940  if ( coneLimit ) {
1941  coneLimit->DebugDraw();
1942  }
1943  }
1944 }
1945 
1946 /*
1947 ================
1948 idAFConstraint_Hinge::Save
1949 ================
1950 */
1951 void idAFConstraint_Hinge::Save( idSaveGame *saveFile ) const {
1952  idAFConstraint::Save( saveFile );
1953  saveFile->WriteVec3( anchor1 );
1954  saveFile->WriteVec3( anchor2 );
1955  saveFile->WriteVec3( axis1 );
1956  saveFile->WriteVec3( axis2 );
1957  saveFile->WriteMat3( initialAxis );
1958  saveFile->WriteFloat( friction );
1959  if ( coneLimit ) {
1960  saveFile->WriteBool( true );
1961  coneLimit->Save( saveFile );
1962  } else {
1963  saveFile->WriteBool( false );
1964  }
1965  if ( steering ) {
1966  saveFile->WriteBool( true );
1967  steering->Save( saveFile );
1968  } else {
1969  saveFile->WriteBool( false );
1970  }
1971  if ( fc ) {
1972  saveFile->WriteBool( true );
1973  fc->Save( saveFile );
1974  } else {
1975  saveFile->WriteBool( false );
1976  }
1977 }
1978 
1979 /*
1980 ================
1981 idAFConstraint_Hinge::Restore
1982 ================
1983 */
1984 void idAFConstraint_Hinge::Restore( idRestoreGame *saveFile ) {
1985  bool b;
1986  idAFConstraint::Restore( saveFile );
1987  saveFile->ReadVec3( anchor1 );
1988  saveFile->ReadVec3( anchor2 );
1989  saveFile->ReadVec3( axis1 );
1990  saveFile->ReadVec3( axis2 );
1991  saveFile->ReadMat3( initialAxis );
1992  saveFile->ReadFloat( friction );
1993 
1994  saveFile->ReadBool( b );
1995  if ( b ) {
1996  if ( !coneLimit ) {
1997  coneLimit = new idAFConstraint_ConeLimit;
1998  }
1999  coneLimit->SetPhysics( physics );
2000  coneLimit->Restore( saveFile );
2001  }
2002  saveFile->ReadBool( b );
2003  if ( b ) {
2004  if ( !steering ) {
2005  steering = new idAFConstraint_HingeSteering;
2006  }
2007  steering->Setup( this );
2008  steering->Restore( saveFile );
2009  }
2010  saveFile->ReadBool( b );
2011  if ( b ) {
2012  if ( !fc ) {
2013  fc = new idAFConstraint_HingeFriction;
2014  }
2015  fc->Setup( this );
2016  fc->Restore( saveFile );
2017  }
2018 }
2019 
2020 
2021 //===============================================================
2022 //
2023 // idAFConstraint_HingeFriction
2024 //
2025 //===============================================================
2026 
2027 /*
2028 ================
2029 idAFConstraint_HingeFriction::idAFConstraint_HingeFriction
2030 ================
2031 */
2032 idAFConstraint_HingeFriction::idAFConstraint_HingeFriction( void ) {
2033  type = CONSTRAINT_FRICTION;
2034  name = "hingeFriction";
2035  InitSize( 1 );
2036  hinge = NULL;
2037  fl.allowPrimary = false;
2038  fl.frameConstraint = true;
2039 }
2040 
2041 /*
2042 ================
2043 idAFConstraint_HingeFriction::Setup
2044 ================
2045 */
2046 void idAFConstraint_HingeFriction::Setup( idAFConstraint_Hinge *h ) {
2047  this->hinge = h;
2048  body1 = h->GetBody1();
2049  body2 = h->GetBody2();
2050 }
2051 
2052 /*
2053 ================
2054 idAFConstraint_HingeFriction::Evaluate
2055 ================
2056 */
2057 void idAFConstraint_HingeFriction::Evaluate( float invTimeStep ) {
2058  // do nothing
2059 }
2060 
2061 /*
2062 ================
2063 idAFConstraint_HingeFriction::ApplyFriction
2064 ================
2065 */
2066 void idAFConstraint_HingeFriction::ApplyFriction( float invTimeStep ) {
2067  // do nothing
2068 }
2069 
2070 /*
2071 ================
2072 idAFConstraint_HingeFriction::Add
2073 ================
2074 */
2075 bool idAFConstraint_HingeFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
2076  idVec3 a1, a2;
2077  float f;
2078 
2079  physics = phys;
2080 
2081  f = hinge->GetFriction() * hinge->GetMultiplier().Length();
2082  if ( f == 0.0f ) {
2083  return false;
2084  }
2085 
2086  lo[0] = -f;
2087  hi[0] = f;
2088 
2089  hinge->GetAxis( a1, a2 );
2090 
2091  a1 *= body1->GetWorldAxis();
2092 
2093  J1.SetSize( 1, 6 );
2094  J1.SubVec6(0).SubVec3(0).Zero();
2095  J1.SubVec6(0).SubVec3(1) = a1;
2096 
2097  if ( body2 ) {
2098  a2 *= body2->GetWorldAxis();
2099 
2100  J2.SetSize( 1, 6 );
2101  J2.SubVec6(0).SubVec3(0).Zero();
2102  J2.SubVec6(0).SubVec3(1) = -a2;
2103  }
2104 
2105  physics->AddFrameConstraint( this );
2106 
2107  return true;
2108 }
2109 
2110 /*
2111 ================
2112 idAFConstraint_HingeFriction::Translate
2113 ================
2114 */
2115 void idAFConstraint_HingeFriction::Translate( const idVec3 &translation ) {
2116 }
2117 
2118 /*
2119 ================
2120 idAFConstraint_HingeFriction::Rotate
2121 ================
2122 */
2123 void idAFConstraint_HingeFriction::Rotate( const idRotation &rotation ) {
2124 }
2125 
2126 
2127 //===============================================================
2128 //
2129 // idAFConstraint_HingeSteering
2130 //
2131 //===============================================================
2132 
2133 /*
2134 ================
2135 idAFConstraint_HingeSteering::idAFConstraint_HingeSteering
2136 ================
2137 */
2138 idAFConstraint_HingeSteering::idAFConstraint_HingeSteering( void ) {
2139  type = CONSTRAINT_HINGESTEERING;
2140  name = "hingeFriction";
2141  InitSize( 1 );
2142  hinge = NULL;
2143  fl.allowPrimary = false;
2144  fl.frameConstraint = true;
2145  steerSpeed = 0.0f;
2146  epsilon = LCP_EPSILON;
2147 }
2148 
2149 /*
2150 ================
2151 idAFConstraint_HingeSteering::Save
2152 ================
2153 */
2154 void idAFConstraint_HingeSteering::Save( idSaveGame *saveFile ) const {
2155  saveFile->WriteFloat(steerAngle);
2156  saveFile->WriteFloat(steerSpeed);
2157  saveFile->WriteFloat(epsilon);
2158 }
2159 
2160 /*
2161 ================
2162 idAFConstraint_HingeSteering::Restore
2163 ================
2164 */
2165 void idAFConstraint_HingeSteering::Restore( idRestoreGame *saveFile ) {
2166  saveFile->ReadFloat(steerAngle);
2167  saveFile->ReadFloat(steerSpeed);
2168  saveFile->ReadFloat(epsilon);
2169 }
2170 
2171 /*
2172 ================
2173 idAFConstraint_HingeSteering::Setup
2174 ================
2175 */
2176 void idAFConstraint_HingeSteering::Setup( idAFConstraint_Hinge *h ) {
2177  this->hinge = h;
2178  body1 = h->GetBody1();
2179  body2 = h->GetBody2();
2180 }
2181 
2182 /*
2183 ================
2184 idAFConstraint_HingeSteering::Evaluate
2185 ================
2186 */
2187 void idAFConstraint_HingeSteering::Evaluate( float invTimeStep ) {
2188  // do nothing
2189 }
2190 
2191 /*
2192 ================
2193 idAFConstraint_HingeSteering::ApplyFriction
2194 ================
2195 */
2196 void idAFConstraint_HingeSteering::ApplyFriction( float invTimeStep ) {
2197  // do nothing
2198 }
2199 
2200 /*
2201 ================
2202 idAFConstraint_HingeSteering::Add
2203 ================
2204 */
2205 bool idAFConstraint_HingeSteering::Add( idPhysics_AF *phys, float invTimeStep ) {
2206  float angle, speed;
2207  idVec3 a1, a2;
2208 
2209  physics = phys;
2210 
2211  hinge->GetAxis( a1, a2 );
2212  angle = hinge->GetAngle();
2213 
2214  a1 *= body1->GetWorldAxis();
2215 
2216  J1.SetSize( 1, 6 );
2217  J1.SubVec6(0).SubVec3(0).Zero();
2218  J1.SubVec6(0).SubVec3(1) = a1;
2219 
2220  if ( body2 ) {
2221  a2 *= body2->GetWorldAxis();
2222 
2223  J2.SetSize( 1, 6 );
2224  J2.SubVec6(0).SubVec3(0).Zero();
2225  J2.SubVec6(0).SubVec3(1) = -a2;
2226  }
2227 
2228  speed = steerAngle - angle;
2229  if ( steerSpeed != 0.0f ) {
2230  if ( speed > steerSpeed ) {
2231  speed = steerSpeed;
2232  }
2233  else if ( speed < -steerSpeed ) {
2234  speed = -steerSpeed;
2235  }
2236  }
2237 
2238  c1[0] = DEG2RAD( speed ) * invTimeStep;
2239 
2240  physics->AddFrameConstraint( this );
2241 
2242  return true;
2243 }
2244 
2245 /*
2246 ================
2247 idAFConstraint_HingeSteering::Translate
2248 ================
2249 */
2250 void idAFConstraint_HingeSteering::Translate( const idVec3 &translation ) {
2251 }
2252 
2253 /*
2254 ================
2255 idAFConstraint_HingeSteering::Rotate
2256 ================
2257 */
2258 void idAFConstraint_HingeSteering::Rotate( const idRotation &rotation ) {
2259 }
2260 
2261 
2262 //===============================================================
2263 //
2264 // idAFConstraint_Slider
2265 //
2266 //===============================================================
2267 
2268 /*
2269 ================
2270 idAFConstraint_Slider::idAFConstraint_Slider
2271 ================
2272 */
2273 idAFConstraint_Slider::idAFConstraint_Slider( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
2274  assert( body1 );
2275  type = CONSTRAINT_SLIDER;
2276  this->name = name;
2277  this->body1 = body1;
2278  this->body2 = body2;
2279  InitSize( 5 );
2280  fl.allowPrimary = true;
2281  fl.noCollision = true;
2282 
2283  if ( body2 ) {
2284  offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
2285  relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose();
2286  }
2287  else {
2288  offset = body1->GetWorldOrigin();
2289  relAxis = body1->GetWorldAxis();
2290  }
2291 }
2292 
2293 /*
2294 ================
2295 idAFConstraint_Slider::SetAxis
2296 ================
2297 */
2298 void idAFConstraint_Slider::SetAxis( const idVec3 &ax ) {
2299  idVec3 normAxis;
2300 
2301  // get normalized axis relative to body1
2302  normAxis = ax;
2303  normAxis.Normalize();
2304  if ( body2 ) {
2305  axis = normAxis * body2->GetWorldAxis().Transpose();
2306  }
2307  else {
2308  axis = normAxis;
2309  }
2310 }
2311 
2312 /*
2313 ================
2314 idAFConstraint_Slider::Evaluate
2315 ================
2316 */
2317 void idAFConstraint_Slider::Evaluate( float invTimeStep ) {
2318  idVec3 vecX, vecY, ofs;
2319  idRotation r;
2320  idAFBody *master;
2321 
2322  master = body2 ? body2 : physics->GetMasterBody();
2323 
2324  if ( master ) {
2325  (axis * master->GetWorldAxis()).OrthogonalBasis( vecX, vecY );
2326  ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
2327  r = ( body1->GetWorldAxis().Transpose() * (relAxis * master->GetWorldAxis()) ).ToRotation();
2328  }
2329  else {
2330  axis.OrthogonalBasis( vecX, vecY );
2331  ofs = offset - body1->GetWorldOrigin();
2332  r = ( body1->GetWorldAxis().Transpose() * relAxis ).ToRotation();
2333  }
2334 
2335  J1.Set( mat3_zero, mat3_identity,
2336  idMat3( vecX, vecY, vec3_origin ), mat3_zero );
2337  J1.SetSize( 5, 6 );
2338 
2339  if ( body2 ) {
2340 
2341  J2.Set( mat3_zero, -mat3_identity,
2342  idMat3( -vecX, -vecY, vec3_origin ), mat3_zero );
2343  J2.SetSize( 5, 6 );
2344  }
2345  else {
2346  J2.Zero( 5, 6 );
2347  }
2348 
2349  c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * - (float) DEG2RAD( r.GetAngle() ) );
2350 
2351  c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( vecX * ofs );
2352  c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( vecY * ofs );
2353 
2354  c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
2355 }
2356 
2357 /*
2358 ================
2359 idAFConstraint_Slider::ApplyFriction
2360 ================
2361 */
2362 void idAFConstraint_Slider::ApplyFriction( float invTimeStep ) {
2363  // no friction
2364 }
2365 
2366 /*
2367 ================
2368 idAFConstraint_Slider::Translate
2369 ================
2370 */
2371 void idAFConstraint_Slider::Translate( const idVec3 &translation ) {
2372  if ( !body2 ) {
2373  offset += translation;
2374  }
2375 }
2376 
2377 /*
2378 ================
2379 idAFConstraint_Slider::Rotate
2380 ================
2381 */
2382 void idAFConstraint_Slider::Rotate( const idRotation &rotation ) {
2383  if ( !body2 ) {
2384  offset *= rotation;
2385  }
2386 }
2387 
2388 /*
2389 ================
2390 idAFConstraint_Slider::GetCenter
2391 ================
2392 */
2393 void idAFConstraint_Slider::GetCenter( idVec3 &center ) {
2394  idAFBody *master;
2395 
2396  master = body2 ? body2 : physics->GetMasterBody();
2397  if ( master ) {
2398  center = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
2399  }
2400  else {
2401  center = offset - body1->GetWorldOrigin();
2402  }
2403 }
2404 
2405 /*
2406 ================
2407 idAFConstraint_Slider::DebugDraw
2408 ================
2409 */
2410 void idAFConstraint_Slider::DebugDraw( void ) {
2411  idVec3 ofs;
2412  idAFBody *master;
2413 
2414  master = body2 ? body2 : physics->GetMasterBody();
2415  if ( master ) {
2416  ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
2417  }
2418  else {
2419  ofs = offset - body1->GetWorldOrigin();
2420  }
2421  gameRenderWorld->DebugLine( colorGreen, ofs, ofs + axis * body1->GetWorldAxis() );
2422 }
2423 
2424 /*
2425 ================
2426 idAFConstraint_Slider::Save
2427 ================
2428 */
2429 void idAFConstraint_Slider::Save( idSaveGame *saveFile ) const {
2430  idAFConstraint::Save( saveFile );
2431  saveFile->WriteVec3( axis );
2432  saveFile->WriteVec3( offset );
2433  saveFile->WriteMat3( relAxis );
2434 }
2435 
2436 /*
2437 ================
2438 idAFConstraint_Slider::Restore
2439 ================
2440 */
2441 void idAFConstraint_Slider::Restore( idRestoreGame *saveFile ) {
2442  idAFConstraint::Restore( saveFile );
2443  saveFile->ReadVec3( axis );
2444  saveFile->ReadVec3( offset );
2445  saveFile->ReadMat3( relAxis );
2446 }
2447 
2448 
2449 //===============================================================
2450 //
2451 // idAFConstraint_Line
2452 //
2453 //===============================================================
2454 
2455 /*
2456 ================
2457 idAFConstraint_Line::idAFConstraint_Line
2458 ================
2459 */
2460 idAFConstraint_Line::idAFConstraint_Line( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
2461  assert( 0 ); // FIXME: implement
2462 }
2463 
2464 /*
2465 ================
2466 idAFConstraint_Line::Evaluate
2467 ================
2468 */
2469 void idAFConstraint_Line::Evaluate( float invTimeStep ) {
2470  assert( 0 ); // FIXME: implement
2471 }
2472 
2473 /*
2474 ================
2475 idAFConstraint_Line::ApplyFriction
2476 ================
2477 */
2478 void idAFConstraint_Line::ApplyFriction( float invTimeStep ) {
2479  assert( 0 ); // FIXME: implement
2480 }
2481 
2482 /*
2483 ================
2484 idAFConstraint_Line::Translate
2485 ================
2486 */
2487 void idAFConstraint_Line::Translate( const idVec3 &translation ) {
2488  assert( 0 ); // FIXME: implement
2489 }
2490 
2491 /*
2492 ================
2493 idAFConstraint_Line::Rotate
2494 ================
2495 */
2496 void idAFConstraint_Line::Rotate( const idRotation &rotation ) {
2497  assert( 0 ); // FIXME: implement
2498 }
2499 
2500 /*
2501 ================
2502 idAFConstraint_Line::DebugDraw
2503 ================
2504 */
2505 void idAFConstraint_Line::DebugDraw( void ) {
2506  assert( 0 ); // FIXME: implement
2507 }
2508 
2509 
2510 //===============================================================
2511 //
2512 // idAFConstraint_Plane
2513 //
2514 //===============================================================
2515 
2516 /*
2517 ================
2518 idAFConstraint_Plane::idAFConstraint_Plane
2519 ================
2520 */
2521 idAFConstraint_Plane::idAFConstraint_Plane( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
2522  assert( body1 );
2523  type = CONSTRAINT_PLANE;
2524  this->name = name;
2525  this->body1 = body1;
2526  this->body2 = body2;
2527  InitSize( 1 );
2528  fl.allowPrimary = true;
2529  fl.noCollision = true;
2530 }
2531 
2532 /*
2533 ================
2534 idAFConstraint_Plane::SetPlane
2535 ================
2536 */
2537 void idAFConstraint_Plane::SetPlane( const idVec3 &normal, const idVec3 &anchor ) {
2538  // get anchor relative to center of mass of body1
2539  anchor1 = ( anchor - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
2540  if ( body2 ) {
2541  // get anchor relative to center of mass of body2
2542  anchor2 = ( anchor - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
2543  planeNormal = normal * body2->GetWorldAxis().Transpose();
2544  }
2545  else {
2546  anchor2 = anchor;
2547  planeNormal = normal;
2548  }
2549 }
2550 
2551 /*
2552 ================
2553 idAFConstraint_Plane::Evaluate
2554 ================
2555 */
2556 void idAFConstraint_Plane::Evaluate( float invTimeStep ) {
2557  idVec3 a1, a2, normal, p;
2558  idVec6 v;
2559  idAFBody *master;
2560 
2561  master = body2 ? body2 : physics->GetMasterBody();
2562 
2563  a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
2564  if ( master ) {
2565  a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
2566  normal = planeNormal * master->GetWorldAxis();
2567  }
2568  else {
2569  a2 = anchor2;
2570  normal = planeNormal;
2571  }
2572 
2573  p = a1 - body1->GetWorldOrigin();
2574  v.SubVec3(0) = normal;
2575  v.SubVec3(1) = p.Cross( normal );
2576  J1.Set( 1, 6, v.ToFloatPtr() );
2577 
2578  if ( body2 ) {
2579  p = a1 - body2->GetWorldOrigin();
2580  v.SubVec3(0) = -normal;
2581  v.SubVec3(1) = p.Cross( -normal );
2582  J2.Set( 1, 6, v.ToFloatPtr() );
2583  }
2584 
2585  c1[0] = -( invTimeStep * ERROR_REDUCTION ) * (a1 * normal - a2 * normal);
2586 
2587  c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
2588 }
2589 
2590 /*
2591 ================
2592 idAFConstraint_Plane::ApplyFriction
2593 ================
2594 */
2595 void idAFConstraint_Plane::ApplyFriction( float invTimeStep ) {
2596  // no friction
2597 }
2598 
2599 /*
2600 ================
2601 idAFConstraint_Plane::Translate
2602 ================
2603 */
2604 void idAFConstraint_Plane::Translate( const idVec3 &translation ) {
2605  if ( !body2 ) {
2606  anchor2 += translation;
2607  }
2608 }
2609 
2610 /*
2611 ================
2612 idAFConstraint_Plane::Rotate
2613 ================
2614 */
2615 void idAFConstraint_Plane::Rotate( const idRotation &rotation ) {
2616  if ( !body2 ) {
2617  anchor2 *= rotation;
2618  planeNormal *= rotation.ToMat3();
2619  }
2620 }
2621 
2622 /*
2623 ================
2624 idAFConstraint_Plane::DebugDraw
2625 ================
2626 */
2627 void idAFConstraint_Plane::DebugDraw( void ) {
2628  idVec3 a1, normal, right, up;
2629  idAFBody *master;
2630 
2631  master = body2 ? body2 : physics->GetMasterBody();
2632 
2633  a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
2634  if ( master ) {
2635  normal = planeNormal * master->GetWorldAxis();
2636  }
2637  else {
2638  normal = planeNormal;
2639  }
2640  normal.NormalVectors( right, up );
2641  normal *= 4.0f;
2642  right *= 4.0f;
2643  up *= 4.0f;
2644 
2645  gameRenderWorld->DebugLine( colorCyan, a1 - right, a1 + right );
2646  gameRenderWorld->DebugLine( colorCyan, a1 - up, a1 + up );
2647  gameRenderWorld->DebugArrow( colorCyan, a1, a1 + normal, 1 );
2648 }
2649 
2650 /*
2651 ================
2652 idAFConstraint_Plane::Save
2653 ================
2654 */
2655 void idAFConstraint_Plane::Save( idSaveGame *saveFile ) const {
2656  idAFConstraint::Save( saveFile );
2657  saveFile->WriteVec3( anchor1 );
2658  saveFile->WriteVec3( anchor2 );
2659  saveFile->WriteVec3( planeNormal );
2660 }
2661 
2662 /*
2663 ================
2664 idAFConstraint_Plane::Restore
2665 ================
2666 */
2667 void idAFConstraint_Plane::Restore( idRestoreGame *saveFile ) {
2668  idAFConstraint::Restore( saveFile );
2669  saveFile->ReadVec3( anchor1 );
2670  saveFile->ReadVec3( anchor2 );
2671  saveFile->ReadVec3( planeNormal );
2672 }
2673 
2674 
2675 //===============================================================
2676 //
2677 // idAFConstraint_Spring
2678 //
2679 //===============================================================
2680 
2681 /*
2682 ================
2683 idAFConstraint_Spring::idAFConstraint_Spring
2684 ================
2685 */
2686 idAFConstraint_Spring::idAFConstraint_Spring( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
2687  assert( body1 );
2688  type = CONSTRAINT_SPRING;
2689  this->name = name;
2690  this->body1 = body1;
2691  this->body2 = body2;
2692  InitSize( 1 );
2693  fl.allowPrimary = false;
2694  kstretch = kcompress = damping = 1.0f;
2695  minLength = maxLength = restLength = 0.0f;
2696 }
2697 
2698 /*
2699 ================
2700 idAFConstraint_Spring::SetAnchor
2701 ================
2702 */
2703 void idAFConstraint_Spring::SetAnchor( const idVec3 &worldAnchor1, const idVec3 &worldAnchor2 ) {
2704  // get anchor relative to center of mass of body1
2705  anchor1 = ( worldAnchor1 - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
2706  if ( body2 ) {
2707  // get anchor relative to center of mass of body2
2708  anchor2 = ( worldAnchor2 - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
2709  }
2710  else {
2711  anchor2 = worldAnchor2;
2712  }
2713 }
2714 
2715 /*
2716 ================
2717 idAFConstraint_Spring::SetSpring
2718 ================
2719 */
2720 void idAFConstraint_Spring::SetSpring( const float stretch, const float compress, const float damping, const float restLength ) {
2721  assert( stretch >= 0.0f && compress >= 0.0f && restLength >= 0.0f );
2722  this->kstretch = stretch;
2723  this->kcompress = compress;
2724  this->damping = damping;
2725  this->restLength = restLength;
2726 }
2727 
2728 /*
2729 ================
2730 idAFConstraint_Spring::SetLimit
2731 ================
2732 */
2733 void idAFConstraint_Spring::SetLimit( const float minLength, const float maxLength ) {
2734  assert( minLength >= 0.0f && maxLength >= 0.0f && maxLength >= minLength );
2735  this->minLength = minLength;
2736  this->maxLength = maxLength;
2737 }
2738 
2739 /*
2740 ================
2741 idAFConstraint_Spring::Evaluate
2742 ================
2743 */
2744 void idAFConstraint_Spring::Evaluate( float invTimeStep ) {
2745  idVec3 a1, a2, velocity1, velocity2, force;
2746  idVec6 v1, v2;
2747  float d, dampingForce, length, error;
2748  bool limit;
2749  idAFBody *master;
2750 
2751  master = body2 ? body2 : physics->GetMasterBody();
2752 
2753  a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
2754  velocity1 = body1->GetPointVelocity( a1 );
2755 
2756  if ( master ) {
2757  a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
2758  velocity2 = master->GetPointVelocity( a2 );
2759  }
2760  else {
2761  a2 = anchor2;
2762  velocity2.Zero();
2763  }
2764 
2765  force = a2 - a1;
2766  d = force * force;
2767  if ( d != 0.0f ) {
2768  dampingForce = damping * idMath::Fabs( (velocity2 - velocity1) * force ) / d;
2769  }
2770  else {
2771  dampingForce = 0.0f;
2772  }
2773  length = force.Normalize();
2774 
2775  if ( length > restLength ) {
2776  if ( kstretch > 0.0f ) {
2777  idVec3 springForce = force * ( Square( length - restLength ) * kstretch - dampingForce );
2778  body1->AddForce( a1, springForce );
2779  if ( master ) {
2780  master->AddForce( a2, -springForce );
2781  }
2782  }
2783  }
2784  else {
2785  if ( kcompress > 0.0f ) {
2786  idVec3 springForce = force * -( Square( restLength - length ) * kcompress - dampingForce );
2787  body1->AddForce( a1, springForce );
2788  if ( master ) {
2789  master->AddForce( a2, -springForce );
2790  }
2791  }
2792  }
2793 
2794  // check for spring limits
2795  if ( length < minLength ) {
2796  force = -force;
2797  error = minLength - length;
2798  limit = true;
2799  }
2800  else if ( maxLength > 0.0f && length > maxLength ) {
2801  error = length - maxLength;
2802  limit = true;
2803  }
2804  else {
2805  error = 0.0f;
2806  limit = false;
2807  }
2808 
2809  if ( limit ) {
2810  a1 -= body1->GetWorldOrigin();
2811  v1.SubVec3(0) = force;
2812  v1.SubVec3(1) = a1.Cross( force );
2813  J1.Set( 1, 6, v1.ToFloatPtr() );
2814  if ( body2 ) {
2815  a2 -= body2->GetWorldOrigin();
2816  v2.SubVec3(0) = -force;
2817  v2.SubVec3(1) = a2.Cross( -force );
2818  J2.Set( 1, 6, v2.ToFloatPtr() );
2819  }
2820  c1[0] = -( invTimeStep * ERROR_REDUCTION ) * error;
2821  lo[0] = 0.0f;
2822  }
2823  else {
2824  J1.Zero( 0, 0 );
2825  J2.Zero( 0, 0 );
2826  }
2827 
2828  c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
2829 }
2830 
2831 /*
2832 ================
2833 idAFConstraint_Spring::ApplyFriction
2834 ================
2835 */
2836 void idAFConstraint_Spring::ApplyFriction( float invTimeStep ) {
2837  // no friction
2838 }
2839 
2840 /*
2841 ================
2842 idAFConstraint_Spring::Translate
2843 ================
2844 */
2845 void idAFConstraint_Spring::Translate( const idVec3 &translation ) {
2846  if ( !body2 ) {
2847  anchor2 += translation;
2848  }
2849 }
2850 
2851 /*
2852 ================
2853 idAFConstraint_Spring::Rotate
2854 ================
2855 */
2856 void idAFConstraint_Spring::Rotate( const idRotation &rotation ) {
2857  if ( !body2 ) {
2858  anchor2 *= rotation;
2859  }
2860 }
2861 
2862 /*
2863 ================
2864 idAFConstraint_Spring::GetCenter
2865 ================
2866 */
2867 void idAFConstraint_Spring::GetCenter( idVec3 &center ) {
2868  idAFBody *master;
2869  idVec3 a1, a2;
2870 
2871  master = body2 ? body2 : physics->GetMasterBody();
2872  a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
2873  if ( master ) {
2874  a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
2875  }
2876  else {
2877  a2 = anchor2;
2878  }
2879  center = ( a1 + a2 ) * 0.5f;
2880 }
2881 
2882 /*
2883 ================
2884 idAFConstraint_Spring::DebugDraw
2885 ================
2886 */
2887 void idAFConstraint_Spring::DebugDraw( void ) {
2888  idAFBody *master;
2889  float length;
2890  idVec3 a1, a2, dir, mid, p;
2891 
2892  master = body2 ? body2 : physics->GetMasterBody();
2893  a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
2894  if ( master ) {
2895  a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
2896  }
2897  else {
2898  a2 = anchor2;
2899  }
2900  dir = a2 - a1;
2901  mid = a1 + 0.5f * dir;
2902  length = dir.Normalize();
2903 
2904  // draw spring
2905  gameRenderWorld->DebugLine( colorGreen, a1, a2 );
2906 
2907  // draw rest length
2908  p = restLength * 0.5f * dir;
2909  gameRenderWorld->DebugCircle( colorWhite, mid + p, dir, 1.0f, 10 );
2910  gameRenderWorld->DebugCircle( colorWhite, mid - p, dir, 1.0f, 10 );
2911  if ( restLength > length ) {
2912  gameRenderWorld->DebugLine( colorWhite, a2, mid + p );
2913  gameRenderWorld->DebugLine( colorWhite, a1, mid - p );
2914  }
2915 
2916  if ( minLength > 0.0f ) {
2917  // draw min length
2918  gameRenderWorld->DebugCircle( colorBlue, mid + minLength * 0.5f * dir, dir, 2.0f, 10 );
2919  gameRenderWorld->DebugCircle( colorBlue, mid - minLength * 0.5f * dir, dir, 2.0f, 10 );
2920  }
2921 
2922  if ( maxLength > 0.0f ) {
2923  // draw max length
2924  gameRenderWorld->DebugCircle( colorRed, mid + maxLength * 0.5f * dir, dir, 2.0f, 10 );
2925  gameRenderWorld->DebugCircle( colorRed, mid - maxLength * 0.5f * dir, dir, 2.0f, 10 );
2926  }
2927 }
2928 
2929 /*
2930 ================
2931 idAFConstraint_Spring::Save
2932 ================
2933 */
2934 void idAFConstraint_Spring::Save( idSaveGame *saveFile ) const {
2935  idAFConstraint::Save( saveFile );
2936  saveFile->WriteVec3( anchor1 );
2937  saveFile->WriteVec3( anchor2 );
2938  saveFile->WriteFloat( kstretch );
2939  saveFile->WriteFloat( kcompress );
2940  saveFile->WriteFloat( damping );
2941  saveFile->WriteFloat( restLength );
2942  saveFile->WriteFloat( minLength );
2943  saveFile->WriteFloat( maxLength );
2944 }
2945 
2946 /*
2947 ================
2948 idAFConstraint_Spring::Restore
2949 ================
2950 */
2951 void idAFConstraint_Spring::Restore( idRestoreGame *saveFile ) {
2952  idAFConstraint::Restore( saveFile );
2953  saveFile->ReadVec3( anchor1 );
2954  saveFile->ReadVec3( anchor2 );
2955  saveFile->ReadFloat( kstretch );
2956  saveFile->ReadFloat( kcompress );
2957  saveFile->ReadFloat( damping );
2958  saveFile->ReadFloat( restLength );
2959  saveFile->ReadFloat( minLength );
2960  saveFile->ReadFloat( maxLength );
2961 }
2962 
2963 
2964 //===============================================================
2965 //
2966 // idAFConstraint_Contact
2967 //
2968 //===============================================================
2969 
2970 /*
2971 ================
2972 idAFConstraint_Contact::idAFConstraint_Contact
2973 ================
2974 */
2975 idAFConstraint_Contact::idAFConstraint_Contact( void ) {
2976  name = "contact";
2977  type = CONSTRAINT_CONTACT;
2978  InitSize( 1 );
2979  fc = NULL;
2980  fl.allowPrimary = false;
2981  fl.frameConstraint = true;
2982 }
2983 
2984 /*
2985 ================
2986 idAFConstraint_Contact::~idAFConstraint_Contact
2987 ================
2988 */
2989 idAFConstraint_Contact::~idAFConstraint_Contact( void ) {
2990  if ( fc ) {
2991  delete fc;
2992  }
2993 }
2994 
2995 /*
2996 ================
2997 idAFConstraint_Contact::Setup
2998 ================
2999 */
3000 void idAFConstraint_Contact::Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c ) {
3001  idVec3 p;
3002  idVec6 v;
3003  float vel;
3004  float minBounceVelocity = 2.0f;
3005 
3006  assert( b1 );
3007 
3008  body1 = b1;
3009  body2 = b2;
3010  contact = c;
3011 
3012  p = c.point - body1->GetWorldOrigin();
3013  v.SubVec3(0) = c.normal;
3014  v.SubVec3(1) = p.Cross( c.normal );
3015  J1.Set( 1, 6, v.ToFloatPtr() );
3016  vel = v.SubVec3(0) * body1->GetLinearVelocity() + v.SubVec3(1) * body1->GetAngularVelocity();
3017 
3018  if ( body2 ) {
3019  p = c.point - body2->GetWorldOrigin();
3020  v.SubVec3(0) = -c.normal;
3021  v.SubVec3(1) = p.Cross( -c.normal );
3022  J2.Set( 1, 6, v.ToFloatPtr() );
3023  vel += v.SubVec3(0) * body2->GetLinearVelocity() + v.SubVec3(1) * body2->GetAngularVelocity();
3024  c2[0] = 0.0f;
3025  }
3026 
3027  if ( body1->GetBouncyness() > 0.0f && -vel > minBounceVelocity ) {
3028  c1[0] = body1->GetBouncyness() * vel;
3029  }
3030  else {
3031  c1[0] = 0.0f;
3032  }
3033 
3034  e[0] = CONTACT_LCP_EPSILON;
3035  lo[0] = 0.0f;
3036  hi[0] = idMath::INFINITY;
3037  boxConstraint = NULL;
3038  boxIndex[0] = -1;
3039 }
3040 
3041 /*
3042 ================
3043 idAFConstraint_Contact::Evaluate
3044 ================
3045 */
3046 void idAFConstraint_Contact::Evaluate( float invTimeStep ) {
3047  // do nothing
3048 }
3049 
3050 /*
3051 ================
3052 idAFConstraint_Contact::ApplyFriction
3053 ================
3054 */
3055 void idAFConstraint_Contact::ApplyFriction( float invTimeStep ) {
3056  idVec3 r, velocity, normal, dir1, dir2;
3057  float friction, magnitude, forceNumerator, forceDenominator;
3058  idVecX impulse, dv;
3059 
3060  friction = body1->GetContactFriction();
3061  if ( body2 && body2->GetContactFriction() < friction ) {
3062  friction = body2->GetContactFriction();
3063  }
3064 
3065  friction *= physics->GetContactFrictionScale();
3066 
3067  if ( friction <= 0.0f ) {
3068  return;
3069  }
3070 
3071  // seperate friction per contact is silly but it's fast and often looks close enough
3072  if ( af_useImpulseFriction.GetBool() ) {
3073 
3074  impulse.SetData( 6, VECX_ALLOCA( 6 ) );
3075  dv.SetData( 6, VECX_ALLOCA( 6 ) );
3076 
3077  // calculate velocity in the contact plane
3078  r = contact.point - body1->GetWorldOrigin();
3079  velocity = body1->GetLinearVelocity() + body1->GetAngularVelocity().Cross( r );
3080  velocity -= contact.normal * velocity * contact.normal;
3081 
3082  // get normalized direction of friction and magnitude of velocity
3083  normal = -velocity;
3084  magnitude = normal.Normalize();
3085 
3086  forceNumerator = friction * magnitude;
3087  forceDenominator = body1->GetInverseMass() + ( ( body1->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal );
3088  impulse.SubVec3(0) = (forceNumerator / forceDenominator) * normal;
3089  impulse.SubVec3(1) = r.Cross( impulse.SubVec3(0) );
3090  body1->InverseWorldSpatialInertiaMultiply( dv, impulse.ToFloatPtr() );
3091 
3092  // modify velocity with friction force
3093  body1->SetLinearVelocity( body1->GetLinearVelocity() + dv.SubVec3(0) );
3094  body1->SetAngularVelocity( body1->GetAngularVelocity() + dv.SubVec3(1) );
3095  }
3096  else {
3097 
3098  if ( !fc ) {
3099  fc = new idAFConstraint_ContactFriction;
3100  }
3101  // call setup each frame because contact constraints are re-used for different bodies
3102  fc->Setup( this );
3103  fc->Add( physics, invTimeStep );
3104  }
3105 }
3106 
3107 /*
3108 ================
3109 idAFConstraint_Contact::Translate
3110 ================
3111 */
3112 void idAFConstraint_Contact::Translate( const idVec3 &translation ) {
3113  assert( 0 ); // contact should never be translated
3114 }
3115 
3116 /*
3117 ================
3118 idAFConstraint_Contact::Rotate
3119 ================
3120 */
3121 void idAFConstraint_Contact::Rotate( const idRotation &rotation ) {
3122  assert( 0 ); // contact should never be rotated
3123 }
3124 
3125 /*
3126 ================
3127 idAFConstraint_Contact::GetCenter
3128 ================
3129 */
3130 void idAFConstraint_Contact::GetCenter( idVec3 &center ) {
3131  center = contact.point;
3132 }
3133 
3134 /*
3135 ================
3136 idAFConstraint_Contact::DebugDraw
3137 ================
3138 */
3139 void idAFConstraint_Contact::DebugDraw( void ) {
3140  idVec3 x, y;
3141  contact.normal.NormalVectors( x, y );
3142  gameRenderWorld->DebugLine( colorWhite, contact.point, contact.point + 6.0f * contact.normal );
3143  gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * x, contact.point + 2.0f * x );
3144  gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * y, contact.point + 2.0f * y );
3145 }
3146 
3147 
3148 //===============================================================
3149 //
3150 // idAFConstraint_ContactFriction
3151 //
3152 //===============================================================
3153 
3154 /*
3155 ================
3156 idAFConstraint_ContactFriction::idAFConstraint_ContactFriction
3157 ================
3158 */
3159 idAFConstraint_ContactFriction::idAFConstraint_ContactFriction( void ) {
3160  type = CONSTRAINT_FRICTION;
3161  name = "contactFriction";
3162  InitSize( 2 );
3163  cc = NULL;
3164  fl.allowPrimary = false;
3165  fl.frameConstraint = true;
3166 }
3167 
3168 /*
3169 ================
3170 idAFConstraint_ContactFriction::Setup
3171 ================
3172 */
3173 void idAFConstraint_ContactFriction::Setup( idAFConstraint_Contact *cc ) {
3174  this->cc = cc;
3175  body1 = cc->GetBody1();
3176  body2 = cc->GetBody2();
3177 }
3178 
3179 /*
3180 ================
3181 idAFConstraint_ContactFriction::Evaluate
3182 ================
3183 */
3184 void idAFConstraint_ContactFriction::Evaluate( float invTimeStep ) {
3185  // do nothing
3186 }
3187 
3188 /*
3189 ================
3190 idAFConstraint_ContactFriction::ApplyFriction
3191 ================
3192 */
3193 void idAFConstraint_ContactFriction::ApplyFriction( float invTimeStep ) {
3194  // do nothing
3195 }
3196 
3197 /*
3198 ================
3199 idAFConstraint_ContactFriction::Add
3200 ================
3201 */
3202 bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
3203  idVec3 r, dir1, dir2;
3204  float friction;
3205  int newRow;
3206 
3207  physics = phys;
3208 
3209  friction = body1->GetContactFriction() * physics->GetContactFrictionScale();
3210 
3211  // if the body only has friction in one direction
3212  if ( body1->GetFrictionDirection( dir1 ) ) {
3213  // project the friction direction into the contact plane
3214  dir1 -= dir1 * cc->GetContact().normal * dir1;
3215  dir1.Normalize();
3216 
3217  r = cc->GetContact().point - body1->GetWorldOrigin();
3218 
3219  J1.SetSize( 1, 6 );
3220  J1.SubVec6(0).SubVec3(0) = dir1;
3221  J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 );
3222  c1.SetSize( 1 );
3223  c1[0] = 0.0f;
3224 
3225  if ( body2 ) {
3226  r = cc->GetContact().point - body2->GetWorldOrigin();
3227 
3228  J2.SetSize( 1, 6 );
3229  J2.SubVec6(0).SubVec3(0) = -dir1;
3230  J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 );
3231  c2.SetSize( 1 );
3232  c2[0] = 0.0f;
3233  }
3234 
3235  lo[0] = -friction;
3236  hi[0] = friction;
3237  boxConstraint = cc;
3238  boxIndex[0] = 0;
3239  }
3240  else {
3241  // get two friction directions orthogonal to contact normal
3242  cc->GetContact().normal.NormalVectors( dir1, dir2 );
3243 
3244  r = cc->GetContact().point - body1->GetWorldOrigin();
3245 
3246  J1.SetSize( 2, 6 );
3247  J1.SubVec6(0).SubVec3(0) = dir1;
3248  J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 );
3249  J1.SubVec6(1).SubVec3(0) = dir2;
3250  J1.SubVec6(1).SubVec3(1) = r.Cross( dir2 );
3251  c1.SetSize( 2 );
3252  c1[0] = c1[1] = 0.0f;
3253 
3254  if ( body2 ) {
3255  r = cc->GetContact().point - body2->GetWorldOrigin();
3256 
3257  J2.SetSize( 2, 6 );
3258  J2.SubVec6(0).SubVec3(0) = -dir1;
3259  J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 );
3260  J2.SubVec6(1).SubVec3(0) = -dir2;
3261  J2.SubVec6(1).SubVec3(1) = r.Cross( -dir2 );
3262  c2.SetSize( 2 );
3263  c2[0] = c2[1] = 0.0f;
3264 
3265  if ( body2->GetContactFriction() < friction ) {
3266  friction = body2->GetContactFriction();
3267  }
3268  }
3269 
3270  lo[0] = -friction;
3271  hi[0] = friction;
3272  boxConstraint = cc;
3273  boxIndex[0] = 0;
3274  lo[1] = -friction;
3275  hi[1] = friction;
3276  boxIndex[1] = 0;
3277  }
3278 
3279  if ( body1->GetContactMotorDirection( dir1 ) && body1->GetContactMotorForce() > 0.0f ) {
3280  // project the motor force direction into the contact plane
3281  dir1 -= dir1 * cc->GetContact().normal * dir1;
3282  dir1.Normalize();
3283 
3284  r = cc->GetContact().point - body1->GetWorldOrigin();
3285 
3286  newRow = J1.GetNumRows();
3287  J1.ChangeSize( newRow+1, J1.GetNumColumns() );
3288  J1.SubVec6(newRow).SubVec3(0) = -dir1;
3289  J1.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 );
3290  c1.ChangeSize( newRow+1 );
3291  c1[newRow] = body1->GetContactMotorVelocity();
3292 
3293  if ( body2 ) {
3294  r = cc->GetContact().point - body2->GetWorldOrigin();
3295 
3296  J2.ChangeSize( newRow+1, J2.GetNumColumns() );
3297  J2.SubVec6(newRow).SubVec3(0) = -dir1;
3298  J2.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 );
3299  c2.ChangeSize( newRow+1 );
3300  c2[newRow] = 0.0f;
3301  }
3302 
3303  lo[newRow] = -body1->GetContactMotorForce();
3304  hi[newRow] = body1->GetContactMotorForce();
3305  boxIndex[newRow] = -1;
3306  }
3307 
3308  physics->AddFrameConstraint( this );
3309 
3310  return true;
3311 }
3312 
3313 /*
3314 ================
3315 idAFConstraint_ContactFriction::Translate
3316 ================
3317 */
3318 void idAFConstraint_ContactFriction::Translate( const idVec3 &translation ) {
3319 }
3320 
3321 /*
3322 ================
3323 idAFConstraint_ContactFriction::Rotate
3324 ================
3325 */
3326 void idAFConstraint_ContactFriction::Rotate( const idRotation &rotation ) {
3327 }
3328 
3329 /*
3330 ================
3331 idAFConstraint_ContactFriction::DebugDraw
3332 ================
3333 */
3334 void idAFConstraint_ContactFriction::DebugDraw( void ) {
3335 }
3336 
3337 
3338 //===============================================================
3339 //
3340 // idAFConstraint_ConeLimit
3341 //
3342 //===============================================================
3343 
3344 /*
3345 ================
3346 idAFConstraint_ConeLimit::idAFConstraint_ConeLimit
3347 ================
3348 */
3349 idAFConstraint_ConeLimit::idAFConstraint_ConeLimit( void ) {
3350  type = CONSTRAINT_CONELIMIT;
3351  name = "coneLimit";
3352  InitSize( 1 );
3353  fl.allowPrimary = false;
3354  fl.frameConstraint = true;
3355 }
3356 
3357 /*
3358 ================
3359 idAFConstraint_ConeLimit::Setup
3360 
3361  the coneAnchor is the top of the cone in body2 space
3362  the coneAxis is the axis of the cone in body2 space
3363  the coneAngle is the angle the cone hull makes at the top
3364  the body1Axis is the axis in body1 space that should stay within the cone
3365 ================
3366 */
3367 void idAFConstraint_ConeLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) {
3368  this->body1 = b1;
3369  this->body2 = b2;
3370  this->coneAxis = coneAxis;
3371  this->coneAxis.Normalize();
3372  this->coneAnchor = coneAnchor;
3373  this->body1Axis = body1Axis;
3374  this->body1Axis.Normalize();
3375  this->cosAngle = (float) cos( DEG2RAD( coneAngle * 0.5f ) );
3376  this->sinHalfAngle = (float) sin( DEG2RAD( coneAngle * 0.25f ) );
3377  this->cosHalfAngle = (float) cos( DEG2RAD( coneAngle * 0.25f ) );
3378 }
3379 
3380 /*
3381 ================
3382 idAFConstraint_ConeLimit::SetAnchor
3383 ================
3384 */
3385 void idAFConstraint_ConeLimit::SetAnchor( const idVec3 &coneAnchor ) {
3386  this->coneAnchor = coneAnchor;
3387 }
3388 
3389 /*
3390 ================
3391 idAFConstraint_ConeLimit::SetBody1Axis
3392 ================
3393 */
3394 void idAFConstraint_ConeLimit::SetBody1Axis( const idVec3 &body1Axis ) {
3395  this->body1Axis = body1Axis;
3396 }
3397 
3398 /*
3399 ================
3400 idAFConstraint_ConeLimit::Evaluate
3401 ================
3402 */
3403 void idAFConstraint_ConeLimit::Evaluate( float invTimeStep ) {
3404  // do nothing
3405 }
3406 
3407 /*
3408 ================
3409 idAFConstraint_ConeLimit::ApplyFriction
3410 ================
3411 */
3412 void idAFConstraint_ConeLimit::ApplyFriction( float invTimeStep ) {
3413 }
3414 
3415 /*
3416 ================
3417 idAFConstraint_ConeLimit::Add
3418 ================
3419 */
3420 bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) {
3421  float a;
3422  idVec6 J1row, J2row;
3423  idVec3 ax, anchor, body1ax, normal, coneVector, p1, p2;
3424  idQuat q;
3425  idAFBody *master;
3426 
3427  if ( af_skipLimits.GetBool() ) {
3428  lm.Zero(); // constraint exerts no force
3429  return false;
3430  }
3431 
3432  physics = phys;
3433 
3434  master = body2 ? body2 : physics->GetMasterBody();
3435 
3436  if ( master ) {
3437  ax = coneAxis * master->GetWorldAxis();
3438  anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis();
3439  }
3440  else {
3441  ax = coneAxis;
3442  anchor = coneAnchor;
3443  }
3444 
3445  body1ax = body1Axis * body1->GetWorldAxis();
3446 
3447  a = ax * body1ax;
3448 
3449  // if the body1 axis is inside the cone
3450  if ( a > cosAngle ) {
3451  lm.Zero(); // constraint exerts no force
3452  return false;
3453  }
3454 
3455  // calculate the inward cone normal for the position the body1 axis went outside the cone
3456  normal = body1ax.Cross( ax );
3457  normal.Normalize();
3458  q.x = normal.x * sinHalfAngle;
3459  q.y = normal.y * sinHalfAngle;
3460  q.z = normal.z * sinHalfAngle;
3461  q.w = cosHalfAngle;
3462  coneVector = ax * q.ToMat3();
3463  normal = coneVector.Cross( ax ).Cross( coneVector );
3464  normal.Normalize();
3465 
3466  p1 = anchor + 32.0f * coneVector - body1->GetWorldOrigin();
3467 
3468  J1row.SubVec3(0) = normal;
3469  J1row.SubVec3(1) = p1.Cross( normal );
3470  J1.Set( 1, 6, J1row.ToFloatPtr() );
3471 
3472  c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) );
3473 
3474  if ( body2 ) {
3475 
3476  p2 = anchor + 32.0f * coneVector - master->GetWorldOrigin();
3477 
3478  J2row.SubVec3(0) = -normal;
3479  J2row.SubVec3(1) = p2.Cross( -normal );
3480  J2.Set( 1, 6, J2row.ToFloatPtr() );
3481 
3482  c2[0] = 0.0f;
3483  }
3484 
3485  lo[0] = 0.0f;
3486  e[0] = LIMIT_LCP_EPSILON;
3487 
3488  physics->AddFrameConstraint( this );
3489 
3490  return true;
3491 }
3492 
3493 /*
3494 ================
3495 idAFConstraint_ConeLimit::Translate
3496 ================
3497 */
3498 void idAFConstraint_ConeLimit::Translate( const idVec3 &translation ) {
3499  if ( !body2 ) {
3500  coneAnchor += translation;
3501  }
3502 }
3503 
3504 /*
3505 ================
3506 idAFConstraint_ConeLimit::Rotate
3507 ================
3508 */
3509 void idAFConstraint_ConeLimit::Rotate( const idRotation &rotation ) {
3510  if ( !body2 ) {
3511  coneAnchor *= rotation;
3512  coneAxis *= rotation.ToMat3();
3513  }
3514 }
3515 
3516 /*
3517 ================
3518 idAFConstraint_ConeLimit::DebugDraw
3519 ================
3520 */
3521 void idAFConstraint_ConeLimit::DebugDraw( void ) {
3522  idVec3 ax, anchor, x, y, z, start, end;
3523  float sinAngle, a, size = 10.0f;
3524  idAFBody *master;
3525 
3526  master = body2 ? body2 : physics->GetMasterBody();
3527 
3528  if ( master ) {
3529  ax = coneAxis * master->GetWorldAxis();
3530  anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis();
3531  }
3532  else {
3533  ax = coneAxis;
3534  anchor = coneAnchor;
3535  }
3536 
3537  // draw body1 axis
3538  gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) );
3539 
3540  // draw cone
3541  ax.NormalVectors( x, y );
3542  sinAngle = idMath::Sqrt( 1.0f - cosAngle * cosAngle );
3543  x *= size * sinAngle;
3544  y *= size * sinAngle;
3545  z = anchor + ax * size * cosAngle;
3546  start = x + z;
3547  for ( a = 0.0f; a < 360.0f; a += 45.0f ) {
3548  end = x * (float) cos( DEG2RAD(a + 45.0f) ) + y * (float) sin( DEG2RAD(a + 45.0f) ) + z;
3549  gameRenderWorld->DebugLine( colorMagenta, anchor, start );
3550  gameRenderWorld->DebugLine( colorMagenta, start, end );
3551  start = end;
3552  }
3553 }
3554 
3555 /*
3556 ================
3557 idAFConstraint_ConeLimit::Save
3558 ================
3559 */
3560 void idAFConstraint_ConeLimit::Save( idSaveGame *saveFile ) const {
3561  idAFConstraint::Save( saveFile );
3562  saveFile->WriteVec3( coneAnchor );
3563  saveFile->WriteVec3( coneAxis );
3564  saveFile->WriteVec3( body1Axis );
3565  saveFile->WriteFloat( cosAngle );
3566  saveFile->WriteFloat( sinHalfAngle );
3567  saveFile->WriteFloat( cosHalfAngle );
3568  saveFile->WriteFloat( epsilon );
3569 }
3570 
3571 /*
3572 ================
3573 idAFConstraint_ConeLimit::Restore
3574 ================
3575 */
3576 void idAFConstraint_ConeLimit::Restore( idRestoreGame *saveFile ) {
3577  idAFConstraint::Restore( saveFile );
3578  saveFile->ReadVec3( coneAnchor );
3579  saveFile->ReadVec3( coneAxis );
3580  saveFile->ReadVec3( body1Axis );
3581  saveFile->ReadFloat( cosAngle );
3582  saveFile->ReadFloat( sinHalfAngle );
3583  saveFile->ReadFloat( cosHalfAngle );
3584  saveFile->ReadFloat( epsilon );
3585 }
3586 
3587 
3588 //===============================================================
3589 //
3590 // idAFConstraint_PyramidLimit
3591 //
3592 //===============================================================
3593 
3594 /*
3595 ================
3596 idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit
3597 ================
3598 */
3599 idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit( void ) {
3600  type = CONSTRAINT_PYRAMIDLIMIT;
3601  name = "pyramidLimit";
3602  InitSize( 1 );
3603  fl.allowPrimary = false;
3604  fl.frameConstraint = true;
3605 }
3606 
3607 /*
3608 ================
3609 idAFConstraint_PyramidLimit::Setup
3610 ================
3611 */
3612 void idAFConstraint_PyramidLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor,
3613  const idVec3 &pyramidAxis, const idVec3 &baseAxis,
3614  const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis ) {
3615  body1 = b1;
3616  body2 = b2;
3617  // setup the base and make sure the basis is orthonormal
3618  pyramidBasis[2] = pyramidAxis;
3619  pyramidBasis[2].Normalize();
3620  pyramidBasis[0] = baseAxis;
3621  pyramidBasis[0] -= pyramidBasis[2] * baseAxis * pyramidBasis[2];
3622  pyramidBasis[0].Normalize();
3623  pyramidBasis[1] = pyramidBasis[0].Cross( pyramidBasis[2] );
3624  // pyramid top
3625  this->pyramidAnchor = pyramidAnchor;
3626  // angles
3627  cosAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.5f ) );
3628  cosAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.5f ) );
3629  sinHalfAngle[0] = (float) sin( DEG2RAD( pyramidAngle1 * 0.25f ) );
3630  sinHalfAngle[1] = (float) sin( DEG2RAD( pyramidAngle2 * 0.25f ) );
3631  cosHalfAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.25f ) );
3632  cosHalfAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.25f ) );
3633 
3634  this->body1Axis = body1Axis;
3635 }
3636 
3637 /*
3638 ================
3639 idAFConstraint_PyramidLimit::SetAnchor
3640 ================
3641 */
3642 void idAFConstraint_PyramidLimit::SetAnchor( const idVec3 &pyramidAnchor ) {
3643  this->pyramidAnchor = pyramidAnchor;
3644 }
3645 
3646 /*
3647 ================
3648 idAFConstraint_PyramidLimit::SetBody1Axis
3649 ================
3650 */
3651 void idAFConstraint_PyramidLimit::SetBody1Axis( const idVec3 &body1Axis ) {
3652  this->body1Axis = body1Axis;
3653 }
3654 
3655 /*
3656 ================
3657 idAFConstraint_PyramidLimit::Evaluate
3658 ================
3659 */
3660 void idAFConstraint_PyramidLimit::Evaluate( float invTimeStep ) {
3661  // do nothing
3662 }
3663 
3664 /*
3665 ================
3666 idAFConstraint_PyramidLimit::ApplyFriction
3667 ================
3668 */
3669 void idAFConstraint_PyramidLimit::ApplyFriction( float invTimeStep ) {
3670 }
3671 
3672 /*
3673 ================
3674 idAFConstraint_PyramidLimit::Add
3675 ================
3676 */
3677 bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) {
3678  int i;
3679  float a[2];
3680  idVec6 J1row, J2row;
3681  idMat3 worldBase;
3682  idVec3 anchor, body1ax, ax[2], v, normal, pyramidVector, p1, p2;
3683  idQuat q;
3684  idAFBody *master;
3685 
3686  if ( af_skipLimits.GetBool() ) {
3687  lm.Zero(); // constraint exerts no force
3688  return false;
3689  }
3690 
3691  physics = phys;
3692  master = body2 ? body2 : physics->GetMasterBody();
3693 
3694  if ( master ) {
3695  worldBase[0] = pyramidBasis[0] * master->GetWorldAxis();
3696  worldBase[1] = pyramidBasis[1] * master->GetWorldAxis();
3697  worldBase[2] = pyramidBasis[2] * master->GetWorldAxis();
3698  anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis();
3699  }
3700  else {
3701  worldBase = pyramidBasis;
3702  anchor = pyramidAnchor;
3703  }
3704 
3705  body1ax = body1Axis * body1->GetWorldAxis();
3706 
3707  for ( i = 0; i < 2; i++ ) {
3708  ax[i] = body1ax - worldBase[!i] * body1ax * worldBase[!i];
3709  ax[i].Normalize();
3710  a[i] = worldBase[2] * ax[i];
3711  }
3712 
3713  // if the body1 axis is inside the pyramid
3714  if ( a[0] > cosAngle[0] && a[1] > cosAngle[1] ) {
3715  lm.Zero(); // constraint exerts no force
3716  return false;
3717  }
3718 
3719  // calculate the inward pyramid normal for the position the body1 axis went outside the pyramid
3720  pyramidVector = worldBase[2];
3721  for ( i = 0; i < 2; i++ ) {
3722  if ( a[i] <= cosAngle[i] ) {
3723  v = ax[i].Cross( worldBase[2] );
3724  v.Normalize();
3725  q.x = v.x * sinHalfAngle[i];
3726  q.y = v.y * sinHalfAngle[i];
3727  q.z = v.z * sinHalfAngle[i];
3728  q.w = cosHalfAngle[i];
3729  pyramidVector *= q.ToMat3();
3730  }
3731  }
3732  normal = pyramidVector.Cross( worldBase[2] ).Cross( pyramidVector );
3733  normal.Normalize();
3734 
3735  p1 = anchor + 32.0f * pyramidVector - body1->GetWorldOrigin();
3736 
3737  J1row.SubVec3(0) = normal;
3738  J1row.SubVec3(1) = p1.Cross( normal );
3739  J1.Set( 1, 6, J1row.ToFloatPtr() );
3740 
3741  c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) );
3742 
3743  if ( body2 ) {
3744 
3745  p2 = anchor + 32.0f * pyramidVector - master->GetWorldOrigin();
3746 
3747  J2row.SubVec3(0) = -normal;
3748  J2row.SubVec3(1) = p2.Cross( -normal );
3749  J2.Set( 1, 6, J2row.ToFloatPtr() );
3750 
3751  c2[0] = 0.0f;
3752  }
3753 
3754  lo[0] = 0.0f;
3755  e[0] = LIMIT_LCP_EPSILON;
3756 
3757  physics->AddFrameConstraint( this );
3758 
3759  return true;
3760 }
3761 
3762 /*
3763 ================
3764 idAFConstraint_PyramidLimit::Translate
3765 ================
3766 */
3767 void idAFConstraint_PyramidLimit::Translate( const idVec3 &translation ) {
3768  if ( !body2 ) {
3769  pyramidAnchor += translation;
3770  }
3771 }
3772 
3773 /*
3774 ================
3775 idAFConstraint_PyramidLimit::Rotate
3776 ================
3777 */
3778 void idAFConstraint_PyramidLimit::Rotate( const idRotation &rotation ) {
3779  if ( !body2 ) {
3780  pyramidAnchor *= rotation;
3781  pyramidBasis[0] *= rotation.ToMat3();
3782  pyramidBasis[1] *= rotation.ToMat3();
3783  pyramidBasis[2] *= rotation.ToMat3();
3784  }
3785 }
3786 
3787 /*
3788 ================
3789 idAFConstraint_PyramidLimit::DebugDraw
3790 ================
3791 */
3792 void idAFConstraint_PyramidLimit::DebugDraw( void ) {
3793  int i;
3794  float size = 10.0f;
3795  idVec3 anchor, dir, p[4];
3796  idMat3 worldBase, m[2];
3797  idQuat q;
3798  idAFBody *master;
3799 
3800  master = body2 ? body2 : physics->GetMasterBody();
3801 
3802  if ( master ) {
3803  worldBase[0] = pyramidBasis[0] * master->GetWorldAxis();
3804  worldBase[1] = pyramidBasis[1] * master->GetWorldAxis();
3805  worldBase[2] = pyramidBasis[2] * master->GetWorldAxis();
3806  anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis();
3807  }
3808  else {
3809  worldBase = pyramidBasis;
3810  anchor = pyramidAnchor;
3811  }
3812 
3813  // draw body1 axis
3814  gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) );
3815 
3816  // draw the pyramid
3817  for ( i = 0; i < 2; i++ ) {
3818  q.x = worldBase[!i].x * sinHalfAngle[i];
3819  q.y = worldBase[!i].y * sinHalfAngle[i];
3820  q.z = worldBase[!i].z * sinHalfAngle[i];
3821  q.w = cosHalfAngle[i];
3822  m[i] = q.ToMat3();
3823  }
3824 
3825  dir = worldBase[2] * size;
3826  p[0] = anchor + m[0] * (m[1] * dir);
3827  p[1] = anchor + m[0] * (m[1].Transpose() * dir);
3828  p[2] = anchor + m[0].Transpose() * (m[1].Transpose() * dir);
3829  p[3] = anchor + m[0].Transpose() * (m[1] * dir);
3830 
3831  for ( i = 0; i < 4; i++ ) {
3832  gameRenderWorld->DebugLine( colorMagenta, anchor, p[i] );
3833  gameRenderWorld->DebugLine( colorMagenta, p[i], p[(i+1)&3] );
3834  }
3835 }
3836 
3837 /*
3838 ================
3839 idAFConstraint_PyramidLimit::Save
3840 ================
3841 */
3842 void idAFConstraint_PyramidLimit::Save( idSaveGame *saveFile ) const {
3843  idAFConstraint::Save( saveFile );
3844  saveFile->WriteVec3( pyramidAnchor );
3845  saveFile->WriteMat3( pyramidBasis );
3846  saveFile->WriteVec3( body1Axis );
3847  saveFile->WriteFloat( cosAngle[0] );
3848  saveFile->WriteFloat( cosAngle[1] );
3849  saveFile->WriteFloat( sinHalfAngle[0] );
3850  saveFile->WriteFloat( sinHalfAngle[1] );
3851  saveFile->WriteFloat( cosHalfAngle[0] );
3852  saveFile->WriteFloat( cosHalfAngle[1] );
3853  saveFile->WriteFloat( epsilon );
3854 }
3855 
3856 /*
3857 ================
3858 idAFConstraint_PyramidLimit::Restore
3859 ================
3860 */
3861 void idAFConstraint_PyramidLimit::Restore( idRestoreGame *saveFile ) {
3862  idAFConstraint::Restore( saveFile );
3863  saveFile->ReadVec3( pyramidAnchor );
3864  saveFile->ReadMat3( pyramidBasis );
3865  saveFile->ReadVec3( body1Axis );
3866  saveFile->ReadFloat( cosAngle[0] );
3867  saveFile->ReadFloat( cosAngle[1] );
3868  saveFile->ReadFloat( sinHalfAngle[0] );
3869  saveFile->ReadFloat( sinHalfAngle[1] );
3870  saveFile->ReadFloat( cosHalfAngle[0] );
3871  saveFile->ReadFloat( cosHalfAngle[1] );
3872  saveFile->ReadFloat( epsilon );
3873 }
3874 
3875 
3876 //===============================================================
3877 //
3878 // idAFConstraint_Suspension
3879 //
3880 //===============================================================
3881 
3882 /*
3883 ================
3884 idAFConstraint_Suspension::idAFConstraint_Suspension
3885 ================
3886 */
3887 idAFConstraint_Suspension::idAFConstraint_Suspension( void ) {
3888  type = CONSTRAINT_SUSPENSION;
3889  name = "suspension";
3890  InitSize( 3 );
3891  fl.allowPrimary = false;
3892  fl.frameConstraint = true;
3893 
3894  localOrigin.Zero();
3895  localAxis.Identity();
3896  suspensionUp = 0.0f;
3897  suspensionDown = 0.0f;
3898  suspensionKCompress = 0.0f;
3899  suspensionDamping = 0.0f;
3900  steerAngle = 0.0f;
3901  friction = 2.0f;
3902  motorEnabled = false;
3903  motorForce = 0.0f;
3904  motorVelocity = 0.0f;
3905  wheelModel = NULL;
3906  memset( &trace, 0, sizeof( trace ) );
3907  epsilon = LCP_EPSILON;
3908 }
3909 
3910 /*
3911 ================
3912 idAFConstraint_Suspension::Setup
3913 ================
3914 */
3915 void idAFConstraint_Suspension::Setup( const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel ) {
3916  this->name = name;
3917  body1 = body;
3918  body2 = NULL;
3919  localOrigin = ( origin - body->GetWorldOrigin() ) * body->GetWorldAxis().Transpose();
3920  localAxis = axis * body->GetWorldAxis().Transpose();
3921  wheelModel = clipModel;
3922 }
3923 
3924 /*
3925 ================
3926 idAFConstraint_Suspension::SetSuspension
3927 ================
3928 */
3929 void idAFConstraint_Suspension::SetSuspension( const float up, const float down, const float k, const float d, const float f ) {
3930  suspensionUp = up;
3931  suspensionDown = down;
3932  suspensionKCompress = k;
3933  suspensionDamping = d;
3934  friction = f;
3935 }
3936 
3937 /*
3938 ================
3939 idAFConstraint_Suspension::GetWheelOrigin
3940 ================
3941 */
3942 const idVec3 idAFConstraint_Suspension::GetWheelOrigin( void ) const {
3943  return body1->GetWorldOrigin() + wheelOffset * body1->GetWorldAxis();
3944 }
3945 
3946 /*
3947 ================
3948 idAFConstraint_Suspension::Evaluate
3949 ================
3950 */
3951 void idAFConstraint_Suspension::Evaluate( float invTimeStep ) {
3952  float velocity, suspensionLength, springLength, compression, dampingForce, springForce;
3953  idVec3 origin, start, end, vel1, vel2, springDir, r, frictionDir, motorDir;
3954  idMat3 axis;
3955  idRotation rotation;
3956 
3957  axis = localAxis * body1->GetWorldAxis();
3958  origin = body1->GetWorldOrigin() + localOrigin * body1->GetWorldAxis();
3959  start = origin + suspensionUp * axis[2];
3960  end = origin - suspensionDown * axis[2];
3961 
3962  rotation.SetVec( axis[2] );
3963  rotation.SetAngle( steerAngle );
3964 
3965  axis *= rotation.ToMat3();
3966 
3967  gameLocal.clip.Translation( trace, start, end, wheelModel, axis, MASK_SOLID, NULL );
3968 
3969  wheelOffset = ( trace.endpos - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
3970 
3971  if ( trace.fraction >= 1.0f ) {
3972  J1.SetSize( 0, 6 );
3973  if ( body2 ) {
3974  J2.SetSize( 0, 6 );
3975  }
3976  return;
3977  }
3978 
3979  // calculate and add spring force
3980  vel1 = body1->GetPointVelocity( start );
3981  if ( body2 ) {
3982  vel2 = body2->GetPointVelocity( trace.c.point );
3983  } else {
3984  vel2.Zero();
3985  }
3986 
3987  suspensionLength = suspensionUp + suspensionDown;
3988  springDir = trace.endpos - start;
3989  springLength = trace.fraction * suspensionLength;
3990  dampingForce = suspensionDamping * idMath::Fabs( ( vel2 - vel1 ) * springDir ) / ( 1.0f + springLength * springLength );
3991  compression = suspensionLength - springLength;
3992  springForce = compression * compression * suspensionKCompress - dampingForce;
3993 
3994  r = trace.c.point - body1->GetWorldOrigin();
3995  J1.SetSize( 2, 6 );
3996  J1.SubVec6(0).SubVec3(0) = trace.c.normal;
3997  J1.SubVec6(0).SubVec3(1) = r.Cross( trace.c.normal );
3998  c1.SetSize( 2 );
3999  c1[0] = 0.0f;
4000  velocity = J1.SubVec6(0).SubVec3(0) * body1->GetLinearVelocity() + J1.SubVec6(0).SubVec3(1) * body1->GetAngularVelocity();
4001 
4002  if ( body2 ) {
4003  r = trace.c.point - body2->GetWorldOrigin();
4004  J2.SetSize( 2, 6 );
4005  J2.SubVec6(0).SubVec3(0) = -trace.c.normal;
4006  J2.SubVec6(0).SubVec3(1) = r.Cross( -trace.c.normal );
4007  c2.SetSize( 2 );
4008  c2[0] = 0.0f;
4009  velocity += J2.SubVec6(0).SubVec3(0) * body2->GetLinearVelocity() + J2.SubVec6(0).SubVec3(1) * body2->GetAngularVelocity();
4010  }
4011 
4012  c1[0] = -compression; // + 0.5f * -velocity;
4013 
4014  e[0] = 1e-4f;
4015  lo[0] = 0.0f;
4016  hi[0] = springForce;
4017  boxConstraint = NULL;
4018  boxIndex[0] = -1;
4019 
4020  // project the friction direction into the contact plane
4021  frictionDir = axis[1] - axis[1] * trace.c.normal * axis[1];
4022  frictionDir.Normalize();
4023 
4024  r = trace.c.point - body1->GetWorldOrigin();
4025 
4026  J1.SubVec6(1).SubVec3(0) = frictionDir;
4027  J1.SubVec6(1).SubVec3(1) = r.Cross( frictionDir );
4028  c1[1] = 0.0f;
4029 
4030  if ( body2 ) {
4031  r = trace.c.point - body2->GetWorldOrigin();
4032 
4033  J2.SubVec6(1).SubVec3(0) = -frictionDir;
4034  J2.SubVec6(1).SubVec3(1) = r.Cross( -frictionDir );
4035  c2[1] = 0.0f;
4036  }
4037 
4038  lo[1] = -friction * physics->GetContactFrictionScale();
4039  hi[1] = friction * physics->GetContactFrictionScale();
4040 
4041  boxConstraint = this;
4042  boxIndex[1] = 0;
4043 
4044 
4045  if ( motorEnabled ) {
4046  // project the motor force direction into the contact plane
4047  motorDir = axis[0] - axis[0] * trace.c.normal * axis[0];
4048  motorDir.Normalize();
4049 
4050  r = trace.c.point - body1->GetWorldOrigin();
4051 
4052  J1.ChangeSize( 3, J1.GetNumColumns() );
4053  J1.SubVec6(2).SubVec3(0) = -motorDir;
4054  J1.SubVec6(2).SubVec3(1) = r.Cross( -motorDir );
4055  c1.ChangeSize( 3 );
4056  c1[2] = motorVelocity;
4057 
4058  if ( body2 ) {
4059  r = trace.c.point - body2->GetWorldOrigin();
4060 
4061  J2.ChangeSize( 3, J2.GetNumColumns() );
4062  J2.SubVec6(2).SubVec3(0) = -motorDir;
4063  J2.SubVec6(2).SubVec3(1) = r.Cross( -motorDir );
4064  c2.ChangeSize( 3 );
4065  c2[2] = 0.0f;
4066  }
4067 
4068  lo[2] = -motorForce;
4069  hi[2] = motorForce;
4070  boxIndex[2] = -1;
4071  }
4072 }
4073 
4074 /*
4075 ================
4076 idAFConstraint_Suspension::ApplyFriction
4077 ================
4078 */
4079 void idAFConstraint_Suspension::ApplyFriction( float invTimeStep ) {
4080  // do nothing
4081 }
4082 
4083 /*
4084 ================
4085 idAFConstraint_Suspension::Translate
4086 ================
4087 */
4088 void idAFConstraint_Suspension::Translate( const idVec3 &translation ) {
4089 }
4090 
4091 /*
4092 ================
4093 idAFConstraint_Suspension::Rotate
4094 ================
4095 */
4096 void idAFConstraint_Suspension::Rotate( const idRotation &rotation ) {
4097 }
4098 
4099 /*
4100 ================
4101 idAFConstraint_Suspension::DebugDraw
4102 ================
4103 */
4104 void idAFConstraint_Suspension::DebugDraw( void ) {
4105  idVec3 origin;
4106  idMat3 axis;
4107  idRotation rotation;
4108 
4109  axis = localAxis * body1->GetWorldAxis();
4110 
4111  rotation.SetVec( axis[2] );
4112  rotation.SetAngle( steerAngle );
4113 
4114  axis *= rotation.ToMat3();
4115 
4116  if ( trace.fraction < 1.0f ) {
4117  origin = trace.c.point;
4118 
4119  gameRenderWorld->DebugLine( colorWhite, origin, origin + 6.0f * axis[2] );
4120  gameRenderWorld->DebugLine( colorWhite, origin - 4.0f * axis[0], origin + 4.0f * axis[0] );
4121  gameRenderWorld->DebugLine( colorWhite, origin - 2.0f * axis[1], origin + 2.0f * axis[1] );
4122  }
4123 }
4124 
4125 
4126 //===============================================================
4127 //
4128 // idAFBody
4129 //
4130 //===============================================================
4131 
4132 /*
4133 ================
4134 idAFBody::idAFBody
4135 ================
4136 */
4137 idAFBody::idAFBody( void ) {
4138  Init();
4139 }
4140 
4141 /*
4142 ================
4143 idAFBody::idAFBody
4144 ================
4145 */
4146 idAFBody::idAFBody( const idStr &name, idClipModel *clipModel, float density ) {
4147 
4148  assert( clipModel );
4149  assert( clipModel->IsTraceModel() );
4150 
4151  Init();
4152 
4153  this->name = name;
4154  this->clipModel = NULL;
4155 
4156  SetClipModel( clipModel );
4157  SetDensity( density );
4158 
4159  current->worldOrigin = clipModel->GetOrigin();
4160  current->worldAxis = clipModel->GetAxis();
4161  *next = *current;
4162 
4163 }
4164 
4165 /*
4166 ================
4167 idAFBody::~idAFBody
4168 ================
4169 */
4170 idAFBody::~idAFBody( void ) {
4171  delete clipModel;
4172 }
4173 
4174 /*
4175 ================
4176 idAFBody::Init
4177 ================
4178 */
4179 void idAFBody::Init( void ) {
4180  name = "noname";
4181  parent = NULL;
4182  clipModel = NULL;
4183  primaryConstraint = NULL;
4184  tree = NULL;
4185 
4186  linearFriction = -1.0f;
4187  angularFriction = -1.0f;
4188  contactFriction = -1.0f;
4189  bouncyness = -1.0f;
4190  clipMask = 0;
4191 
4192  frictionDir = vec3_zero;
4193  contactMotorDir = vec3_zero;
4194  contactMotorVelocity = 0.0f;
4195  contactMotorForce = 0.0f;
4196 
4197  mass = 1.0f;
4198  invMass = 1.0f;
4199  centerOfMass = vec3_zero;
4200  inertiaTensor = mat3_identity;
4201  inverseInertiaTensor = mat3_identity;
4202 
4203  current = &state[0];
4204  next = &state[1];
4205  current->worldOrigin = vec3_zero;
4206  current->worldAxis = mat3_identity;
4207  current->spatialVelocity = vec6_zero;
4208  current->externalForce = vec6_zero;
4209  *next = *current;
4210  saved = *current;
4211  atRestOrigin = vec3_zero;
4212  atRestAxis = mat3_identity;
4213 
4214  s.Zero( 6 );
4215  totalForce.Zero( 6 );
4216  auxForce.Zero( 6 );
4217  acceleration.Zero( 6 );
4218 
4219  response = NULL;
4220  responseIndex = NULL;
4221  numResponses = 0;
4222  maxAuxiliaryIndex = 0;
4223  maxSubTreeAuxiliaryIndex = 0;
4224 
4225  memset( &fl, 0, sizeof( fl ) );
4226 
4227  fl.selfCollision = true;
4228  fl.isZero = true;
4229 }
4230 
4231 /*
4232 ================
4233 idAFBody::SetClipModel
4234 ================
4235 */
4236 void idAFBody::SetClipModel( idClipModel *clipModel ) {
4237  if ( this->clipModel && this->clipModel != clipModel ) {
4238  delete this->clipModel;
4239  }
4240  this->clipModel = clipModel;
4241 }
4242 
4243 /*
4244 ================
4245 idAFBody::SetFriction
4246 ================
4247 */
4248 void idAFBody::SetFriction( float linear, float angular, float contact ) {
4249  if ( linear < 0.0f || linear > 1.0f ||
4250  angular < 0.0f || angular > 1.0f ||
4251  contact < 0.0f ) {
4252  gameLocal.Warning( "idAFBody::SetFriction: friction out of range, linear = %.1f, angular = %.1f, contact = %.1f", linear, angular, contact );
4253  return;
4254  }
4255  linearFriction = linear;
4256  angularFriction = angular;
4257  contactFriction = contact;
4258 }
4259 
4260 /*
4261 ================
4262 idAFBody::SetBouncyness
4263 ================
4264 */
4265 void idAFBody::SetBouncyness( float bounce ) {
4266  if ( bounce < 0.0f || bounce > 1.0f ) {
4267  gameLocal.Warning( "idAFBody::SetBouncyness: bouncyness out of range, bounce = %.1f", bounce );
4268  return;
4269  }
4270  bouncyness = bounce;
4271 }
4272 
4273 /*
4274 ================
4275 idAFBody::SetDensity
4276 ================
4277 */
4278 void idAFBody::SetDensity( float density, const idMat3 &inertiaScale ) {
4279 
4280  // get the body mass properties
4281  clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor );
4282 
4283  // make sure we have a valid mass
4284  if ( mass <= 0.0f || FLOAT_IS_NAN( mass ) ) {
4285  gameLocal.Warning( "idAFBody::SetDensity: invalid mass for body '%s'", name.c_str() );
4286  mass = 1.0f;
4287  centerOfMass.Zero();
4288  inertiaTensor.Identity();
4289  }
4290 
4291  // make sure the center of mass is at the body origin
4292  if ( !centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) {
4293  gameLocal.Warning( "idAFBody::SetDentity: center of mass not at origin for body '%s'", name.c_str() );
4294  }
4295  centerOfMass.Zero();
4296 
4297  // calculate the inverse mass and inverse inertia tensor
4298  invMass = 1.0f / mass;
4299  if ( inertiaScale != mat3_identity ) {
4300  inertiaTensor *= inertiaScale;
4301  }
4302  if ( inertiaTensor.IsDiagonal( 1e-3f ) ) {
4303  inertiaTensor[0][1] = inertiaTensor[0][2] = 0.0f;
4304  inertiaTensor[1][0] = inertiaTensor[1][2] = 0.0f;
4305  inertiaTensor[2][0] = inertiaTensor[2][1] = 0.0f;
4306  inverseInertiaTensor.Identity();
4307  inverseInertiaTensor[0][0] = 1.0f / inertiaTensor[0][0];
4308  inverseInertiaTensor[1][1] = 1.0f / inertiaTensor[1][1];
4309  inverseInertiaTensor[2][2] = 1.0f / inertiaTensor[2][2];
4310  }
4311  else {
4312  inverseInertiaTensor = inertiaTensor.Inverse();
4313  }
4314 }
4315 
4316 /*
4317 ================
4318 idAFBody::SetFrictionDirection
4319 ================
4320 */
4321 void idAFBody::SetFrictionDirection( const idVec3 &dir ) {
4322  frictionDir = dir * current->worldAxis.Transpose();
4323  fl.useFrictionDir = true;
4324 }
4325 
4326 /*
4327 ================
4328 idAFBody::GetFrictionDirection
4329 ================
4330 */
4331 bool idAFBody::GetFrictionDirection( idVec3 &dir ) const {
4332  if ( fl.useFrictionDir ) {
4333  dir = frictionDir * current->worldAxis;
4334  return true;
4335  }
4336  return false;
4337 }
4338 
4339 /*
4340 ================
4341 idAFBody::SetContactMotorDirection
4342 ================
4343 */
4344 void idAFBody::SetContactMotorDirection( const idVec3 &dir ) {
4345  contactMotorDir = dir * current->worldAxis.Transpose();
4346  fl.useContactMotorDir = true;
4347 }
4348 
4349 /*
4350 ================
4351 idAFBody::GetContactMotorDirection
4352 ================
4353 */
4354 bool idAFBody::GetContactMotorDirection( idVec3 &dir ) const {
4355  if ( fl.useContactMotorDir ) {
4356  dir = contactMotorDir * current->worldAxis;
4357  return true;
4358  }
4359  return false;
4360 }
4361 
4362 /*
4363 ================
4364 idAFBody::GetPointVelocity
4365 ================
4366 */
4367 idVec3 idAFBody::GetPointVelocity( const idVec3 &point ) const {
4368  idVec3 r = point - current->worldOrigin;
4369  return current->spatialVelocity.SubVec3(0) + current->spatialVelocity.SubVec3(1).Cross( r );
4370 }
4371 
4372 /*
4373 ================
4374 idAFBody::AddForce
4375 ================
4376 */
4377 void idAFBody::AddForce( const idVec3 &point, const idVec3 &force ) {
4378  current->externalForce.SubVec3(0) += force;
4379  current->externalForce.SubVec3(1) += (point - current->worldOrigin).Cross( force );
4380 }
4381 
4382 /*
4383 ================
4384 idAFBody::InverseWorldSpatialInertiaMultiply
4385 
4386  dst = this->inverseWorldSpatialInertia * v;
4387 ================
4388 */
4389 ID_INLINE void idAFBody::InverseWorldSpatialInertiaMultiply( idVecX &dst, const float *v ) const {
4390  const float *mPtr = inverseWorldSpatialInertia.ToFloatPtr();
4391  const float *vPtr = v;
4392  float *dstPtr = dst.ToFloatPtr();
4393 
4394  if ( fl.spatialInertiaSparse ) {
4395  dstPtr[0] = mPtr[0*6+0] * vPtr[0];
4396  dstPtr[1] = mPtr[1*6+1] * vPtr[1];
4397  dstPtr[2] = mPtr[2*6+2] * vPtr[2];
4398  dstPtr[3] = mPtr[3*6+3] * vPtr[3] + mPtr[3*6+4] * vPtr[4] + mPtr[3*6+5] * vPtr[5];
4399  dstPtr[4] = mPtr[4*6+3] * vPtr[3] + mPtr[4*6+4] * vPtr[4] + mPtr[4*6+5] * vPtr[5];
4400  dstPtr[5] = mPtr[5*6+3] * vPtr[3] + mPtr[5*6+4] * vPtr[4] + mPtr[5*6+5] * vPtr[5];
4401  } else {
4402  gameLocal.Warning( "spatial inertia is not sparse for body %s", name.c_str() );
4403  }
4404 }
4405 
4406 /*
4407 ================
4408 idAFBody::Save
4409 ================
4410 */
4411 void idAFBody::Save( idSaveGame *saveFile ) {
4412  saveFile->WriteFloat( linearFriction );
4413  saveFile->WriteFloat( angularFriction );
4414  saveFile->WriteFloat( contactFriction );
4415  saveFile->WriteFloat( bouncyness );
4416  saveFile->WriteInt( clipMask );
4417  saveFile->WriteVec3( frictionDir );
4418  saveFile->WriteVec3( contactMotorDir );
4419  saveFile->WriteFloat( contactMotorVelocity );
4420  saveFile->WriteFloat( contactMotorForce );
4421 
4422  saveFile->WriteFloat( mass );
4423  saveFile->WriteFloat( invMass );
4424  saveFile->WriteVec3( centerOfMass );
4425  saveFile->WriteMat3( inertiaTensor );
4426  saveFile->WriteMat3( inverseInertiaTensor );
4427 
4428  saveFile->WriteVec3( current->worldOrigin );
4429  saveFile->WriteMat3( current->worldAxis );
4430  saveFile->WriteVec6( current->spatialVelocity );
4431  saveFile->WriteVec6( current->externalForce );
4432  saveFile->WriteVec3( atRestOrigin );
4433  saveFile->WriteMat3( atRestAxis );
4434 }
4435 
4436 /*
4437 ================
4438 idAFBody::Restore
4439 ================
4440 */
4441 void idAFBody::Restore( idRestoreGame *saveFile ) {
4442  saveFile->ReadFloat( linearFriction );
4443  saveFile->ReadFloat( angularFriction );
4444  saveFile->ReadFloat( contactFriction );
4445  saveFile->ReadFloat( bouncyness );
4446  saveFile->ReadInt( clipMask );
4447  saveFile->ReadVec3( frictionDir );
4448  saveFile->ReadVec3( contactMotorDir );
4449  saveFile->ReadFloat( contactMotorVelocity );
4450  saveFile->ReadFloat( contactMotorForce );
4451 
4452  saveFile->ReadFloat( mass );
4453  saveFile->ReadFloat( invMass );
4454  saveFile->ReadVec3( centerOfMass );
4455  saveFile->ReadMat3( inertiaTensor );
4456  saveFile->ReadMat3( inverseInertiaTensor );
4457 
4458  saveFile->ReadVec3( current->worldOrigin );
4459  saveFile->ReadMat3( current->worldAxis );
4460  saveFile->ReadVec6( current->spatialVelocity );
4461  saveFile->ReadVec6( current->externalForce );
4462  saveFile->ReadVec3( atRestOrigin );
4463  saveFile->ReadMat3( atRestAxis );
4464 }
4465 
4466 
4467 
4468 //===============================================================
4469 // M
4470 // idAFTree MrE
4471 // E
4472 //===============================================================
4473 
4474 /*
4475 ================
4476 idAFTree::Factor
4477 
4478  factor matrix for the primary constraints in the tree
4479 ================
4480 */
4481 void idAFTree::Factor( void ) const {
4482  int i, j;
4483  idAFBody *body;
4484  idAFConstraint *child;
4485  idMatX childI;
4486 
4487  childI.SetData( 6, 6, MATX_ALLOCA( 6 * 6 ) );
4488 
4489  // from the leaves up towards the root
4490  for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
4491  body = sortedBodies[i];
4492 
4493  if ( body->children.Num() ) {
4494 
4495  for ( j = 0; j < body->children.Num(); j++ ) {
4496 
4497  child = body->children[j]->primaryConstraint;
4498 
4499  // child->I = - child->body1->J.Transpose() * child->body1->I * child->body1->J;
4500  childI.SetSize( child->J1.GetNumRows(), child->J1.GetNumRows() );
4501  child->body1->J.TransposeMultiply( child->body1->I ).Multiply( childI, child->body1->J );
4502  childI.Negate();
4503 
4504  child->invI = childI;
4505  if ( !child->invI.InverseFastSelf() ) {
4506  gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for constraint '%s'",
4507  child->invI.GetNumRows(), child->invI.GetNumColumns(), child->GetName().c_str() );
4508  }
4509  child->J = child->invI * child->J;
4510 
4511  body->I -= child->J.TransposeMultiply( childI ) * child->J;
4512  }
4513 
4514  body->invI = body->I;
4515  if ( !body->invI.InverseFastSelf() ) {
4516  gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for body %s",
4517  child->invI.GetNumRows(), child->invI.GetNumColumns(), body->GetName().c_str() );
4518  }
4519  if ( body->primaryConstraint ) {
4520  body->J = body->invI * body->J;
4521  }
4522  }
4523  else if ( body->primaryConstraint ) {
4524  body->J = body->inverseWorldSpatialInertia * body->J;
4525  }
4526  }
4527 }
4528 
4529 /*
4530 ================
4531 idAFTree::Solve
4532 
4533  solve for primary constraints in the tree
4534 ================
4535 */
4536 void idAFTree::Solve( int auxiliaryIndex ) const {
4537  int i, j;
4538  idAFBody *body, *child;
4539  idAFConstraint *primaryConstraint;
4540 
4541  // from the leaves up towards the root
4542  for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
4543  body = sortedBodies[i];
4544 
4545  for ( j = 0; j < body->children.Num(); j++ ) {
4546  child = body->children[j];
4547  primaryConstraint = child->primaryConstraint;
4548 
4549  if ( !child->fl.isZero ) {
4550  child->J.TransposeMultiplySub( primaryConstraint->s, child->s );
4551  primaryConstraint->fl.isZero = false;
4552  }
4553  if ( !primaryConstraint->fl.isZero ) {
4554  primaryConstraint->J.TransposeMultiplySub( body->s, primaryConstraint->s );
4555  body->fl.isZero = false;
4556  }
4557  }
4558  }
4559 
4560  bool useSymmetry = af_useSymmetry.GetBool();
4561 
4562  // from the root down towards the leaves
4563  for ( i = 0; i < sortedBodies.Num(); i++ ) {
4564  body = sortedBodies[i];
4565  primaryConstraint = body->primaryConstraint;
4566 
4567  if ( primaryConstraint ) {
4568 
4569  if ( useSymmetry && body->parent->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) {
4570  continue;
4571  }
4572 
4573  if ( !primaryConstraint->fl.isZero ) {
4574  primaryConstraint->s = primaryConstraint->invI * primaryConstraint->s;
4575  }
4576  primaryConstraint->J.MultiplySub( primaryConstraint->s, primaryConstraint->body2->s );
4577 
4578  primaryConstraint->lm = primaryConstraint->s;
4579 
4580  if ( useSymmetry && body->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) {
4581  continue;
4582  }
4583 
4584  if ( body->children.Num() ) {
4585  if ( !body->fl.isZero ) {
4586  body->s = body->invI * body->s;
4587  }
4588  body->J.MultiplySub( body->s, primaryConstraint->s );
4589  }
4590  } else if ( body->children.Num() ) {
4591  body->s = body->invI * body->s;
4592  }
4593  }
4594 }
4595 
4596 /*
4597 ================
4598 idAFTree::Response
4599 
4600  calculate body forces in the tree in response to a constraint force
4601 ================
4602 */
4603 void idAFTree::Response( const idAFConstraint *constraint, int row, int auxiliaryIndex ) const {
4604  int i, j;
4605  idAFBody *body;
4606  idAFConstraint *child, *primaryConstraint;
4607  idVecX v;
4608 
4609  // if a single body don't waste time because there aren't any primary constraints
4610  if ( sortedBodies.Num() == 1 ) {
4611  body = constraint->body1;
4612  if ( body->tree == this ) {
4613  body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row );
4614  body->responseIndex[body->numResponses++] = auxiliaryIndex;
4615  }
4616  else {
4617  body = constraint->body2;
4618  body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row );
4619  body->responseIndex[body->numResponses++] = auxiliaryIndex;
4620  }
4621  return;
4622  }
4623 
4624  v.SetData( 6, VECX_ALLOCA( 6 ) );
4625 
4626  // initialize right hand side to zero
4627  for ( i = 0; i < sortedBodies.Num(); i++ ) {
4628  body = sortedBodies[i];
4629  primaryConstraint = body->primaryConstraint;
4630  if ( primaryConstraint ) {
4631  primaryConstraint->s.Zero();
4632  primaryConstraint->fl.isZero = true;
4633  }
4634  body->s.Zero();
4635  body->fl.isZero = true;
4636  body->GetResponseForce( body->numResponses ).Zero();
4637  }
4638 
4639  // set right hand side for first constrained body
4640  body = constraint->body1;
4641  if ( body->tree == this ) {
4642  body->InverseWorldSpatialInertiaMultiply( v, constraint->J1[row] );
4643  primaryConstraint = body->primaryConstraint;
4644  if ( primaryConstraint ) {
4645  primaryConstraint->J1.Multiply( primaryConstraint->s, v );
4646  primaryConstraint->fl.isZero = false;
4647  }
4648  for ( i = 0; i < body->children.Num(); i++ ) {
4649  child = body->children[i]->primaryConstraint;
4650  child->J2.Multiply( child->s, v );
4651  child->fl.isZero = false;
4652  }
4653  body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row );
4654  }
4655 
4656  // set right hand side for second constrained body
4657  body = constraint->body2;
4658  if ( body && body->tree == this ) {
4659  body->InverseWorldSpatialInertiaMultiply( v, constraint->J2[row] );
4660  primaryConstraint = body->primaryConstraint;
4661  if ( primaryConstraint ) {
4662  primaryConstraint->J1.MultiplyAdd( primaryConstraint->s, v );
4663  primaryConstraint->fl.isZero = false;
4664  }
4665  for ( i = 0; i < body->children.Num(); i++ ) {
4666  child = body->children[i]->primaryConstraint;
4667  child->J2.MultiplyAdd( child->s, v );
4668  child->fl.isZero = false;
4669  }
4670  body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row );
4671  }
4672 
4673 
4674  // solve for primary constraints
4675  Solve( auxiliaryIndex );
4676 
4677  bool useSymmetry = af_useSymmetry.GetBool();
4678 
4679  // store body forces in response to the constraint force
4680  idVecX force;
4681  for ( i = 0; i < sortedBodies.Num(); i++ ) {
4682  body = sortedBodies[i];
4683 
4684  if ( useSymmetry && body->maxAuxiliaryIndex < auxiliaryIndex ) {
4685  continue;
4686  }
4687 
4688  force.SetData( 6, body->response + body->numResponses * 8 );
4689 
4690  // add forces of all primary constraints acting on this body
4691  primaryConstraint = body->primaryConstraint;
4692  if ( primaryConstraint ) {
4693  primaryConstraint->J1.TransposeMultiplyAdd( force, primaryConstraint->lm );
4694  }
4695  for ( j = 0; j < body->children.Num(); j++ ) {
4696  child = body->children[j]->primaryConstraint;
4697  child->J2.TransposeMultiplyAdd( force, child->lm );
4698  }
4699 
4700  body->responseIndex[body->numResponses++] = auxiliaryIndex;
4701  }
4702 }
4703 
4704 /*
4705 ================
4706 idAFTree::CalculateForces
4707 
4708  calculate forces on the bodies in the tree
4709 ================
4710 */
4711 void idAFTree::CalculateForces( float timeStep ) const {
4712  int i, j;
4713  float invStep;
4714  idAFBody *body;
4715  idAFConstraint *child, *c, *primaryConstraint;
4716 
4717  // forces on bodies
4718  for ( i = 0; i < sortedBodies.Num(); i++ ) {
4719  body = sortedBodies[i];
4720 
4721  body->totalForce.SubVec6(0) = body->current->externalForce + body->auxForce.SubVec6(0);
4722  }
4723 
4724  // if a single body don't waste time because there aren't any primary constraints
4725  if ( sortedBodies.Num() == 1 ) {
4726  return;
4727  }
4728 
4729  invStep = 1.0f / timeStep;
4730 
4731  // initialize right hand side
4732  for ( i = 0; i < sortedBodies.Num(); i++ ) {
4733  body = sortedBodies[i];
4734 
4735  body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
4736  body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep;
4737  primaryConstraint = body->primaryConstraint;
4738  if ( primaryConstraint ) {
4739  // b = ( J * acc + c )
4740  c = primaryConstraint;
4741  c->s = c->J1 * c->body1->acceleration + c->J2 * c->body2->acceleration + invStep * ( c->c1 + c->c2 );
4742  c->fl.isZero = false;
4743  }
4744  body->s.Zero();
4745  body->fl.isZero = true;
4746  }
4747 
4748  // solve for primary constraints
4749  Solve();
4750 
4751  // calculate forces on bodies after applying primary constraints
4752  for ( i = 0; i < sortedBodies.Num(); i++ ) {
4753  body = sortedBodies[i];
4754 
4755  // add forces of all primary constraints acting on this body
4756  primaryConstraint = body->primaryConstraint;
4757  if ( primaryConstraint ) {
4758  primaryConstraint->J1.TransposeMultiplyAdd( body->totalForce, primaryConstraint->lm );
4759  }
4760  for ( j = 0; j < body->children.Num(); j++ ) {
4761  child = body->children[j]->primaryConstraint;
4762  child->J2.TransposeMultiplyAdd( body->totalForce, child->lm );
4763  }
4764  }
4765 }
4766 
4767 /*
4768 ================
4769 idAFTree::SetMaxSubTreeAuxiliaryIndex
4770 ================
4771 */
4772 void idAFTree::SetMaxSubTreeAuxiliaryIndex( void ) {
4773  int i, j;
4774  idAFBody *body, *child;
4775 
4776  // from the leaves up towards the root
4777  for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
4778  body = sortedBodies[i];
4779 
4780  body->maxSubTreeAuxiliaryIndex = body->maxAuxiliaryIndex;
4781  for ( j = 0; j < body->children.Num(); j++ ) {
4782  child = body->children[j];
4783  if ( child->maxSubTreeAuxiliaryIndex > body->maxSubTreeAuxiliaryIndex ) {
4784  body->maxSubTreeAuxiliaryIndex = child->maxSubTreeAuxiliaryIndex;
4785  }
4786  }
4787  }
4788 }
4789 
4790 /*
4791 ================
4792 idAFTree::SortBodies_r
4793 ================
4794 */
4795 void idAFTree::SortBodies_r( idList<idAFBody*>&sortedList, idAFBody *body ) {
4796  int i;
4797 
4798  for ( i = 0; i < body->children.Num(); i++ ) {
4799  sortedList.Append( body->children[i] );
4800  }
4801  for ( i = 0; i < body->children.Num(); i++ ) {
4802  SortBodies_r( sortedList, body->children[i] );
4803  }
4804 }
4805 
4806 /*
4807 ================
4808 idAFTree::SortBodies
4809 
4810  sort body list to make sure parents come first
4811 ================
4812 */
4813 void idAFTree::SortBodies( void ) {
4814  int i;
4815  idAFBody *body;
4816 
4817  // find the root
4818  for ( i = 0; i < sortedBodies.Num(); i++ ) {
4819  if ( !sortedBodies[i]->parent ) {
4820  break;
4821  }
4822  }
4823 
4824  if ( i >= sortedBodies.Num() ) {
4825  gameLocal.Error( "Articulated figure tree has no root." );
4826  }
4827 
4828  body = sortedBodies[i];
4829  sortedBodies.Clear();
4830  sortedBodies.Append( body );
4831  SortBodies_r( sortedBodies, body );
4832 }
4833 
4834 /*
4835 ================
4836 idAFTree::DebugDraw
4837 ================
4838 */
4839 void idAFTree::DebugDraw( const idVec4 &color ) const {
4840  int i;
4841  idAFBody *body;
4842 
4843  for ( i = 1; i < sortedBodies.Num(); i++ ) {
4844  body = sortedBodies[i];
4845  gameRenderWorld->DebugArrow( color, body->parent->current->worldOrigin, body->current->worldOrigin, 1 );
4846  }
4847 }
4848 
4849 
4850 //===============================================================
4851 // M
4852 // idPhysics_AF MrE
4853 // E
4854 //===============================================================
4855 
4856 /*
4857 ================
4858 idPhysics_AF::EvaluateConstraints
4859 ================
4860 */
4861 void idPhysics_AF::EvaluateConstraints( float timeStep ) {
4862  int i;
4863  float invTimeStep;
4864  idAFBody *body;
4865  idAFConstraint *c;
4866 
4867  invTimeStep = 1.0f / timeStep;
4868 
4869  // setup the constraint equations for the current position and orientation of the bodies
4870  for ( i = 0; i < primaryConstraints.Num(); i++ ) {
4871  c = primaryConstraints[i];
4872  c->Evaluate( invTimeStep );
4873  c->J = c->J2;
4874  }
4875  for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
4876  auxiliaryConstraints[i]->Evaluate( invTimeStep );
4877  }
4878 
4879  // add contact constraints to the list with frame constraints
4880  for ( i = 0; i < contactConstraints.Num(); i++ ) {
4881  AddFrameConstraint( contactConstraints[i] );
4882  }
4883 
4884  // setup body primary constraint matrix
4885  for ( i = 0; i < bodies.Num(); i++ ) {
4886  body = bodies[i];
4887 
4888  if ( body->primaryConstraint ) {
4889  body->J = body->primaryConstraint->J1.Transpose();
4890  }
4891  }
4892 }
4893 
4894 /*
4895 ================
4896 idPhysics_AF::EvaluateBodies
4897 ================
4898 */
4899 void idPhysics_AF::EvaluateBodies( float timeStep ) {
4900  int i;
4901  idAFBody *body;
4902  idMat3 axis;
4903 
4904  for ( i = 0; i < bodies.Num(); i++ ) {
4905  body = bodies[i];
4906 
4907  // we transpose the axis before using it because idMat3 is column-major
4908  axis = body->current->worldAxis.Transpose();
4909 
4910  // if the center of mass is at the body point of reference
4911  if ( body->centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) {
4912 
4913  // spatial inertia in world space
4914  body->I.Set( body->mass * mat3_identity, mat3_zero,
4915  mat3_zero, axis * body->inertiaTensor * axis.Transpose() );
4916 
4917  // inverse spatial inertia in world space
4918  body->inverseWorldSpatialInertia.Set( body->invMass * mat3_identity, mat3_zero,
4919  mat3_zero, axis * body->inverseInertiaTensor * axis.Transpose() );
4920 
4921  body->fl.spatialInertiaSparse = true;
4922  }
4923  else {
4924  idMat3 massMoment = body->mass * SkewSymmetric( body->centerOfMass );
4925 
4926  // spatial inertia in world space
4927  body->I.Set( body->mass * mat3_identity, massMoment,
4928  massMoment.Transpose(), axis * body->inertiaTensor * axis.Transpose() );
4929 
4930  // inverse spatial inertia in world space
4931  body->inverseWorldSpatialInertia = body->I.InverseFast();
4932 
4933  body->fl.spatialInertiaSparse = false;
4934  }
4935 
4936  // initialize auxiliary constraint force to zero
4937  body->auxForce.Zero();
4938  }
4939 }
4940 
4941 /*
4942 ================
4943 idPhysics_AF::AddFrameConstraints
4944 ================
4945 */
4946 void idPhysics_AF::AddFrameConstraints( void ) {
4947  int i;
4948 
4949  // add frame constraints to auxiliary constraints
4950  for ( i = 0; i < frameConstraints.Num(); i++ ) {
4951  auxiliaryConstraints.Append( frameConstraints[i] );
4952  }
4953 }
4954 
4955 /*
4956 ================
4957 idPhysics_AF::RemoveFrameConstraints
4958 ================
4959 */
4960 void idPhysics_AF::RemoveFrameConstraints( void ) {
4961  // remove all the frame constraints from the auxiliary constraints
4962  auxiliaryConstraints.SetNum( auxiliaryConstraints.Num() - frameConstraints.Num(), false );
4963  frameConstraints.SetNum( 0, false );
4964 }
4965 
4966 /*
4967 ================
4968 idPhysics_AF::ApplyFriction
4969 ================
4970 */
4971 void idPhysics_AF::ApplyFriction( float timeStep, float endTimeMSec ) {
4972  int i;
4973  float invTimeStep;
4974 
4975  if ( af_skipFriction.GetBool() ) {
4976  return;
4977  }
4978 
4979  if ( jointFrictionDentStart < MS2SEC( endTimeMSec ) && jointFrictionDentEnd > MS2SEC( endTimeMSec ) ) {
4980  float halfTime = ( jointFrictionDentEnd - jointFrictionDentStart ) * 0.5f;
4981  if ( jointFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) {
4982  jointFrictionDentScale = 1.0f - ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart ) / halfTime;
4983  } else {
4984  jointFrictionDentScale = jointFrictionDent + ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart - halfTime ) / halfTime;
4985  }
4986  } else {
4987  jointFrictionDentScale = 0.0f;
4988  }
4989 
4990  if ( contactFrictionDentStart < MS2SEC( endTimeMSec ) && contactFrictionDentEnd > MS2SEC( endTimeMSec ) ) {
4991  float halfTime = ( contactFrictionDentEnd - contactFrictionDentStart ) * 0.5f;
4992  if ( contactFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) {
4993  contactFrictionDentScale = 1.0f - ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart ) / halfTime;
4994  } else {
4995  contactFrictionDentScale = contactFrictionDent + ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart - halfTime ) / halfTime;
4996  }
4997  } else {
4998  contactFrictionDentScale = 0.0f;
4999  }
5000 
5001  invTimeStep = 1.0f / timeStep;
5002 
5003  for ( i = 0; i < primaryConstraints.Num(); i++ ) {
5004  primaryConstraints[i]->ApplyFriction( invTimeStep );
5005  }
5006  for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5007  auxiliaryConstraints[i]->ApplyFriction( invTimeStep );
5008  }
5009  for ( i = 0; i < frameConstraints.Num(); i++ ) {
5010  frameConstraints[i]->ApplyFriction( invTimeStep );
5011  }
5012 }
5013 
5014 /*
5015 ================
5016 idPhysics_AF::PrimaryFactor
5017 ================
5018 */
5019 void idPhysics_AF::PrimaryFactor( void ) {
5020  int i;
5021 
5022  for ( i = 0; i < trees.Num(); i++ ) {
5023  trees[i]->Factor();
5024  }
5025 }
5026 
5027 /*
5028 ================
5029 idPhysics_AF::PrimaryForces
5030 ================
5031 */
5032 void idPhysics_AF::PrimaryForces( float timeStep ) {
5033  int i;
5034 
5035  for ( i = 0; i < trees.Num(); i++ ) {
5036  trees[i]->CalculateForces( timeStep );
5037  }
5038 }
5039 
5040 /*
5041 ================
5042 idPhysics_AF::AuxiliaryForces
5043 ================
5044 */
5045 void idPhysics_AF::AuxiliaryForces( float timeStep ) {
5046  int i, j, k, l, n, m, s, numAuxConstraints, *index, *boxIndex;
5047  float *ptr, *j1, *j2, *dstPtr, *forcePtr;
5048  float invStep, u;
5049  idAFBody *body;
5050  idAFConstraint *constraint;
5051  idVecX tmp;
5052  idMatX jmk;
5053  idVecX rhs, w, lm, lo, hi;
5054 
5055  // get the number of one dimensional auxiliary constraints
5056  for ( numAuxConstraints = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5057  numAuxConstraints += auxiliaryConstraints[i]->J1.GetNumRows();
5058  }
5059 
5060  if ( numAuxConstraints == 0 ) {
5061  return;
5062  }
5063 
5064  // allocate memory to store the body response to auxiliary constraint forces
5065  forcePtr = (float *) _alloca16( bodies.Num() * numAuxConstraints * 8 * sizeof( float ) );
5066  index = (int *) _alloca16( bodies.Num() * numAuxConstraints * sizeof( int ) );
5067  for ( i = 0; i < bodies.Num(); i++ ) {
5068  body = bodies[i];
5069  body->response = forcePtr;
5070  body->responseIndex = index;
5071  body->numResponses = 0;
5072  body->maxAuxiliaryIndex = 0;
5073  forcePtr += numAuxConstraints * 8;
5074  index += numAuxConstraints;
5075  }
5076 
5077  // set on each body the largest index of an auxiliary constraint constraining the body
5078  if ( af_useSymmetry.GetBool() ) {
5079  for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5080  constraint = auxiliaryConstraints[i];
5081  for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5082  if ( k > constraint->body1->maxAuxiliaryIndex ) {
5083  constraint->body1->maxAuxiliaryIndex = k;
5084  }
5085  if ( constraint->body2 && k > constraint->body2->maxAuxiliaryIndex ) {
5086  constraint->body2->maxAuxiliaryIndex = k;
5087  }
5088  }
5089  }
5090  for ( i = 0; i < trees.Num(); i++ ) {
5091  trees[i]->SetMaxSubTreeAuxiliaryIndex();
5092  }
5093  }
5094 
5095  // calculate forces of primary constraints in response to the auxiliary constraint forces
5096  for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5097  constraint = auxiliaryConstraints[i];
5098 
5099  for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5100 
5101  // calculate body forces in the tree in response to the constraint force
5102  constraint->body1->tree->Response( constraint, j, k );
5103  // if there is a second body which is part of a different tree
5104  if ( constraint->body2 && constraint->body2->tree != constraint->body1->tree ) {
5105  // calculate body forces in the second tree in response to the constraint force
5106  constraint->body2->tree->Response( constraint, j, k );
5107  }
5108  }
5109  }
5110 
5111  // NOTE: the rows are 16 byte padded
5112  jmk.SetData( numAuxConstraints, ((numAuxConstraints+3)&~3), MATX_ALLOCA( numAuxConstraints * ((numAuxConstraints+3)&~3) ) );
5113  tmp.SetData( 6, VECX_ALLOCA( 6 ) );
5114 
5115  // create constraint matrix for auxiliary constraints using a mass matrix adjusted for the primary constraints
5116  for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5117  constraint = auxiliaryConstraints[i];
5118 
5119  for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5120  constraint->body1->InverseWorldSpatialInertiaMultiply( tmp, constraint->J1[j] );
5121  j1 = tmp.ToFloatPtr();
5122  ptr = constraint->body1->response;
5123  index = constraint->body1->responseIndex;
5124  dstPtr = jmk[k];
5125  s = af_useSymmetry.GetBool() ? k + 1 : numAuxConstraints;
5126  for ( l = n = 0, m = index[n]; n < constraint->body1->numResponses && m < s; n++, m = index[n] ) {
5127  while( l < m ) {
5128  dstPtr[l++] = 0.0f;
5129  }
5130  dstPtr[l++] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] +
5131  j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5];
5132  ptr += 8;
5133  }
5134 
5135  while( l < s ) {
5136  dstPtr[l++] = 0.0f;
5137  }
5138 
5139  if ( constraint->body2 ) {
5140  constraint->body2->InverseWorldSpatialInertiaMultiply( tmp, constraint->J2[j] );
5141  j2 = tmp.ToFloatPtr();
5142  ptr = constraint->body2->response;
5143  index = constraint->body2->responseIndex;
5144  for ( n = 0, m = index[n]; n < constraint->body2->numResponses && m < s; n++, m = index[n] ) {
5145  dstPtr[m] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] +
5146  j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5];
5147  ptr += 8;
5148  }
5149  }
5150  }
5151  }
5152 
5153  if ( af_useSymmetry.GetBool() ) {
5154  n = jmk.GetNumColumns();
5155  for ( i = 0; i < numAuxConstraints; i++ ) {
5156  ptr = jmk.ToFloatPtr() + ( i + 1 ) * n + i;
5157  dstPtr = jmk.ToFloatPtr() + i * n + i + 1;
5158  for ( j = i+1; j < numAuxConstraints; j++ ) {
5159  *dstPtr++ = *ptr;
5160  ptr += n;
5161  }
5162  }
5163  }
5164 
5165  invStep = 1.0f / timeStep;
5166 
5167  // calculate body acceleration
5168  for ( i = 0; i < bodies.Num(); i++ ) {
5169  body = bodies[i];
5170  body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
5171  body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep;
5172  }
5173 
5174  rhs.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
5175  lo.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
5176  hi.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
5177  lm.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
5178  boxIndex = (int *) _alloca16( numAuxConstraints * sizeof( int ) );
5179 
5180  // set first index for special box constrained variables
5181  for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5182  auxiliaryConstraints[i]->firstIndex = k;
5183  k += auxiliaryConstraints[i]->J1.GetNumRows();
5184  }
5185 
5186  // initialize right hand side and low and high bounds for auxiliary constraints
5187  for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5188  constraint = auxiliaryConstraints[i];
5189  n = k;
5190 
5191  for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5192 
5193  j1 = constraint->J1[j];
5194  ptr = constraint->body1->acceleration.ToFloatPtr();
5195  rhs[k] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] + j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5];
5196  rhs[k] += constraint->c1[j] * invStep;
5197 
5198  if ( constraint->body2 ) {
5199  j2 = constraint->J2[j];
5200  ptr = constraint->body2->acceleration.ToFloatPtr();
5201  rhs[k] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] + j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5];
5202  rhs[k] += constraint->c2[j] * invStep;
5203  }
5204 
5205  rhs[k] = -rhs[k];
5206  lo[k] = constraint->lo[j];
5207  hi[k] = constraint->hi[j];
5208 
5209  if ( constraint->boxIndex[j] >= 0 ) {
5210  if ( constraint->boxConstraint->fl.isPrimary ) {
5211  gameLocal.Error( "cannot reference primary constraints for the box index" );
5212  }
5213  boxIndex[k] = constraint->boxConstraint->firstIndex + constraint->boxIndex[j];
5214  }
5215  else {
5216  boxIndex[k] = -1;
5217  }
5218  jmk[k][k] += constraint->e[j] * invStep;
5219  }
5220  }
5221 
5222 #ifdef AF_TIMINGS
5223  timer_lcp.Start();
5224 #endif
5225 
5226  // calculate lagrange multipliers for auxiliary constraints
5227  if ( !lcp->Solve( jmk, lm, rhs, lo, hi, boxIndex ) ) {
5228  return; // bad monkey!
5229  }
5230 
5231 #ifdef AF_TIMINGS
5232  timer_lcp.Stop();
5233 #endif
5234 
5235  // calculate auxiliary constraint forces
5236  for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5237  constraint = auxiliaryConstraints[i];
5238 
5239  for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5240  constraint->lm[j] = u = lm[k];
5241 
5242  j1 = constraint->J1[j];
5243  ptr = constraint->body1->auxForce.ToFloatPtr();
5244  ptr[0] += j1[0] * u; ptr[1] += j1[1] * u; ptr[2] += j1[2] * u;
5245  ptr[3] += j1[3] * u; ptr[4] += j1[4] * u; ptr[5] += j1[5] * u;
5246 
5247  if ( constraint->body2 ) {
5248  j2 = constraint->J2[j];
5249  ptr = constraint->body2->auxForce.ToFloatPtr();
5250  ptr[0] += j2[0] * u; ptr[1] += j2[1] * u; ptr[2] += j2[2] * u;
5251  ptr[3] += j2[3] * u; ptr[4] += j2[4] * u; ptr[5] += j2[5] * u;
5252  }
5253  }
5254  }
5255 
5256  // recalculate primary constraint forces in response to auxiliary constraint forces
5257  PrimaryForces( timeStep );
5258 
5259  // clear pointers pointing to stack space so tools don't get confused
5260  for ( i = 0; i < bodies.Num(); i++ ) {
5261  body = bodies[i];
5262  body->response = NULL;
5263  body->responseIndex = NULL;
5264  }
5265 }
5266 
5267 /*
5268 ================
5269 idPhysics_AF::VerifyContactConstraints
5270 ================
5271 */
5272 void idPhysics_AF::VerifyContactConstraints( void ) {
5273 #if 0
5274  int i;
5275  float impulseNumerator, impulseDenominator;
5276  idVec3 r, velocity, normalVelocity, normal, impulse;
5277  idAFBody *body;
5278 
5279  for ( i = 0; i < contactConstraints.Num(); i++ ) {
5280  body = contactConstraints[i]->body1;
5281  const contactInfo_t &contact = contactConstraints[i]->GetContact();
5282 
5283  r = contact.point - body->GetCenterOfMass();
5284 
5285  // calculate velocity at contact point
5286  velocity = body->GetLinearVelocity() + body->GetAngularVelocity().Cross( r );
5287 
5288  // velocity along normal vector
5289  normalVelocity = ( velocity * contact.normal ) * contact.normal;
5290 
5291  // if moving towards the surface at the contact point
5292  if ( normalVelocity * contact.normal < 0.0f ) {
5293  // calculate impulse
5294  normal = -normalVelocity;
5295  impulseNumerator = normal.Normalize();
5296  impulseDenominator = body->GetInverseMass() + ( ( body->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal );
5297  impulse = (impulseNumerator / impulseDenominator) * normal * 1.0001f;
5298 
5299  // apply impulse
5300  body->SetLinearVelocity( body->GetLinearVelocity() + impulse );
5301  body->SetAngularVelocity( body->GetAngularVelocity() + r.Cross( impulse ) );
5302  }
5303  }
5304 #else
5305  int i;
5306  idAFBody *body;
5307  idVec3 normal;
5308 
5309  for ( i = 0; i < contactConstraints.Num(); i++ ) {
5310  body = contactConstraints[i]->body1;
5311  normal = contactConstraints[i]->GetContact().normal;
5312  if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) {
5313  body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal;
5314  }
5315  body = contactConstraints[i]->body2;
5316  if ( !body ) {
5317  continue;
5318  }
5319  normal = -normal;
5320  if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) {
5321  body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal;
5322  }
5323  }
5324 #endif
5325 }
5326 
5327 /*
5328 ================
5329 idPhysics_AF::Evolve
5330 ================
5331 */
5332 void idPhysics_AF::Evolve( float timeStep ) {
5333  int i;
5334  float angle;
5335  idVec3 vec;
5336  idAFBody *body;
5337  idVec6 force;
5338  idRotation rotation;
5339  float vSqr, maxLinearVelocity, maxAngularVelocity;
5340 
5341  maxLinearVelocity = af_maxLinearVelocity.GetFloat() / timeStep;
5342  maxAngularVelocity = af_maxAngularVelocity.GetFloat() / timeStep;
5343 
5344  for ( i = 0; i < bodies.Num(); i++ ) {
5345  body = bodies[i];
5346 
5347  // calculate the spatial velocity for the next physics state
5348  body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
5349  body->next->spatialVelocity = body->current->spatialVelocity + timeStep * body->acceleration.SubVec6(0);
5350 
5351  if ( maxLinearVelocity > 0.0f ) {
5352  // cap the linear velocity
5353  vSqr = body->next->spatialVelocity.SubVec3(0).LengthSqr();
5354  if ( vSqr > Square( maxLinearVelocity ) ) {
5355  body->next->spatialVelocity.SubVec3(0) *= idMath::InvSqrt( vSqr ) * maxLinearVelocity;
5356  }
5357  }
5358 
5359  if ( maxAngularVelocity > 0.0f ) {
5360  // cap the angular velocity
5361  vSqr = body->next->spatialVelocity.SubVec3(1).LengthSqr();
5362  if ( vSqr > Square( maxAngularVelocity ) ) {
5363  body->next->spatialVelocity.SubVec3(1) *= idMath::InvSqrt( vSqr ) * maxAngularVelocity;
5364  }
5365  }
5366  }
5367 
5368  // make absolutely sure all contact constraints are satisfied
5369  VerifyContactConstraints();
5370 
5371  // calculate the position of the bodies for the next physics state
5372  for ( i = 0; i < bodies.Num(); i++ ) {
5373  body = bodies[i];
5374 
5375  // translate world origin
5376  body->next->worldOrigin = body->current->worldOrigin + timeStep * body->next->spatialVelocity.SubVec3( 0 );
5377 
5378  // convert angular velocity to a rotation matrix
5379  vec = body->next->spatialVelocity.SubVec3( 1 );
5380  angle = -timeStep * (float) RAD2DEG( vec.Normalize() );
5381  rotation = idRotation( vec3_origin, vec, angle );
5382  rotation.Normalize180();
5383 
5384  // rotate world axis
5385  body->next->worldAxis = body->current->worldAxis * rotation.ToMat3();
5386  body->next->worldAxis.OrthoNormalizeSelf();
5387 
5388  // linear and angular friction
5389  body->next->spatialVelocity.SubVec3(0) -= body->linearFriction * body->next->spatialVelocity.SubVec3(0);
5390  body->next->spatialVelocity.SubVec3(1) -= body->angularFriction * body->next->spatialVelocity.SubVec3(1);
5391  }
5392 }
5393 
5394 /*
5395 ================
5396 idPhysics_AF::CollisionImpulse
5397 
5398  apply impulse to the colliding bodies
5399  the current state of the body should be set to the moment of impact
5400  this is silly as it doesn't take the AF structure into account
5401 ================
5402 */
5403 bool idPhysics_AF::CollisionImpulse( float timeStep, idAFBody *body, trace_t &collision ) {
5404  idVec3 r, velocity, impulse;
5405  idMat3 inverseWorldInertiaTensor;
5406  float impulseNumerator, impulseDenominator;
5407  impactInfo_t info;
5408  idEntity *ent;
5409 
5410  ent = gameLocal.entities[collision.c.entityNum];
5411  if ( ent == self ) {
5412  return false;
5413  }
5414 
5415  // get info from other entity involved
5416  ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info );
5417  // collision point relative to the body center of mass
5418  r = collision.c.point - (body->current->worldOrigin + body->centerOfMass * body->current->worldAxis);
5419  // the velocity at the collision point
5420  velocity = body->current->spatialVelocity.SubVec3(0) + body->current->spatialVelocity.SubVec3(1).Cross(r);
5421  // subtract velocity of other entity
5422  velocity -= info.velocity;
5423  // never stick
5424  if ( velocity * collision.c.normal > 0.0f ) {
5425  velocity = collision.c.normal;
5426  }
5427  inverseWorldInertiaTensor = body->current->worldAxis.Transpose() * body->inverseInertiaTensor * body->current->worldAxis;
5428  impulseNumerator = -( 1.0f + body->bouncyness ) * ( velocity * collision.c.normal );
5429  impulseDenominator = body->invMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal );
5430  if ( info.invMass ) {
5431  impulseDenominator += info.invMass + ( ( info.invInertiaTensor * info.position.Cross( collision.c.normal ) ).Cross( info.position ) * collision.c.normal );
5432  }
5433  impulse = (impulseNumerator / impulseDenominator) * collision.c.normal;
5434 
5435  // apply impact to other entity
5436  ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse );
5437 
5438  // callback to self to let the entity know about the impact
5439  return self->Collide( collision, velocity );
5440 }
5441 
5442 /*
5443 ================
5444 idPhysics_AF::ApplyCollisions
5445 ================
5446 */
5447 bool idPhysics_AF::ApplyCollisions( float timeStep ) {
5448  int i;
5449 
5450  for ( i = 0; i < collisions.Num(); i++ ) {
5451  if ( CollisionImpulse( timeStep, collisions[i].body, collisions[i].trace ) ) {
5452  return true;
5453  }
5454  }
5455  return false;
5456 }
5457 
5458 /*
5459 ================
5460 idPhysics_AF::SetupCollisionForBody
5461 ================
5462 */
5463 idEntity *idPhysics_AF::SetupCollisionForBody( idAFBody *body ) const {
5464  int i;
5465  idAFBody *b;
5466  idEntity *passEntity;
5467 
5468  passEntity = NULL;
5469 
5470  if ( !selfCollision || !body->fl.selfCollision || af_skipSelfCollision.GetBool() ) {
5471 
5472  // disable all bodies
5473  for ( i = 0; i < bodies.Num(); i++ ) {
5474  bodies[i]->clipModel->Disable();
5475  }
5476 
5477  // don't collide with world collision model if attached to the world
5478  for ( i = 0; i < body->constraints.Num(); i++ ) {
5479  if ( !body->constraints[i]->fl.noCollision ) {
5480  continue;
5481  }
5482  // if this constraint attaches the body to the world
5483  if ( body->constraints[i]->body2 == NULL ) {
5484  // don't collide with the world collision model
5485  passEntity = gameLocal.world;
5486  }
5487  }
5488 
5489  } else {
5490 
5491  // enable all bodies that have self collision
5492  for ( i = 0; i < bodies.Num(); i++ ) {
5493  if ( bodies[i]->fl.selfCollision ) {
5494  bodies[i]->clipModel->Enable();
5495  } else {
5496  bodies[i]->clipModel->Disable();
5497  }
5498  }
5499 
5500  // don't let the body collide with itself
5501  body->clipModel->Disable();
5502 
5503  // disable any bodies attached with constraints
5504  for ( i = 0; i < body->constraints.Num(); i++ ) {
5505  if ( !body->constraints[i]->fl.noCollision ) {
5506  continue;
5507  }
5508  // if this constraint attaches the body to the world
5509  if ( body->constraints[i]->body2 == NULL ) {
5510  // don't collide with the world collision model
5511  passEntity = gameLocal.world;
5512  } else {
5513  if ( body->constraints[i]->body1 == body ) {
5514  b = body->constraints[i]->body2;
5515  } else if ( body->constraints[i]->body2 == body ) {
5516  b = body->constraints[i]->body1;
5517  } else {
5518  continue;
5519  }
5520  // don't collide with this body
5521  b->clipModel->Disable();
5522  }
5523  }
5524  }
5525 
5526  return passEntity;
5527 }
5528 
5529 /*
5530 ================
5531 idPhysics_AF::CheckForCollisions
5532 
5533  check for collisions between the current and next state
5534  if there is a collision the next state is set to the state at the moment of impact
5535  assumes all bodies are linked for collision detection and relinks all bodies after moving them
5536 ================
5537 */
5538 void idPhysics_AF::CheckForCollisions( float timeStep ) {
5539 // #define TEST_COLLISION_DETECTION
5540  int i, index;
5541  idAFBody *body;
5542  idMat3 axis;
5543  idRotation rotation;
5544  trace_t collision;
5545  idEntity *passEntity;
5546 
5547  // clear list with collisions
5548  collisions.SetNum( 0, false );
5549 
5550  if ( !enableCollision ) {
5551  return;
5552  }
5553 
5554  for ( i = 0; i < bodies.Num(); i++ ) {
5555  body = bodies[i];
5556 
5557  if ( body->clipMask != 0 ) {
5558 
5559  passEntity = SetupCollisionForBody( body );
5560 
5561 #ifdef TEST_COLLISION_DETECTION
5562  bool startsolid = false;
5563  if ( gameLocal.clip.Contents( body->current->worldOrigin, body->clipModel,
5564  body->current->worldAxis, body->clipMask, passEntity ) ) {
5565  startsolid = true;
5566  }
5567 #endif
5568 
5569  TransposeMultiply( body->current->worldAxis, body->next->worldAxis, axis );
5570  rotation = axis.ToRotation();
5571  rotation.SetOrigin( body->current->worldOrigin );
5572 
5573  // if there was a collision
5574  if ( gameLocal.clip.Motion( collision, body->current->worldOrigin, body->next->worldOrigin, rotation,
5575  body->clipModel, body->current->worldAxis, body->clipMask, passEntity ) ) {
5576 
5577  // set the next state to the state at the moment of impact
5578  body->next->worldOrigin = collision.endpos;
5579  body->next->worldAxis = collision.endAxis;
5580 
5581  // add collision to the list
5582  index = collisions.Num();
5583  collisions.SetNum( index + 1, false );
5584  collisions[index].trace = collision;
5585  collisions[index].body = body;
5586  }
5587 
5588 #ifdef TEST_COLLISION_DETECTION
5589  if ( gameLocal.clip.Contents( body->next->worldOrigin, body->clipModel,
5590  body->next->worldAxis, body->clipMask, passEntity ) ) {
5591  if ( !startsolid ) {
5592  int bah = 1;
5593  }
5594  }
5595 #endif
5596  }
5597 
5598  body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->next->worldOrigin, body->next->worldAxis );
5599  }
5600 }
5601 
5602 /*
5603 ================
5604 idPhysics_AF::EvaluateContacts
5605 ================
5606 */
5607 bool idPhysics_AF::EvaluateContacts( void ) {
5608  int i, j, k, numContacts, numBodyContacts;
5609  idAFBody *body;
5610  contactInfo_t contactInfo[10];
5611  idEntity *passEntity;
5612  idVecX dir( 6, VECX_ALLOCA( 6 ) );
5613 
5614  // evaluate bodies
5615  EvaluateBodies( current.lastTimeStep );
5616 
5617  // remove all existing contacts
5618  ClearContacts();
5619 
5620  contactBodies.SetNum( 0, false );
5621 
5622  if ( !enableCollision ) {
5623  return false;
5624  }
5625 
5626  // find all the contacts
5627  for ( i = 0; i < bodies.Num(); i++ ) {
5628  body = bodies[i];
5629 
5630  if ( body->clipMask == 0 ) {
5631  continue;
5632  }
5633 
5634  passEntity = SetupCollisionForBody( body );
5635 
5636  body->InverseWorldSpatialInertiaMultiply( dir, body->current->externalForce.ToFloatPtr() );
5637  dir.SubVec6(0) = body->current->spatialVelocity + current.lastTimeStep * dir.SubVec6(0);
5638  dir.SubVec3(0).Normalize();
5639  dir.SubVec3(1).Normalize();
5640 
5641  numContacts = gameLocal.clip.Contacts( contactInfo, 10, body->current->worldOrigin, dir.SubVec6(0), 2.0f, //CONTACT_EPSILON,
5642  body->clipModel, body->current->worldAxis, body->clipMask, passEntity );
5643 
5644 #if 1
5645  // merge nearby contacts between the same bodies
5646  // and assure there are at most three planar contacts between any pair of bodies
5647  for ( j = 0; j < numContacts; j++ ) {
5648 
5649  numBodyContacts = 0;
5650  for ( k = 0; k < contacts.Num(); k++ ) {
5651  if ( contacts[k].entityNum == contactInfo[j].entityNum ) {
5652  if ( ( contacts[k].id == i && contactInfo[j].id == contactBodies[k] ) ||
5653  ( contactBodies[k] == i && contacts[k].id == contactInfo[j].id ) ) {
5654 
5655  if ( ( contacts[k].point - contactInfo[j].point ).LengthSqr() < Square( 2.0f ) ) {
5656  break;
5657  }
5658  if ( idMath::Fabs( contacts[k].normal * contactInfo[j].normal ) > 0.9f ) {
5659  numBodyContacts++;
5660  }
5661  }
5662  }
5663  }
5664 
5665  if ( k >= contacts.Num() && numBodyContacts < 3 ) {
5666  contacts.Append( contactInfo[j] );
5667  contactBodies.Append( i );
5668  }
5669  }
5670 
5671 #else
5672 
5673  for ( j = 0; j < numContacts; j++ ) {
5674  contacts.Append( contactInfo[j] );
5675  contactBodies.Append( i );
5676  }
5677 #endif
5678 
5679  }
5680 
5681  AddContactEntitiesForContacts();
5682 
5683  return ( contacts.Num() != 0 );
5684 }
5685 
5686 /*
5687 ================
5688 idPhysics_AF::SetupContactConstraints
5689 ================
5690 */
5691 void idPhysics_AF::SetupContactConstraints( void ) {
5692  int i;
5693 
5694  // make sure enough contact constraints are allocated
5695  contactConstraints.AssureSizeAlloc( contacts.Num(), idListNewElement<idAFConstraint_Contact> );
5696  contactConstraints.SetNum( contacts.Num(), false );
5697 
5698  // setup contact constraints
5699  for ( i = 0; i < contacts.Num(); i++ ) {
5700  // add contact constraint
5701  contactConstraints[i]->physics = this;
5702  if ( contacts[i].entityNum == self->entityNumber ) {
5703  contactConstraints[i]->Setup( bodies[contactBodies[i]], bodies[ contacts[i].id ], contacts[i] );
5704  }
5705  else {
5706  contactConstraints[i]->Setup( bodies[contactBodies[i]], NULL, contacts[i] );
5707  }
5708  }
5709 }
5710 
5711 /*
5712 ================
5713 idPhysics_AF::ApplyContactForces
5714 ================
5715 */
5716 void idPhysics_AF::ApplyContactForces( void ) {
5717 #if 0
5718  int i;
5719  idEntity *ent;
5720  idVec3 force;
5721 
5722  for ( i = 0; i < contactConstraints.Num(); i++ ) {
5723  if ( contactConstraints[i]->body2 != NULL ) {
5724  continue;
5725  }
5726  const contactInfo_t &contact = contactConstraints[i]->GetContact();
5727  ent = gameLocal.entities[contact.entityNum];
5728  if ( !ent ) {
5729  continue;
5730  }
5731  force.Zero();
5732  ent->AddForce( self, contact.id, contact.point, force );
5733  }
5734 #endif
5735 }
5736 
5737 /*
5738 ================
5739 idPhysics_AF::ClearExternalForce
5740 ================
5741 */
5742 void idPhysics_AF::ClearExternalForce( void ) {
5743  int i;
5744  idAFBody *body;
5745 
5746  for ( i = 0; i < bodies.Num(); i++ ) {
5747  body = bodies[i];
5748 
5749  // clear external force
5750  body->current->externalForce.Zero();
5751  body->next->externalForce.Zero();
5752  }
5753 }
5754 
5755 /*
5756 ================
5757 idPhysics_AF::AddGravity
5758 ================
5759 */
5760 void idPhysics_AF::AddGravity( void ) {
5761  int i;
5762  idAFBody *body;
5763 
5764  for ( i = 0; i < bodies.Num(); i++ ) {
5765  body = bodies[i];
5766  // add gravitational force
5767  body->current->externalForce.SubVec3( 0 ) += body->mass * gravityVector;
5768  }
5769 }
5770 
5771 /*
5772 ================
5773 idPhysics_AF::SwapStates
5774 ================
5775 */
5776 void idPhysics_AF::SwapStates( void ) {
5777  int i;
5778  idAFBody *body;
5779  AFBodyPState_t *swap;
5780 
5781  for ( i = 0; i < bodies.Num(); i++ ) {
5782 
5783  body = bodies[i];
5784 
5785  // swap the current and next state for next simulation step
5786  swap = body->current;
5787  body->current = body->next;
5788  body->next = swap;
5789  }
5790 }
5791 
5792 /*
5793 ================
5794 idPhysics_AF::UpdateClipModels
5795 ================
5796 */
5797 void idPhysics_AF::UpdateClipModels( void ) {
5798  int i;
5799  idAFBody *body;
5800 
5801  for ( i = 0; i < bodies.Num(); i++ ) {
5802  body = bodies[i];
5803  body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->current->worldOrigin, body->current->worldAxis );
5804  }
5805 }
5806 
5807 /*
5808 ================
5809 idPhysics_AF::SetSuspendSpeed
5810 ================
5811 */
5812 void idPhysics_AF::SetSuspendSpeed( const idVec2 &velocity, const idVec2 &acceleration ) {
5813  this->suspendVelocity = velocity;
5814  this->suspendAcceleration = acceleration;
5815 }
5816 
5817 /*
5818 ================
5819 idPhysics_AF::SetSuspendTime
5820 ================
5821 */
5822 void idPhysics_AF::SetSuspendTime( const float minTime, const float maxTime ) {
5823  this->minMoveTime = minTime;
5824  this->maxMoveTime = maxTime;
5825 }
5826 
5827 /*
5828 ================
5829 idPhysics_AF::SetSuspendTolerance
5830 ================
5831 */
5832 void idPhysics_AF::SetSuspendTolerance( const float noMoveTime, const float noMoveTranslation, const float noMoveRotation ) {
5833  this->noMoveTime = noMoveTime;
5834  this->noMoveTranslation = noMoveTranslation;
5835  this->noMoveRotation = noMoveRotation;
5836 }
5837 
5838 /*
5839 ================
5840 idPhysics_AF::SetTimeScaleRamp
5841 ================
5842 */
5843 void idPhysics_AF::SetTimeScaleRamp( const float start, const float end ) {
5844  timeScaleRampStart = start;
5845  timeScaleRampEnd = end;
5846 }
5847 
5848 /*
5849 ================
5850 idPhysics_AF::SetJointFrictionDent
5851 ================
5852 */
5853 void idPhysics_AF::SetJointFrictionDent( const float dent, const float start, const float end ) {
5854  jointFrictionDent = dent;
5855  jointFrictionDentStart = start;
5856  jointFrictionDentEnd = end;
5857 }
5858 
5859 /*
5860 ================
5861 idPhysics_AF::GetJointFrictionScale
5862 ================
5863 */
5864 float idPhysics_AF::GetJointFrictionScale( void ) const {
5865  if ( jointFrictionDentScale > 0.0f ) {
5866  return jointFrictionDentScale;
5867  } else if ( jointFrictionScale > 0.0f ) {
5868  return jointFrictionScale;
5869  } else if ( af_jointFrictionScale.GetFloat() > 0.0f ) {
5870  return af_jointFrictionScale.GetFloat();
5871  }
5872  return 1.0f;
5873 }
5874 
5875 /*
5876 ================
5877 idPhysics_AF::SetContactFrictionDent
5878 ================
5879 */
5880 void idPhysics_AF::SetContactFrictionDent( const float dent, const float start, const float end ) {
5881  contactFrictionDent = dent;
5882  contactFrictionDentStart = start;
5883  contactFrictionDentEnd = end;
5884 }
5885 
5886 /*
5887 ================
5888 idPhysics_AF::GetContactFrictionScale
5889 ================
5890 */
5891 float idPhysics_AF::GetContactFrictionScale( void ) const {
5892  if ( contactFrictionDentScale > 0.0f ) {
5893  return contactFrictionDentScale;
5894  } else if ( contactFrictionScale > 0.0f ) {
5895  return contactFrictionScale;
5896  } else if ( af_contactFrictionScale.GetFloat() > 0.0f ) {
5897  return af_contactFrictionScale.GetFloat();
5898  }
5899  return 1.0f;
5900 }
5901 
5902 /*
5903 ================
5904 idPhysics_AF::TestIfAtRest
5905 ================
5906 */
5907 bool idPhysics_AF::TestIfAtRest( float timeStep ) {
5908  int i;
5909  float translationSqr, maxTranslationSqr, rotation, maxRotation;
5910  idAFBody *body;
5911 
5912  if ( current.atRest >= 0 ) {
5913  return true;
5914  }
5915 
5916  current.activateTime += timeStep;
5917 
5918  // if the simulation should never be suspended before a certaint amount of time passed
5919  if ( minMoveTime > 0.0f && current.activateTime < minMoveTime ) {
5920  return false;
5921  }
5922 
5923  // if the simulation should always be suspended after a certain amount time passed
5924  if ( maxMoveTime > 0.0f && current.activateTime > maxMoveTime ) {
5925  return true;
5926  }
5927 
5928  // test if all bodies hardly moved over a period of time
5929  if ( current.noMoveTime == 0.0f ) {
5930  for ( i = 0; i < bodies.Num(); i++ ) {
5931  body = bodies[i];
5932  body->atRestOrigin = body->current->worldOrigin;
5933  body->atRestAxis = body->current->worldAxis;
5934  }
5935  current.noMoveTime += timeStep;
5936  }
5937  else if ( current.noMoveTime > noMoveTime ) {
5938  current.noMoveTime = 0.0f;
5939  maxTranslationSqr = 0.0f;
5940  maxRotation = 0.0f;
5941  for ( i = 0; i < bodies.Num(); i++ ) {
5942  body = bodies[i];
5943 
5944  translationSqr = ( body->current->worldOrigin - body->atRestOrigin ).LengthSqr();
5945  if ( translationSqr > maxTranslationSqr ) {
5946  maxTranslationSqr = translationSqr;
5947  }
5948  rotation = ( body->atRestAxis.Transpose() * body->current->worldAxis ).ToRotation().GetAngle();
5949  if ( rotation > maxRotation ) {
5950  maxRotation = rotation;
5951  }
5952  }
5953 
5954  if ( maxTranslationSqr < Square( noMoveTranslation ) && maxRotation < noMoveRotation ) {
5955  // hardly moved over a period of time so the articulated figure may come to rest
5956  return true;
5957  }
5958  } else {
5959  current.noMoveTime += timeStep;
5960  }
5961 
5962  // test if the velocity or acceleration of any body is still too large to come to rest
5963  for ( i = 0; i < bodies.Num(); i++ ) {
5964  body = bodies[i];
5965 
5966  if ( body->current->spatialVelocity.SubVec3(0).LengthSqr() > Square( suspendVelocity[0] ) ) {
5967  return false;
5968  }
5969  if ( body->current->spatialVelocity.SubVec3(1).LengthSqr() > Square( suspendVelocity[1] ) ) {
5970  return false;
5971  }
5972  if ( body->acceleration.SubVec3(0).LengthSqr() > Square( suspendAcceleration[0] ) ) {
5973  return false;
5974  }
5975  if ( body->acceleration.SubVec3(1).LengthSqr() > Square( suspendAcceleration[1] ) ) {
5976  return false;
5977  }
5978  }
5979 
5980  // all bodies have a velocity and acceleration small enough to come to rest
5981  return true;
5982 }
5983 
5984 /*
5985 ================
5986 idPhysics_AF::Rest
5987 ================
5988 */
5989 void idPhysics_AF::Rest( void ) {
5990  int i;
5991 
5992  current.atRest = gameLocal.time;
5993 
5994  for ( i = 0; i < bodies.Num(); i++ ) {
5995  bodies[i]->current->spatialVelocity.Zero();
5996  bodies[i]->current->externalForce.Zero();
5997  }
5998 
5999  self->BecomeInactive( TH_PHYSICS );
6000 }
6001 
6002 /*
6003 ================
6004 idPhysics_AF::Activate
6005 ================
6006 */
6007 void idPhysics_AF::Activate( void ) {
6008  // if the articulated figure was at rest
6009  if ( current.atRest >= 0 ) {
6010  // normally gravity is added at the end of a simulation frame
6011  // if the figure was at rest add gravity here so it is applied this simulation frame
6012  AddGravity();
6013  // reset the active time for the max move time
6014  current.activateTime = 0.0f;
6015  }
6016  current.atRest = -1;
6017  current.noMoveTime = 0.0f;
6018  self->BecomeActive( TH_PHYSICS );
6019 }
6020 
6021 /*
6022 ================
6023 idPhysics_AF::PutToRest
6024 
6025  put to rest untill something collides with this physics object
6026 ================
6027 */
6028 void idPhysics_AF::PutToRest( void ) {
6029  Rest();
6030 }
6031 
6032 /*
6033 ================
6034 idPhysics_AF::EnableImpact
6035 ================
6036 */
6037 void idPhysics_AF::EnableImpact( void ) {
6038  noImpact = false;
6039 }
6040 
6041 /*
6042 ================
6043 idPhysics_AF::DisableImpact
6044 ================
6045 */
6046 void idPhysics_AF::DisableImpact( void ) {
6047  noImpact = true;
6048 }
6049 
6050 /*
6051 ================
6052 idPhysics_AF::AddPushVelocity
6053 ================
6054 */
6055 void idPhysics_AF::AddPushVelocity( const idVec6 &pushVelocity ) {
6056  int i;
6057 
6058  if ( pushVelocity != vec6_origin ) {
6059  for ( i = 0; i < bodies.Num(); i++ ) {
6060  bodies[i]->current->spatialVelocity += pushVelocity;
6061  }
6062  }
6063 }
6064 
6065 /*
6066 ================
6067 idPhysics_AF::SetClipModel
6068 ================
6069 */
6070 void idPhysics_AF::SetClipModel( idClipModel *model, float density, int id, bool freeOld ) {
6071 }
6072 
6073 /*
6074 ================
6075 idPhysics_AF::GetClipModel
6076 ================
6077 */
6078 idClipModel *idPhysics_AF::GetClipModel( int id ) const {
6079  if ( id >= 0 && id < bodies.Num() ) {
6080  return bodies[id]->GetClipModel();
6081  }
6082  return NULL;
6083 }
6084 
6085 /*
6086 ================
6087 idPhysics_AF::GetNumClipModels
6088 ================
6089 */
6090 int idPhysics_AF::GetNumClipModels( void ) const {
6091  return bodies.Num();
6092 }
6093 
6094 /*
6095 ================
6096 idPhysics_AF::SetMass
6097 ================
6098 */
6099 void idPhysics_AF::SetMass( float mass, int id ) {
6100  if ( id >= 0 && id < bodies.Num() ) {
6101  }
6102  else {
6103  forceTotalMass = mass;
6104  }
6105  SetChanged();
6106 }
6107 
6108 /*
6109 ================
6110 idPhysics_AF::GetMass
6111 ================
6112 */
6113 float idPhysics_AF::GetMass( int id ) const {
6114  if ( id >= 0 && id < bodies.Num() ) {
6115  return bodies[id]->mass;
6116  }
6117  return totalMass;
6118 }
6119 
6120 /*
6121 ================
6122 idPhysics_AF::SetContents
6123 ================
6124 */
6125 void idPhysics_AF::SetContents( int contents, int id ) {
6126  int i;
6127 
6128  if ( id >= 0 && id < bodies.Num() ) {
6129  bodies[id]->GetClipModel()->SetContents( contents );
6130  }
6131  else {
6132  for ( i = 0; i < bodies.Num(); i++ ) {
6133  bodies[i]->GetClipModel()->SetContents( contents );
6134  }
6135  }
6136 }
6137 
6138 /*
6139 ================
6140 idPhysics_AF::GetContents
6141 ================
6142 */
6143 int idPhysics_AF::GetContents( int id ) const {
6144  int i, contents;
6145 
6146  if ( id >= 0 && id < bodies.Num() ) {
6147  return bodies[id]->GetClipModel()->GetContents();
6148  }
6149  else {
6150  contents = 0;
6151  for ( i = 0; i < bodies.Num(); i++ ) {
6152  contents |= bodies[i]->GetClipModel()->GetContents();
6153  }
6154  return contents;
6155  }
6156 }
6157 
6158 /*
6159 ================
6160 idPhysics_AF::GetBounds
6161 ================
6162 */
6163 const idBounds &idPhysics_AF::GetBounds( int id ) const {
6164  int i;
6165  static idBounds relBounds;
6166 
6167  if ( id >= 0 && id < bodies.Num() ) {
6168  return bodies[id]->GetClipModel()->GetBounds();
6169  }
6170  else if ( !bodies.Num() ) {
6171  relBounds.Zero();
6172  return relBounds;
6173  }
6174  else {
6175  relBounds = bodies[0]->GetClipModel()->GetBounds();
6176  for ( i = 1; i < bodies.Num(); i++ ) {
6177  idBounds bounds;
6178  idVec3 origin = ( bodies[i]->GetWorldOrigin() - bodies[0]->GetWorldOrigin() ) * bodies[0]->GetWorldAxis().Transpose();
6179  idMat3 axis = bodies[i]->GetWorldAxis() * bodies[0]->GetWorldAxis().Transpose();
6180  bounds.FromTransformedBounds( bodies[i]->GetClipModel()->GetBounds(), origin, axis );
6181  relBounds += bounds;
6182  }
6183  return relBounds;
6184  }
6185 }
6186 
6187 /*
6188 ================
6189 idPhysics_AF::GetAbsBounds
6190 ================
6191 */
6192 const idBounds &idPhysics_AF::GetAbsBounds( int id ) const {
6193  int i;
6194  static idBounds absBounds;
6195 
6196  if ( id >= 0 && id < bodies.Num() ) {
6197  return bodies[id]->GetClipModel()->GetAbsBounds();
6198  }
6199  else if ( !bodies.Num() ) {
6200  absBounds.Zero();
6201  return absBounds;
6202  }
6203  else {
6204  absBounds = bodies[0]->GetClipModel()->GetAbsBounds();
6205  for ( i = 1; i < bodies.Num(); i++ ) {
6206  absBounds += bodies[i]->GetClipModel()->GetAbsBounds();
6207  }
6208  return absBounds;
6209  }
6210 }
6211 
6212 /*
6213 ================
6214 idPhysics_AF::Evaluate
6215 ================
6216 */
6217 bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) {
6218  float timeStep;
6219 
6220  if ( timeScaleRampStart < MS2SEC( endTimeMSec ) && timeScaleRampEnd > MS2SEC( endTimeMSec ) ) {
6221  timeStep = MS2SEC( timeStepMSec ) * ( MS2SEC( endTimeMSec ) - timeScaleRampStart ) / ( timeScaleRampEnd - timeScaleRampStart );
6222  } else if ( af_timeScale.GetFloat() != 1.0f ) {
6223  timeStep = MS2SEC( timeStepMSec ) * af_timeScale.GetFloat();
6224  } else {
6225  timeStep = MS2SEC( timeStepMSec ) * timeScale;
6226  }
6227  current.lastTimeStep = timeStep;
6228 
6229 
6230  // if the articulated figure changed
6231  if ( changedAF || ( linearTime != af_useLinearTime.GetBool() ) ) {
6232  BuildTrees();
6233  changedAF = false;
6234  linearTime = af_useLinearTime.GetBool();
6235  }
6236 
6237  // get the new master position
6238  if ( masterBody ) {
6239  idVec3 masterOrigin;
6240  idMat3 masterAxis;
6241  self->GetMasterPosition( masterOrigin, masterAxis );
6242  if ( current.atRest >= 0 && ( masterBody->current->worldOrigin != masterOrigin || masterBody->current->worldAxis != masterAxis ) ) {
6243  Activate();
6244  }
6245  masterBody->current->worldOrigin = masterOrigin;
6246  masterBody->current->worldAxis = masterAxis;
6247  }
6248 
6249  // if the simulation is suspended because the figure is at rest
6250  if ( current.atRest >= 0 || timeStep <= 0.0f ) {
6251  DebugDraw();
6252  return false;
6253  }
6254 
6255  // move the af velocity into the frame of a pusher
6256  AddPushVelocity( -current.pushVelocity );
6257 
6258 #ifdef AF_TIMINGS
6259  timer_total.Start();
6260 #endif
6261 
6262 #ifdef AF_TIMINGS
6263  timer_collision.Start();
6264 #endif
6265 
6266  // evaluate contacts
6267  EvaluateContacts();
6268 
6269  // setup contact constraints
6270  SetupContactConstraints();
6271 
6272 #ifdef AF_TIMINGS
6273  timer_collision.Stop();
6274 #endif
6275 
6276  // evaluate constraint equations
6277  EvaluateConstraints( timeStep );
6278 
6279  // apply friction
6280  ApplyFriction( timeStep, endTimeMSec );
6281 
6282  // add frame constraints
6283  AddFrameConstraints();
6284 
6285 #ifdef AF_TIMINGS
6286  int i, numPrimary = 0, numAuxiliary = 0;
6287  for ( i = 0; i < primaryConstraints.Num(); i++ ) {
6288  numPrimary += primaryConstraints[i]->J1.GetNumRows();
6289  }
6290  for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
6291  numAuxiliary += auxiliaryConstraints[i]->J1.GetNumRows();
6292  }
6293  timer_pc.Start();
6294 #endif
6295 
6296  // factor matrices for primary constraints
6297  PrimaryFactor();
6298 
6299  // calculate forces on bodies after applying primary constraints
6300  PrimaryForces( timeStep );
6301 
6302 #ifdef AF_TIMINGS
6303  timer_pc.Stop();
6304  timer_ac.Start();
6305 #endif
6306 
6307  // calculate and apply auxiliary constraint forces
6308  AuxiliaryForces( timeStep );
6309 
6310 #ifdef AF_TIMINGS
6311  timer_ac.Stop();
6312 #endif
6313 
6314  // evolve current state to next state
6315  Evolve( timeStep );
6316 
6317  // debug graphics
6318  DebugDraw();
6319 
6320  // clear external forces on all bodies
6321  ClearExternalForce();
6322 
6323  // apply contact force to other entities
6324  ApplyContactForces();
6325 
6326  // remove all frame constraints
6327  RemoveFrameConstraints();
6328 
6329 #ifdef AF_TIMINGS
6330  timer_collision.Start();
6331 #endif
6332 
6333  // check for collisions between current and next state
6334  CheckForCollisions( timeStep );
6335 
6336 #ifdef AF_TIMINGS
6337  timer_collision.Stop();
6338 #endif
6339 
6340  // swap the current and next state
6341  SwapStates();
6342 
6343  // make sure all clip models are disabled in case they were enabled for self collision
6344  if ( selfCollision && !af_skipSelfCollision.GetBool() ) {
6345  DisableClip();
6346  }
6347 
6348  // apply collision impulses
6349  if ( ApplyCollisions( timeStep ) ) {
6350  current.atRest = gameLocal.time;
6351  comeToRest = true;
6352  }
6353 
6354  // test if the simulation can be suspended because the whole figure is at rest
6355  if ( comeToRest && TestIfAtRest( timeStep ) ) {
6356  Rest();
6357  } else {
6358  ActivateContactEntities();
6359  }
6360 
6361  // add gravitational force
6362  AddGravity();
6363 
6364  // move the af velocity back into the world frame
6365  AddPushVelocity( current.pushVelocity );
6366  current.pushVelocity.Zero();
6367 
6368  if ( IsOutsideWorld() ) {
6369  gameLocal.Warning( "articulated figure moved outside world bounds for entity '%s' type '%s' at (%s)",
6370  self->name.c_str(), self->GetType()->classname, bodies[0]->current->worldOrigin.ToString(0) );
6371  Rest();
6372  }
6373 
6374 #ifdef AF_TIMINGS
6375  timer_total.Stop();
6376 
6377  if ( af_showTimings.GetInteger() == 1 ) {
6378  gameLocal.Printf( "%12s: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n",
6379  self->name.c_str(),
6380  timer_total.Milliseconds(),
6381  numPrimary, timer_pc.Milliseconds(),
6382  numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(),
6383  timer_lcp.Milliseconds(), timer_collision.Milliseconds() );
6384  }
6385  else if ( af_showTimings.GetInteger() == 2 ) {
6386  numArticulatedFigures++;
6387  if ( endTimeMSec > lastTimerReset ) {
6388  gameLocal.Printf( "af %d: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n",
6389  numArticulatedFigures,
6390  timer_total.Milliseconds(),
6391  numPrimary, timer_pc.Milliseconds(),
6392  numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(),
6393  timer_lcp.Milliseconds(), timer_collision.Milliseconds() );
6394  }
6395  }
6396 
6397  if ( endTimeMSec > lastTimerReset ) {
6398  lastTimerReset = endTimeMSec;
6399  numArticulatedFigures = 0;
6400  timer_total.Clear();
6401  timer_pc.Clear();
6402  timer_ac.Clear();
6403  timer_collision.Clear();
6404  timer_lcp.Clear();
6405  }
6406 #endif
6407 
6408  return true;
6409 }
6410 
6411 /*
6412 ================
6413 idPhysics_AF::UpdateTime
6414 ================
6415 */
6416 void idPhysics_AF::UpdateTime( int endTimeMSec ) {
6417 }
6418 
6419 /*
6420 ================
6421 idPhysics_AF::GetTime
6422 ================
6423 */
6424 int idPhysics_AF::GetTime( void ) const {
6425  return gameLocal.time;
6426 }
6427 
6428 /*
6429 ================
6430 DrawTraceModelSilhouette
6431 ================
6432 */
6433 void DrawTraceModelSilhouette( const idVec3 &projectionOrigin, const idClipModel *clipModel ) {
6434  int i, numSilEdges;
6435  int silEdges[MAX_TRACEMODEL_EDGES];
6436  idVec3 v1, v2;
6437  const idTraceModel *trm = clipModel->GetTraceModel();
6438  const idVec3 &origin = clipModel->GetOrigin();
6439  const idMat3 &axis = clipModel->GetAxis();
6440 
6441  numSilEdges = trm->GetProjectionSilhouetteEdges( ( projectionOrigin - origin ) * axis.Transpose(), silEdges );
6442  for ( i = 0; i < numSilEdges; i++ ) {
6443  v1 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITSET( silEdges[i] ) ] ];
6444  v2 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITNOTSET( silEdges[i] ) ] ];
6445  gameRenderWorld->DebugArrow( colorRed, origin + v1 * axis, origin + v2 * axis, 1 );
6446  }
6447 }
6448 
6449 /*
6450 ================
6451 idPhysics_AF::DebugDraw
6452 ================
6453 */
6454 void idPhysics_AF::DebugDraw( void ) {
6455  int i;
6456  idAFBody *body, *highlightBody = NULL, *constrainedBody1 = NULL, *constrainedBody2 = NULL;
6457  idAFConstraint *constraint;
6458  idVec3 center;
6459  idMat3 axis;
6460 
6461  if ( af_highlightConstraint.GetString()[0] ) {
6462  constraint = GetConstraint( af_highlightConstraint.GetString() );
6463  if ( constraint ) {
6464  constraint->GetCenter( center );
6465  axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3();
6466  gameRenderWorld->DebugCone( colorYellow, center, (axis[2] - axis[1]) * 4.0f, 0.0f, 1.0f, 0 );
6467 
6468  if ( af_showConstrainedBodies.GetBool() ) {
6469  cvarSystem->SetCVarString( "cm_drawColor", colorCyan.ToString( 0 ) );
6470  constrainedBody1 = constraint->body1;
6471  if ( constrainedBody1 ) {
6472  collisionModelManager->DrawModel( constrainedBody1->clipModel->Handle(), constrainedBody1->clipModel->GetOrigin(),
6473  constrainedBody1->clipModel->GetAxis(), vec3_origin, 0.0f );
6474  }
6475  cvarSystem->SetCVarString( "cm_drawColor", colorBlue.ToString( 0 ) );
6476  constrainedBody2 = constraint->body2;
6477  if ( constrainedBody2 ) {
6478  collisionModelManager->DrawModel( constrainedBody2->clipModel->Handle(), constrainedBody2->clipModel->GetOrigin(),
6479  constrainedBody2->clipModel->GetAxis(), vec3_origin, 0.0f );
6480  }
6481  cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) );
6482  }
6483  }
6484  }
6485 
6486  if ( af_highlightBody.GetString()[0] ) {
6487  highlightBody = GetBody( af_highlightBody.GetString() );
6488  if ( highlightBody ) {
6489  cvarSystem->SetCVarString( "cm_drawColor", colorYellow.ToString( 0 ) );
6490  collisionModelManager->DrawModel( highlightBody->clipModel->Handle(), highlightBody->clipModel->GetOrigin(),
6491  highlightBody->clipModel->GetAxis(), vec3_origin, 0.0f );
6492  cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) );
6493  }
6494  }
6495 
6496  if ( af_showBodies.GetBool() ) {
6497  for ( i = 0; i < bodies.Num(); i++ ) {
6498  body = bodies[i];
6499  if ( body == constrainedBody1 || body == constrainedBody2 ) {
6500  continue;
6501  }
6502  if ( body == highlightBody ) {
6503  continue;
6504  }
6505  collisionModelManager->DrawModel( body->clipModel->Handle(), body->clipModel->GetOrigin(),
6506  body->clipModel->GetAxis(), vec3_origin, 0.0f );
6507  //DrawTraceModelSilhouette( gameLocal.GetLocalPlayer()->GetEyePosition(), body->clipModel );
6508  }
6509  }
6510 
6511  if ( af_showBodyNames.GetBool() ) {
6512  for ( i = 0; i < bodies.Num(); i++ ) {
6513  body = bodies[i];
6514  gameRenderWorld->DrawText( body->GetName().c_str(), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6515  }
6516  }
6517 
6518  if ( af_showMass.GetBool() ) {
6519  for ( i = 0; i < bodies.Num(); i++ ) {
6520  body = bodies[i];
6521  gameRenderWorld->DrawText( va( "\n%1.2f", 1.0f / body->GetInverseMass() ), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6522  }
6523  }
6524 
6525  if ( af_showTotalMass.GetBool() ) {
6526  axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3();
6527  gameRenderWorld->DrawText( va( "\n%1.2f", totalMass ), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1 );
6528  }
6529 
6530  if ( af_showInertia.GetBool() ) {
6531  for ( i = 0; i < bodies.Num(); i++ ) {
6532  body = bodies[i];
6533  idMat3 &I = body->inertiaTensor;
6534  gameRenderWorld->DrawText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )",
6535  I[0].x, I[0].y, I[0].z,
6536  I[1].x, I[1].y, I[1].z,
6537  I[2].x, I[2].y, I[2].z ),
6538  body->GetWorldOrigin(), 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6539  }
6540  }
6541 
6542  if ( af_showVelocity.GetBool() ) {
6543  for ( i = 0; i < bodies.Num(); i++ ) {
6544  DrawVelocity( bodies[i]->clipModel->GetId(), 0.1f, 4.0f );
6545  }
6546  }
6547 
6548  if ( af_showConstraints.GetBool() ) {
6549  for ( i = 0; i < primaryConstraints.Num(); i++ ) {
6550  constraint = primaryConstraints[i];
6551  constraint->DebugDraw();
6552  }
6553  if ( !af_showPrimaryOnly.GetBool() ) {
6554  for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
6555  constraint = auxiliaryConstraints[i];
6556  constraint->DebugDraw();
6557  }
6558  }
6559  }
6560 
6561  if ( af_showConstraintNames.GetBool() ) {
6562  for ( i = 0; i < primaryConstraints.Num(); i++ ) {
6563  constraint = primaryConstraints[i];
6564  constraint->GetCenter( center );
6565  gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6566  }
6567  if ( !af_showPrimaryOnly.GetBool() ) {
6568  for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
6569  constraint = auxiliaryConstraints[i];
6570  constraint->GetCenter( center );
6571  gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6572  }
6573  }
6574  }
6575 
6576  if ( af_showTrees.GetBool() || ( af_showActive.GetBool() && current.atRest < 0 ) ) {
6577  for ( i = 0; i < trees.Num(); i++ ) {
6578  trees[i]->DebugDraw( idStr::ColorForIndex( i+3 ) );
6579  }
6580  }
6581 }
6582 
6583 /*
6584 ================
6585 idPhysics_AF::idPhysics_AF
6586 ================
6587 */
6588 idPhysics_AF::idPhysics_AF( void ) {
6589  trees.Clear();
6590  bodies.Clear();
6591  constraints.Clear();
6592  primaryConstraints.Clear();
6593  auxiliaryConstraints.Clear();
6594  frameConstraints.Clear();
6595  contacts.Clear();
6596  collisions.Clear();
6597  changedAF = true;
6598  masterBody = NULL;
6599 
6600  lcp = idLCP::AllocSymmetric();
6601 
6602  memset( &current, 0, sizeof( current ) );
6603  current.atRest = -1;
6604  current.lastTimeStep = USERCMD_MSEC;
6605  saved = current;
6606 
6607  linearFriction = 0.005f;
6608  angularFriction = 0.005f;
6609  contactFriction = 0.8f;
6610  bouncyness = 0.4f;
6611  totalMass = 0.0f;
6612  forceTotalMass = -1.0f;
6613 
6614  suspendVelocity.Set( SUSPEND_LINEAR_VELOCITY, SUSPEND_ANGULAR_VELOCITY );
6615  suspendAcceleration.Set( SUSPEND_LINEAR_ACCELERATION, SUSPEND_LINEAR_ACCELERATION );
6616  noMoveTime = NO_MOVE_TIME;
6617  noMoveTranslation = NO_MOVE_TRANSLATION_TOLERANCE;
6618  noMoveRotation = NO_MOVE_ROTATION_TOLERANCE;
6619  minMoveTime = MIN_MOVE_TIME;
6620  maxMoveTime = MAX_MOVE_TIME;
6621  impulseThreshold = IMPULSE_THRESHOLD;
6622 
6623  timeScale = 1.0f;
6624  timeScaleRampStart = 0.0f;
6625  timeScaleRampEnd = 0.0f;
6626 
6627  jointFrictionScale = 0.0f;
6628  jointFrictionDent = 0.0f;
6629  jointFrictionDentStart = 0.0f;
6630  jointFrictionDentEnd = 0.0f;
6631  jointFrictionDentScale = 0.0f;
6632 
6633  contactFrictionScale = 0.0f;
6634  contactFrictionDent = 0.0f;
6635  contactFrictionDentStart = 0.0f;
6636  contactFrictionDentEnd = 0.0f;
6637  contactFrictionDentScale = 0.0f;
6638 
6639  enableCollision = true;
6640  selfCollision = true;
6641  comeToRest = true;
6642  linearTime = true;
6643  noImpact = false;
6644  worldConstraintsLocked = false;
6645  forcePushable = false;
6646 
6647 #ifdef AF_TIMINGS
6648  lastTimerReset = 0;
6649 #endif
6650 }
6651 
6652 /*
6653 ================
6654 idPhysics_AF::~idPhysics_AF
6655 ================
6656 */
6657 idPhysics_AF::~idPhysics_AF( void ) {
6658  int i;
6659 
6660  trees.DeleteContents( true );
6661 
6662  for ( i = 0; i < bodies.Num(); i++ ) {
6663  delete bodies[i];
6664  }
6665 
6666  for ( i = 0; i < constraints.Num(); i++ ) {
6667  delete constraints[i];
6668  }
6669 
6670  contactConstraints.SetNum( contactConstraints.NumAllocated(), false );
6671  for ( i = 0; i < contactConstraints.NumAllocated(); i++ ) {
6672  delete contactConstraints[i];
6673  }
6674 
6675  delete lcp;
6676 
6677  if ( masterBody ) {
6678  delete masterBody;
6679  }
6680 }
6681 
6682 /*
6683 ================
6684 idPhysics_AF_SavePState
6685 ================
6686 */
6687 void idPhysics_AF_SavePState( idSaveGame *saveFile, const AFPState_t &state ) {
6688  saveFile->WriteInt( state.atRest );
6689  saveFile->WriteFloat( state.noMoveTime );
6690  saveFile->WriteFloat( state.activateTime );
6691  saveFile->WriteFloat( state.lastTimeStep );
6692  saveFile->WriteVec6( state.pushVelocity );
6693 }
6694 
6695 /*
6696 ================
6697 idPhysics_AF_RestorePState
6698 ================
6699 */
6700 void idPhysics_AF_RestorePState( idRestoreGame *saveFile, AFPState_t &state ) {
6701  saveFile->ReadInt( state.atRest );
6702  saveFile->ReadFloat( state.noMoveTime );
6703  saveFile->ReadFloat( state.activateTime );
6704  saveFile->ReadFloat( state.lastTimeStep );
6705  saveFile->ReadVec6( state.pushVelocity );
6706 }
6707 
6708 /*
6709 ================
6710 idPhysics_AF::Save
6711 ================
6712 */
6713 void idPhysics_AF::Save( idSaveGame *saveFile ) const {
6714  int i;
6715 
6716  // the articulated figure structure is handled by the owner
6717 
6718  idPhysics_AF_SavePState( saveFile, current );
6719  idPhysics_AF_SavePState( saveFile, saved );
6720 
6721  saveFile->WriteInt( bodies.Num() );
6722  for ( i = 0; i < bodies.Num(); i++ ) {
6723  bodies[i]->Save( saveFile );
6724  }
6725  if ( masterBody ) {
6726  saveFile->WriteBool( true );
6727  masterBody->Save( saveFile );
6728  } else {
6729  saveFile->WriteBool( false );
6730  }
6731 
6732  saveFile->WriteInt( constraints.Num() );
6733  for ( i = 0; i < constraints.Num(); i++ ) {
6734  constraints[i]->Save( saveFile );
6735  }
6736 
6737  saveFile->WriteBool( changedAF );
6738 
6739  saveFile->WriteFloat( linearFriction );
6740  saveFile->WriteFloat( angularFriction );
6741  saveFile->WriteFloat( contactFriction );
6742  saveFile->WriteFloat( bouncyness );
6743  saveFile->WriteFloat( totalMass );
6744  saveFile->WriteFloat( forceTotalMass );
6745 
6746  saveFile->WriteVec2( suspendVelocity );
6747  saveFile->WriteVec2( suspendAcceleration );
6748  saveFile->WriteFloat( noMoveTime );
6749  saveFile->WriteFloat( noMoveTranslation );
6750  saveFile->WriteFloat( noMoveRotation );
6751  saveFile->WriteFloat( minMoveTime );
6752  saveFile->WriteFloat( maxMoveTime );
6753  saveFile->WriteFloat( impulseThreshold );
6754 
6755  saveFile->WriteFloat( timeScale );
6756  saveFile->WriteFloat( timeScaleRampStart );
6757  saveFile->WriteFloat( timeScaleRampEnd );
6758 
6759  saveFile->WriteFloat( jointFrictionScale );
6760  saveFile->WriteFloat( jointFrictionDent );
6761  saveFile->WriteFloat( jointFrictionDentStart );
6762  saveFile->WriteFloat( jointFrictionDentEnd );
6763  saveFile->WriteFloat( jointFrictionDentScale );
6764 
6765  saveFile->WriteFloat( contactFrictionScale );
6766  saveFile->WriteFloat( contactFrictionDent );
6767  saveFile->WriteFloat( contactFrictionDentStart );
6768  saveFile->WriteFloat( contactFrictionDentEnd );
6769  saveFile->WriteFloat( contactFrictionDentScale );
6770 
6771  saveFile->WriteBool( enableCollision );
6772  saveFile->WriteBool( selfCollision );
6773  saveFile->WriteBool( comeToRest );
6774  saveFile->WriteBool( linearTime );
6775  saveFile->WriteBool( noImpact );
6776  saveFile->WriteBool( worldConstraintsLocked );
6777  saveFile->WriteBool( forcePushable );
6778 }
6779 
6780 /*
6781 ================
6782 idPhysics_AF::Restore
6783 ================
6784 */
6785 void idPhysics_AF::Restore( idRestoreGame *saveFile ) {
6786  int i, num;
6787  bool hasMaster;
6788 
6789  // the articulated figure structure should have already been restored
6790 
6791  idPhysics_AF_RestorePState( saveFile, current );
6792  idPhysics_AF_RestorePState( saveFile, saved );
6793 
6794  saveFile->ReadInt( num );
6795  assert( num == bodies.Num() );
6796  for ( i = 0; i < bodies.Num(); i++ ) {
6797  bodies[i]->Restore( saveFile );
6798  }
6799  saveFile->ReadBool( hasMaster );
6800  if ( hasMaster ) {
6801  masterBody = new idAFBody();
6802  masterBody->Restore( saveFile );
6803  }
6804 
6805  saveFile->ReadInt( num );
6806  assert( num == constraints.Num() );
6807  for ( i = 0; i < constraints.Num(); i++ ) {
6808  constraints[i]->Restore( saveFile );
6809  }
6810 
6811  saveFile->ReadBool( changedAF );
6812 
6813  saveFile->ReadFloat( linearFriction );
6814  saveFile->ReadFloat( angularFriction );
6815  saveFile->ReadFloat( contactFriction );
6816  saveFile->ReadFloat( bouncyness );
6817  saveFile->ReadFloat( totalMass );
6818  saveFile->ReadFloat( forceTotalMass );
6819 
6820  saveFile->ReadVec2( suspendVelocity );
6821  saveFile->ReadVec2( suspendAcceleration );
6822  saveFile->ReadFloat( noMoveTime );
6823  saveFile->ReadFloat( noMoveTranslation );
6824  saveFile->ReadFloat( noMoveRotation );
6825  saveFile->ReadFloat( minMoveTime );
6826  saveFile->ReadFloat( maxMoveTime );
6827  saveFile->ReadFloat( impulseThreshold );
6828 
6829  saveFile->ReadFloat( timeScale );
6830  saveFile->ReadFloat( timeScaleRampStart );
6831  saveFile->ReadFloat( timeScaleRampEnd );
6832 
6833  saveFile->ReadFloat( jointFrictionScale );
6834  saveFile->ReadFloat( jointFrictionDent );
6835  saveFile->ReadFloat( jointFrictionDentStart );
6836  saveFile->ReadFloat( jointFrictionDentEnd );
6837  saveFile->ReadFloat( jointFrictionDentScale );
6838 
6839  saveFile->ReadFloat( contactFrictionScale );
6840  saveFile->ReadFloat( contactFrictionDent );
6841  saveFile->ReadFloat( contactFrictionDentStart );
6842  saveFile->ReadFloat( contactFrictionDentEnd );
6843  saveFile->ReadFloat( contactFrictionDentScale );
6844 
6845  saveFile->ReadBool( enableCollision );
6846  saveFile->ReadBool( selfCollision );
6847  saveFile->ReadBool( comeToRest );
6848  saveFile->ReadBool( linearTime );
6849  saveFile->ReadBool( noImpact );
6850  saveFile->ReadBool( worldConstraintsLocked );
6851  saveFile->ReadBool( forcePushable );
6852 
6853  changedAF = true;
6854 
6855  UpdateClipModels();
6856 }
6857 
6858 /*
6859 ================
6860 idPhysics_AF::IsClosedLoop
6861 ================
6862 */
6863 bool idPhysics_AF::IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) const {
6864  const idAFBody *b1, *b2;
6865 
6866  for ( b1 = body1; b1->parent; b1 = b1->parent ) {
6867  }
6868  for ( b2 = body2; b2->parent; b2 = b2->parent ) {
6869  }
6870  return ( b1 == b2 );
6871 }
6872 
6873 /*
6874 ================
6875 idPhysics_AF::BuildTrees
6876 ================
6877 */
6878 void idPhysics_AF::BuildTrees( void ) {
6879  int i;
6880  float scale;
6881  idAFBody *b;
6882  idAFConstraint *c;
6883  idAFTree *tree;
6884 
6885  primaryConstraints.Clear();
6886  auxiliaryConstraints.Clear();
6887  trees.DeleteContents( true );
6888 
6889  totalMass = 0.0f;
6890  for ( i = 0; i < bodies.Num(); i++ ) {
6891  b = bodies[i];
6892  b->parent = NULL;
6893  b->primaryConstraint = NULL;
6894  b->constraints.SetNum( 0, false );
6895  b->children.Clear();
6896  b->tree = NULL;
6897  totalMass += b->mass;
6898  }
6899 
6900  if ( forceTotalMass > 0.0f ) {
6901  scale = forceTotalMass / totalMass;
6902  for ( i = 0; i < bodies.Num(); i++ ) {
6903  b = bodies[i];
6904  b->mass *= scale;
6905  b->invMass = 1.0f / b->mass;
6906  b->inertiaTensor *= scale;
6907  b->inverseInertiaTensor = b->inertiaTensor.Inverse();
6908  }
6909  totalMass = forceTotalMass;
6910  }
6911 
6912  if ( af_useLinearTime.GetBool() ) {
6913 
6914  for ( i = 0; i < constraints.Num(); i++ ) {
6915  c = constraints[i];
6916 
6917  c->body1->constraints.Append( c );
6918  if ( c->body2 ) {
6919  c->body2->constraints.Append( c );
6920  }
6921 
6922  // only bilateral constraints between two non-world bodies that do not
6923  // create loops can be used as primary constraints
6924  if ( !c->body1->primaryConstraint && c->fl.allowPrimary && c->body2 != NULL && !IsClosedLoop( c->body1, c->body2 ) ) {
6925  c->body1->primaryConstraint = c;
6926  c->body1->parent = c->body2;
6927  c->body2->children.Append( c->body1 );
6928  c->fl.isPrimary = true;
6929  c->firstIndex = 0;
6930  primaryConstraints.Append( c );
6931  } else {
6932  c->fl.isPrimary = false;
6933  auxiliaryConstraints.Append( c );
6934  }
6935  }
6936 
6937  // create trees for all parent bodies
6938  for ( i = 0; i < bodies.Num(); i++ ) {
6939  if ( !bodies[i]->parent ) {
6940  tree = new idAFTree();
6941  tree->sortedBodies.Clear();
6942  tree->sortedBodies.Append( bodies[i] );
6943  bodies[i]->tree = tree;
6944  trees.Append( tree );
6945  }
6946  }
6947 
6948  // add each child body to the appropriate tree
6949  for ( i = 0; i < bodies.Num(); i++ ) {
6950  if ( bodies[i]->parent ) {
6951  for ( b = bodies[i]->parent; !b->tree; b = b->parent ) {
6952  }
6953  b->tree->sortedBodies.Append( bodies[i] );
6954  bodies[i]->tree = b->tree;
6955  }
6956  }
6957 
6958  if ( trees.Num() > 1 ) {
6959  gameLocal.Warning( "Articulated figure has multiple seperate tree structures for entity '%s' type '%s'.",
6960  self->name.c_str(), self->GetType()->classname );
6961  }
6962 
6963  // sort bodies in each tree to make sure parents come first
6964  for ( i = 0; i < trees.Num(); i++ ) {
6965  trees[i]->SortBodies();
6966  }
6967 
6968  } else {
6969 
6970  // create a tree for each body
6971  for ( i = 0; i < bodies.Num(); i++ ) {
6972  tree = new idAFTree();
6973  tree->sortedBodies.Clear();
6974  tree->sortedBodies.Append( bodies[i] );
6975  bodies[i]->tree = tree;
6976  trees.Append( tree );
6977  }
6978 
6979  for ( i = 0; i < constraints.Num(); i++ ) {
6980  c = constraints[i];
6981 
6982  c->body1->constraints.Append( c );
6983  if ( c->body2 ) {
6984  c->body2->constraints.Append( c );
6985  }
6986 
6987  c->fl.isPrimary = false;
6988  auxiliaryConstraints.Append( c );
6989  }
6990  }
6991 }
6992 
6993 /*
6994 ================
6995 idPhysics_AF::AddBody
6996 
6997  bodies get an id in the order they are added starting at zero
6998  as such the first body added will get id zero
6999 ================
7000 */
7001 int idPhysics_AF::AddBody( idAFBody *body ) {
7002  int id = 0;
7003 
7004  if ( !body->clipModel ) {
7005  gameLocal.Error( "idPhysics_AF::AddBody: body '%s' has no clip model.", body->name.c_str() );
7006  }
7007 
7008  if ( bodies.Find( body ) ) {
7009  gameLocal.Error( "idPhysics_AF::AddBody: body '%s' added twice.", body->name.c_str() );
7010  }
7011 
7012  if ( GetBody( body->name ) ) {
7013  gameLocal.Error( "idPhysics_AF::AddBody: a body with the name '%s' already exists.", body->name.c_str() );
7014  }
7015 
7016  id = bodies.Num();
7017  body->clipModel->SetId( id );
7018  if ( body->linearFriction < 0.0f ) {
7019  body->linearFriction = linearFriction;
7020  body->angularFriction = angularFriction;
7021  body->contactFriction = contactFriction;
7022  }
7023  if ( body->bouncyness < 0.0f ) {
7024  body->bouncyness = bouncyness;
7025  }
7026  if ( !body->fl.clipMaskSet ) {
7027  body->clipMask = clipMask;
7028  }
7029 
7030  bodies.Append( body );
7031 
7032  changedAF = true;
7033 
7034  return id;
7035 }
7036 
7037 /*
7038 ================
7039 idPhysics_AF::AddConstraint
7040 ================
7041 */
7042 void idPhysics_AF::AddConstraint( idAFConstraint *constraint ) {
7043 
7044  if ( constraints.Find( constraint ) ) {
7045  gameLocal.Error( "idPhysics_AF::AddConstraint: constraint '%s' added twice.", constraint->name.c_str() );
7046  }
7047  if ( GetConstraint( constraint->name ) ) {
7048  gameLocal.Error( "idPhysics_AF::AddConstraint: a constraint with the name '%s' already exists.", constraint->name.c_str() );
7049  }
7050  if ( !constraint->body1 ) {
7051  gameLocal.Error( "idPhysics_AF::AddConstraint: body1 == NULL on constraint '%s'.", constraint->name.c_str() );
7052  }
7053  if ( !bodies.Find( constraint->body1 ) ) {
7054  gameLocal.Error( "idPhysics_AF::AddConstraint: body1 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() );
7055  }
7056  if ( constraint->body2 && !bodies.Find( constraint->body2 ) ) {
7057  gameLocal.Error( "idPhysics_AF::AddConstraint: body2 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() );
7058  }
7059  if ( constraint->body1 == constraint->body2 ) {
7060  gameLocal.Error( "idPhysics_AF::AddConstraint: body1 and body2 of constraint '%s' are the same.", constraint->name.c_str() );
7061  }
7062 
7063  constraints.Append( constraint );
7064  constraint->physics = this;
7065 
7066  changedAF = true;
7067 }
7068 
7069 /*
7070 ================
7071 idPhysics_AF::AddFrameConstraint
7072 ================
7073 */
7074 void idPhysics_AF::AddFrameConstraint( idAFConstraint *constraint ) {
7075  frameConstraints.Append( constraint );
7076  constraint->physics = this;
7077 }
7078 
7079 /*
7080 ================
7081 idPhysics_AF::ForceBodyId
7082 ================
7083 */
7084 void idPhysics_AF::ForceBodyId( idAFBody *body, int newId ) {
7085  int id;
7086 
7087  id = bodies.FindIndex( body );
7088  if ( id == -1 ) {
7089  gameLocal.Error( "ForceBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() );
7090  }
7091  if ( id != newId ) {
7092  idAFBody *b = bodies[newId];
7093  bodies[newId] = bodies[id];
7094  bodies[id] = b;
7095  changedAF = true;
7096  }
7097 }
7098 
7099 /*
7100 ================
7101 idPhysics_AF::GetBodyId
7102 ================
7103 */
7104 int idPhysics_AF::GetBodyId( idAFBody *body ) const {
7105  int id;
7106 
7107  id = bodies.FindIndex( body );
7108  if ( id == -1 && body ) {
7109  gameLocal.Error( "GetBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() );
7110  }
7111  return id;
7112 }
7113 
7114 /*
7115 ================
7116 idPhysics_AF::GetBodyId
7117 ================
7118 */
7119 int idPhysics_AF::GetBodyId( const char *bodyName ) const {
7120  int i;
7121 
7122  for ( i = 0; i < bodies.Num(); i++ ) {
7123  if ( !bodies[i]->name.Icmp( bodyName ) ) {
7124  return i;
7125  }
7126  }
7127  gameLocal.Error( "GetBodyId: no body with the name '%s' is not part of the articulated figure.\n", bodyName );
7128  return 0;
7129 }
7130 
7131 /*
7132 ================
7133 idPhysics_AF::GetConstraintId
7134 ================
7135 */
7136 int idPhysics_AF::GetConstraintId( idAFConstraint *constraint ) const {
7137  int id;
7138 
7139  id = constraints.FindIndex( constraint );
7140  if ( id == -1 && constraint ) {
7141  gameLocal.Error( "GetConstraintId: constraint '%s' is not part of the articulated figure.\n", constraint->name.c_str() );
7142  }
7143  return id;
7144 }
7145 
7146 /*
7147 ================
7148 idPhysics_AF::GetConstraintId
7149 ================
7150 */
7151 int idPhysics_AF::GetConstraintId( const char *constraintName ) const {
7152  int i;
7153 
7154  for ( i = 0; i < constraints.Num(); i++ ) {
7155  if ( constraints[i]->name.Icmp( constraintName ) == 0 ) {
7156  return i;
7157  }
7158  }
7159  gameLocal.Error( "GetConstraintId: no constraint with the name '%s' is not part of the articulated figure.\n", constraintName );
7160  return 0;
7161 }
7162 
7163 /*
7164 ================
7165 idPhysics_AF::GetNumBodies
7166 ================
7167 */
7168 int idPhysics_AF::GetNumBodies( void ) const {
7169  return bodies.Num();
7170 }
7171 
7172 /*
7173 ================
7174 idPhysics_AF::GetNumConstraints
7175 ================
7176 */
7177 int idPhysics_AF::GetNumConstraints( void ) const {
7178  return constraints.Num();
7179 }
7180 
7181 /*
7182 ================
7183 idPhysics_AF::GetBody
7184 ================
7185 */
7186 idAFBody *idPhysics_AF::GetBody( const char *bodyName ) const {
7187  int i;
7188 
7189  for ( i = 0; i < bodies.Num(); i++ ) {
7190  if ( !bodies[i]->name.Icmp( bodyName ) ) {
7191  return bodies[i];
7192  }
7193  }
7194 
7195  return NULL;
7196 }
7197 
7198 /*
7199 ================
7200 idPhysics_AF::GetBody
7201 ================
7202 */
7203 idAFBody *idPhysics_AF::GetBody( const int id ) const {
7204  if ( id < 0 || id >= bodies.Num() ) {
7205  gameLocal.Error( "GetBody: no body with id %d exists\n", id );
7206  return NULL;
7207  }
7208  return bodies[id];
7209 }
7210 
7211 /*
7212 ================
7213 idPhysics_AF::GetConstraint
7214 ================
7215 */
7216 idAFConstraint *idPhysics_AF::GetConstraint( const char *constraintName ) const {
7217  int i;
7218 
7219  for ( i = 0; i < constraints.Num(); i++ ) {
7220  if ( constraints[i]->name.Icmp( constraintName ) == 0 ) {
7221  return constraints[i];
7222  }
7223  }
7224 
7225  return NULL;
7226 }
7227 
7228 /*
7229 ================
7230 idPhysics_AF::GetConstraint
7231 ================
7232 */
7233 idAFConstraint *idPhysics_AF::GetConstraint( const int id ) const {
7234  if ( id < 0 || id >= constraints.Num() ) {
7235  gameLocal.Error( "GetConstraint: no constraint with id %d exists\n", id );
7236  return NULL;
7237  }
7238  return constraints[id];
7239 }
7240 
7241 /*
7242 ================
7243 idPhysics_AF::DeleteBody
7244 ================
7245 */
7246 void idPhysics_AF::DeleteBody( const char *bodyName ) {
7247  int i;
7248 
7249  // find the body with the given name
7250  for ( i = 0; i < bodies.Num(); i++ ) {
7251  if ( !bodies[i]->name.Icmp( bodyName ) ) {
7252  break;
7253  }
7254  }
7255 
7256  if ( i >= bodies.Num() ) {
7257  gameLocal.Warning( "DeleteBody: no body found in the articulated figure with the name '%s' for entity '%s' type '%s'.",
7258  bodyName, self->name.c_str(), self->GetType()->classname );
7259  return;
7260  }
7261 
7262  DeleteBody( i );
7263 }
7264 
7265 /*
7266 ================
7267 idPhysics_AF::DeleteBody
7268 ================
7269 */
7270 void idPhysics_AF::DeleteBody( const int id ) {
7271  int j;
7272 
7273  if ( id < 0 || id > bodies.Num() ) {
7274  gameLocal.Error( "DeleteBody: no body with id %d.", id );
7275  return;
7276  }
7277 
7278  // remove any constraints attached to this body
7279  for ( j = 0; j < constraints.Num(); j++ ) {
7280  if ( constraints[j]->body1 == bodies[id] || constraints[j]->body2 == bodies[id] ) {
7281  delete constraints[j];
7282  constraints.RemoveIndex( j );
7283  j--;
7284  }
7285  }
7286 
7287  // remove the body
7288  delete bodies[id];
7289  bodies.RemoveIndex( id );
7290 
7291  // set new body ids
7292  for ( j = 0; j < bodies.Num(); j++ ) {
7293  bodies[j]->clipModel->SetId( j );
7294  }
7295 
7296  changedAF = true;
7297 }
7298 
7299 /*
7300 ================
7301 idPhysics_AF::DeleteConstraint
7302 ================
7303 */
7304 void idPhysics_AF::DeleteConstraint( const char *constraintName ) {
7305  int i;
7306 
7307  // find the constraint with the given name
7308  for ( i = 0; i < constraints.Num(); i++ ) {
7309  if ( !constraints[i]->name.Icmp( constraintName ) ) {
7310  break;
7311  }
7312  }
7313 
7314  if ( i >= constraints.Num() ) {
7315  gameLocal.Warning( "DeleteConstraint: no constriant found in the articulated figure with the name '%s' for entity '%s' type '%s'.",
7316  constraintName, self->name.c_str(), self->GetType()->classname );
7317  return;
7318  }
7319 
7320  DeleteConstraint( i );
7321 }
7322 
7323 /*
7324 ================
7325 idPhysics_AF::DeleteConstraint
7326 ================
7327 */
7328 void idPhysics_AF::DeleteConstraint( const int id ) {
7329 
7330  if ( id < 0 || id >= constraints.Num() ) {
7331  gameLocal.Error( "DeleteConstraint: no constraint with id %d.", id );
7332  return;
7333  }
7334 
7335  // remove the constraint
7336  delete constraints[id];
7337  constraints.RemoveIndex( id );
7338 
7339  changedAF = true;
7340 }
7341 
7342 /*
7343 ================
7344 idPhysics_AF::GetBodyContactConstraints
7345 ================
7346 */
7347 int idPhysics_AF::GetBodyContactConstraints( const int id, idAFConstraint_Contact *contacts[], int maxContacts ) const {
7348  int i, numContacts;
7349  idAFBody *body;
7350  idAFConstraint_Contact *contact;
7351 
7352  if ( id < 0 || id >= bodies.Num() || maxContacts <= 0 ) {
7353  return 0;
7354  }
7355 
7356  numContacts = 0;
7357  body = bodies[id];
7358  for ( i = 0; i < contactConstraints.Num(); i++ ) {
7359  contact = contactConstraints[i];
7360  if ( contact->body1 == body || contact->body2 == body ) {
7361  contacts[numContacts++] = contact;
7362  if ( numContacts >= maxContacts ) {
7363  return numContacts;
7364  }
7365  }
7366  }
7367  return numContacts;
7368 }
7369 
7370 /*
7371 ================
7372 idPhysics_AF::SetDefaultFriction
7373 ================
7374 */
7375 void idPhysics_AF::SetDefaultFriction( float linear, float angular, float contact ) {
7376  if ( linear < 0.0f || linear > 1.0f ||
7377  angular < 0.0f || angular > 1.0f ||
7378  contact < 0.0f || contact > 1.0f ) {
7379  return;
7380  }
7381  linearFriction = linear;
7382  angularFriction = angular;
7383  contactFriction = contact;
7384 }
7385 
7386 /*
7387 ================
7388 idPhysics_AF::GetImpactInfo
7389 ================
7390 */
7391 void idPhysics_AF::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const {
7392  if ( id < 0 || id >= bodies.Num() ) {
7393  memset( info, 0, sizeof( *info ) );
7394  return;
7395  }
7396  info->invMass = 1.0f / bodies[id]->mass;
7397  info->invInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis;
7398  info->position = point - bodies[id]->current->worldOrigin;
7399  info->velocity = bodies[id]->current->spatialVelocity.SubVec3(0) + bodies[id]->current->spatialVelocity.SubVec3(1).Cross( info->position );
7400 }
7401 
7402 /*
7403 ================
7404 idPhysics_AF::ApplyImpulse
7405 ================
7406 */
7407 void idPhysics_AF::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) {
7408  if ( id < 0 || id >= bodies.Num() ) {
7409  return;
7410  }
7411  if ( noImpact || impulse.LengthSqr() < Square( impulseThreshold ) ) {
7412  return;
7413  }
7414  idMat3 invWorldInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis;
7415  bodies[id]->current->spatialVelocity.SubVec3(0) += bodies[id]->invMass * impulse;
7416  bodies[id]->current->spatialVelocity.SubVec3(1) += invWorldInertiaTensor * (point - bodies[id]->current->worldOrigin).Cross( impulse );
7417  Activate();
7418 }
7419 
7420 /*
7421 ================
7422 idPhysics_AF::AddForce
7423 ================
7424 */
7425 void idPhysics_AF::AddForce( const int id, const idVec3 &point, const idVec3 &force ) {
7426  if ( noImpact ) {
7427  return;
7428  }
7429  if ( id < 0 || id >= bodies.Num() ) {
7430  return;
7431  }
7432  bodies[id]->current->externalForce.SubVec3( 0 ) += force;
7433  bodies[id]->current->externalForce.SubVec3( 1 ) += (point - bodies[id]->current->worldOrigin).Cross( force );
7434  Activate();
7435 }
7436 
7437 /*
7438 ================
7439 idPhysics_AF::IsAtRest
7440 ================
7441 */
7442 bool idPhysics_AF::IsAtRest( void ) const {
7443  return current.atRest >= 0;
7444 }
7445 
7446 /*
7447 ================
7448 idPhysics_AF::GetRestStartTime
7449 ================
7450 */
7451 int idPhysics_AF::GetRestStartTime( void ) const {
7452  return current.atRest;
7453 }
7454 
7455 /*
7456 ================
7457 idPhysics_AF::IsPushable
7458 ================
7459 */
7460 bool idPhysics_AF::IsPushable( void ) const {
7461  return ( !noImpact && ( masterBody == NULL || forcePushable ) );
7462 }
7463 
7464 /*
7465 ================
7466 idPhysics_AF::SaveState
7467 ================
7468 */
7469 void idPhysics_AF::SaveState( void ) {
7470  int i;
7471 
7472  saved = current;
7473 
7474  for ( i = 0; i < bodies.Num(); i++ ) {
7475  memcpy( &bodies[i]->saved, bodies[i]->current, sizeof( AFBodyPState_t ) );
7476  }
7477 }
7478 
7479 /*
7480 ================
7481 idPhysics_AF::RestoreState
7482 ================
7483 */
7484 void idPhysics_AF::RestoreState( void ) {
7485  int i;
7486 
7487  current = saved;
7488 
7489  for ( i = 0; i < bodies.Num(); i++ ) {
7490  *(bodies[i]->current) = bodies[i]->saved;
7491  }
7492 
7493  EvaluateContacts();
7494 }
7495 
7496 /*
7497 ================
7498 idPhysics_AF::SetOrigin
7499 ================
7500 */
7501 void idPhysics_AF::SetOrigin( const idVec3 &newOrigin, int id ) {
7502  if ( masterBody ) {
7503  Translate( masterBody->current->worldOrigin + masterBody->current->worldAxis * newOrigin - bodies[0]->current->worldOrigin );
7504  } else {
7505  Translate( newOrigin - bodies[0]->current->worldOrigin );
7506  }
7507 }
7508 
7509 /*
7510 ================
7511 idPhysics_AF::SetAxis
7512 ================
7513 */
7514 void idPhysics_AF::SetAxis( const idMat3 &newAxis, int id ) {
7515  idMat3 axis;
7516  idRotation rotation;
7517 
7518  if ( masterBody ) {
7519  axis = bodies[0]->current->worldAxis.Transpose() * ( newAxis * masterBody->current->worldAxis );
7520  } else {
7521  axis = bodies[0]->current->worldAxis.Transpose() * newAxis;
7522  }
7523  rotation = axis.ToRotation();
7524  rotation.SetOrigin( bodies[0]->current->worldOrigin );
7525 
7526  Rotate( rotation );
7527 }
7528 
7529 /*
7530 ================
7531 idPhysics_AF::Translate
7532 ================
7533 */
7534 void idPhysics_AF::Translate( const idVec3 &translation, int id ) {
7535  int i;
7536  idAFBody *body;
7537 
7538  if ( !worldConstraintsLocked ) {
7539  // translate constraints attached to the world
7540  for ( i = 0; i < constraints.Num(); i++ ) {
7541  constraints[i]->Translate( translation );
7542  }
7543  }
7544 
7545  // translate all the bodies
7546  for ( i = 0; i < bodies.Num(); i++ ) {
7547 
7548  body = bodies[i];
7549  body->current->worldOrigin += translation;
7550  }
7551 
7552  Activate();
7553 
7554  UpdateClipModels();
7555 }
7556 
7557 /*
7558 ================
7559 idPhysics_AF::Rotate
7560 ================
7561 */
7562 void idPhysics_AF::Rotate( const idRotation &rotation, int id ) {
7563  int i;
7564  idAFBody *body;
7565 
7566  if ( !worldConstraintsLocked ) {
7567  // rotate constraints attached to the world
7568  for ( i = 0; i < constraints.Num(); i++ ) {
7569  constraints[i]->Rotate( rotation );
7570  }
7571  }
7572 
7573  // rotate all the bodies
7574  for ( i = 0; i < bodies.Num(); i++ ) {
7575  body = bodies[i];
7576 
7577  body->current->worldOrigin *= rotation;
7578  body->current->worldAxis *= rotation.ToMat3();
7579  }
7580 
7581  Activate();
7582 
7583  UpdateClipModels();
7584 }
7585 
7586 /*
7587 ================
7588 idPhysics_AF::GetOrigin
7589 ================
7590 */
7591 const idVec3 &idPhysics_AF::GetOrigin( int id ) const {
7592  if ( id < 0 || id >= bodies.Num() ) {
7593  return vec3_origin;
7594  }
7595  else {
7596  return bodies[id]->current->worldOrigin;
7597  }
7598 }
7599 
7600 /*
7601 ================
7602 idPhysics_AF::GetAxis
7603 ================
7604 */
7605 const idMat3 &idPhysics_AF::GetAxis( int id ) const {
7606  if ( id < 0 || id >= bodies.Num() ) {
7607  return mat3_identity;
7608  }
7609  else {
7610  return bodies[id]->current->worldAxis;
7611  }
7612 }
7613 
7614 /*
7615 ================
7616 idPhysics_AF::SetLinearVelocity
7617 ================
7618 */
7619 void idPhysics_AF::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) {
7620  if ( id < 0 || id >= bodies.Num() ) {
7621  return;
7622  }
7623  bodies[id]->current->spatialVelocity.SubVec3( 0 ) = newLinearVelocity;
7624  Activate();
7625 }
7626 
7627 /*
7628 ================
7629 idPhysics_AF::SetAngularVelocity
7630 ================
7631 */
7632 void idPhysics_AF::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) {
7633  if ( id < 0 || id >= bodies.Num() ) {
7634  return;
7635  }
7636  bodies[id]->current->spatialVelocity.SubVec3( 1 ) = newAngularVelocity;
7637  Activate();
7638 }
7639 
7640 /*
7641 ================
7642 idPhysics_AF::GetLinearVelocity
7643 ================
7644 */
7645 const idVec3 &idPhysics_AF::GetLinearVelocity( int id ) const {
7646  if ( id < 0 || id >= bodies.Num() ) {
7647  return vec3_origin;
7648  }
7649  else {
7650  return bodies[id]->current->spatialVelocity.SubVec3( 0 );
7651  }
7652 }
7653 
7654 /*
7655 ================
7656 idPhysics_AF::GetAngularVelocity
7657 ================
7658 */
7659 const idVec3 &idPhysics_AF::GetAngularVelocity( int id ) const {
7660  if ( id < 0 || id >= bodies.Num() ) {
7661  return vec3_origin;
7662  }
7663  else {
7664  return bodies[id]->current->spatialVelocity.SubVec3( 1 );
7665  }
7666 }
7667 
7668 /*
7669 ================
7670 idPhysics_AF::ClipTranslation
7671 ================
7672 */
7673 void idPhysics_AF::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const {
7674  int i;
7675  idAFBody *body;
7676  trace_t bodyResults;
7677 
7678  results.fraction = 1.0f;
7679 
7680  for ( i = 0; i < bodies.Num(); i++ ) {
7681  body = bodies[i];
7682 
7683  if ( body->clipModel->IsTraceModel() ) {
7684  if ( model ) {
7685  gameLocal.clip.TranslationModel( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation,
7686  body->clipModel, body->current->worldAxis, body->clipMask,
7687  model->Handle(), model->GetOrigin(), model->GetAxis() );
7688  }
7689  else {
7690  gameLocal.clip.Translation( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation,
7691  body->clipModel, body->current->worldAxis, body->clipMask, self );
7692  }
7693  if ( bodyResults.fraction < results.fraction ) {
7694  results = bodyResults;
7695  }
7696  }
7697  }
7698 
7699  results.endpos = bodies[0]->current->worldOrigin + results.fraction * translation;
7700  results.endAxis = bodies[0]->current->worldAxis;
7701 }
7702 
7703 /*
7704 ================
7705 idPhysics_AF::ClipRotation
7706 ================
7707 */
7708 void idPhysics_AF::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const {
7709  int i;
7710  idAFBody *body;
7711  trace_t bodyResults;
7712  idRotation partialRotation;
7713 
7714  results.fraction = 1.0f;
7715 
7716  for ( i = 0; i < bodies.Num(); i++ ) {
7717  body = bodies[i];
7718 
7719  if ( body->clipModel->IsTraceModel() ) {
7720  if ( model ) {
7721  gameLocal.clip.RotationModel( bodyResults, body->current->worldOrigin, rotation,
7722  body->clipModel, body->current->worldAxis, body->clipMask,
7723  model->Handle(), model->GetOrigin(), model->GetAxis() );
7724  }
7725  else {
7726  gameLocal.clip.Rotation( bodyResults, body->current->worldOrigin, rotation,
7727  body->clipModel, body->current->worldAxis, body->clipMask, self );
7728  }
7729  if ( bodyResults.fraction < results.fraction ) {
7730  results = bodyResults;
7731  }
7732  }
7733  }
7734 
7735  partialRotation = rotation * results.fraction;
7736  results.endpos = bodies[0]->current->worldOrigin * partialRotation;
7737  results.endAxis = bodies[0]->current->worldAxis * partialRotation.ToMat3();
7738 }
7739 
7740 /*
7741 ================
7742 idPhysics_AF::ClipContents
7743 ================
7744 */
7745 int idPhysics_AF::ClipContents( const idClipModel *model ) const {
7746  int i, contents;
7747  idAFBody *body;
7748 
7749  contents = 0;
7750 
7751  for ( i = 0; i < bodies.Num(); i++ ) {
7752  body = bodies[i];
7753 
7754  if ( body->clipModel->IsTraceModel() ) {
7755  if ( model ) {
7756  contents |= gameLocal.clip.ContentsModel( body->current->worldOrigin,
7757  body->clipModel, body->current->worldAxis, -1,
7758  model->Handle(), model->GetOrigin(), model->GetAxis() );
7759  }
7760  else {
7761  contents |= gameLocal.clip.Contents( body->current->worldOrigin,
7762  body->clipModel, body->current->worldAxis, -1, NULL );
7763  }
7764  }
7765  }
7766 
7767  return contents;
7768 }
7769 
7770 /*
7771 ================
7772 idPhysics_AF::DisableClip
7773 ================
7774 */
7775 void idPhysics_AF::DisableClip( void ) {
7776  int i;
7777 
7778  for ( i = 0; i < bodies.Num(); i++ ) {
7779  bodies[i]->clipModel->Disable();
7780  }
7781 }
7782 
7783 /*
7784 ================
7785 idPhysics_AF::EnableClip
7786 ================
7787 */
7788 void idPhysics_AF::EnableClip( void ) {
7789  int i;
7790 
7791  for ( i = 0; i < bodies.Num(); i++ ) {
7792  bodies[i]->clipModel->Enable();
7793  }
7794 }
7795 
7796 /*
7797 ================
7798 idPhysics_AF::UnlinkClip
7799 ================
7800 */
7801 void idPhysics_AF::UnlinkClip( void ) {
7802  int i;
7803 
7804  for ( i = 0; i < bodies.Num(); i++ ) {
7805  bodies[i]->clipModel->Unlink();
7806  }
7807 }
7808 
7809 /*
7810 ================
7811 idPhysics_AF::LinkClip
7812 ================
7813 */
7814 void idPhysics_AF::LinkClip( void ) {
7815  UpdateClipModels();
7816 }
7817 
7818 /*
7819 ================
7820 idPhysics_AF::SetPushed
7821 ================
7822 */
7823 void idPhysics_AF::SetPushed( int deltaTime ) {
7824  idAFBody *body;
7825  idRotation rotation;
7826 
7827  if ( bodies.Num() ) {
7828  body = bodies[0];
7829  rotation = ( body->saved.worldAxis.Transpose() * body->current->worldAxis ).ToRotation();
7830 
7831  // velocity with which the af is pushed
7832  current.pushVelocity.SubVec3(0) += ( body->current->worldOrigin - body->saved.worldOrigin ) / ( deltaTime * idMath::M_MS2SEC );
7833  current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( deltaTime * idMath::M_MS2SEC );
7834  }
7835 }
7836 
7837 /*
7838 ================
7839 idPhysics_AF::GetPushedLinearVelocity
7840 ================
7841 */
7842 const idVec3 &idPhysics_AF::GetPushedLinearVelocity( const int id ) const {
7843  return current.pushVelocity.SubVec3(0);
7844 }
7845 
7846 /*
7847 ================
7848 idPhysics_AF::GetPushedAngularVelocity
7849 ================
7850 */
7851 const idVec3 &idPhysics_AF::GetPushedAngularVelocity( const int id ) const {
7852  return current.pushVelocity.SubVec3(1);
7853 }
7854 
7855 /*
7856 ================
7857 idPhysics_AF::SetMaster
7858 
7859  the binding is orientated based on the constraints being used
7860 ================
7861 */
7862 void idPhysics_AF::SetMaster( idEntity *master, const bool orientated ) {
7863  int i;
7864  idVec3 masterOrigin;
7865  idMat3 masterAxis;
7866  idRotation rotation;
7867 
7868  if ( master ) {
7869  self->GetMasterPosition( masterOrigin, masterAxis );
7870  if ( !masterBody ) {
7871  masterBody = new idAFBody();
7872  // translate and rotate all the constraints with body2 == NULL from world space to master space
7873  rotation = masterAxis.Transpose().ToRotation();
7874  for ( i = 0; i < constraints.Num(); i++ ) {
7875  if ( constraints[i]->GetBody2() == NULL ) {
7876  constraints[i]->Translate( -masterOrigin );
7877  constraints[i]->Rotate( rotation );
7878  }
7879  }
7880  Activate();
7881  }
7882  masterBody->current->worldOrigin = masterOrigin;
7883  masterBody->current->worldAxis = masterAxis;
7884  }
7885  else {
7886  if ( masterBody ) {
7887  // translate and rotate all the constraints with body2 == NULL from master space to world space
7888  rotation = masterBody->current->worldAxis.ToRotation();
7889  for ( i = 0; i < constraints.Num(); i++ ) {
7890  if ( constraints[i]->GetBody2() == NULL ) {
7891  constraints[i]->Rotate( rotation );
7892  constraints[i]->Translate( masterBody->current->worldOrigin );
7893  }
7894  }
7895  delete masterBody;
7896  masterBody = NULL;
7897  Activate();
7898  }
7899  }
7900 }
7901 
7902 
7903 const float AF_VELOCITY_MAX = 16000;
7904 const int AF_VELOCITY_TOTAL_BITS = 16;
7905 const int AF_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( AF_VELOCITY_MAX ) ) + 1;
7906 const int AF_VELOCITY_MANTISSA_BITS = AF_VELOCITY_TOTAL_BITS - 1 - AF_VELOCITY_EXPONENT_BITS;
7907 const float AF_FORCE_MAX = 1e20f;
7908 const int AF_FORCE_TOTAL_BITS = 16;
7909 const int AF_FORCE_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( AF_FORCE_MAX ) ) + 1;
7910 const int AF_FORCE_MANTISSA_BITS = AF_FORCE_TOTAL_BITS - 1 - AF_FORCE_EXPONENT_BITS;
7911 
7912 /*
7913 ================
7914 idPhysics_AF::WriteToSnapshot
7915 ================
7916 */
7917 void idPhysics_AF::WriteToSnapshot( idBitMsgDelta &msg ) const {
7918  int i;
7919  idCQuat quat;
7920 
7921  msg.WriteLong( current.atRest );
7922  msg.WriteFloat( current.noMoveTime );
7923  msg.WriteFloat( current.activateTime );
7924  msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7925  msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7926  msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7927  msg.WriteDeltaFloat( 0.0f, current.pushVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7928  msg.WriteDeltaFloat( 0.0f, current.pushVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7929  msg.WriteDeltaFloat( 0.0f, current.pushVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7930 
7931  msg.WriteByte( bodies.Num() );
7932 
7933  for ( i = 0; i < bodies.Num(); i++ ) {
7934  AFBodyPState_t *state = bodies[i]->current;
7935  quat = state->worldAxis.ToCQuat();
7936 
7937  msg.WriteFloat( state->worldOrigin[0] );
7938  msg.WriteFloat( state->worldOrigin[1] );
7939  msg.WriteFloat( state->worldOrigin[2] );
7940  msg.WriteFloat( quat.x );
7941  msg.WriteFloat( quat.y );
7942  msg.WriteFloat( quat.z );
7943  msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7944  msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7945  msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7946  msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7947  msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7948  msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7949 /* msg.WriteDeltaFloat( 0.0f, state->externalForce[0], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7950  msg.WriteDeltaFloat( 0.0f, state->externalForce[1], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7951  msg.WriteDeltaFloat( 0.0f, state->externalForce[2], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7952  msg.WriteDeltaFloat( 0.0f, state->externalForce[3], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7953  msg.WriteDeltaFloat( 0.0f, state->externalForce[4], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7954  msg.WriteDeltaFloat( 0.0f, state->externalForce[5], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7955 */
7956  }
7957 }
7958 
7959 /*
7960 ================
7961 idPhysics_AF::ReadFromSnapshot
7962 ================
7963 */
7964 void idPhysics_AF::ReadFromSnapshot( const idBitMsgDelta &msg ) {
7965  int i, num;
7966  idCQuat quat;
7967 
7968  current.atRest = msg.ReadLong();
7969  current.noMoveTime = msg.ReadFloat();
7970  current.activateTime = msg.ReadFloat();
7971  current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7972  current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7973  current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7974  current.pushVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7975  current.pushVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7976  current.pushVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7977 
7978  num = msg.ReadByte();
7979  assert( num == bodies.Num() );
7980 
7981  for ( i = 0; i < bodies.Num(); i++ ) {
7982  AFBodyPState_t *state = bodies[i]->current;
7983 
7984  state->worldOrigin[0] = msg.ReadFloat();
7985  state->worldOrigin[1] = msg.ReadFloat();
7986  state->worldOrigin[2] = msg.ReadFloat();
7987  quat.x = msg.ReadFloat();
7988  quat.y = msg.ReadFloat();
7989  quat.z = msg.ReadFloat();
7990  state->spatialVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7991  state->spatialVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7992  state->spatialVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7993  state->spatialVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7994  state->spatialVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7995  state->spatialVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7996 /* state->externalForce[0] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7997  state->externalForce[1] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7998  state->externalForce[2] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7999  state->externalForce[3] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
8000  state->externalForce[4] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
8001  state->externalForce[5] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
8002 */
8003  state->worldAxis = quat.ToMat3();
8004  }
8005 
8006  UpdateClipModels();
8007 }
idAFBody::maxAuxiliaryIndex
int maxAuxiliaryIndex
Definition: Physics_AF.h:754
idPhysics_AF::AddForce
void AddForce(const int id, const idVec3 &point, const idVec3 &force)
Definition: Physics_AF.cpp:7425
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:2959
idAFConstraint_Contact::GetContact
const contactInfo_t & GetContact(void) const
Definition: Physics_AF.h:503
idMat3::ToCQuat
idCQuat ToCQuat(void) const
Definition: Matrix.cpp:233
idGameLocal::GetLocalPlayer
idPlayer * GetLocalPlayer() const
Definition: Game_local.cpp:2128
idAFConstraint_BallAndSocketJoint::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:778
idAFBody::bodyFlags_s::useContactMotorDir
bool useContactMotorDir
Definition: Physics_AF.h:762
idMatX::InverseFastSelf
bool InverseFastSelf(void)
Definition: Matrix.h:2628
idAFBody::SetBouncyness
void SetBouncyness(float bounce)
Definition: Physics_AF.cpp:4265
idAFConstraint::GetBody1
idAFBody * GetBody1(void) const
Definition: Physics_AF.h:103
idAFConstraint_ContactFriction::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:3318
idPhysics_AF::GetImpactInfo
void GetImpactInfo(const int id, const idVec3 &point, impactInfo_t *info) const
Definition: Physics_AF.cpp:7391
idMatX::TransposeMultiply
idVecX TransposeMultiply(const idVecX &vec) const
Definition: Matrix.h:2683
color
byte color[4]
Definition: MegaTexture.cpp:54
idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint
~idAFConstraint_UniversalJoint(void)
Definition: Physics_AF.cpp:939
idVecX::SetSize
void SetSize(int size)
Definition: Vector.h:1707
idAFConstraint_BallAndSocketJoint::GetCenter
virtual void GetCenter(idVec3 &center)
Definition: Physics_AF.cpp:748
idPhysics_AF::ForceBodyId
void ForceBodyId(idAFBody *body, int newId)
Definition: Physics_AF.cpp:7084
idAFConstraint_UniversalJointFriction::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:1487
idAFConstraint::body1
idAFBody * body1
Definition: Physics_AF.h:120
idPhysics_AF::bodies
idList< idAFBody * > bodies
Definition: Physics_AF.h:961
idPhysics_AF::EnableImpact
void EnableImpact(void)
Definition: Physics_AF.cpp:6037
idAFConstraint_Hinge::SetSteerAngle
void SetSteerAngle(const float degrees)
Definition: Physics_AF.cpp:1751
idAFBody::InverseWorldSpatialInertiaMultiply
void InverseWorldSpatialInertiaMultiply(idVecX &dst, const float *v) const
Definition: Physics_AF.cpp:4389
idAFBody::linearFriction
float linearFriction
Definition: Physics_AF.h:718
idAFConstraint_ConeLimit::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:3509
idPhysics_AF::enableCollision
bool enableCollision
Definition: Physics_AF.h:1004
idAFConstraint_Contact::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:3139
idAFConstraint_BallAndSocketJoint::GetForce
virtual void GetForce(idAFBody *body, idVec6 &force)
Definition: Physics_AF.cpp:704
idAFBody::frictionDir
idVec3 frictionDir
Definition: Physics_AF.h:723
idAFConstraint_Hinge::GetFriction
float GetFriction(void) const
Definition: Physics_AF.cpp:1720
idMath::INFINITY
static const float INFINITY
Definition: Math.h:218
idPhysics_Base::DrawVelocity
void DrawVelocity(int id, float linearScale, float angularScale) const
Definition: Physics_Base.cpp:781
idAFConstraint::InitSize
void InitSize(int size)
Definition: Physics_AF.cpp:228
idPhysics_AF::minMoveTime
float minMoveTime
Definition: Physics_AF.h:984
idAFBody::clipMask
int clipMask
Definition: Physics_AF.h:722
idAFConstraint_CylindricalJoint::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:1546
idVec3::Compare
bool Compare(const idVec3 &a) const
Definition: Vector.h:496
idAFConstraint_ContactFriction::cc
idAFConstraint_Contact * cc
Definition: Physics_AF.h:530
idVec3::Normalize
float Normalize(void)
Definition: Vector.h:646
colorGreen
idVec4 colorGreen
Definition: Lib.cpp:118
af_useJointImpulseFriction
idCVar af_useJointImpulseFriction("af_useJointImpulseFriction","0", CVAR_GAME|CVAR_BOOL,"use impulse based joint friction")
idAFConstraint_HingeSteering::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:2258
idAFConstraint::boxConstraint
idAFConstraint * boxConstraint
Definition: Physics_AF.h:128
idClipModel::SetId
void SetId(int newId)
Definition: Clip.h:182
idAFConstraint_UniversalJoint::SetAnchor
void SetAnchor(const idVec3 &worldPosition)
Definition: Physics_AF.cpp:956
idPhysics_AF::constraints
idList< idAFConstraint * > constraints
Definition: Physics_AF.h:962
idAFBody::inverseWorldSpatialInertia
idMatX inverseWorldSpatialInertia
Definition: Physics_AF.h:744
idAFConstraint_Line::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:2505
idAFConstraint_BallAndSocketJoint::SetPyramidLimit
void SetPyramidLimit(const idVec3 &pyramidAxis, const idVec3 &baseAxis, const float angle1, const float angle2, const idVec3 &body1Axis)
Definition: Physics_AF.cpp:571
idPhysics_AF::AddBody
int AddBody(idAFBody *body)
Definition: Physics_AF.cpp:7001
idAFConstraint_Fixed::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:433
idAFConstraint::SetPhysics
void SetPhysics(idPhysics_AF *p)
Definition: Physics_AF.h:105
cross
void cross(float a[], float b[], float c[])
Definition: Model_lwo.cpp:3889
idAFConstraint_UniversalJointFriction::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:1425
assert
assert(prefInfo.fullscreenBtn)
idAFConstraint_HingeFriction::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:2057
idAFConstraint_BallAndSocketJointFriction
Definition: Physics_AF.h:217
idPhysics_AF::GetNumConstraints
int GetNumConstraints(void) const
Definition: Physics_AF.cpp:7177
DrawTraceModelSilhouette
void DrawTraceModelSilhouette(const idVec3 &projectionOrigin, const idClipModel *clipModel)
Definition: Physics_AF.cpp:6433
idPhysics_AF::PrimaryFactor
void PrimaryFactor(void)
Definition: Physics_AF.cpp:5019
idPhysics_AF::SetupContactConstraints
void SetupContactConstraints(void)
Definition: Physics_AF.cpp:5691
idClipModel::Link
void Link(idClip &clp)
Definition: Clip.cpp:545
af_forceFriction
idCVar af_forceFriction("af_forceFriction","-1", CVAR_GAME|CVAR_FLOAT,"force the given friction value")
cvarSystem
idCVarSystem * cvarSystem
Definition: CVarSystem.cpp:487
idAFTree::DebugDraw
void DebugDraw(const idVec4 &color) const
Definition: Physics_AF.cpp:4839
idAFConstraint::GetCenter
virtual void GetCenter(idVec3 &center)
Definition: Physics_AF.cpp:211
idAFConstraint::GetBody2
idAFBody * GetBody2(void) const
Definition: Physics_AF.h:104
af_skipLimits
idCVar af_skipLimits("af_skipLimits","0", CVAR_GAME|CVAR_BOOL,"skip joint limits")
idAFConstraint_Slider::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:2371
idPhysics_AF::SetDefaultFriction
void SetDefaultFriction(float linear, float angular, float contact)
Definition: Physics_AF.cpp:7375
mat3_identity
idMat3 mat3_identity(idVec3(1, 0, 0), idVec3(0, 1, 0), idVec3(0, 0, 1))
idRestoreGame::ReadVec2
void ReadVec2(idVec2 &vec)
Definition: SaveGame.cpp:1002
idAFBody::GetContactMotorDirection
bool GetContactMotorDirection(idVec3 &dir) const
Definition: Physics_AF.cpp:4354
idAFConstraint_HingeSteering::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:2165
idCQuat
Definition: Quat.h:306
idPhysics_AF::SetContactFrictionDent
void SetContactFrictionDent(const float dent, const float start, const float end)
Definition: Physics_AF.cpp:5880
idAFConstraint_Hinge::SetSteerSpeed
void SetSteerSpeed(const float speed)
Definition: Physics_AF.cpp:1768
idPhysics_AF::DebugDraw
void DebugDraw(void)
Definition: Physics_AF.cpp:6454
idAFConstraint_UniversalJointFriction
Definition: Physics_AF.h:278
idAFConstraint_Suspension::wheelOffset
idVec3 wheelOffset
Definition: Physics_AF.h:633
idAFConstraint::SetBody2
virtual void SetBody2(idAFBody *body)
Definition: Physics_AF.cpp:132
idPhysics_AF::ApplyImpulse
void ApplyImpulse(const int id, const idVec3 &point, const idVec3 &impulse)
Definition: Physics_AF.cpp:7407
CONSTRAINT_CONELIMIT
Definition: Physics_AF.h:80
idPhysics_AF::GetClipModel
idClipModel * GetClipModel(int id=0) const
Definition: Physics_AF.cpp:6078
idRenderWorld::DebugArrow
virtual void DebugArrow(const idVec4 &color, const idVec3 &start, const idVec3 &end, int size, const int lifetime=0)=0
idAFConstraint_ConeLimit::sinHalfAngle
float sinHalfAngle
Definition: Physics_AF.h:559
idPhysics_AF::GetNumBodies
int GetNumBodies(void) const
Definition: Physics_AF.cpp:7168
idPhysics_AF::GetConstraint
idAFConstraint * GetConstraint(const char *constraintName) const
Definition: Physics_AF.cpp:7216
idAFConstraint::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:220
idGameLocal::clip
idClip clip
Definition: Game_local.h:296
idPhysics_AF::GetConstraintId
int GetConstraintId(idAFConstraint *constraint) const
Definition: Physics_AF.cpp:7136
idAFConstraint_Contact::contact
contactInfo_t contact
Definition: Physics_AF.h:510
colorWhite
idVec4 colorWhite
Definition: Lib.cpp:116
idAFConstraint_BallAndSocketJoint::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:714
TransposeMultiply
ID_INLINE void TransposeMultiply(const idMat3 &transpose, const idMat3 &b, idMat3 &dst)
Definition: Matrix.h:729
idPhysics_AF::bouncyness
float bouncyness
Definition: Physics_AF.h:975
idGameLocal::Printf
void Printf(const char *fmt,...) const id_attribute((format(printf
Definition: Game_local.cpp:699
idTimer
Definition: Timer.h:40
idBounds::Zero
void Zero(void)
Definition: Bounds.h:206
idAFConstraint_Suspension::suspensionUp
float suspensionUp
Definition: Physics_AF.h:623
af_showLimits
idCVar af_showLimits("af_showLimits","0", CVAR_GAME|CVAR_BOOL,"show joint limits")
idAFBody::atRestAxis
idMat3 atRestAxis
Definition: Physics_AF.h:741
af_showInertia
idCVar af_showInertia("af_showInertia","0", CVAR_GAME|CVAR_BOOL,"show the inertia tensor of each body")
idAFConstraint_HingeFriction::hinge
idAFConstraint_Hinge * hinge
Definition: Physics_AF.h:367
idAFConstraint_Fixed::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:416
idAFBody::maxSubTreeAuxiliaryIndex
int maxSubTreeAuxiliaryIndex
Definition: Physics_AF.h:755
af_showConstrainedBodies
idCVar af_showConstrainedBodies("af_showConstrainedBodies","0", CVAR_GAME|CVAR_BOOL,"show the two bodies contrained by the highlighted constraint")
idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint
idAFConstraint_BallAndSocketJoint(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:462
idTraceModel::GetProjectionSilhouetteEdges
int GetProjectionSilhouetteEdges(const idVec3 &projectionOrigin, int silEdges[MAX_TRACEMODEL_EDGES]) const
Definition: TraceModel.cpp:1207
idPhysics_AF::VerifyContactConstraints
void VerifyContactConstraints(void)
Definition: Physics_AF.cpp:5272
idAFConstraint_PyramidLimit::SetAnchor
void SetAnchor(const idVec3 &pyramidAxis)
Definition: Physics_AF.cpp:3642
idAFConstraint_Hinge::coneLimit
idAFConstraint_ConeLimit * coneLimit
Definition: Physics_AF.h:347
j1
GLint GLint GLint j1
Definition: qgl.h:262
idAFBody::SetDensity
void SetDensity(float density, const idMat3 &inertiaScale=mat3_identity)
Definition: Physics_AF.cpp:4278
idAFBody::GetFrictionDirection
bool GetFrictionDirection(idVec3 &dir) const
Definition: Physics_AF.cpp:4331
idAFConstraint_Hinge::GetAxis
idVec3 GetAxis(void) const
Definition: Physics_AF.cpp:1667
idAFConstraint_HingeFriction::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:2115
idAFConstraint_UniversalJoint::GetForce
virtual void GetForce(idAFBody *body, idVec6 &force)
Definition: Physics_AF.cpp:1238
idAFBody::SetContactMotorDirection
void SetContactMotorDirection(const idVec3 &dir)
Definition: Physics_AF.cpp:4344
idAFConstraint::GetMultiplier
const idVecX & GetMultiplier(void)
Definition: Physics_AF.cpp:146
idCVar::GetFloat
float GetFloat(void) const
Definition: CVarSystem.h:144
idTraceModel::verts
traceModelVert_t verts[MAX_TRACEMODEL_VERTS]
Definition: TraceModel.h:88
v
const GLdouble * v
Definition: glext.h:2936
idRotation::SetVec
void SetVec(const idVec3 &rotationVec)
Definition: Rotation.h:119
idClip::Contacts
int Contacts(contactInfo_t *contacts, const int maxContacts, const idVec3 &start, const idVec6 &dir, const float depth, const idClipModel *mdl, const idMat3 &trmAxis, int contentMask, const idEntity *passEntity)
Definition: Clip.cpp:1358
MAX_MOVE_TIME
const float MAX_MOVE_TIME
Definition: Physics_AF.cpp:48
idPhysics_AF::masterBody
idAFBody * masterBody
Definition: Physics_AF.h:1016
x2
GLdouble GLdouble x2
Definition: qgl.h:415
idList::SetNum
void SetNum(int newnum, bool resize=true)
Definition: List.h:289
idAFConstraint_HingeSteering::idAFConstraint_HingeSteering
idAFConstraint_HingeSteering(void)
Definition: Physics_AF.cpp:2138
idPhysics_AF::ApplyFriction
void ApplyFriction(float timeStep, float endTimeMSec)
Definition: Physics_AF.cpp:4971
idQuat::ToMat3
idMat3 ToMat3(void) const
Definition: Quat.cpp:70
idVecX::Zero
void Zero(void)
Definition: Vector.h:1767
contactInfo_t::normal
idVec3 normal
Definition: CollisionModel.h:63
idAFBody::GetInverseMass
float GetInverseMass(void) const
Definition: Physics_AF.h:689
idPhysics_AF::TestIfAtRest
bool TestIfAtRest(float timeStep)
Definition: Physics_AF.cpp:5907
idAFConstraint_Plane::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:2615
idPhysics_AF::Save
void Save(idSaveGame *savefile) const
Definition: Physics_AF.cpp:6713
idAFConstraint_UniversalJoint::GetFriction
float GetFriction(void) const
Definition: Physics_AF.cpp:1111
idAFTree::Solve
void Solve(int auxiliaryIndex=0) const
Definition: Physics_AF.cpp:4536
const
#define const
Definition: getdate.c:251
idAFConstraint_HingeSteering::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:2250
idAFConstraint_Line::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:2469
idAFBody::tree
idAFTree * tree
Definition: Physics_AF.h:717
idPhysics_AF::GetBounds
const idBounds & GetBounds(int id=-1) const
Definition: Physics_AF.cpp:6163
idAFBody::state
AFBodyPState_t state[2]
Definition: Physics_AF.h:736
scale
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:4804
idMat3::Transpose
idMat3 Transpose(void) const
Definition: Matrix.h:677
idGameLocal::Error
void void void void void Error(const char *fmt,...) const id_attribute((format(printf
Definition: Game_local.cpp:783
idPhysics_AF::SetPushed
void SetPushed(int deltaTime)
Definition: Physics_AF.cpp:7823
idAFConstraint_PyramidLimit::sinHalfAngle
float sinHalfAngle[2]
Definition: Physics_AF.h:591
idAFBody::contactMotorDir
idVec3 contactMotorDir
Definition: Physics_AF.h:724
idAFConstraint_Slider::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:2382
idAFConstraint_Suspension::motorForce
float motorForce
Definition: Physics_AF.h:630
idAFConstraint_Hinge::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:1905
vec6_origin
idVec6 vec6_origin(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)
idPhysics_AF::changedAF
bool changedAF
Definition: Physics_AF.h:969
idAFConstraint_Suspension::Setup
void Setup(const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel)
Definition: Physics_AF.cpp:3915
idAFConstraint_Spring::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:2845
idMatX::Set
void Set(int rows, int columns, const float *src)
Definition: Matrix.h:2018
idQuat::w
float w
Definition: Quat.h:53
idAFConstraint_Plane::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:2604
idPhysics_Base::IsOutsideWorld
bool IsOutsideWorld(void) const
Definition: Physics_Base.cpp:769
idAFConstraint::constraintFlags_s::isZero
bool isZero
Definition: Physics_AF.h:143
idPhysics_AF::GetBody
idAFBody * GetBody(const char *bodyName) const
Definition: Physics_AF.cpp:7186
y
GLenum GLint GLint y
Definition: glext.h:2849
idAFBody::GetWorldAxis
const idMat3 & GetWorldAxis(void) const
Definition: Physics_AF.h:670
idAFConstraint_Hinge::axis2
idVec3 axis2
Definition: Physics_AF.h:344
idClipModel::GetAxis
const idMat3 & GetAxis(void) const
Definition: Clip.h:210
idPhysics_AF::noMoveTime
float noMoveTime
Definition: Physics_AF.h:981
n
GLenum GLsizei n
Definition: glext.h:3705
SUSPEND_LINEAR_VELOCITY
const float SUSPEND_LINEAR_VELOCITY
Definition: Physics_AF.cpp:50
NO_MOVE_ROTATION_TOLERANCE
const float NO_MOVE_ROTATION_TOLERANCE
Definition: Physics_AF.cpp:46
idPhysics_AF::linearTime
bool linearTime
Definition: Physics_AF.h:1007
idVec3::z
float z
Definition: Vector.h:320
idAFConstraint_UniversalJoint::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:1248
MASK_SOLID
#define MASK_SOLID
Definition: Game_local.h:735
int
case const int
Definition: Callbacks.cpp:52
idAFBody::GetCenterOfMass
const idVec3 & GetCenterOfMass(void) const
Definition: Physics_AF.h:674
idAFConstraint_BallAndSocketJoint::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:623
idAFBody::saved
AFBodyPState_t saved
Definition: Physics_AF.h:739
idAFConstraint_Hinge::fc
idAFConstraint_HingeFriction * fc
Definition: Physics_AF.h:349
VECX_ALLOCA
#define VECX_ALLOCA(n)
Definition: Vector.h:1432
idAFBody::constraints
idList< idAFConstraint * > constraints
Definition: Physics_AF.h:716
idAFConstraint_Spring::anchor2
idVec3 anchor2
Definition: Physics_AF.h:483
idAFConstraint::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:242
idPhysics_AF::timeScaleRampStart
float timeScaleRampStart
Definition: Physics_AF.h:989
idAFConstraint_UniversalJoint::SetShafts
void SetShafts(const idVec3 &cardanShaft1, const idVec3 &cardanShaft2)
Definition: Physics_AF.cpp:993
idAFConstraint_UniversalJoint::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:1293
idAFConstraint_ConeLimit::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:3576
af_showConstraintNames
idCVar af_showConstraintNames("af_showConstraintNames","0", CVAR_GAME|CVAR_BOOL,"show constraint names")
idAFConstraint_UniversalJoint::coneLimit
idAFConstraint_ConeLimit * coneLimit
Definition: Physics_AF.h:268
vec6_lcp_epsilon
const idVec6 vec6_lcp_epsilon
Definition: Physics_AF.cpp:54
idPhysics_AF::idPhysics_AF
idPhysics_AF(void)
Definition: Physics_AF.cpp:6588
idAFConstraint_ConeLimit::Add
bool Add(idPhysics_AF *phys, float invTimeStep)
Definition: Physics_AF.cpp:3420
af_showConstraints
idCVar af_showConstraints("af_showConstraints","0", CVAR_GAME|CVAR_BOOL,"show constraints")
idVec6::SubVec3
const idVec3 & SubVec3(int index) const
Definition: Vector.h:1402
idPhysics_AF::GetMass
float GetMass(int id=-1) const
Definition: Physics_AF.cpp:6113
idPhysics_AF::comeToRest
bool comeToRest
Definition: Physics_AF.h:1006
idAFConstraint::constraintFlags_s::allowPrimary
bool allowPrimary
Definition: Physics_AF.h:139
idVec3
Definition: Vector.h:316
float
case const float
Definition: Callbacks.cpp:62
NO_MOVE_TRANSLATION_TOLERANCE
const float NO_MOVE_TRANSLATION_TOLERANCE
Definition: Physics_AF.cpp:45
idRestoreGame::ReadBool
void ReadBool(bool &value)
Definition: SaveGame.cpp:976
idAFConstraint_Hinge::anchor2
idVec3 anchor2
Definition: Physics_AF.h:342
contactInfo_t::id
int id
Definition: CollisionModel.h:70
idAFConstraint_HingeFriction
Definition: Physics_AF.h:357
AF_VELOCITY_EXPONENT_BITS
const int AF_VELOCITY_EXPONENT_BITS
Definition: Physics_AF.cpp:7905
idAFBody::contactFriction
float contactFriction
Definition: Physics_AF.h:720
idAFConstraint_UniversalJoint::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:1197
contactInfo_t::point
idVec3 point
Definition: CollisionModel.h:62
idMath::Sqrt
static float Sqrt(float x)
Definition: Math.h:302
Square
ID_INLINE T Square(T x)
Definition: Math.h:104
idAFBody::bodyFlags_s::useFrictionDir
bool useFrictionDir
Definition: Physics_AF.h:761
idAFConstraint_Contact::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:3112
idAFConstraint_Suspension::GetWheelOrigin
const idVec3 GetWheelOrigin(void) const
Definition: Physics_AF.cpp:3942
FLOAT_IS_NAN
#define FLOAT_IS_NAN(x)
Definition: Math.h:74
idAFConstraint_Spring::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:2744
idPhysics_AF::Rest
void Rest(void)
Definition: Physics_AF.cpp:5989
af_showMass
idCVar af_showMass("af_showMass","0", CVAR_GAME|CVAR_BOOL,"show the mass of each body")
idAFConstraint_UniversalJointFriction::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:1434
idTimer::Start
void Start(void)
Definition: Timer.h:144
idAFConstraint_Spring::minLength
float minLength
Definition: Physics_AF.h:488
idCVarSystem::SetCVarString
virtual void SetCVarString(const char *name, const char *value, int flags=0)=0
idClipModel::GetOrigin
const idVec3 & GetOrigin(void) const
Definition: Clip.h:206
idPhysics_AF::auxiliaryConstraints
idList< idAFConstraint * > auxiliaryConstraints
Definition: Physics_AF.h:964
vec6_zero
#define vec6_zero
Definition: Vector.h:1205
idAFBody::totalForce
idVecX totalForce
Definition: Physics_AF.h:748
idAFConstraint_ContactFriction::Add
bool Add(idPhysics_AF *phys, float invTimeStep)
Definition: Physics_AF.cpp:3202
idAFConstraint_Fixed::idAFConstraint_Fixed
idAFConstraint_Fixed(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:269
af_showVelocity
idCVar af_showVelocity("af_showVelocity","0", CVAR_GAME|CVAR_BOOL,"show the velocity of each body")
LCP_EPSILON
const float LCP_EPSILON
Definition: Physics_AF.cpp:40
type
GLuint GLuint GLsizei GLenum type
Definition: glext.h:2845
MIN_MOVE_TIME
const float MIN_MOVE_TIME
Definition: Physics_AF.cpp:47
CONSTRAINT_SPRING
Definition: Physics_AF.h:77
idTraceModel::edges
traceModelEdge_t edges[MAX_TRACEMODEL_EDGES+1]
Definition: TraceModel.h:90
af_showPrimaryOnly
idCVar af_showPrimaryOnly("af_showPrimaryOnly","0", CVAR_GAME|CVAR_BOOL,"show primary constraints only")
contactInfo_t
Definition: CollisionModel.h:60
idAFConstraint_Hinge::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:1840
idAFConstraint_UniversalJoint::axis1
idVec3 axis1
Definition: Physics_AF.h:265
idAFConstraint_Spring::maxLength
float maxLength
Definition: Physics_AF.h:489
idAFBody::GetInverseWorldInertia
idMat3 GetInverseWorldInertia(void) const
Definition: Physics_AF.h:690
idMatX::Transpose
idMatX Transpose(void) const
Definition: Matrix.h:2562
idPhysics_AF::GetContactFrictionScale
float GetContactFrictionScale(void) const
Definition: Physics_AF.cpp:5891
idAFBody::GetContactMotorForce
float GetContactMotorForce(void) const
Definition: Physics_AF.h:700
AF_FORCE_EXPONENT_BITS
const int AF_FORCE_EXPONENT_BITS
Definition: Physics_AF.cpp:7909
CONSTRAINT_INVALID
Definition: Physics_AF.h:67
idAFTree::SortBodies_r
void SortBodies_r(idList< idAFBody * > &sortedList, idAFBody *body)
Definition: Physics_AF.cpp:4795
idBitMsgDelta::WriteFloat
void WriteFloat(float f)
Definition: BitMsg.h:558
s
GLdouble s
Definition: glext.h:2935
idEntity::ApplyImpulse
virtual void ApplyImpulse(idEntity *ent, int id, const idVec3 &point, const idVec3 &impulse)
Definition: Entity.cpp:2887
idMatX::MultiplySub
void MultiplySub(idVecX &dst, const idVecX &vec) const
Definition: Matrix.h:2755
idAFConstraint_Hinge::GetAngle
float GetAngle(void) const
Definition: Physics_AF.cpp:1732
AFBodyPState_s::worldAxis
idMat3 worldAxis
Definition: Physics_AF.h:651
idTimer::Milliseconds
double Milliseconds(void) const
Definition: Timer.h:191
right
GLdouble right
Definition: qgl.h:273
idAFConstraint::boxIndex
int boxIndex[6]
Definition: Physics_AF.h:129
idVec3::Cross
idVec3 Cross(const idVec3 &a) const
Definition: Vector.h:619
idAFTree::sortedBodies
idList< idAFBody * > sortedBodies
Definition: Physics_AF.h:788
idAFConstraint_BallAndSocketJointFriction::Setup
void Setup(idAFConstraint_BallAndSocketJoint *cc)
Definition: Physics_AF.cpp:835
idPhysics_AF::DisableImpact
void DisableImpact(void)
Definition: Physics_AF.cpp:6046
idCollisionModelManager::DrawModel
virtual void DrawModel(cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis, const idVec3 &viewOrigin, const float radius)=0
idMat3::Identity
void Identity(void)
Definition: Matrix.h:591
idAFConstraint_PyramidLimit::Setup
void Setup(idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor, const idVec3 &pyramidAxis, const idVec3 &baseAxis, const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis)
Definition: Physics_AF.cpp:3612
idVec3::x
float x
Definition: Vector.h:318
idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction
idAFConstraint_BallAndSocketJointFriction(void)
Definition: Physics_AF.cpp:821
LIMIT_LCP_EPSILON
const float LIMIT_LCP_EPSILON
Definition: Physics_AF.cpp:41
idAFConstraint_ConeLimit::SetEpsilon
void SetEpsilon(const float e)
Definition: Physics_AF.h:546
x
GLenum GLint x
Definition: glext.h:2849
process.i
int i
Definition: process.py:33
idVec6::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Vector.h:1410
AFBodyPState_s
Definition: Physics_AF.h:649
offset
GLintptr offset
Definition: glext.h:3113
idVec3::OrthogonalBasis
void OrthogonalBasis(idVec3 &left, idVec3 &up) const
Definition: Vector.h:744
trace_s::c
contactInfo_t c
Definition: CollisionModel.h:78
idAFBody::contactMotorVelocity
float contactMotorVelocity
Definition: Physics_AF.h:725
idAFBody::AddForce
void AddForce(const idVec3 &point, const idVec3 &force)
Definition: Physics_AF.cpp:4377
idAFConstraint_UniversalJointFriction::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:1495
idAFConstraint_BallAndSocketJoint::GetFriction
float GetFriction(void) const
Definition: Physics_AF.cpp:611
num
GLuint GLuint num
Definition: glext.h:5390
idSaveGame::WriteVec3
void WriteVec3(const idVec3 &vec)
Definition: SaveGame.cpp:253
idPhysics_AF::GetAxis
const idMat3 & GetAxis(int id=0) const
Definition: Physics_AF.cpp:7605
af_highlightConstraint
idCVar af_highlightConstraint("af_highlightConstraint","", CVAR_GAME,"name of the constraint to highlight")
idAFConstraint_Hinge::steering
idAFConstraint_HingeSteering * steering
Definition: Physics_AF.h:348
idPhysics_AF::SetMaster
void SetMaster(idEntity *master, const bool orientated=true)
Definition: Physics_AF.cpp:7862
AFPState_s::lastTimeStep
float lastTimeStep
Definition: Physics_AF.h:802
idAFConstraint_BallAndSocketJoint
Definition: Physics_AF.h:181
idAFConstraint_UniversalJointFriction::Add
bool Add(idPhysics_AF *phys, float invTimeStep)
Definition: Physics_AF.cpp:1443
idAFConstraint_PyramidLimit::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:3669
idPhysics_AF::Activate
void Activate(void)
Definition: Physics_AF.cpp:6007
idPhysics_AF::contactFrictionDentStart
float contactFrictionDentStart
Definition: Physics_AF.h:1000
idStr::ColorForIndex
static idVec4 & ColorForIndex(int i)
Definition: Str.cpp:71
af_showTrees
idCVar af_showTrees("af_showTrees","0", CVAR_GAME|CVAR_BOOL,"show tree-like structures")
idRestoreGame::ReadVec6
void ReadVec6(idVec6 &vec)
Definition: SaveGame.cpp:1029
idAFConstraint_ConeLimit::coneAnchor
idVec3 coneAnchor
Definition: Physics_AF.h:555
CONSTRAINT_HINGESTEERING
Definition: Physics_AF.h:72
idMatX::InverseFast
idMatX InverseFast(void) const
Definition: Matrix.h:2618
idAFBody::bodyFlags_s::selfCollision
bool selfCollision
Definition: Physics_AF.h:759
idAFBody::response
float * response
Definition: Physics_AF.h:751
idAFConstraint_Slider::SetAxis
void SetAxis(const idVec3 &ax)
Definition: Physics_AF.cpp:2298
idAFConstraint_Line::idAFConstraint_Line
idAFConstraint_Line(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:2460
idAFConstraint_Fixed::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:395
idAFBody::I
idMatX I
Definition: Physics_AF.h:745
idPhysics_AF::contactConstraints
idList< idAFConstraint_Contact * > contactConstraints
Definition: Physics_AF.h:966
idAFBody::acceleration
idVecX acceleration
Definition: Physics_AF.h:750
idAFConstraint_HingeFriction::idAFConstraint_HingeFriction
idAFConstraint_HingeFriction(void)
Definition: Physics_AF.cpp:2032
af_highlightBody
idCVar af_highlightBody("af_highlightBody","", CVAR_GAME,"name of the body to highlight")
idPhysics_AF::GetAngularVelocity
const idVec3 & GetAngularVelocity(int id=0) const
Definition: Physics_AF.cpp:7659
idPhysics_AF::selfCollision
bool selfCollision
Definition: Physics_AF.h:1005
AF_FORCE_TOTAL_BITS
const int AF_FORCE_TOTAL_BITS
Definition: Physics_AF.cpp:7908
idPhysics_Base::clipMask
int clipMask
Definition: Physics_Base.h:146
trace_s::fraction
float fraction
Definition: CollisionModel.h:75
idAFConstraint_Fixed::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:375
trace_s::endpos
idVec3 endpos
Definition: CollisionModel.h:76
idAFConstraint_ConeLimit::SetAnchor
void SetAnchor(const idVec3 &coneAnchor)
Definition: Physics_AF.cpp:3385
idAFConstraint_HingeFriction::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:2123
idAFConstraint_Suspension::SetSuspension
void SetSuspension(const float up, const float down, const float k, const float d, const float f)
Definition: Physics_AF.cpp:3929
idQuat::x
float x
Definition: Quat.h:50
idMatX::MultiplyAdd
void MultiplyAdd(idVecX &dst, const idVecX &vec) const
Definition: Matrix.h:2733
idAFConstraint_PyramidLimit::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:3792
prepare.l
list l
Definition: prepare.py:17
idAFBody::SetFriction
void SetFriction(float linear, float angular, float contact)
Definition: Physics_AF.cpp:4248
idSaveGame::WriteBool
void WriteBool(const bool value)
Definition: SaveGame.cpp:222
idAFConstraint::constraintFlags_s::noCollision
bool noCollision
Definition: Physics_AF.h:141
colorRed
idVec4 colorRed
Definition: Lib.cpp:117
idPhysics_AF::AddFrameConstraint
void AddFrameConstraint(idAFConstraint *constraint)
Definition: Physics_AF.cpp:7074
idPhysics_AF
Definition: Physics_AF.h:812
CONSTRAINT_FRICTION
Definition: Physics_AF.h:79
idPhysics_AF::Evaluate
bool Evaluate(int timeStepMSec, int endTimeMSec)
Definition: Physics_AF.cpp:6217
idAFConstraint_BallAndSocketJoint::anchor2
idVec3 anchor2
Definition: Physics_AF.h:205
idPhysics_AF::UnlinkClip
void UnlinkClip(void)
Definition: Physics_AF.cpp:7801
idAFConstraint_Hinge::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:1891
idAFConstraint_BallAndSocketJointFriction::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:855
idAFBody::mass
float mass
Definition: Physics_AF.h:729
idPhysics_Base::self
idEntity * self
Definition: Physics_Base.h:145
idAFBody::auxForce
idVecX auxForce
Definition: Physics_AF.h:749
AFBodyPState_s::worldOrigin
idVec3 worldOrigin
Definition: Physics_AF.h:650
idAFConstraint_CylindricalJoint::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:1528
idMatX::TransposeMultiplySub
void TransposeMultiplySub(idVecX &dst, const idVecX &vec) const
Definition: Matrix.h:2821
idLCP::AllocSymmetric
static idLCP * AllocSymmetric(void)
Definition: Lcp.cpp:1614
idAFConstraint_UniversalJoint::shaft1
idVec3 shaft1
Definition: Physics_AF.h:263
idAFConstraint::fl
struct idAFConstraint::constraintFlags_s fl
idAFConstraint_Contact::Setup
void Setup(idAFBody *b1, idAFBody *b2, contactInfo_t &c)
Definition: Physics_AF.cpp:3000
idGameLocal::Warning
void void void Warning(const char *fmt,...) const id_attribute((format(printf
Definition: Game_local.cpp:735
idAFBody::invI
idMatX invI
Definition: Physics_AF.h:745
idQuat::y
float y
Definition: Quat.h:51
idAFConstraint_UniversalJoint::SetConeLimit
void SetConeLimit(const idVec3 &coneAxis, const float coneAngle)
Definition: Physics_AF.cpp:1051
idAFConstraint_ConeLimit::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:3412
idAFConstraint_BallAndSocketJointFriction::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:846
AFPState_s
Definition: Physics_AF.h:798
idPhysics_AF::AddFrameConstraints
void AddFrameConstraints(void)
Definition: Physics_AF.cpp:4946
NO_MOVE_TIME
const float NO_MOVE_TIME
Definition: Physics_AF.cpp:44
idPhysics_AF::DeleteBody
void DeleteBody(const char *bodyName)
Definition: Physics_AF.cpp:7246
idPhysics_AF::noImpact
bool noImpact
Definition: Physics_AF.h:1008
IMPULSE_THRESHOLD
const float IMPULSE_THRESHOLD
Definition: Physics_AF.cpp:49
idPhysics_AF::impulseThreshold
float impulseThreshold
Definition: Physics_AF.h:986
idAFConstraint_Fixed::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:444
idAFConstraint_Hinge::SetAnchor
void SetAnchor(const idVec3 &worldPosition)
Definition: Physics_AF.cpp:1612
idPhysics_AF::GetAbsBounds
const idBounds & GetAbsBounds(int id=-1) const
Definition: Physics_AF.cpp:6192
idPhysics_AF::saved
AFPState_t saved
Definition: Physics_AF.h:1014
idAFBody::numResponses
int numResponses
Definition: Physics_AF.h:753
idAFConstraint_BallAndSocketJointFriction::joint
idAFConstraint_BallAndSocketJoint * joint
Definition: Physics_AF.h:227
USERCMD_MSEC
const int USERCMD_MSEC
Definition: UsercmdGen.h:41
idAFConstraint_Spring::SetLimit
void SetLimit(const float minLength, const float maxLength)
Definition: Physics_AF.cpp:2733
v2
GLfloat GLfloat GLfloat v2
Definition: glext.h:3608
idAFConstraint_Contact
Definition: Physics_AF.h:497
idMatX::Multiply
idVecX Multiply(const idVecX &vec) const
Definition: Matrix.h:2655
dst
GLuint dst
Definition: glext.h:5285
idAFBody::clipModel
idClipModel * clipModel
Definition: Physics_AF.h:714
CONSTRAINT_UNIVERSALJOINT
Definition: Physics_AF.h:70
impactInfo_s::invMass
float invMass
Definition: Physics.h:69
idPhysics_AF::ApplyContactForces
void ApplyContactForces(void)
Definition: Physics_AF.cpp:5716
impactInfo_s
Definition: Physics.h:68
idAFConstraint_HingeSteering::Add
bool Add(idPhysics_AF *phys, float invTimeStep)
Definition: Physics_AF.cpp:2205
idRotation::GetAngle
float GetAngle(void) const
Definition: Rotation.h:154
idAFBody::SetFrictionDirection
void SetFrictionDirection(const idVec3 &dir)
Definition: Physics_AF.cpp:4321
colorYellow
idVec4 colorYellow
Definition: Lib.cpp:120
j2
GLint GLint GLint GLint j2
Definition: qgl.h:262
idMatX::GetNumColumns
int GetNumColumns(void) const
Definition: Matrix.h:1822
idAFConstraint_Suspension::trace
trace_t trace
Definition: Physics_AF.h:634
AFBodyPState_s::externalForce
idVec6 externalForce
Definition: Physics_AF.h:653
idAFBody::idAFBody
idAFBody(void)
Definition: Physics_AF.cpp:4137
idAFConstraint_Hinge::GetAnchor
idVec3 GetAnchor(void) const
Definition: Physics_AF.cpp:1633
idAFConstraint_Hinge::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:1984
idAFConstraint_ConeLimit::body1Axis
idVec3 body1Axis
Definition: Physics_AF.h:557
idAFBody::centerOfMass
idVec3 centerOfMass
Definition: Physics_AF.h:731
idRenderWorld::DebugCone
virtual void DebugCone(const idVec4 &color, const idVec3 &apex, const idVec3 &dir, float radius1, float radius2, const int lifetime=0)=0
idVec2
Definition: Vector.h:52
idAFConstraint_Hinge::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:1779
idPhysics_AF::SetTimeScaleRamp
void SetTimeScaleRamp(const float start, const float end)
Definition: Physics_AF.cpp:5843
idAFConstraint_UniversalJoint::GetAnchor
idVec3 GetAnchor(void) const
Definition: Physics_AF.cpp:981
af_showTimings
idCVar af_showTimings("af_showTimings","0", CVAR_GAME|CVAR_BOOL,"show articulated figure cpu usage")
idAFConstraint_Spring::GetCenter
virtual void GetCenter(idVec3 &center)
Definition: Physics_AF.cpp:2867
idRenderWorld::DebugLine
virtual void DebugLine(const idVec4 &color, const idVec3 &start, const idVec3 &end, const int lifetime=0, const bool depthTest=false)=0
impactInfo_s::velocity
idVec3 velocity
Definition: Physics.h:72
idBitMsgDelta::WriteLong
void WriteLong(int c)
Definition: BitMsg.h:554
idAFConstraint_ConeLimit::coneAxis
idVec3 coneAxis
Definition: Physics_AF.h:556
idPhysics_AF::contactFrictionDentEnd
float contactFrictionDentEnd
Definition: Physics_AF.h:1001
idClipModel::GetMassProperties
void GetMassProperties(const float density, float &mass, idVec3 &centerOfMass, idMat3 &inertiaTensor) const
Definition: Clip.cpp:475
idAFConstraint_BallAndSocketJointFriction::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:904
idAFConstraint_Slider::idAFConstraint_Slider
idAFConstraint_Slider(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:2273
idPhysics_AF::SetMass
void SetMass(float mass, int id=-1)
Definition: Physics_AF.cpp:6099
index
GLuint index
Definition: glext.h:3476
idPhysics_AF::WriteToSnapshot
void WriteToSnapshot(idBitMsgDelta &msg) const
Definition: Physics_AF.cpp:7917
idAFConstraint_UniversalJoint::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:1371
c
const GLubyte * c
Definition: glext.h:4677
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3454
idRestoreGame::ReadFloat
void ReadFloat(float &value)
Definition: SaveGame.cpp:967
idVec4
Definition: Vector.h:808
idAFConstraint_HingeSteering::SetSteerSpeed
void SetSteerSpeed(const float speed)
Definition: Physics_AF.h:381
idPhysics_AF::AddGravity
void AddGravity(void)
Definition: Physics_AF.cpp:5760
idPhysics_AF::EvaluateContacts
bool EvaluateContacts(void)
Definition: Physics_AF.cpp:5607
idAFTree::Factor
void Factor(void) const
Definition: Physics_AF.cpp:4481
idBitMsgDelta::WriteDeltaFloat
void WriteDeltaFloat(float oldValue, float newValue)
Definition: BitMsg.h:595
idGameLocal::world
idWorldspawn * world
Definition: Game_local.h:280
idAFConstraint_BallAndSocketJoint::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:731
AFBodyPState_s::spatialVelocity
idVec6 spatialVelocity
Definition: Physics_AF.h:652
idPhysics_AF::SetContents
void SetContents(int contents, int id=-1)
Definition: Physics_AF.cpp:6125
idAFConstraint_Plane::SetPlane
void SetPlane(const idVec3 &normal, const idVec3 &anchor)
Definition: Physics_AF.cpp:2537
idAFConstraint_Plane::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:2595
idAFConstraint_Spring::SetAnchor
void SetAnchor(const idVec3 &worldAnchor1, const idVec3 &worldAnchor2)
Definition: Physics_AF.cpp:2703
idPhysics_AF::GetNumClipModels
int GetNumClipModels(void) const
Definition: Physics_AF.cpp:6090
vec3_origin
idVec3 vec3_origin(0.0f, 0.0f, 0.0f)
AFPState_s::pushVelocity
idVec6 pushVelocity
Definition: Physics_AF.h:803
idAFConstraint_PyramidLimit::Add
bool Add(idPhysics_AF *phys, float invTimeStep)
Definition: Physics_AF.cpp:3677
idAFConstraint_Slider::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:2441
idMat3::IsDiagonal
bool IsDiagonal(const float epsilon=MATRIX_EPSILON) const
Definition: Matrix.h:612
idVecX::SubVec3
const idVec3 & SubVec3(int index) const
Definition: Vector.h:1895
idPhysics_AF::EnableClip
void EnableClip(void)
Definition: Physics_AF.cpp:7788
idAFBody::Init
void Init(void)
Definition: Physics_AF.cpp:4179
idPhysics_AF::SetOrigin
void SetOrigin(const idVec3 &newOrigin, int id=-1)
Definition: Physics_AF.cpp:7501
CONSTRAINT_SUSPENSION
Definition: Physics_AF.h:82
idPhysics_AF::GetJointFrictionScale
float GetJointFrictionScale(void) const
Definition: Physics_AF.cpp:5864
idAFConstraint_BallAndSocketJoint::GetAnchor
idVec3 GetAnchor(void) const
Definition: Physics_AF.cpp:521
vec3_zero
#define vec3_zero
Definition: Vector.h:390
idPhysics_AF::contactFrictionScale
float contactFrictionScale
Definition: Physics_AF.h:998
idPhysics_AF::GetContents
int GetContents(int id=-1) const
Definition: Physics_AF.cpp:6143
idMatX::SubVec6
const idVec6 & SubVec6(int row) const
Definition: Matrix.h:2911
idAFConstraint_CylindricalJoint::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:1537
end
GLuint GLuint end
Definition: glext.h:2845
idEntity::GetImpactInfo
virtual void GetImpactInfo(idEntity *ent, int id, const idVec3 &point, impactInfo_t *info)
Definition: Entity.cpp:2878
idSaveGame::WriteFloat
void WriteFloat(const float value)
Definition: SaveGame.cpp:213
idAFConstraint_Slider::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:2429
idPhysics_AF::Restore
void Restore(idRestoreGame *savefile)
Definition: Physics_AF.cpp:6785
idAFConstraint_Slider::GetCenter
virtual void GetCenter(idVec3 &center)
Definition: Physics_AF.cpp:2393
idAFConstraint_Slider::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:2410
idMath::Fabs
static float Fabs(float f)
Definition: Math.h:779
idAFBody::GetResponseForce
idVec6 & GetResponseForce(int index)
Definition: Physics_AF.h:704
INTSIGNBITNOTSET
#define INTSIGNBITNOTSET(i)
Definition: Math.h:72
idAFConstraint_Suspension::wheelModel
idClipModel * wheelModel
Definition: Physics_AF.h:632
idClipModel::Handle
cmHandle_t Handle(void) const
Definition: Clip.cpp:457
idAFBody
Definition: Physics_AF.h:657
idPhysics_AF::noMoveRotation
float noMoveRotation
Definition: Physics_AF.h:983
idPhysics_AF::suspendAcceleration
idVec2 suspendAcceleration
Definition: Physics_AF.h:980
af_showTotalMass
idCVar af_showTotalMass("af_showTotalMass","0", CVAR_GAME|CVAR_BOOL,"show the total mass of each articulated figure")
idAFConstraint_Fixed::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:333
NULL
#define NULL
Definition: Lib.h:88
idAFConstraint_HingeFriction::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:2066
idAFConstraint_Slider::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:2362
idAFBody::next
AFBodyPState_t * next
Definition: Physics_AF.h:738
MATX_ALLOCA
#define MATX_ALLOCA(n)
Definition: Matrix.h:1783
AF_VELOCITY_MANTISSA_BITS
const int AF_VELOCITY_MANTISSA_BITS
Definition: Physics_AF.cpp:7906
idPhysics_AF::CheckForCollisions
void CheckForCollisions(float timeStep)
Definition: Physics_AF.cpp:5538
contactInfo_t::entityNum
int entityNum
Definition: CollisionModel.h:69
idPhysics_AF::totalMass
float totalMass
Definition: Physics_AF.h:976
idPhysics_AF::PrimaryForces
void PrimaryForces(float timeStep)
Definition: Physics_AF.cpp:5032
idAFConstraint_Contact::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:3055
idVecX::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Vector.h:1910
idVec3::y
float y
Definition: Vector.h:319
idAFConstraint_BallAndSocketJoint::SetNoLimit
void SetNoLimit(void)
Definition: Physics_AF.cpp:533
af_maxAngularVelocity
idCVar af_maxAngularVelocity("af_maxAngularVelocity","1.57", CVAR_GAME|CVAR_FLOAT,"maximum angular velocity")
idAFConstraint::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:155
idCVar::GetInteger
int GetInteger(void) const
Definition: CVarSystem.h:143
idPhysics_AF::suspendVelocity
idVec2 suspendVelocity
Definition: Physics_AF.h:979
af_showActive
idCVar af_showActive("af_showActive","0", CVAR_GAME|CVAR_BOOL,"show tree-like structures of articulated figures not at rest")
idAFConstraint_UniversalJoint::pyramidLimit
idAFConstraint_PyramidLimit * pyramidLimit
Definition: Physics_AF.h:269
idVec6::Zero
void Zero(void)
Definition: Vector.h:1358
idAFConstraint_Hinge::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:1929
idAFConstraint::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:193
idSaveGame::WriteVec6
void WriteVec6(const idVec6 &vec)
Definition: SaveGame.cpp:271
idAFConstraint_UniversalJoint
Definition: Physics_AF.h:236
idAFConstraint_BallAndSocketJoint::SetLimitEpsilon
void SetLimitEpsilon(const float e)
Definition: Physics_AF.cpp:597
idAFTree::CalculateForces
void CalculateForces(float timeStep) const
Definition: Physics_AF.cpp:4711
idAFConstraint_ContactFriction::idAFConstraint_ContactFriction
idAFConstraint_ContactFriction(void)
Definition: Physics_AF.cpp:3159
idAFConstraint::constraintFlags_s::isPrimary
bool isPrimary
Definition: Physics_AF.h:142
idAFConstraint_ContactFriction::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:3326
idAFConstraint_Suspension::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:3951
idAFConstraint_Suspension::suspensionDown
float suspensionDown
Definition: Physics_AF.h:624
idAFConstraint::physics
idPhysics_AF * physics
Definition: Physics_AF.h:122
idBitMsgDelta::ReadLong
int ReadLong(void) const
Definition: BitMsg.h:621
idPhysics_AF::SetSuspendSpeed
void SetSuspendSpeed(const idVec2 &velocity, const idVec2 &acceleration)
Definition: Physics_AF.cpp:5812
idAFBody::s
idVecX s
Definition: Physics_AF.h:747
idAFConstraint_Hinge::axis1
idVec3 axis1
Definition: Physics_AF.h:343
idAFBody::SetClipModel
void SetClipModel(idClipModel *clipModel)
Definition: Physics_AF.cpp:4236
idAFConstraint_Suspension::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:4079
af_useLinearTime
idCVar af_useLinearTime("af_useLinearTime","1", CVAR_GAME|CVAR_BOOL,"use linear time algorithm for tree-like structures")
idVec4::ToString
const char * ToString(int precision=2) const
Definition: Vector.cpp:307
idAFConstraint_Plane::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:2655
idAFConstraint_Spring::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:2951
idClipModel::IsTraceModel
bool IsTraceModel(void) const
Definition: Clip.h:218
idAFBody::J
idMatX J
Definition: Physics_AF.h:746
idMatX::GetNumRows
int GetNumRows(void) const
Definition: Matrix.h:1821
idAFBody::bodyFlags_s::clipMaskSet
bool clipMaskSet
Definition: Physics_AF.h:758
idMatX::TransposeMultiplyAdd
void TransposeMultiplyAdd(idVecX &dst, const idVecX &vec) const
Definition: Matrix.h:2799
CENTER_OF_MASS_EPSILON
const float CENTER_OF_MASS_EPSILON
Definition: Physics_AF.cpp:43
idPhysics_AF::IsAtRest
bool IsAtRest(void) const
Definition: Physics_AF.cpp:7442
af_showBodyNames
idCVar af_showBodyNames("af_showBodyNames","0", CVAR_GAME|CVAR_BOOL,"show body names")
idAFConstraint_UniversalJoint::SetNoLimit
void SetNoLimit(void)
Definition: Physics_AF.cpp:1035
idAFConstraint_Spring::damping
float damping
Definition: Physics_AF.h:486
idPhysics_AF::forceTotalMass
float forceTotalMass
Definition: Physics_AF.h:977
idAFBody::Save
void Save(idSaveGame *saveFile)
Definition: Physics_AF.cpp:4411
idPhysics_AF::ClipContents
int ClipContents(const idClipModel *model) const
Definition: Physics_AF.cpp:7745
idAFConstraint_PyramidLimit::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:3778
trace_s::endAxis
idMat3 endAxis
Definition: CollisionModel.h:77
idAFConstraint_Suspension::localOrigin
idVec3 localOrigin
Definition: Physics_AF.h:621
idPhysics_AF::jointFrictionDent
float jointFrictionDent
Definition: Physics_AF.h:993
ERROR_REDUCTION
END_CLASS const float ERROR_REDUCTION
Definition: Physics_AF.cpp:37
idPhysics_AF_SavePState
void idPhysics_AF_SavePState(idSaveGame *saveFile, const AFPState_t &state)
Definition: Physics_AF.cpp:6687
ERROR_REDUCTION_MAX
const float ERROR_REDUCTION_MAX
Definition: Physics_AF.cpp:38
idPhysics_AF::SetChanged
void SetChanged(void)
Definition: Physics_AF.h:881
idAFConstraint_UniversalJointFriction::joint
idAFConstraint_UniversalJoint * joint
Definition: Physics_AF.h:288
idAFBody::angularFriction
float angularFriction
Definition: Physics_AF.h:719
idPhysics_AF::lcp
idLCP * lcp
Definition: Physics_AF.h:1017
idAFConstraint_Fixed::InitOffset
void InitOffset(void)
Definition: Physics_AF.cpp:287
idList::Find
type * Find(type const &obj) const
Definition: List.h:782
idAFConstraint_Spring::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:2934
idCQuat::z
float z
Definition: Quat.h:310
idAFConstraint_Suspension::idAFConstraint_Suspension
idAFConstraint_Suspension(void)
Definition: Physics_AF.cpp:3887
idAFConstraint_Fixed::relAxis
idMat3 relAxis
Definition: Physics_AF.h:171
idAFConstraint_HingeSteering::steerSpeed
float steerSpeed
Definition: Physics_AF.h:393
idPhysics_AF::GetLinearVelocity
const idVec3 & GetLinearVelocity(int id=0) const
Definition: Physics_AF.cpp:7645
idBitMsgDelta::ReadDeltaFloat
float ReadDeltaFloat(float oldValue) const
Definition: BitMsg.h:664
idBitMsgDelta::WriteByte
void WriteByte(int c)
Definition: BitMsg.h:542
idPhysics_AF::GetPushedLinearVelocity
const idVec3 & GetPushedLinearVelocity(const int id=0) const
Definition: Physics_AF.cpp:7842
idClipModel::GetId
int GetId(void) const
Definition: Clip.h:186
idPhysics_AF::jointFrictionDentStart
float jointFrictionDentStart
Definition: Physics_AF.h:994
idAFBody::GetBouncyness
float GetBouncyness(void) const
Definition: Physics_AF.h:687
idList::DeleteContents
void DeleteContents(bool clear)
Definition: List.h:207
idAFBody::bouncyness
float bouncyness
Definition: Physics_AF.h:721
idAFBody::~idAFBody
~idAFBody(void)
Definition: Physics_AF.cpp:4170
idMatX::Zero
void Zero(void)
Definition: Matrix.h:2291
idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint
~idAFConstraint_BallAndSocketJoint(void)
Definition: Physics_AF.cpp:482
idAFConstraint_UniversalJoint::axis2
idVec3 axis2
Definition: Physics_AF.h:266
idAFConstraint_Line::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:2487
idAFTree::Response
void Response(const idAFConstraint *constraint, int row, int auxiliaryIndex) const
Definition: Physics_AF.cpp:4603
idPhysics_AF::~idPhysics_AF
~idPhysics_AF(void)
Definition: Physics_AF.cpp:6657
idRenderWorld::DrawText
virtual void DrawText(const char *text, const idVec3 &origin, float scale, const idVec4 &color, const idMat3 &viewAxis, const int align=1, const int lifetime=0, bool depthTest=false)=0
idAFConstraint_Suspension::localAxis
idMat3 localAxis
Definition: Physics_AF.h:622
idAFConstraint_Hinge
Definition: Physics_AF.h:314
idBounds::FromTransformedBounds
void FromTransformedBounds(const idBounds &bounds, const idVec3 &origin, const idMat3 &axis)
Definition: Bounds.cpp:232
idAFConstraint_Suspension::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:4088
idAFConstraint_PyramidLimit::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:3861
idAFConstraint_BallAndSocketJoint::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:757
CONSTRAINT_HINGE
Definition: Physics_AF.h:71
idMath::InvSqrt
static float InvSqrt(float x)
Definition: Math.h:268
idAFConstraint_Contact::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:3046
idAFConstraint_Suspension::motorVelocity
float motorVelocity
Definition: Physics_AF.h:631
idPhysics_AF_RestorePState
void idPhysics_AF_RestorePState(idRestoreGame *saveFile, AFPState_t &state)
Definition: Physics_AF.cpp:6700
idAFConstraint_HingeSteering::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:2187
idAFConstraint_Hinge::idAFConstraint_Hinge
idAFConstraint_Hinge(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:1571
gameLocal
idGameLocal gameLocal
Definition: Game_local.cpp:64
idVec3::LengthSqr
float LengthSqr(void) const
Definition: Vector.h:635
idPhysics_AF::SetAxis
void SetAxis(const idMat3 &newAxis, int id=-1)
Definition: Physics_AF.cpp:7514
idAFConstraint_Fixed::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:384
idAFConstraint_Suspension::epsilon
float epsilon
Definition: Physics_AF.h:635
END_CLASS
#define END_CLASS
Definition: Class.h:54
idPhysics_AF::collisions
idList< AFCollision_t > collisions
Definition: Physics_AF.h:968
idPhysics_AF::SetLinearVelocity
void SetLinearVelocity(const idVec3 &newLinearVelocity, int id=0)
Definition: Physics_AF.cpp:7619
idAFBody::invMass
float invMass
Definition: Physics_AF.h:730
idAFBody::SetLinearVelocity
void SetLinearVelocity(const idVec3 &linear) const
Definition: Physics_AF.h:682
a
GLubyte GLubyte GLubyte a
Definition: glext.h:4662
idAFConstraint_BallAndSocketJoint::SetAnchor
void SetAnchor(const idVec3 &worldPosition)
Definition: Physics_AF.cpp:496
idAFConstraint_BallAndSocketJoint::fc
idAFConstraint_BallAndSocketJointFriction * fc
Definition: Physics_AF.h:209
idAFConstraint_Hinge::SetLimit
void SetLimit(const idVec3 &axis, const float angle, const idVec3 &body1Axis)
Definition: Physics_AF.cpp:1691
idPhysics_AF::AuxiliaryForces
void AuxiliaryForces(float timeStep)
Definition: Physics_AF.cpp:5045
DEG2RAD
#define DEG2RAD(a)
Definition: Math.h:56
idPhysics_AF::GetPushedAngularVelocity
const idVec3 & GetPushedAngularVelocity(const int id=0) const
Definition: Physics_AF.cpp:7851
idVecX::SubVec6
const idVec6 & SubVec6(int index) const
Definition: Vector.h:1905
idAFConstraint::body2
idAFBody * body2
Definition: Physics_AF.h:121
idPhysics_AF::SetJointFrictionDent
void SetJointFrictionDent(const float dent, const float start, const float end)
Definition: Physics_AF.cpp:5853
idPhysics_AF::ClearExternalForce
void ClearExternalForce(void)
Definition: Physics_AF.cpp:5742
idMath::BitsForInteger
static int BitsForInteger(int i)
Definition: Math.h:727
idList::AssureSizeAlloc
void AssureSizeAlloc(int newSize, new_t *allocator)
Definition: List.h:503
idPhysics_AF::contactFrictionDentScale
float contactFrictionDentScale
Definition: Physics_AF.h:1002
idMat3::OrthoNormalizeSelf
idMat3 & OrthoNormalizeSelf(void)
Definition: Matrix.h:668
idAFConstraint::~idAFConstraint
virtual ~idAFConstraint(void)
Definition: Physics_AF.cpp:110
idAFConstraint_Hinge::SetNoLimit
void SetNoLimit(void)
Definition: Physics_AF.cpp:1679
TH_PHYSICS
Definition: Entity.h:66
idAFConstraint_ConeLimit::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:3403
idAFConstraint_PyramidLimit::pyramidAnchor
idVec3 pyramidAnchor
Definition: Physics_AF.h:587
idAFConstraint_BallAndSocketJointFriction::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:896
af_showBodies
idCVar af_showBodies("af_showBodies","0", CVAR_GAME|CVAR_BOOL,"show bodies")
v1
GLfloat GLfloat v1
Definition: glext.h:3607
idAFBody::inertiaTensor
idMat3 inertiaTensor
Definition: Physics_AF.h:732
idSaveGame::WriteInt
void WriteInt(const int value)
Definition: SaveGame.cpp:168
idPhysics_AF::ApplyCollisions
bool ApplyCollisions(float timeStep)
Definition: Physics_AF.cpp:5447
idAFConstraint_Hinge::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:1951
af_jointFrictionScale
idCVar af_jointFrictionScale("af_jointFrictionScale","0", CVAR_GAME|CVAR_FLOAT,"scales the joint friction")
idCQuat::x
float x
Definition: Quat.h:308
idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction
idAFConstraint_UniversalJointFriction(void)
Definition: Physics_AF.cpp:1400
CONSTRAINT_FIXED
Definition: Physics_AF.h:68
idAFConstraint_Suspension::steerAngle
float steerAngle
Definition: Physics_AF.h:627
idAFConstraint::constraintFlags_s::frameConstraint
bool frameConstraint
Definition: Physics_AF.h:140
idMat3::Zero
void Zero(void)
Definition: Matrix.h:587
idAFConstraint_Spring::idAFConstraint_Spring
idAFConstraint_Spring(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:2686
idSaveGame::WriteMat3
void WriteMat3(const idMat3 &mat)
Definition: SaveGame.cpp:309
idAFConstraint_Hinge::SetAxis
void SetAxis(const idVec3 &axis)
Definition: Physics_AF.cpp:1645
idAFConstraint_PyramidLimit::body1Axis
idVec3 body1Axis
Definition: Physics_AF.h:589
idPhysics_AF::trees
idList< idAFTree * > trees
Definition: Physics_AF.h:960
idAFBody::SetAngularVelocity
void SetAngularVelocity(const idVec3 &angular) const
Definition: Physics_AF.h:683
idAFConstraint_HingeSteering::hinge
idAFConstraint_Hinge * hinge
Definition: Physics_AF.h:391
idAFConstraint_ConeLimit::epsilon
float epsilon
Definition: Physics_AF.h:561
idPhysics_AF::worldConstraintsLocked
bool worldConstraintsLocked
Definition: Physics_AF.h:1009
b
GLubyte GLubyte b
Definition: glext.h:4662
idAFConstraint_Plane::anchor2
idVec3 anchor2
Definition: Physics_AF.h:457
idAFConstraint_Slider::axis
idVec3 axis
Definition: Physics_AF.h:416
idPhysics_Base::contacts
idList< contactInfo_t > contacts
Definition: Physics_Base.h:149
idAFConstraint_Spring::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:2887
idPhysics_AF::GetBodyId
int GetBodyId(idAFBody *body) const
Definition: Physics_AF.cpp:7104
idRotation::GetVec
const idVec3 & GetVec(void) const
Definition: Rotation.h:150
idRestoreGame::ReadMat3
void ReadMat3(idMat3 &mat)
Definition: SaveGame.cpp:1064
idGameLocal::entities
idEntity * entities[MAX_GENTITIES]
Definition: Game_local.h:275
idQuat
Definition: Quat.h:48
idPhysics_AF::ClipTranslation
void ClipTranslation(trace_t &results, const idVec3 &translation, const idClipModel *model) const
Definition: Physics_AF.cpp:7673
idAFConstraint_Hinge::SetLimitEpsilon
void SetLimitEpsilon(const float e)
Definition: Physics_AF.cpp:1709
idTimer::Stop
void Stop(void)
Definition: Timer.h:155
idPhysics_AF::BuildTrees
void BuildTrees(void)
Definition: Physics_AF.cpp:6878
af_skipSelfCollision
idCVar af_skipSelfCollision("af_skipSelfCollision","0", CVAR_GAME|CVAR_BOOL,"skip self collision detection")
idPhysics_AF::Evolve
void Evolve(float timeStep)
Definition: Physics_AF.cpp:5332
idPhysics_AF::forcePushable
bool forcePushable
Definition: Physics_AF.h:1010
idCVar::GetString
const char * GetString(void) const
Definition: CVarSystem.h:141
idAFConstraint_PyramidLimit::cosHalfAngle
float cosHalfAngle[2]
Definition: Physics_AF.h:592
idQuat::z
float z
Definition: Quat.h:52
SUSPEND_ANGULAR_ACCELERATION
const float SUSPEND_ANGULAR_ACCELERATION
Definition: Physics_AF.cpp:53
idPhysics_AF::ClipRotation
void ClipRotation(trace_t &results, const idRotation &rotation, const idClipModel *model) const
Definition: Physics_AF.cpp:7708
MAX_TRACEMODEL_EDGES
#define MAX_TRACEMODEL_EDGES
Definition: TraceModel.h:64
impactInfo_s::position
idVec3 position
Definition: Physics.h:71
colorCyan
idVec4 colorCyan
Definition: Lib.cpp:122
idAFConstraint_HingeFriction::Add
bool Add(idPhysics_AF *phys, float invTimeStep)
Definition: Physics_AF.cpp:2075
idAFConstraint_UniversalJoint::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:1349
idAFBody::parent
idAFBody * parent
Definition: Physics_AF.h:712
idAFConstraint_PyramidLimit::SetEpsilon
void SetEpsilon(const float e)
Definition: Physics_AF.h:578
idAFBody::name
idStr name
Definition: Physics_AF.h:711
idPhysics_Base::AddContactEntitiesForContacts
void AddContactEntitiesForContacts(void)
Definition: Physics_Base.cpp:733
idAFConstraint_BallAndSocketJoint::pyramidLimit
idAFConstraint_PyramidLimit * pyramidLimit
Definition: Physics_AF.h:208
idAFConstraint_BallAndSocketJoint::anchor1
idVec3 anchor1
Definition: Physics_AF.h:204
idList::Append
int Append(const type &obj)
Definition: List.h:646
r
GLdouble GLdouble GLdouble r
Definition: glext.h:2951
idPhysics_AF::SetSuspendTolerance
void SetSuspendTolerance(const float noMoveTime, const float translationTolerance, const float rotationTolerance)
Definition: Physics_AF.cpp:5832
idVecX::Length
float Length(void) const
Definition: Vector.h:1837
idAFBody::inverseInertiaTensor
idMat3 inverseInertiaTensor
Definition: Physics_AF.h:733
idAFConstraint_Fixed::offset
idVec3 offset
Definition: Physics_AF.h:170
idAFConstraint_ConeLimit::Setup
void Setup(idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis)
Definition: Physics_AF.cpp:3367
idPhysics_AF::ReadFromSnapshot
void ReadFromSnapshot(const idBitMsgDelta &msg)
Definition: Physics_AF.cpp:7964
idPhysics_AF::GetTime
int GetTime(void) const
Definition: Physics_AF.cpp:6424
idAFConstraint_HingeSteering::SetSteerAngle
void SetSteerAngle(const float degrees)
Definition: Physics_AF.h:380
idMat3
Definition: Matrix.h:333
idPhysics_AF::UpdateClipModels
void UpdateClipModels(void)
Definition: Physics_AF.cpp:5797
idPhysics_AF::AddConstraint
void AddConstraint(idAFConstraint *constraint)
Definition: Physics_AF.cpp:7042
idMatX::SetSize
void SetSize(int rows, int columns)
Definition: Matrix.h:2247
idAFConstraint_Slider::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:2317
idAFConstraint_UniversalJoint::anchor1
idVec3 anchor1
Definition: Physics_AF.h:261
idPhysics_AF::RestoreState
void RestoreState(void)
Definition: Physics_AF.cpp:7484
idAFConstraint_BallAndSocketJoint::SetConeLimit
void SetConeLimit(const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis)
Definition: Physics_AF.cpp:549
idPhysics_AF::noMoveTranslation
float noMoveTranslation
Definition: Physics_AF.h:982
idAFConstraint_Plane::anchor1
idVec3 anchor1
Definition: Physics_AF.h:456
row
GLenum GLenum GLvoid * row
Definition: glext.h:2866
idAFBody::GetWorldOrigin
const idVec3 & GetWorldOrigin(void) const
Definition: Physics_AF.h:669
idAFConstraint_Suspension::motorEnabled
bool motorEnabled
Definition: Physics_AF.h:629
id
GLuint id
Definition: glext.h:3103
idRotation::SetOrigin
void SetOrigin(const idVec3 &rotationOrigin)
Definition: Rotation.h:115
idCVar::GetBool
bool GetBool(void) const
Definition: CVarSystem.h:142
INTSIGNBITSET
#define INTSIGNBITSET(i)
Definition: Math.h:71
idAFConstraint_PyramidLimit::epsilon
float epsilon
Definition: Physics_AF.h:593
idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit
idAFConstraint_PyramidLimit(void)
Definition: Physics_AF.cpp:3599
CONTACT_LCP_EPSILON
const float CONTACT_LCP_EPSILON
Definition: Physics_AF.cpp:42
idAFConstraint_ConeLimit::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:3560
idal.f
tuple f
Definition: idal.py:89
idAFConstraint_Plane::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:2667
idAFConstraint_BallAndSocketJointFriction::Add
bool Add(idPhysics_AF *phys, float invTimeStep)
Definition: Physics_AF.cpp:864
idCQuat::ToMat3
idMat3 ToMat3(void) const
Definition: Quat.cpp:232
idAFConstraint_PyramidLimit::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:3767
idAFConstraint_HingeSteering::Setup
void Setup(idAFConstraint_Hinge *cc)
Definition: Physics_AF.cpp:2176
idPhysics_AF::frameConstraints
idList< idAFConstraint * > frameConstraints
Definition: Physics_AF.h:965
idPlayer::viewAngles
idAngles viewAngles
Definition: Player.h:258
idRotation::ToMat3
const idMat3 & ToMat3(void) const
Definition: Rotation.cpp:60
idEntity::AddForce
virtual void AddForce(idEntity *ent, int id, const idVec3 &point, const idVec3 &force)
Definition: Entity.cpp:2896
impactInfo_s::invInertiaTensor
idMat3 invInertiaTensor
Definition: Physics.h:70
idList::Num
int Num(void) const
Definition: List.h:265
idList::RemoveIndex
bool RemoveIndex(int index)
Definition: List.h:849
idAFConstraint_BallAndSocketJoint::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:663
idAFConstraint_ConeLimit::idAFConstraint_ConeLimit
idAFConstraint_ConeLimit(void)
Definition: Physics_AF.cpp:3349
idAFBody::GetContactFriction
float GetContactFriction(void) const
Definition: Physics_AF.h:685
idAFConstraint_Fixed::GetCenter
virtual void GetCenter(idVec3 &center)
Definition: Physics_AF.cpp:407
idAngles::ToMat3
idMat3 ToMat3(void) const
Definition: Angles.cpp:199
idAFConstraint::SetBody1
virtual void SetBody1(idAFBody *body)
Definition: Physics_AF.cpp:118
af_timeScale
idCVar af_timeScale("af_timeScale","1", CVAR_GAME|CVAR_FLOAT,"scales the time")
idPhysics_AF::PutToRest
void PutToRest(void)
Definition: Physics_AF.cpp:6028
idAFConstraint_HingeSteering::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:2154
idAFConstraint_UniversalJoint::friction
float friction
Definition: Physics_AF.h:267
idPhysics_AF::timeScale
float timeScale
Definition: Physics_AF.h:988
idAFConstraint_Spring::kstretch
float kstretch
Definition: Physics_AF.h:484
idVecX::ChangeSize
void ChangeSize(int size, bool makeZero=false)
Definition: Vector.h:1720
idPhysics_AF::contactBodies
idList< int > contactBodies
Definition: Physics_AF.h:967
idAFBody::primaryConstraint
idAFConstraint * primaryConstraint
Definition: Physics_AF.h:715
idAFConstraint_Plane::planeNormal
idVec3 planeNormal
Definition: Physics_AF.h:458
idAFBody::atRestOrigin
idVec3 atRestOrigin
Definition: Physics_AF.h:740
idAFConstraint_Hinge::GetCenter
virtual void GetCenter(idVec3 &center)
Definition: Physics_AF.cpp:1920
name
const GLcharARB * name
Definition: glext.h:3629
idPhysics_AF::UpdateTime
void UpdateTime(int endTimeMSec)
Definition: Physics_AF.cpp:6416
RAD2DEG
#define RAD2DEG(a)
Definition: Math.h:57
size
GLsizeiptr size
Definition: glext.h:3112
SUSPEND_LINEAR_ACCELERATION
const float SUSPEND_LINEAR_ACCELERATION
Definition: Physics_AF.cpp:52
idClip::Rotation
bool Rotation(trace_t &results, const idVec3 &start, const idRotation &rotation, const idClipModel *mdl, const idMat3 &trmAxis, int contentMask, const idEntity *passEntity)
Definition: Clip.cpp:1130
idAFConstraint_Slider::relAxis
idMat3 relAxis
Definition: Physics_AF.h:418
idAFConstraint::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:202
idAFConstraint_Contact::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:3121
idMatX::SetData
void SetData(int rows, int columns, float *data)
Definition: Matrix.h:2278
idPhysics_AF::SwapStates
void SwapStates(void)
Definition: Physics_AF.cpp:5776
idPhysics_AF::Translate
void Translate(const idVec3 &translation, int id=-1)
Definition: Physics_AF.cpp:7534
idPhysics_AF::DeleteConstraint
void DeleteConstraint(const char *constraintName)
Definition: Physics_AF.cpp:7304
idClip::Contents
int Contents(const idVec3 &start, const idClipModel *mdl, const idMat3 &trmAxis, int contentMask, const idEntity *passEntity)
Definition: Clip.cpp:1429
idClip::RotationModel
void RotationModel(trace_t &results, const idVec3 &start, const idRotation &rotation, const idClipModel *mdl, const idMat3 &trmAxis, int contentMask, cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis)
Definition: Clip.cpp:1506
idClip::ContentsModel
int ContentsModel(const idVec3 &start, const idClipModel *mdl, const idMat3 &trmAxis, int contentMask, cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis)
Definition: Clip.cpp:1532
CLASS_DECLARATION
#define CLASS_DECLARATION(nameofsuperclass, nameofclass)
Definition: Class.h:110
idStr
Definition: Str.h:116
idBitMsgDelta::ReadByte
int ReadByte(void) const
Definition: BitMsg.h:609
idAFBody::GetAngularVelocity
const idVec3 & GetAngularVelocity(void) const
Definition: Physics_AF.h:672
idAFBody::current
AFBodyPState_t * current
Definition: Physics_AF.h:737
length
GLsizei const GLcharARB const GLint * length
Definition: glext.h:3599
idSaveGame::WriteVec2
void WriteVec2(const idVec2 &vec)
Definition: SaveGame.cpp:244
idPhysics_AF::GetMasterBody
idAFBody * GetMasterBody(void) const
Definition: Physics_AF.h:840
idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint
idAFConstraint_CylindricalJoint(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:1510
idAFConstraint_HingeSteering::steerAngle
float steerAngle
Definition: Physics_AF.h:392
SUSPEND_ANGULAR_VELOCITY
const float SUSPEND_ANGULAR_VELOCITY
Definition: Physics_AF.cpp:51
af_maxLinearVelocity
idCVar af_maxLinearVelocity("af_maxLinearVelocity","128", CVAR_GAME|CVAR_FLOAT,"maximum linear velocity")
idPhysics_AF::jointFrictionDentScale
float jointFrictionDentScale
Definition: Physics_AF.h:996
idAFConstraint_Spring::SetSpring
void SetSpring(const float stretch, const float compress, const float damping, const float restLength)
Definition: Physics_AF.cpp:2720
idPhysics_AF::SetSuspendTime
void SetSuspendTime(const float minTime, const float maxTime)
Definition: Physics_AF.cpp:5822
idPhysics_AF::LinkClip
void LinkClip(void)
Definition: Physics_AF.cpp:7814
idRestoreGame::ReadVec3
void ReadVec3(idVec3 &vec)
Definition: SaveGame.cpp:1011
idAFConstraint_Hinge::GetAxis
void GetAxis(idVec3 &a1, idVec3 &a2) const
Definition: Physics_AF.h:322
idRenderWorld::DebugCircle
virtual void DebugCircle(const idVec4 &color, const idVec3 &origin, const idVec3 &dir, const float radius, const int numSteps, const int lifetime=0, const bool depthTest=false)=0
idPhysics_AF::DisableClip
void DisableClip(void)
Definition: Physics_AF.cpp:7775
idAFConstraint_Plane::idAFConstraint_Plane
idAFConstraint_Plane(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:2521
idAFConstraint_Hinge::~idAFConstraint_Hinge
~idAFConstraint_Hinge(void)
Definition: Physics_AF.cpp:1595
idVecX::SetData
void SetData(int length, float *data)
Definition: Vector.h:1756
idAFConstraint_Spring::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:2836
idList::NumAllocated
int NumAllocated(void) const
Definition: List.h:277
idPhysics_AF::SetClipModel
void SetClipModel(idClipModel *model, float density, int id=0, bool freeOld=true)
Definition: Physics_AF.cpp:6070
idAFConstraint_UniversalJoint::GetShafts
void GetShafts(idVec3 &cardanShaft1, idVec3 &cardanShaft2)
Definition: Physics_AF.h:244
idStr::c_str
const char * c_str(void) const
Definition: Str.h:487
idAFConstraint_Hinge::anchor1
idVec3 anchor1
Definition: Physics_AF.h:341
idPhysics_Base::ClearContacts
void ClearContacts(void)
Definition: Physics_Base.cpp:532
idAFConstraint_Hinge::GetForce
virtual void GetForce(idAFBody *body, idVec6 &force)
Definition: Physics_AF.cpp:1881
idAFConstraint_Spring::kcompress
float kcompress
Definition: Physics_AF.h:485
idAFConstraint_ConeLimit::cosAngle
float cosAngle
Definition: Physics_AF.h:558
idAFConstraint_UniversalJoint::anchor2
idVec3 anchor2
Definition: Physics_AF.h:262
AFPState_s::noMoveTime
float noMoveTime
Definition: Physics_AF.h:800
idMat3::ToRotation
idRotation ToRotation(void) const
Definition: Matrix.cpp:246
AFPState_s::activateTime
float activateTime
Definition: Physics_AF.h:801
maxLength
GLsizei maxLength
Definition: glext.h:3627
idPhysics_AF::AddPushVelocity
void AddPushVelocity(const idVec6 &pushVelocity)
Definition: Physics_AF.cpp:6055
idAFConstraint_HingeSteering::epsilon
float epsilon
Definition: Physics_AF.h:394
CONSTRAINT_PYRAMIDLIMIT
Definition: Physics_AF.h:81
idAFConstraint_ConeLimit::cosHalfAngle
float cosHalfAngle
Definition: Physics_AF.h:560
idClip::TranslationModel
void TranslationModel(trace_t &results, const idVec3 &start, const idVec3 &end, const idClipModel *mdl, const idMat3 &trmAxis, int contentMask, cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis)
Definition: Clip.cpp:1493
idPhysics_AF::IsPushable
bool IsPushable(void) const
Definition: Physics_AF.cpp:7460
idAFConstraint_Hinge::initialAxis
idMat3 initialAxis
Definition: Physics_AF.h:345
traceModelEdge_t::v
int v[2]
Definition: TraceModel.h:71
vec6_infinity
idVec6 vec6_infinity(idMath::INFINITY, idMath::INFINITY, idMath::INFINITY, idMath::INFINITY, idMath::INFINITY, idMath::INFINITY)
idPhysics_Base::gravityVector
idVec3 gravityVector
Definition: Physics_Base.h:147
idAFBody::fl
struct idAFBody::bodyFlags_s fl
idPhysics_AF::primaryConstraints
idList< idAFConstraint * > primaryConstraints
Definition: Physics_AF.h:963
idList::FindIndex
int FindIndex(const type &obj) const
Definition: List.h:761
idAFConstraint_CylindricalJoint::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:1555
mat3_zero
idMat3 mat3_zero(idVec3(0, 0, 0), idVec3(0, 0, 0), idVec3(0, 0, 0))
idPhysics_AF::timeScaleRampEnd
float timeScaleRampEnd
Definition: Physics_AF.h:990
idCQuat::y
float y
Definition: Quat.h:309
idPhysics_AF::SaveState
void SaveState(void)
Definition: Physics_AF.cpp:7469
idAFBody::bodyFlags_s::isZero
bool isZero
Definition: Physics_AF.h:763
idBitMsgDelta::ReadFloat
float ReadFloat(void) const
Definition: BitMsg.h:625
idAFConstraint_Plane::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:2627
idAFConstraint_HingeFriction::Setup
void Setup(idAFConstraint_Hinge *cc)
Definition: Physics_AF.cpp:2046
idAFConstraint_PyramidLimit::cosAngle
float cosAngle[2]
Definition: Physics_AF.h:590
idAFTree::SortBodies
void SortBodies(void)
Definition: Physics_AF.cpp:4813
idAFConstraint_BallAndSocketJoint::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:796
idAFConstraint_UniversalJoint::SetPyramidLimit
void SetPyramidLimit(const idVec3 &pyramidAxis, const idVec3 &baseAxis, const float angle1, const float angle2)
Definition: Physics_AF.cpp:1073
idVec2::Set
void Set(const float x, const float y)
Definition: Vector.h:114
idPhysics_AF::contactFrictionDent
float contactFrictionDent
Definition: Physics_AF.h:999
idAFConstraint_ContactFriction::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:3184
idAFConstraint_Fixed::SetBody1
virtual void SetBody1(idAFBody *body)
Definition: Physics_AF.cpp:303
idPhysics_AF::SetAngularVelocity
void SetAngularVelocity(const idVec3 &newAngularVelocity, int id=0)
Definition: Physics_AF.cpp:7632
idAFConstraint_HingeSteering
Definition: Physics_AF.h:375
idClipModel::Disable
void Disable(void)
Definition: Clip.h:154
idAFConstraint::Restore
virtual void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:251
idAFConstraint_Suspension::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:4104
idAFConstraint::GetName
const idStr & GetName(void) const
Definition: Physics_AF.h:102
SkewSymmetric
ID_INLINE idMat3 SkewSymmetric(idVec3 const &src)
Definition: Matrix.h:741
idPhysics_AF::contactFriction
float contactFriction
Definition: Physics_AF.h:974
idPhysics_Base::ActivateContactEntities
void ActivateContactEntities(void)
Definition: Physics_Base.cpp:750
j
GLint j
Definition: qgl.h:264
idClipModel::GetTraceModel
const idTraceModel * GetTraceModel(void) const
Definition: Clip.h:234
idPhysics_AF::linearFriction
float linearFriction
Definition: Physics_AF.h:972
idAFBody::GetPointVelocity
idVec3 GetPointVelocity(const idVec3 &point) const
Definition: Physics_AF.cpp:4367
idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint
idAFConstraint_UniversalJoint(const idStr &name, idAFBody *body1, idAFBody *body2)
Definition: Physics_AF.cpp:919
idVecX::Clamp
void Clamp(float min, float max)
Definition: Vector.h:1818
idAFConstraint_Contact::GetCenter
virtual void GetCenter(idVec3 &center)
Definition: Physics_AF.cpp:3130
af_useSymmetry
idCVar af_useSymmetry("af_useSymmetry","1", CVAR_GAME|CVAR_BOOL,"use constraint matrix symmetry")
idAFConstraint_CylindricalJoint::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:1519
if
if(!ValidDisplayID(prefInfo.prefDisplayID)) prefInfo.prefDisplayID
AF_FORCE_MANTISSA_BITS
const int AF_FORCE_MANTISSA_BITS
Definition: Physics_AF.cpp:7910
gameRenderWorld
idRenderWorld * gameRenderWorld
Definition: Game_local.cpp:55
va
char * va(const char *fmt,...)
Definition: Str.cpp:1568
idPhysics_AF::Rotate
void Rotate(const idRotation &rotation, int id=-1)
Definition: Physics_AF.cpp:7562
idAFBody::contactMotorForce
float contactMotorForce
Definition: Physics_AF.h:726
idClip::Translation
bool Translation(trace_t &results, const idVec3 &start, const idVec3 &end, const idClipModel *mdl, const idMat3 &trmAxis, int contentMask, const idEntity *passEntity)
Definition: Clip.cpp:1056
idAFConstraint_Line::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:2478
CONSTRAINT_CONTACT
Definition: Physics_AF.h:78
af_useImpulseFriction
idCVar af_useImpulseFriction("af_useImpulseFriction","0", CVAR_GAME|CVAR_BOOL,"use impulse based contact friction")
idPhysics_AF::RemoveFrameConstraints
void RemoveFrameConstraints(void)
Definition: Physics_AF.cpp:4960
idEntity::entityNumber
int entityNumber
Definition: Entity.h:111
p
GLfloat GLfloat p
Definition: glext.h:4674
idAFConstraint_ConeLimit::SetBody1Axis
void SetBody1Axis(const idVec3 &body1Axis)
Definition: Physics_AF.cpp:3394
idEntity::name
idStr name
Definition: Entity.h:121
idAFConstraint_Hinge::friction
float friction
Definition: Physics_AF.h:346
idAFConstraint_PyramidLimit::SetBody1Axis
void SetBody1Axis(const idVec3 &body1Axis)
Definition: Physics_AF.cpp:3651
idPhysics_Base
Definition: Physics_Base.h:42
idAFConstraint_Suspension::suspensionDamping
float suspensionDamping
Definition: Physics_AF.h:626
idPhysics_AF::jointFrictionScale
float jointFrictionScale
Definition: Physics_AF.h:992
idAFConstraint_UniversalJoint::GetCenter
virtual void GetCenter(idVec3 &center)
Definition: Physics_AF.cpp:1284
idAFConstraint_ContactFriction::Setup
void Setup(idAFConstraint_Contact *cc)
Definition: Physics_AF.cpp:3173
idMatX::ChangeSize
void ChangeSize(int rows, int columns, bool makeZero=false)
Definition: Matrix.cpp:2948
idAFConstraint_ContactFriction
Definition: Physics_AF.h:519
idAFConstraint_Suspension::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:4096
idAFConstraint_Spring::restLength
float restLength
Definition: Physics_AF.h:487
idMat3::Inverse
idMat3 Inverse(void) const
Definition: Matrix.h:699
idLCP::Solve
virtual bool Solve(const idMatX &A, idVecX &x, const idVecX &b, const idVecX &lo, const idVecX &hi, const int *boxIndex=NULL)=0
idPhysics_AF::jointFrictionDentEnd
float jointFrictionDentEnd
Definition: Physics_AF.h:995
AF_FORCE_MAX
const float AF_FORCE_MAX
Definition: Physics_AF.cpp:7907
AFPState_s::atRest
int atRest
Definition: Physics_AF.h:799
idAFConstraint_Spring::anchor1
idVec3 anchor1
Definition: Physics_AF.h:482
z
GLdouble GLdouble z
Definition: glext.h:3067
colorMagenta
idVec4 colorMagenta
Definition: Lib.cpp:121
idAFConstraint_ContactFriction::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:3334
idMatX::Negate
void Negate(void)
Definition: Matrix.h:2358
idMatX::ToFloatPtr
const float * ToFloatPtr(void) const
Definition: Matrix.h:2935
idAFConstraint_UniversalJoint::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:1125
idVec3::Zero
void Zero(void)
Definition: Vector.h:415
idAFConstraint_Suspension::suspensionKCompress
float suspensionKCompress
Definition: Physics_AF.h:625
idAFConstraint::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:164
trace_s
Definition: CollisionModel.h:74
idPhysics_AF::maxMoveTime
float maxMoveTime
Definition: Physics_AF.h:985
idAFConstraint_ContactFriction::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:3193
af_contactFrictionScale
idCVar af_contactFrictionScale("af_contactFrictionScale","0", CVAR_GAME|CVAR_FLOAT,"scales the contact friction")
idPhysics_AF::EvaluateConstraints
void EvaluateConstraints(float timeStep)
Definition: Physics_AF.cpp:4861
idAFConstraint_UniversalJoint::fc
idAFConstraint_UniversalJointFriction * fc
Definition: Physics_AF.h:270
CONSTRAINT_SLIDER
Definition: Physics_AF.h:73
idAFConstraint_Plane::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:2556
CONSTRAINT_BALLANDSOCKETJOINT
Definition: Physics_AF.h:69
idAFBody::responseIndex
int * responseIndex
Definition: Physics_AF.h:752
idRestoreGame::ReadInt
void ReadInt(int &value)
Definition: SaveGame.cpp:922
idPhysics_AF::SetupCollisionForBody
idEntity * SetupCollisionForBody(idAFBody *body) const
Definition: Physics_AF.cpp:5463
idVec3::NormalVectors
void NormalVectors(idVec3 &left, idVec3 &down) const
Definition: Vector.h:727
idMath::M_MS2SEC
static const float M_MS2SEC
Definition: Math.h:217
idPhysics_AF::EvaluateBodies
void EvaluateBodies(float timeStep)
Definition: Physics_AF.cpp:4899
idAFBody::GetLinearVelocity
const idVec3 & GetLinearVelocity(void) const
Definition: Physics_AF.h:671
idAFConstraint_UniversalJoint::shaft2
idVec3 shaft2
Definition: Physics_AF.h:264
collisionModelManager
idCollisionModelManager * collisionModelManager
Definition: CollisionModel_load.cpp:55
idPhysics_AF::CollisionImpulse
bool CollisionImpulse(float timeStep, idAFBody *body, trace_t &collision)
Definition: Physics_AF.cpp:5403
idAFBody::children
idList< idAFBody * > children
Definition: Physics_AF.h:713
idAFConstraint_Contact::idAFConstraint_Contact
idAFConstraint_Contact(void)
Definition: Physics_AF.cpp:2975
idAFConstraint_Contact::fc
idAFConstraint_ContactFriction * fc
Definition: Physics_AF.h:511
start
GLuint start
Definition: glext.h:2845
I
#define I(x, y, z)
Definition: md5.c:106
af_skipFriction
idCVar af_skipFriction("af_skipFriction","0", CVAR_GAME|CVAR_BOOL,"skip friction")
idPhysics_AF::IsClosedLoop
bool IsClosedLoop(const idAFBody *body1, const idAFBody *body2) const
Definition: Physics_AF.cpp:6863
idPhysics_AF::angularFriction
float angularFriction
Definition: Physics_AF.h:973
idPhysics_AF::GetRestStartTime
int GetRestStartTime(void) const
Definition: Physics_AF.cpp:7451
idAFConstraint_Spring::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:2856
idAFConstraint_PyramidLimit::Evaluate
virtual void Evaluate(float invTimeStep)
Definition: Physics_AF.cpp:3660
idClip::Motion
bool Motion(trace_t &results, const idVec3 &start, const idVec3 &end, const idRotation &rotation, const idClipModel *mdl, const idMat3 &trmAxis, int contentMask, const idEntity *passEntity)
Definition: Clip.cpp:1197
colorBlue
idVec4 colorBlue
Definition: Lib.cpp:119
idAFConstraint::GetForce
virtual void GetForce(idAFBody *body, idVec6 &force)
Definition: Physics_AF.cpp:172
idAFConstraint_UniversalJoint::SetLimitEpsilon
void SetLimitEpsilon(const float e)
Definition: Physics_AF.cpp:1097
idAFBody::GetContactMotorVelocity
float GetContactMotorVelocity(void) const
Definition: Physics_AF.h:698
idAFConstraint::idAFConstraint
idAFConstraint(void)
Definition: Physics_AF.cpp:78
MS2SEC
#define MS2SEC(t)
Definition: Math.h:60
idAFConstraint_UniversalJointFriction::Setup
void Setup(idAFConstraint_UniversalJoint *cc)
Definition: Physics_AF.cpp:1414
idAFConstraint_BallAndSocketJoint::friction
float friction
Definition: Physics_AF.h:206
AF_VELOCITY_TOTAL_BITS
const int AF_VELOCITY_TOTAL_BITS
Definition: Physics_AF.cpp:7904
idAFBody::Restore
void Restore(idRestoreGame *saveFile)
Definition: Physics_AF.cpp:4441
idPhysics_AF::GetOrigin
const idVec3 & GetOrigin(int id=0) const
Definition: Physics_AF.cpp:7591
CONSTRAINT_PLANE
Definition: Physics_AF.h:76
idAFConstraint_Suspension::friction
float friction
Definition: Physics_AF.h:628
idAFConstraint_PyramidLimit::pyramidBasis
idMat3 pyramidBasis
Definition: Physics_AF.h:588
idAFConstraint_ConeLimit
Definition: Physics_AF.h:538
idAFBody::GetName
const idStr & GetName(void) const
Definition: Physics_AF.h:668
idAFConstraint_UniversalJoint::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:1265
idAFConstraint_HingeSteering::ApplyFriction
virtual void ApplyFriction(float invTimeStep)
Definition: Physics_AF.cpp:2196
idMath::BitsForFloat
static int BitsForFloat(float f)
Definition: Math.h:723
idAFConstraint_Fixed::SetBody2
virtual void SetBody2(idAFBody *body)
Definition: Physics_AF.cpp:318
idAFConstraint_ConeLimit::DebugDraw
virtual void DebugDraw(void)
Definition: Physics_AF.cpp:3521
idAFConstraint_PyramidLimit::Save
virtual void Save(idSaveGame *saveFile) const
Definition: Physics_AF.cpp:3842
idTimer::Clear
void Clear(void)
Definition: Timer.h:172
idRotation::SetAngle
void SetAngle(const float rotationAngle)
Definition: Rotation.h:131
AF_VELOCITY_MAX
const float AF_VELOCITY_MAX
Definition: Physics_AF.cpp:7903
t
GLdouble GLdouble t
Definition: glext.h:2943
constraintType_t
constraintType_t
Definition: Physics_AF.h:66
idAFBody::bodyFlags_s::spatialInertiaSparse
bool spatialInertiaSparse
Definition: Physics_AF.h:760
idPhysics_AF::current
AFPState_t current
Definition: Physics_AF.h:1013
idList::Clear
void Clear(void)
Definition: List.h:184
idAFConstraint_Line::Rotate
virtual void Rotate(const idRotation &rotation)
Definition: Physics_AF.cpp:2496
idAFConstraint_BallAndSocketJoint::coneLimit
idAFConstraint_ConeLimit * coneLimit
Definition: Physics_AF.h:207
idAFConstraint_Contact::~idAFConstraint_Contact
~idAFConstraint_Contact(void)
Definition: Physics_AF.cpp:2989
LIMIT_ERROR_REDUCTION
const float LIMIT_ERROR_REDUCTION
Definition: Physics_AF.cpp:39
idAFTree::SetMaxSubTreeAuxiliaryIndex
void SetMaxSubTreeAuxiliaryIndex(void)
Definition: Physics_AF.cpp:4772
idAFConstraint_ConeLimit::Translate
virtual void Translate(const idVec3 &translation)
Definition: Physics_AF.cpp:3498
idPhysics_AF::GetBodyContactConstraints
int GetBodyContactConstraints(const int id, idAFConstraint_Contact *contacts[], int maxContacts) const
Definition: Physics_AF.cpp:7347
idAFConstraint_PyramidLimit
Definition: Physics_AF.h:569
This page is maintained by Wladimir van der Laan. Generated on Mon Nov 17 2014 12:23:18 for doom3-gpl by doxygen 1.8.6.