neo/game/physics/Physics_AF.cpp

   1 /*
   2 ===========================================================================
   3
   4 Doom 3 GPL Source Code
   5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
   6
   7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
   8
   9 Doom 3 Source Code is free software: you can redistribute it and/or modify
  10 it under the terms of the GNU General Public License as published by
  11 the Free Software Foundation, either version 3 of the License, or
  12 (at your option) any later version.
  13
  14 Doom 3 Source Code is distributed in the hope that it will be useful,
  15 but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 GNU General Public License for more details.
  18
  19 You should have received a copy of the GNU General Public License
  20 along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
  21
  22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
  23
  24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
  25
  26 ===========================================================================
  27 */
  28
  29 #include "../../idlib/precompiled.h"
  30 #pragma hdrstop
  31
  32 #include "../Game_local.h"
  33
  34 CLASS_DECLARATION( idPhysics_Base, idPhysics_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         } else {
3030                 c1[0] = 0.0f;
3031         }
3032
3033         e[0] = CONTACT_LCP_EPSILON;
3034         lo[0] = 0.0f;
3035         hi[0] = idMath::INFINITY;
3036         boxConstraint = NULL;
3037         boxIndex[0] = -1;
3038 }
3039
3040 /*
3041 ================
3042 idAFConstraint_Contact::Evaluate
3043 ================
3044 */
3045 void idAFConstraint_Contact::Evaluate( float invTimeStep ) {
3046         // do nothing
3047 }
3048
3049 /*
3050 ================
3051 idAFConstraint_Contact::ApplyFriction
3052 ================
3053 */
3054 void idAFConstraint_Contact::ApplyFriction( float invTimeStep ) {
3055         idVec3 r, velocity, normal, dir1, dir2;
3056         float friction, magnitude, forceNumerator, forceDenominator;
3057         idVecX impulse, dv;
3058
3059         friction = body1->GetContactFriction();
3060         if ( body2 && body2->GetContactFriction() < friction ) {
3061                 friction = body2->GetContactFriction();
3062         }
3063
3064         friction *= physics->GetContactFrictionScale();
3065
3066         if ( friction <= 0.0f ) {
3067                 return;
3068         }
3069
3070         // seperate friction per contact is silly but it's fast and often looks close enough
3071         if ( af_useImpulseFriction.GetBool() ) {
3072
3073                 impulse.SetData( 6, VECX_ALLOCA( 6 ) );
3074                 dv.SetData( 6, VECX_ALLOCA( 6 ) );
3075
3076                 // calculate velocity in the contact plane
3077                 r = contact.point - body1->GetWorldOrigin();
3078                 velocity = body1->GetLinearVelocity() + body1->GetAngularVelocity().Cross( r );
3079                 velocity -= contact.normal * velocity * contact.normal;
3080
3081                 // get normalized direction of friction and magnitude of velocity
3082                 normal = -velocity;
3083                 magnitude = normal.Normalize();
3084
3085                 forceNumerator = friction * magnitude;
3086                 forceDenominator = body1->GetInverseMass() + ( ( body1->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal );
3087                 impulse.SubVec3(0) = (forceNumerator / forceDenominator) * normal;
3088                 impulse.SubVec3(1) = r.Cross( impulse.SubVec3(0) );
3089                 body1->InverseWorldSpatialInertiaMultiply( dv, impulse.ToFloatPtr() );
3090
3091                 // modify velocity with friction force
3092                 body1->SetLinearVelocity( body1->GetLinearVelocity() + dv.SubVec3(0) );
3093                 body1->SetAngularVelocity( body1->GetAngularVelocity() + dv.SubVec3(1) );
3094         }
3095         else {
3096
3097                 if ( !fc ) {
3098                         fc = new idAFConstraint_ContactFriction;
3099                 }
3100                 // call setup each frame because contact constraints are re-used for different bodies
3101                 fc->Setup( this );
3102                 fc->Add( physics, invTimeStep );
3103         }
3104 }
3105
3106 /*
3107 ================
3108 idAFConstraint_Contact::Translate
3109 ================
3110 */
3111 void idAFConstraint_Contact::Translate( const idVec3 &translation ) {
3112         assert( 0 );    // contact should never be translated
3113 }
3114
3115 /*
3116 ================
3117 idAFConstraint_Contact::Rotate
3118 ================
3119 */
3120 void idAFConstraint_Contact::Rotate( const idRotation &rotation ) {
3121         assert( 0 );    // contact should never be rotated
3122 }
3123
3124 /*
3125 ================
3126 idAFConstraint_Contact::GetCenter
3127 ================
3128 */
3129 void idAFConstraint_Contact::GetCenter( idVec3 &center ) {
3130         center = contact.point;
3131 }
3132
3133 /*
3134 ================
3135 idAFConstraint_Contact::DebugDraw
3136 ================
3137 */
3138 void idAFConstraint_Contact::DebugDraw( void ) {
3139         idVec3 x, y;
3140         contact.normal.NormalVectors( x, y );
3141         gameRenderWorld->DebugLine( colorWhite, contact.point, contact.point + 6.0f * contact.normal );
3142         gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * x, contact.point + 2.0f * x );
3143         gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * y, contact.point + 2.0f * y );
3144 }
3145
3146
3147 //===============================================================
3148 //
3149 //      idAFConstraint_ContactFriction
3150 //
3151 //===============================================================
3152
3153 /*
3154 ================
3155 idAFConstraint_ContactFriction::idAFConstraint_ContactFriction
3156 ================
3157 */
3158 idAFConstraint_ContactFriction::idAFConstraint_ContactFriction( void ) {
3159         type = CONSTRAINT_FRICTION;
3160         name = "contactFriction";
3161         InitSize( 2 );
3162         cc = NULL;
3163         fl.allowPrimary = false;
3164         fl.frameConstraint = true;
3165 }
3166
3167 /*
3168 ================
3169 idAFConstraint_ContactFriction::Setup
3170 ================
3171 */
3172 void idAFConstraint_ContactFriction::Setup( idAFConstraint_Contact *cc ) {
3173         this->cc = cc;
3174         body1 = cc->GetBody1();
3175         body2 = cc->GetBody2();
3176 }
3177
3178 /*
3179 ================
3180 idAFConstraint_ContactFriction::Evaluate
3181 ================
3182 */
3183 void idAFConstraint_ContactFriction::Evaluate( float invTimeStep ) {
3184         // do nothing
3185 }
3186
3187 /*
3188 ================
3189 idAFConstraint_ContactFriction::ApplyFriction
3190 ================
3191 */
3192 void idAFConstraint_ContactFriction::ApplyFriction( float invTimeStep ) {
3193         // do nothing
3194 }
3195
3196 /*
3197 ================
3198 idAFConstraint_ContactFriction::Add
3199 ================
3200 */
3201 bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
3202         idVec3 r, dir1, dir2;
3203         float friction;
3204         int newRow;
3205
3206         physics = phys;
3207
3208         friction = body1->GetContactFriction() * physics->GetContactFrictionScale();
3209
3210         // if the body only has friction in one direction
3211         if ( body1->GetFrictionDirection( dir1 ) ) {
3212                 // project the friction direction into the contact plane
3213                 dir1 -= dir1 * cc->GetContact().normal * dir1;
3214                 dir1.Normalize();
3215
3216                 r = cc->GetContact().point - body1->GetWorldOrigin();
3217
3218                 J1.SetSize( 1, 6 );
3219                 J1.SubVec6(0).SubVec3(0) = dir1;
3220                 J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 );
3221                 c1.SetSize( 1 );
3222                 c1[0] = 0.0f;
3223
3224                 if ( body2 ) {
3225                         r = cc->GetContact().point - body2->GetWorldOrigin();
3226
3227                         J2.SetSize( 1, 6 );
3228                         J2.SubVec6(0).SubVec3(0) = -dir1;
3229                         J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 );
3230                         c2.SetSize( 1 );
3231                         c2[0] = 0.0f;
3232                 }
3233
3234                 lo[0] = -friction;
3235                 hi[0] = friction;
3236                 boxConstraint = cc;
3237                 boxIndex[0] = 0;
3238         }
3239         else {
3240                 // get two friction directions orthogonal to contact normal
3241                 cc->GetContact().normal.NormalVectors( dir1, dir2 );
3242
3243                 r = cc->GetContact().point - body1->GetWorldOrigin();
3244
3245                 J1.SetSize( 2, 6 );
3246                 J1.SubVec6(0).SubVec3(0) = dir1;
3247                 J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 );
3248                 J1.SubVec6(1).SubVec3(0) = dir2;
3249                 J1.SubVec6(1).SubVec3(1) = r.Cross( dir2 );
3250                 c1.SetSize( 2 );
3251                 c1[0] = c1[1] = 0.0f;
3252
3253                 if ( body2 ) {
3254                         r = cc->GetContact().point - body2->GetWorldOrigin();
3255
3256                         J2.SetSize( 2, 6 );
3257                         J2.SubVec6(0).SubVec3(0) = -dir1;
3258                         J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 );
3259                         J2.SubVec6(1).SubVec3(0) = -dir2;
3260                         J2.SubVec6(1).SubVec3(1) = r.Cross( -dir2 );
3261                         c2.SetSize( 2 );
3262                         c2[0] = c2[1] = 0.0f;
3263
3264                         if ( body2->GetContactFriction() < friction ) {
3265                                 friction = body2->GetContactFriction();
3266                         }
3267                 }
3268
3269                 lo[0] = -friction;
3270                 hi[0] = friction;
3271                 boxConstraint = cc;
3272                 boxIndex[0] = 0;
3273                 lo[1] = -friction;
3274                 hi[1] = friction;
3275                 boxIndex[1] = 0;
3276         }
3277
3278         if ( body1->GetContactMotorDirection( dir1 ) && body1->GetContactMotorForce() > 0.0f ) {
3279                 // project the motor force direction into the contact plane
3280                 dir1 -= dir1 * cc->GetContact().normal * dir1;
3281                 dir1.Normalize();
3282
3283                 r = cc->GetContact().point - body1->GetWorldOrigin();
3284
3285                 newRow = J1.GetNumRows();
3286                 J1.ChangeSize( newRow+1, J1.GetNumColumns() );
3287                 J1.SubVec6(newRow).SubVec3(0) = -dir1;
3288                 J1.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 );
3289                 c1.ChangeSize( newRow+1 );
3290                 c1[newRow] = body1->GetContactMotorVelocity();
3291
3292                 if ( body2 ) {
3293                         r = cc->GetContact().point - body2->GetWorldOrigin();
3294
3295                         J2.ChangeSize( newRow+1, J2.GetNumColumns() );
3296                         J2.SubVec6(newRow).SubVec3(0) = -dir1;
3297                         J2.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 );
3298                         c2.ChangeSize( newRow+1 );
3299                         c2[newRow] = 0.0f;
3300                 }
3301
3302                 lo[newRow] = -body1->GetContactMotorForce();
3303                 hi[newRow] = body1->GetContactMotorForce();
3304                 boxIndex[newRow] = -1;
3305         }
3306
3307         physics->AddFrameConstraint( this );
3308
3309         return true;
3310 }
3311
3312 /*
3313 ================
3314 idAFConstraint_ContactFriction::Translate
3315 ================
3316 */
3317 void idAFConstraint_ContactFriction::Translate( const idVec3 &translation ) {
3318 }
3319
3320 /*
3321 ================
3322 idAFConstraint_ContactFriction::Rotate
3323 ================
3324 */
3325 void idAFConstraint_ContactFriction::Rotate( const idRotation &rotation ) {
3326 }
3327
3328 /*
3329 ================
3330 idAFConstraint_ContactFriction::DebugDraw
3331 ================
3332 */
3333 void idAFConstraint_ContactFriction::DebugDraw( void ) {
3334 }
3335
3336
3337 //===============================================================
3338 //
3339 //      idAFConstraint_ConeLimit
3340 //
3341 //===============================================================
3342
3343 /*
3344 ================
3345 idAFConstraint_ConeLimit::idAFConstraint_ConeLimit
3346 ================
3347 */
3348 idAFConstraint_ConeLimit::idAFConstraint_ConeLimit( void ) {
3349         type = CONSTRAINT_CONELIMIT;
3350         name = "coneLimit";
3351         InitSize( 1 );
3352         fl.allowPrimary = false;
3353         fl.frameConstraint = true;
3354 }
3355
3356 /*
3357 ================
3358 idAFConstraint_ConeLimit::Setup
3359
3360   the coneAnchor is the top of the cone in body2 space
3361   the coneAxis is the axis of the cone in body2 space
3362   the coneAngle is the angle the cone hull makes at the top
3363   the body1Axis is the axis in body1 space that should stay within the cone
3364 ================
3365 */
3366 void idAFConstraint_ConeLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) {
3367         this->body1 = b1;
3368         this->body2 = b2;
3369         this->coneAxis = coneAxis;
3370         this->coneAxis.Normalize();
3371         this->coneAnchor = coneAnchor;
3372         this->body1Axis = body1Axis;
3373         this->body1Axis.Normalize();
3374         this->cosAngle = (float) cos( DEG2RAD( coneAngle * 0.5f ) );
3375         this->sinHalfAngle = (float) sin( DEG2RAD( coneAngle * 0.25f ) );
3376         this->cosHalfAngle = (float) cos( DEG2RAD( coneAngle * 0.25f ) );
3377 }
3378
3379 /*
3380 ================
3381 idAFConstraint_ConeLimit::SetAnchor
3382 ================
3383 */
3384 void idAFConstraint_ConeLimit::SetAnchor( const idVec3 &coneAnchor ) {
3385         this->coneAnchor = coneAnchor;
3386 }
3387
3388 /*
3389 ================
3390 idAFConstraint_ConeLimit::SetBody1Axis
3391 ================
3392 */
3393 void idAFConstraint_ConeLimit::SetBody1Axis( const idVec3 &body1Axis ) {
3394         this->body1Axis = body1Axis;
3395 }
3396
3397 /*
3398 ================
3399 idAFConstraint_ConeLimit::Evaluate
3400 ================
3401 */
3402 void idAFConstraint_ConeLimit::Evaluate( float invTimeStep ) {
3403         // do nothing
3404 }
3405
3406 /*
3407 ================
3408 idAFConstraint_ConeLimit::ApplyFriction
3409 ================
3410 */
3411 void idAFConstraint_ConeLimit::ApplyFriction( float invTimeStep ) {
3412 }
3413
3414 /*
3415 ================
3416 idAFConstraint_ConeLimit::Add
3417 ================
3418 */
3419 bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) {
3420         float a;
3421         idVec6 J1row, J2row;
3422         idVec3 ax, anchor, body1ax, normal, coneVector, p1, p2;
3423         idQuat q;
3424         idAFBody *master;
3425
3426         if ( af_skipLimits.GetBool() ) {
3427                 lm.Zero();      // constraint exerts no force
3428                 return false;
3429         }
3430
3431         physics = phys;
3432
3433         master = body2 ? body2 : physics->GetMasterBody();
3434
3435         if ( master ) {
3436                 ax = coneAxis * master->GetWorldAxis();
3437                 anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis();
3438         }
3439         else {
3440                 ax = coneAxis;
3441                 anchor = coneAnchor;
3442         }
3443
3444         body1ax = body1Axis * body1->GetWorldAxis();
3445
3446         a = ax * body1ax;
3447
3448         // if the body1 axis is inside the cone
3449         if ( a > cosAngle ) {
3450                 lm.Zero();      // constraint exerts no force
3451                 return false;
3452         }
3453
3454         // calculate the inward cone normal for the position the body1 axis went outside the cone
3455         normal = body1ax.Cross( ax );
3456         normal.Normalize();
3457         q.x = normal.x * sinHalfAngle;
3458         q.y = normal.y * sinHalfAngle;
3459         q.z = normal.z * sinHalfAngle;
3460         q.w = cosHalfAngle;
3461         coneVector = ax * q.ToMat3();
3462         normal = coneVector.Cross( ax ).Cross( coneVector );
3463         normal.Normalize();
3464
3465         p1 = anchor + 32.0f * coneVector - body1->GetWorldOrigin();
3466
3467         J1row.SubVec3(0) = normal;
3468         J1row.SubVec3(1) = p1.Cross( normal );
3469         J1.Set( 1, 6, J1row.ToFloatPtr() );
3470
3471         c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) );
3472
3473         if ( body2 ) {
3474
3475                 p2 = anchor + 32.0f * coneVector - master->GetWorldOrigin();
3476
3477                 J2row.SubVec3(0) = -normal;
3478                 J2row.SubVec3(1) = p2.Cross( -normal );
3479                 J2.Set( 1, 6, J2row.ToFloatPtr() );
3480
3481                 c2[0] = 0.0f;
3482         }
3483
3484         lo[0] = 0.0f;
3485         e[0] = LIMIT_LCP_EPSILON;
3486
3487         physics->AddFrameConstraint( this );
3488
3489         return true;
3490 }
3491
3492 /*
3493 ================
3494 idAFConstraint_ConeLimit::Translate
3495 ================
3496 */
3497 void idAFConstraint_ConeLimit::Translate( const idVec3 &translation ) {
3498         if ( !body2 ) {
3499                 coneAnchor += translation;
3500         }
3501 }
3502
3503 /*
3504 ================
3505 idAFConstraint_ConeLimit::Rotate
3506 ================
3507 */
3508 void idAFConstraint_ConeLimit::Rotate( const idRotation &rotation ) {
3509         if ( !body2 ) {
3510                 coneAnchor *= rotation;
3511                 coneAxis *= rotation.ToMat3();
3512         }
3513 }
3514
3515 /*
3516 ================
3517 idAFConstraint_ConeLimit::DebugDraw
3518 ================
3519 */
3520 void idAFConstraint_ConeLimit::DebugDraw( void ) {
3521         idVec3 ax, anchor, x, y, z, start, end;
3522         float sinAngle, a, size = 10.0f;
3523         idAFBody *master;
3524
3525         master = body2 ? body2 : physics->GetMasterBody();
3526
3527         if ( master ) {
3528                 ax = coneAxis * master->GetWorldAxis();
3529                 anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis();
3530         }
3531         else {
3532                 ax = coneAxis;
3533                 anchor = coneAnchor;
3534         }
3535
3536         // draw body1 axis
3537         gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) );
3538
3539         // draw cone
3540         ax.NormalVectors( x, y );
3541         sinAngle = idMath::Sqrt( 1.0f - cosAngle * cosAngle );
3542         x *= size * sinAngle;
3543         y *= size * sinAngle;
3544         z = anchor + ax * size * cosAngle;
3545         start = x + z;
3546         for ( a = 0.0f; a < 360.0f; a += 45.0f ) {
3547                 end = x * (float) cos( DEG2RAD(a + 45.0f) ) + y * (float) sin( DEG2RAD(a + 45.0f) ) + z;
3548                 gameRenderWorld->DebugLine( colorMagenta, anchor, start );
3549                 gameRenderWorld->DebugLine( colorMagenta, start, end );
3550                 start = end;
3551         }
3552 }
3553
3554 /*
3555 ================
3556 idAFConstraint_ConeLimit::Save
3557 ================
3558 */
3559 void idAFConstraint_ConeLimit::Save( idSaveGame *saveFile ) const {
3560         idAFConstraint::Save( saveFile );
3561         saveFile->WriteVec3( coneAnchor );
3562         saveFile->WriteVec3( coneAxis );
3563         saveFile->WriteVec3( body1Axis );
3564         saveFile->WriteFloat( cosAngle );
3565         saveFile->WriteFloat( sinHalfAngle );
3566         saveFile->WriteFloat( cosHalfAngle );
3567         saveFile->WriteFloat( epsilon );
3568 }
3569
3570 /*
3571 ================
3572 idAFConstraint_ConeLimit::Restore
3573 ================
3574 */
3575 void idAFConstraint_ConeLimit::Restore( idRestoreGame *saveFile ) {
3576         idAFConstraint::Restore( saveFile );
3577         saveFile->ReadVec3( coneAnchor );
3578         saveFile->ReadVec3( coneAxis );
3579         saveFile->ReadVec3( body1Axis );
3580         saveFile->ReadFloat( cosAngle );
3581         saveFile->ReadFloat( sinHalfAngle );
3582         saveFile->ReadFloat( cosHalfAngle );
3583         saveFile->ReadFloat( epsilon );
3584 }
3585
3586
3587 //===============================================================
3588 //
3589 //      idAFConstraint_PyramidLimit
3590 //
3591 //===============================================================
3592
3593 /*
3594 ================
3595 idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit
3596 ================
3597 */
3598 idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit( void ) {
3599         type = CONSTRAINT_PYRAMIDLIMIT;
3600         name = "pyramidLimit";
3601         InitSize( 1 );
3602         fl.allowPrimary = false;
3603         fl.frameConstraint = true;
3604 }
3605
3606 /*
3607 ================
3608 idAFConstraint_PyramidLimit::Setup
3609 ================
3610 */
3611 void idAFConstraint_PyramidLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor,
3612                                                                 const idVec3 &pyramidAxis, const idVec3 &baseAxis,
3613                                                                 const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis ) {
3614         body1 = b1;
3615         body2 = b2;
3616         // setup the base and make sure the basis is orthonormal
3617         pyramidBasis[2] = pyramidAxis;
3618         pyramidBasis[2].Normalize();
3619         pyramidBasis[0] = baseAxis;
3620         pyramidBasis[0] -= pyramidBasis[2] * baseAxis * pyramidBasis[2];
3621         pyramidBasis[0].Normalize();
3622         pyramidBasis[1] = pyramidBasis[0].Cross( pyramidBasis[2] );
3623         // pyramid top
3624         this->pyramidAnchor = pyramidAnchor;
3625         // angles
3626         cosAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.5f ) );
3627         cosAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.5f ) );
3628         sinHalfAngle[0] = (float) sin( DEG2RAD( pyramidAngle1 * 0.25f ) );
3629         sinHalfAngle[1] = (float) sin( DEG2RAD( pyramidAngle2 * 0.25f ) );
3630         cosHalfAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.25f ) );
3631         cosHalfAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.25f ) );
3632
3633         this->body1Axis = body1Axis;
3634 }
3635
3636 /*
3637 ================
3638 idAFConstraint_PyramidLimit::SetAnchor
3639 ================
3640 */
3641 void idAFConstraint_PyramidLimit::SetAnchor( const idVec3 &pyramidAnchor ) {
3642         this->pyramidAnchor = pyramidAnchor;
3643 }
3644
3645 /*
3646 ================
3647 idAFConstraint_PyramidLimit::SetBody1Axis
3648 ================
3649 */
3650 void idAFConstraint_PyramidLimit::SetBody1Axis( const idVec3 &body1Axis ) {
3651         this->body1Axis = body1Axis;
3652 }
3653
3654 /*
3655 ================
3656 idAFConstraint_PyramidLimit::Evaluate
3657 ================
3658 */
3659 void idAFConstraint_PyramidLimit::Evaluate( float invTimeStep ) {
3660         // do nothing
3661 }
3662
3663 /*
3664 ================
3665 idAFConstraint_PyramidLimit::ApplyFriction
3666 ================
3667 */
3668 void idAFConstraint_PyramidLimit::ApplyFriction( float invTimeStep ) {
3669 }
3670
3671 /*
3672 ================
3673 idAFConstraint_PyramidLimit::Add
3674 ================
3675 */
3676 bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) {
3677         int i;
3678         float a[2];
3679         idVec6 J1row, J2row;
3680         idMat3 worldBase;
3681         idVec3 anchor, body1ax, ax[2], v, normal, pyramidVector, p1, p2;
3682         idQuat q;
3683         idAFBody *master;
3684
3685         if ( af_skipLimits.GetBool() ) {
3686                 lm.Zero();      // constraint exerts no force
3687                 return false;
3688         }
3689
3690         physics = phys;
3691         master = body2 ? body2 : physics->GetMasterBody();
3692
3693         if ( master ) {
3694                 worldBase[0] = pyramidBasis[0] * master->GetWorldAxis();
3695                 worldBase[1] = pyramidBasis[1] * master->GetWorldAxis();
3696                 worldBase[2] = pyramidBasis[2] * master->GetWorldAxis();
3697                 anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis();
3698         }
3699         else {
3700                 worldBase = pyramidBasis;
3701                 anchor = pyramidAnchor;
3702         }
3703
3704         body1ax = body1Axis * body1->GetWorldAxis();
3705
3706         for ( i = 0; i < 2; i++ ) {
3707                 ax[i] = body1ax - worldBase[!i] * body1ax * worldBase[!i];
3708                 ax[i].Normalize();
3709                 a[i] = worldBase[2] * ax[i];
3710         }
3711
3712         // if the body1 axis is inside the pyramid
3713         if ( a[0] > cosAngle[0] && a[1] > cosAngle[1] ) {
3714                 lm.Zero();      // constraint exerts no force
3715                 return false;
3716         }
3717
3718         // calculate the inward pyramid normal for the position the body1 axis went outside the pyramid
3719         pyramidVector = worldBase[2];
3720         for ( i = 0; i < 2; i++ ) {
3721                 if ( a[i] <= cosAngle[i] ) {
3722                         v = ax[i].Cross( worldBase[2] );
3723                         v.Normalize();
3724                         q.x = v.x * sinHalfAngle[i];
3725                         q.y = v.y * sinHalfAngle[i];
3726                         q.z = v.z * sinHalfAngle[i];
3727                         q.w = cosHalfAngle[i];
3728                         pyramidVector *= q.ToMat3();
3729                 }
3730         }
3731         normal = pyramidVector.Cross( worldBase[2] ).Cross( pyramidVector );
3732         normal.Normalize();
3733
3734         p1 = anchor + 32.0f * pyramidVector - body1->GetWorldOrigin();
3735
3736         J1row.SubVec3(0) = normal;
3737         J1row.SubVec3(1) = p1.Cross( normal );
3738         J1.Set( 1, 6, J1row.ToFloatPtr() );
3739
3740         c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) );
3741
3742         if ( body2 ) {
3743
3744                 p2 = anchor + 32.0f * pyramidVector - master->GetWorldOrigin();
3745
3746                 J2row.SubVec3(0) = -normal;
3747                 J2row.SubVec3(1) = p2.Cross( -normal );
3748                 J2.Set( 1, 6, J2row.ToFloatPtr() );
3749
3750                 c2[0] = 0.0f;
3751         }
3752
3753         lo[0] = 0.0f;
3754         e[0] = LIMIT_LCP_EPSILON;
3755
3756         physics->AddFrameConstraint( this );
3757
3758         return true;
3759 }
3760
3761 /*
3762 ================
3763 idAFConstraint_PyramidLimit::Translate
3764 ================
3765 */
3766 void idAFConstraint_PyramidLimit::Translate( const idVec3 &translation ) {
3767         if ( !body2 ) {
3768                 pyramidAnchor += translation;
3769         }
3770 }
3771
3772 /*
3773 ================
3774 idAFConstraint_PyramidLimit::Rotate
3775 ================
3776 */
3777 void idAFConstraint_PyramidLimit::Rotate( const idRotation &rotation ) {
3778         if ( !body2 ) {
3779                 pyramidAnchor *= rotation;
3780                 pyramidBasis[0] *= rotation.ToMat3();
3781                 pyramidBasis[1] *= rotation.ToMat3();
3782                 pyramidBasis[2] *= rotation.ToMat3();
3783         }
3784 }
3785
3786 /*
3787 ================
3788 idAFConstraint_PyramidLimit::DebugDraw
3789 ================
3790 */
3791 void idAFConstraint_PyramidLimit::DebugDraw( void ) {
3792         int i;
3793         float size = 10.0f;
3794         idVec3 anchor, dir, p[4];
3795         idMat3 worldBase, m[2];
3796         idQuat q;
3797         idAFBody *master;
3798
3799         master = body2 ? body2 : physics->GetMasterBody();
3800
3801         if ( master ) {
3802                 worldBase[0] = pyramidBasis[0] * master->GetWorldAxis();
3803                 worldBase[1] = pyramidBasis[1] * master->GetWorldAxis();
3804                 worldBase[2] = pyramidBasis[2] * master->GetWorldAxis();
3805                 anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis();
3806         }
3807         else {
3808                 worldBase = pyramidBasis;
3809                 anchor = pyramidAnchor;
3810         }
3811
3812         // draw body1 axis
3813         gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) );
3814
3815         // draw the pyramid
3816         for ( i = 0; i < 2; i++ ) {
3817                 q.x = worldBase[!i].x * sinHalfAngle[i];
3818                 q.y = worldBase[!i].y * sinHalfAngle[i];
3819                 q.z = worldBase[!i].z * sinHalfAngle[i];
3820                 q.w = cosHalfAngle[i];
3821                 m[i] = q.ToMat3();
3822         }
3823
3824         dir = worldBase[2] * size;
3825         p[0] = anchor + m[0] * (m[1] * dir);
3826         p[1] = anchor + m[0] * (m[1].Transpose() * dir);
3827         p[2] = anchor + m[0].Transpose() * (m[1].Transpose() * dir);
3828         p[3] = anchor + m[0].Transpose() * (m[1] * dir);
3829
3830         for ( i = 0; i < 4; i++ ) {
3831                 gameRenderWorld->DebugLine( colorMagenta, anchor, p[i] );
3832                 gameRenderWorld->DebugLine( colorMagenta, p[i], p[(i+1)&3] );
3833         }
3834 }
3835
3836 /*
3837 ================
3838 idAFConstraint_PyramidLimit::Save
3839 ================
3840 */
3841 void idAFConstraint_PyramidLimit::Save( idSaveGame *saveFile ) const {
3842         idAFConstraint::Save( saveFile );
3843         saveFile->WriteVec3( pyramidAnchor );
3844         saveFile->WriteMat3( pyramidBasis );
3845         saveFile->WriteVec3( body1Axis );
3846         saveFile->WriteFloat( cosAngle[0] );
3847         saveFile->WriteFloat( cosAngle[1] );
3848         saveFile->WriteFloat( sinHalfAngle[0] );
3849         saveFile->WriteFloat( sinHalfAngle[1] );
3850         saveFile->WriteFloat( cosHalfAngle[0] );
3851         saveFile->WriteFloat( cosHalfAngle[1] );
3852         saveFile->WriteFloat( epsilon );
3853 }
3854
3855 /*
3856 ================
3857 idAFConstraint_PyramidLimit::Restore
3858 ================
3859 */
3860 void idAFConstraint_PyramidLimit::Restore( idRestoreGame *saveFile ) {
3861         idAFConstraint::Restore( saveFile );
3862         saveFile->ReadVec3( pyramidAnchor );
3863         saveFile->ReadMat3( pyramidBasis );
3864         saveFile->ReadVec3( body1Axis );
3865         saveFile->ReadFloat( cosAngle[0] );
3866         saveFile->ReadFloat( cosAngle[1] );
3867         saveFile->ReadFloat( sinHalfAngle[0] );
3868         saveFile->ReadFloat( sinHalfAngle[1] );
3869         saveFile->ReadFloat( cosHalfAngle[0] );
3870         saveFile->ReadFloat( cosHalfAngle[1] );
3871         saveFile->ReadFloat( epsilon );
3872 }
3873
3874
3875 //===============================================================
3876 //
3877 //      idAFConstraint_Suspension
3878 //
3879 //===============================================================
3880
3881 /*
3882 ================
3883 idAFConstraint_Suspension::idAFConstraint_Suspension
3884 ================
3885 */
3886 idAFConstraint_Suspension::idAFConstraint_Suspension( void ) {
3887         type = CONSTRAINT_SUSPENSION;
3888         name = "suspension";
3889         InitSize( 3 );
3890         fl.allowPrimary = false;
3891         fl.frameConstraint = true;
3892
3893         localOrigin.Zero();
3894         localAxis.Identity();
3895         suspensionUp = 0.0f;
3896         suspensionDown = 0.0f;
3897         suspensionKCompress = 0.0f;
3898         suspensionDamping = 0.0f;
3899         steerAngle = 0.0f;
3900         friction = 2.0f;
3901         motorEnabled = false;
3902         motorForce = 0.0f;
3903         motorVelocity = 0.0f;
3904         wheelModel = NULL;
3905         memset( &trace, 0, sizeof( trace ) );
3906         epsilon = LCP_EPSILON;
3907 }
3908
3909 /*
3910 ================
3911 idAFConstraint_Suspension::Setup
3912 ================
3913 */
3914 void idAFConstraint_Suspension::Setup( const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel ) {
3915         this->name = name;
3916         body1 = body;
3917         body2 = NULL;
3918         localOrigin = ( origin - body->GetWorldOrigin() ) * body->GetWorldAxis().Transpose();
3919         localAxis = axis * body->GetWorldAxis().Transpose();
3920         wheelModel = clipModel;
3921 }
3922
3923 /*
3924 ================
3925 idAFConstraint_Suspension::SetSuspension
3926 ================
3927 */
3928 void idAFConstraint_Suspension::SetSuspension( const float up, const float down, const float k, const float d, const float f ) {
3929         suspensionUp = up;
3930         suspensionDown = down;
3931         suspensionKCompress = k;
3932         suspensionDamping = d;
3933         friction = f;
3934 }
3935
3936 /*
3937 ================
3938 idAFConstraint_Suspension::GetWheelOrigin
3939 ================
3940 */
3941 const idVec3 idAFConstraint_Suspension::GetWheelOrigin( void ) const {
3942         return body1->GetWorldOrigin() + wheelOffset * body1->GetWorldAxis();
3943 }
3944
3945 /*
3946 ================
3947 idAFConstraint_Suspension::Evaluate
3948 ================
3949 */
3950 void idAFConstraint_Suspension::Evaluate( float invTimeStep ) {
3951         float velocity, suspensionLength, springLength, compression, dampingForce, springForce;
3952         idVec3 origin, start, end, vel1, vel2, springDir, r, frictionDir, motorDir;
3953         idMat3 axis;
3954         idRotation rotation;
3955
3956         axis = localAxis * body1->GetWorldAxis();
3957         origin = body1->GetWorldOrigin() + localOrigin * body1->GetWorldAxis();
3958         start = origin + suspensionUp * axis[2];
3959         end = origin - suspensionDown * axis[2];
3960
3961         rotation.SetVec( axis[2] );
3962         rotation.SetAngle( steerAngle );
3963
3964         axis *= rotation.ToMat3();
3965
3966         gameLocal.clip.Translation( trace, start, end, wheelModel, axis, MASK_SOLID, NULL );
3967
3968         wheelOffset = ( trace.endpos - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
3969
3970         if ( trace.fraction >= 1.0f ) {
3971                 J1.SetSize( 0, 6 );
3972                 if ( body2 ) {
3973                         J2.SetSize( 0, 6 );
3974                 }
3975                 return;
3976         }
3977
3978         // calculate and add spring force
3979         vel1 = body1->GetPointVelocity( start );
3980         if ( body2 ) {
3981                 vel2 = body2->GetPointVelocity( trace.c.point );
3982         } else {
3983                 vel2.Zero();
3984         }
3985
3986         suspensionLength = suspensionUp + suspensionDown;
3987         springDir = trace.endpos - start;
3988         springLength = trace.fraction * suspensionLength;
3989         dampingForce = suspensionDamping * idMath::Fabs( ( vel2 - vel1 ) * springDir ) / ( 1.0f + springLength * springLength );
3990         compression = suspensionLength - springLength;
3991         springForce = compression * compression * suspensionKCompress - dampingForce;
3992
3993         r = trace.c.point - body1->GetWorldOrigin();
3994         J1.SetSize( 2, 6 );
3995         J1.SubVec6(0).SubVec3(0) = trace.c.normal;
3996         J1.SubVec6(0).SubVec3(1) = r.Cross( trace.c.normal );
3997         c1.SetSize( 2 );
3998         c1[0] = 0.0f;
3999         velocity = J1.SubVec6(0).SubVec3(0) * body1->GetLinearVelocity() + J1.SubVec6(0).SubVec3(1) * body1->GetAngularVelocity();
4000
4001         if ( body2 ) {
4002                 r = trace.c.point - body2->GetWorldOrigin();
4003                 J2.SetSize( 2, 6 );
4004                 J2.SubVec6(0).SubVec3(0) = -trace.c.normal;
4005                 J2.SubVec6(0).SubVec3(1) = r.Cross( -trace.c.normal );
4006                 c2.SetSize( 2 );
4007                 c2[0] = 0.0f;
4008                 velocity += J2.SubVec6(0).SubVec3(0) * body2->GetLinearVelocity() + J2.SubVec6(0).SubVec3(1) * body2->GetAngularVelocity();
4009         }
4010
4011         c1[0] = -compression;           // + 0.5f * -velocity;
4012
4013         e[0] = 1e-4f;
4014         lo[0] = 0.0f;
4015         hi[0] = springForce;
4016         boxConstraint = NULL;
4017         boxIndex[0] = -1;
4018
4019         // project the friction direction into the contact plane
4020         frictionDir = axis[1] - axis[1] * trace.c.normal * axis[1];
4021         frictionDir.Normalize();
4022
4023         r = trace.c.point - body1->GetWorldOrigin();
4024
4025         J1.SubVec6(1).SubVec3(0) = frictionDir;
4026         J1.SubVec6(1).SubVec3(1) = r.Cross( frictionDir );
4027         c1[1] = 0.0f;
4028
4029         if ( body2 ) {
4030                 r = trace.c.point - body2->GetWorldOrigin();
4031
4032                 J2.SubVec6(1).SubVec3(0) = -frictionDir;
4033                 J2.SubVec6(1).SubVec3(1) = r.Cross( -frictionDir );
4034                 c2[1] = 0.0f;
4035         }
4036
4037         lo[1] = -friction * physics->GetContactFrictionScale();
4038         hi[1] = friction * physics->GetContactFrictionScale();
4039
4040         boxConstraint = this;
4041         boxIndex[1] = 0;
4042
4043
4044         if ( motorEnabled ) {
4045                 // project the motor force direction into the contact plane
4046                 motorDir = axis[0] - axis[0] * trace.c.normal * axis[0];
4047                 motorDir.Normalize();
4048
4049                 r = trace.c.point - body1->GetWorldOrigin();
4050
4051                 J1.ChangeSize( 3, J1.GetNumColumns() );
4052                 J1.SubVec6(2).SubVec3(0) = -motorDir;
4053                 J1.SubVec6(2).SubVec3(1) = r.Cross( -motorDir );
4054                 c1.ChangeSize( 3 );
4055                 c1[2] = motorVelocity;
4056
4057                 if ( body2 ) {
4058                         r = trace.c.point - body2->GetWorldOrigin();
4059
4060                         J2.ChangeSize( 3, J2.GetNumColumns() );
4061                         J2.SubVec6(2).SubVec3(0) = -motorDir;
4062                         J2.SubVec6(2).SubVec3(1) = r.Cross( -motorDir );
4063                         c2.ChangeSize( 3 );
4064                         c2[2] = 0.0f;
4065                 }
4066
4067                 lo[2] = -motorForce;
4068                 hi[2] = motorForce;
4069                 boxIndex[2] = -1;
4070         }
4071 }
4072
4073 /*
4074 ================
4075 idAFConstraint_Suspension::ApplyFriction
4076 ================
4077 */
4078 void idAFConstraint_Suspension::ApplyFriction( float invTimeStep ) {
4079         // do nothing
4080 }
4081
4082 /*
4083 ================
4084 idAFConstraint_Suspension::Translate
4085 ================
4086 */
4087 void idAFConstraint_Suspension::Translate( const idVec3 &translation ) {
4088 }
4089
4090 /*
4091 ================
4092 idAFConstraint_Suspension::Rotate
4093 ================
4094 */
4095 void idAFConstraint_Suspension::Rotate( const idRotation &rotation ) {
4096 }
4097
4098 /*
4099 ================
4100 idAFConstraint_Suspension::DebugDraw
4101 ================
4102 */
4103 void idAFConstraint_Suspension::DebugDraw( void ) {
4104         idVec3 origin;
4105         idMat3 axis;
4106         idRotation rotation;
4107
4108         axis = localAxis * body1->GetWorldAxis();
4109
4110         rotation.SetVec( axis[2] );
4111         rotation.SetAngle( steerAngle );
4112
4113         axis *= rotation.ToMat3();
4114
4115         if ( trace.fraction < 1.0f ) {
4116                 origin = trace.c.point;
4117
4118                 gameRenderWorld->DebugLine( colorWhite, origin, origin + 6.0f * axis[2] );
4119                 gameRenderWorld->DebugLine( colorWhite, origin - 4.0f * axis[0], origin + 4.0f * axis[0] );
4120                 gameRenderWorld->DebugLine( colorWhite, origin - 2.0f * axis[1], origin + 2.0f * axis[1] );
4121         }
4122 }
4123
4124
4125 //===============================================================
4126 //
4127 //      idAFBody
4128 //
4129 //===============================================================
4130
4131 /*
4132 ================
4133 idAFBody::idAFBody
4134 ================
4135 */
4136 idAFBody::idAFBody( void ) {
4137         Init();
4138 }
4139
4140 /*
4141 ================
4142 idAFBody::idAFBody
4143 ================
4144 */
4145 idAFBody::idAFBody( const idStr &name, idClipModel *clipModel, float density ) {
4146
4147         assert( clipModel );
4148         assert( clipModel->IsTraceModel() );
4149
4150         Init();
4151
4152         this->name = name;
4153         this->clipModel = NULL;
4154
4155         SetClipModel( clipModel );
4156         SetDensity( density );
4157
4158         current->worldOrigin = clipModel->GetOrigin();
4159         current->worldAxis = clipModel->GetAxis();
4160         *next = *current;
4161
4162 }
4163
4164 /*
4165 ================
4166 idAFBody::~idAFBody
4167 ================
4168 */
4169 idAFBody::~idAFBody( void ) {
4170         delete clipModel;
4171 }
4172
4173 /*
4174 ================
4175 idAFBody::Init
4176 ================
4177 */
4178 void idAFBody::Init( void ) {
4179         name                                            = "noname";
4180         parent                                          = NULL;
4181         clipModel                                       = NULL;
4182         primaryConstraint                       = NULL;
4183         tree                                            = NULL;
4184
4185         linearFriction                          = -1.0f;
4186         angularFriction                         = -1.0f;
4187         contactFriction                         = -1.0f;
4188         bouncyness                                      = -1.0f;
4189         clipMask                                        = 0;
4190
4191         frictionDir                                     = vec3_zero;
4192         contactMotorDir                         = vec3_zero;
4193         contactMotorVelocity            = 0.0f;
4194         contactMotorForce                       = 0.0f;
4195
4196         mass                                            = 1.0f;
4197         invMass                                         = 1.0f;
4198         centerOfMass                            = vec3_zero;
4199         inertiaTensor                           = mat3_identity;
4200         inverseInertiaTensor            = mat3_identity;
4201
4202         current                                         = &state[0];
4203         next                                            = &state[1];
4204         current->worldOrigin            = vec3_zero;
4205         current->worldAxis                      = mat3_identity;
4206         current->spatialVelocity        = vec6_zero;
4207         current->externalForce          = vec6_zero;
4208         *next                                           = *current;
4209         saved                                           = *current;
4210         atRestOrigin                            = vec3_zero;
4211         atRestAxis                                      = mat3_identity;
4212
4213         s.Zero( 6 );
4214         totalForce.Zero( 6 );
4215         auxForce.Zero( 6 );
4216         acceleration.Zero( 6 );
4217
4218         response                                        = NULL;
4219         responseIndex                           = NULL;
4220         numResponses                            = 0;
4221         maxAuxiliaryIndex                       = 0;
4222         maxSubTreeAuxiliaryIndex        = 0;
4223
4224         memset( &fl, 0, sizeof( fl ) );
4225
4226         fl.selfCollision                        = true;
4227         fl.isZero                                       = true;
4228 }
4229
4230 /*
4231 ================
4232 idAFBody::SetClipModel
4233 ================
4234 */
4235 void idAFBody::SetClipModel( idClipModel *clipModel ) {
4236         if ( this->clipModel && this->clipModel != clipModel ) {
4237                 delete this->clipModel;
4238         }
4239         this->clipModel = clipModel;
4240 }
4241
4242 /*
4243 ================
4244 idAFBody::SetFriction
4245 ================
4246 */
4247 void idAFBody::SetFriction( float linear, float angular, float contact ) {
4248         if ( linear < 0.0f || linear > 1.0f ||
4249                         angular < 0.0f || angular > 1.0f ||
4250                                 contact < 0.0f ) {
4251                 gameLocal.Warning( "idAFBody::SetFriction: friction out of range, linear = %.1f, angular = %.1f, contact = %.1f", linear, angular, contact );
4252                 return;
4253         }
4254         linearFriction = linear;
4255         angularFriction = angular;
4256         contactFriction = contact;
4257 }
4258
4259 /*
4260 ================
4261 idAFBody::SetBouncyness
4262 ================
4263 */
4264 void idAFBody::SetBouncyness( float bounce ) {
4265         if ( bounce < 0.0f || bounce > 1.0f ) {
4266                 gameLocal.Warning( "idAFBody::SetBouncyness: bouncyness out of range, bounce = %.1f", bounce );
4267                 return;
4268         }
4269         bouncyness = bounce;
4270 }
4271
4272 /*
4273 ================
4274 idAFBody::SetDensity
4275 ================
4276 */
4277 void idAFBody::SetDensity( float density, const idMat3 &inertiaScale ) {
4278
4279         // get the body mass properties
4280         clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor );
4281
4282         // make sure we have a valid mass
4283         if ( mass <= 0.0f || FLOAT_IS_NAN( mass ) ) {
4284                 gameLocal.Warning( "idAFBody::SetDensity: invalid mass for body '%s'", name.c_str() );
4285                 mass = 1.0f;
4286                 centerOfMass.Zero();
4287                 inertiaTensor.Identity();
4288         }
4289
4290         // make sure the center of mass is at the body origin
4291         if ( !centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) {
4292                 gameLocal.Warning( "idAFBody::SetDentity: center of mass not at origin for body '%s'", name.c_str() );
4293         }
4294         centerOfMass.Zero();
4295
4296         // calculate the inverse mass and inverse inertia tensor
4297         invMass = 1.0f / mass;
4298         if ( inertiaScale != mat3_identity ) {
4299                 inertiaTensor *= inertiaScale;
4300         }
4301         if ( inertiaTensor.IsDiagonal( 1e-3f ) ) {
4302                 inertiaTensor[0][1] = inertiaTensor[0][2] = 0.0f;
4303                 inertiaTensor[1][0] = inertiaTensor[1][2] = 0.0f;
4304                 inertiaTensor[2][0] = inertiaTensor[2][1] = 0.0f;
4305                 inverseInertiaTensor.Identity();
4306                 inverseInertiaTensor[0][0] = 1.0f / inertiaTensor[0][0];
4307                 inverseInertiaTensor[1][1] = 1.0f / inertiaTensor[1][1];
4308                 inverseInertiaTensor[2][2] = 1.0f / inertiaTensor[2][2];
4309         }
4310         else {
4311                 inverseInertiaTensor = inertiaTensor.Inverse();
4312         }
4313 }
4314
4315 /*
4316 ================
4317 idAFBody::SetFrictionDirection
4318 ================
4319 */
4320 void idAFBody::SetFrictionDirection( const idVec3 &dir ) {
4321         frictionDir = dir * current->worldAxis.Transpose();
4322         fl.useFrictionDir = true;
4323 }
4324
4325 /*
4326 ================
4327 idAFBody::GetFrictionDirection
4328 ================
4329 */
4330 bool idAFBody::GetFrictionDirection( idVec3 &dir ) const {
4331         if ( fl.useFrictionDir ) {
4332                 dir = frictionDir * current->worldAxis;
4333                 return true;
4334         }
4335         return false;
4336 }
4337
4338 /*
4339 ================
4340 idAFBody::SetContactMotorDirection
4341 ================
4342 */
4343 void idAFBody::SetContactMotorDirection( const idVec3 &dir ) {
4344         contactMotorDir = dir * current->worldAxis.Transpose();
4345         fl.useContactMotorDir = true;
4346 }
4347
4348 /*
4349 ================
4350 idAFBody::GetContactMotorDirection
4351 ================
4352 */
4353 bool idAFBody::GetContactMotorDirection( idVec3 &dir ) const {
4354         if ( fl.useContactMotorDir ) {
4355                 dir = contactMotorDir * current->worldAxis;
4356                 return true;
4357         }
4358         return false;
4359 }
4360
4361 /*
4362 ================
4363 idAFBody::GetPointVelocity
4364 ================
4365 */
4366 idVec3 idAFBody::GetPointVelocity( const idVec3 &point ) const {
4367         idVec3 r = point - current->worldOrigin;
4368         return current->spatialVelocity.SubVec3(0) + current->spatialVelocity.SubVec3(1).Cross( r );
4369 }
4370
4371 /*
4372 ================
4373 idAFBody::AddForce
4374 ================
4375 */
4376 void idAFBody::AddForce( const idVec3 &point, const idVec3 &force ) {
4377         current->externalForce.SubVec3(0) += force;
4378         current->externalForce.SubVec3(1) += (point - current->worldOrigin).Cross( force );
4379 }
4380
4381 /*
4382 ================
4383 idAFBody::InverseWorldSpatialInertiaMultiply
4384
4385   dst = this->inverseWorldSpatialInertia * v;
4386 ================
4387 */
4388 ID_INLINE void idAFBody::InverseWorldSpatialInertiaMultiply( idVecX &dst, const float *v ) const {
4389         const float *mPtr = inverseWorldSpatialInertia.ToFloatPtr();
4390         const float *vPtr = v;
4391         float *dstPtr = dst.ToFloatPtr();
4392
4393         if ( fl.spatialInertiaSparse ) {
4394                 dstPtr[0] = mPtr[0*6+0] * vPtr[0];
4395                 dstPtr[1] = mPtr[1*6+1] * vPtr[1];
4396                 dstPtr[2] = mPtr[2*6+2] * vPtr[2];
4397                 dstPtr[3] = mPtr[3*6+3] * vPtr[3] + mPtr[3*6+4] * vPtr[4] + mPtr[3*6+5] * vPtr[5];
4398                 dstPtr[4] = mPtr[4*6+3] * vPtr[3] + mPtr[4*6+4] * vPtr[4] + mPtr[4*6+5] * vPtr[5];
4399                 dstPtr[5] = mPtr[5*6+3] * vPtr[3] + mPtr[5*6+4] * vPtr[4] + mPtr[5*6+5] * vPtr[5];
4400         } else {
4401                 gameLocal.Warning( "spatial inertia is not sparse for body %s", name.c_str() );
4402         }
4403 }
4404
4405 /*
4406 ================
4407 idAFBody::Save
4408 ================
4409 */
4410 void idAFBody::Save( idSaveGame *saveFile ) {
4411         saveFile->WriteFloat( linearFriction );
4412         saveFile->WriteFloat( angularFriction );
4413         saveFile->WriteFloat( contactFriction );
4414         saveFile->WriteFloat( bouncyness );
4415         saveFile->WriteInt( clipMask );
4416         saveFile->WriteVec3( frictionDir );
4417         saveFile->WriteVec3( contactMotorDir );
4418         saveFile->WriteFloat( contactMotorVelocity );
4419         saveFile->WriteFloat( contactMotorForce );
4420
4421         saveFile->WriteFloat( mass );
4422         saveFile->WriteFloat( invMass );
4423         saveFile->WriteVec3( centerOfMass );
4424         saveFile->WriteMat3( inertiaTensor );
4425         saveFile->WriteMat3( inverseInertiaTensor );
4426
4427         saveFile->WriteVec3( current->worldOrigin );
4428         saveFile->WriteMat3( current->worldAxis );
4429         saveFile->WriteVec6( current->spatialVelocity );
4430         saveFile->WriteVec6( current->externalForce );
4431         saveFile->WriteVec3( atRestOrigin );
4432         saveFile->WriteMat3( atRestAxis );
4433 }
4434
4435 /*
4436 ================
4437 idAFBody::Restore
4438 ================
4439 */
4440 void idAFBody::Restore( idRestoreGame *saveFile ) {
4441         saveFile->ReadFloat( linearFriction );
4442         saveFile->ReadFloat( angularFriction );
4443         saveFile->ReadFloat( contactFriction );
4444         saveFile->ReadFloat( bouncyness );
4445         saveFile->ReadInt( clipMask );
4446         saveFile->ReadVec3( frictionDir );
4447         saveFile->ReadVec3( contactMotorDir );
4448         saveFile->ReadFloat( contactMotorVelocity );
4449         saveFile->ReadFloat( contactMotorForce );
4450
4451         saveFile->ReadFloat( mass );
4452         saveFile->ReadFloat( invMass );
4453         saveFile->ReadVec3( centerOfMass );
4454         saveFile->ReadMat3( inertiaTensor );
4455         saveFile->ReadMat3( inverseInertiaTensor );
4456
4457         saveFile->ReadVec3( current->worldOrigin );
4458         saveFile->ReadMat3( current->worldAxis );
4459         saveFile->ReadVec6( current->spatialVelocity );
4460         saveFile->ReadVec6( current->externalForce );
4461         saveFile->ReadVec3( atRestOrigin );
4462         saveFile->ReadMat3( atRestAxis );
4463 }
4464
4465
4466
4467 //===============================================================
4468 //                                                        M
4469 //  idAFTree                                             MrE
4470 //                                                        E
4471 //===============================================================
4472
4473 /*
4474 ================
4475 idAFTree::Factor
4476
4477   factor matrix for the primary constraints in the tree
4478 ================
4479 */
4480 void idAFTree::Factor( void ) const {
4481         int i, j;
4482         idAFBody *body;
4483         idAFConstraint *child;
4484         idMatX childI;
4485
4486         childI.SetData( 6, 6, MATX_ALLOCA( 6 * 6 ) );
4487
4488         // from the leaves up towards the root
4489         for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
4490                 body = sortedBodies[i];
4491
4492                 if ( body->children.Num() ) {
4493
4494                         for ( j = 0; j < body->children.Num(); j++ ) {
4495
4496                                 child = body->children[j]->primaryConstraint;
4497
4498                                 // child->I = - child->body1->J.Transpose() * child->body1->I * child->body1->J;
4499                                 childI.SetSize( child->J1.GetNumRows(), child->J1.GetNumRows() );
4500                                 child->body1->J.TransposeMultiply( child->body1->I ).Multiply( childI, child->body1->J );
4501                                 childI.Negate();
4502
4503                                 child->invI = childI;
4504                                 if ( !child->invI.InverseFastSelf() ) {
4505                                         gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for constraint '%s'",
4506                                                                         child->invI.GetNumRows(), child->invI.GetNumColumns(), child->GetName().c_str() );
4507                                 }
4508                                 child->J = child->invI * child->J;
4509
4510                                 body->I -= child->J.TransposeMultiply( childI ) * child->J;
4511                         }
4512
4513                         body->invI = body->I;
4514                         if ( !body->invI.InverseFastSelf() ) {
4515                                 gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for body %s",
4516                                                                 child->invI.GetNumRows(), child->invI.GetNumColumns(), body->GetName().c_str() );
4517                         }
4518                         if ( body->primaryConstraint ) {
4519                                 body->J = body->invI * body->J;
4520                         }
4521                 }
4522                 else if ( body->primaryConstraint ) {
4523                         body->J = body->inverseWorldSpatialInertia * body->J;
4524                 }
4525         }
4526 }
4527
4528 /*
4529 ================
4530 idAFTree::Solve
4531
4532   solve for primary constraints in the tree
4533 ================
4534 */
4535 void idAFTree::Solve( int auxiliaryIndex ) const {
4536         int i, j;
4537         idAFBody *body, *child;
4538         idAFConstraint *primaryConstraint;
4539
4540         // from the leaves up towards the root
4541         for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
4542                 body = sortedBodies[i];
4543
4544                 for ( j = 0; j < body->children.Num(); j++ ) {
4545                         child = body->children[j];
4546                         primaryConstraint = child->primaryConstraint;
4547
4548                         if ( !child->fl.isZero ) {
4549                                 child->J.TransposeMultiplySub( primaryConstraint->s, child->s );
4550                                 primaryConstraint->fl.isZero = false;
4551                         }
4552                         if ( !primaryConstraint->fl.isZero ) {
4553                                 primaryConstraint->J.TransposeMultiplySub( body->s, primaryConstraint->s );
4554                                 body->fl.isZero = false;
4555                         }
4556                 }
4557         }
4558
4559         bool useSymmetry = af_useSymmetry.GetBool();
4560
4561         // from the root down towards the leaves
4562         for ( i = 0; i < sortedBodies.Num(); i++ ) {
4563                 body = sortedBodies[i];
4564                 primaryConstraint = body->primaryConstraint;
4565
4566                 if ( primaryConstraint ) {
4567
4568                         if ( useSymmetry && body->parent->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) {
4569                                 continue;
4570                         }
4571
4572                         if ( !primaryConstraint->fl.isZero ) {
4573                                 primaryConstraint->s = primaryConstraint->invI * primaryConstraint->s;
4574                         }
4575                         primaryConstraint->J.MultiplySub( primaryConstraint->s, primaryConstraint->body2->s );
4576
4577                         primaryConstraint->lm = primaryConstraint->s;
4578
4579                         if ( useSymmetry && body->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) {
4580                                 continue;
4581                         }
4582
4583                         if ( body->children.Num() ) {
4584                                 if ( !body->fl.isZero ) {
4585                                         body->s = body->invI * body->s;
4586                                 }
4587                                 body->J.MultiplySub( body->s, primaryConstraint->s );
4588                         }
4589                 } else if ( body->children.Num() ) {
4590                         body->s = body->invI * body->s;
4591                 }
4592         }
4593 }
4594
4595 /*
4596 ================
4597 idAFTree::Response
4598
4599   calculate body forces in the tree in response to a constraint force
4600 ================
4601 */
4602 void idAFTree::Response( const idAFConstraint *constraint, int row, int auxiliaryIndex ) const {
4603         int i, j;
4604         idAFBody *body;
4605         idAFConstraint *child, *primaryConstraint;
4606         idVecX v;
4607
4608         // if a single body don't waste time because there aren't any primary constraints
4609         if ( sortedBodies.Num() == 1 ) {
4610                 body = constraint->body1;
4611                 if ( body->tree == this ) {
4612                         body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row );
4613                         body->responseIndex[body->numResponses++] = auxiliaryIndex;
4614                 }
4615                 else {
4616                         body = constraint->body2;
4617                         body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row );
4618                         body->responseIndex[body->numResponses++] = auxiliaryIndex;
4619                 }
4620                 return;
4621         }
4622
4623         v.SetData( 6, VECX_ALLOCA( 6 ) );
4624
4625         // initialize right hand side to zero
4626         for ( i = 0; i < sortedBodies.Num(); i++ ) {
4627                 body = sortedBodies[i];
4628                 primaryConstraint = body->primaryConstraint;
4629                 if ( primaryConstraint ) {
4630                         primaryConstraint->s.Zero();
4631                         primaryConstraint->fl.isZero = true;
4632                 }
4633                 body->s.Zero();
4634                 body->fl.isZero = true;
4635                 body->GetResponseForce( body->numResponses ).Zero();
4636         }
4637
4638         // set right hand side for first constrained body
4639         body = constraint->body1;
4640         if ( body->tree == this ) {
4641                 body->InverseWorldSpatialInertiaMultiply( v, constraint->J1[row] );
4642                 primaryConstraint = body->primaryConstraint;
4643                 if ( primaryConstraint ) {
4644                         primaryConstraint->J1.Multiply( primaryConstraint->s, v );
4645                         primaryConstraint->fl.isZero = false;
4646                 }
4647                 for ( i = 0; i < body->children.Num(); i++ ) {
4648                         child = body->children[i]->primaryConstraint;
4649                         child->J2.Multiply( child->s, v );
4650                         child->fl.isZero = false;
4651                 }
4652                 body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row );
4653         }
4654
4655         // set right hand side for second constrained body
4656         body = constraint->body2;
4657         if ( body && body->tree == this ) {
4658                 body->InverseWorldSpatialInertiaMultiply( v, constraint->J2[row] );
4659                 primaryConstraint = body->primaryConstraint;
4660                 if ( primaryConstraint ) {
4661                         primaryConstraint->J1.MultiplyAdd( primaryConstraint->s, v );
4662                         primaryConstraint->fl.isZero = false;
4663                 }
4664                 for ( i = 0; i < body->children.Num(); i++ ) {
4665                         child = body->children[i]->primaryConstraint;
4666                         child->J2.MultiplyAdd( child->s, v );
4667                         child->fl.isZero = false;
4668                 }
4669                 body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row );
4670         }
4671
4672
4673         // solve for primary constraints
4674         Solve( auxiliaryIndex );
4675
4676         bool useSymmetry = af_useSymmetry.GetBool();
4677
4678         // store body forces in response to the constraint force
4679         idVecX force;
4680         for ( i = 0; i < sortedBodies.Num(); i++ ) {
4681                 body = sortedBodies[i];
4682
4683                 if ( useSymmetry && body->maxAuxiliaryIndex < auxiliaryIndex ) {
4684                         continue;
4685                 }
4686
4687                 force.SetData( 6, body->response + body->numResponses * 8 );
4688
4689                 // add forces of all primary constraints acting on this body
4690                 primaryConstraint = body->primaryConstraint;
4691                 if ( primaryConstraint ) {
4692                         primaryConstraint->J1.TransposeMultiplyAdd( force, primaryConstraint->lm );
4693                 }
4694                 for ( j = 0; j < body->children.Num(); j++ ) {
4695                         child = body->children[j]->primaryConstraint;
4696                         child->J2.TransposeMultiplyAdd( force, child->lm );
4697                 }
4698
4699                 body->responseIndex[body->numResponses++] = auxiliaryIndex;
4700         }
4701 }
4702
4703 /*
4704 ================
4705 idAFTree::CalculateForces
4706
4707   calculate forces on the bodies in the tree
4708 ================
4709 */
4710 void idAFTree::CalculateForces( float timeStep ) const {
4711         int i, j;
4712         float invStep;
4713         idAFBody *body;
4714         idAFConstraint *child, *c, *primaryConstraint;
4715
4716         // forces on bodies
4717         for ( i = 0; i < sortedBodies.Num(); i++ ) {
4718                 body = sortedBodies[i];
4719
4720                 body->totalForce.SubVec6(0) = body->current->externalForce + body->auxForce.SubVec6(0);
4721         }
4722
4723         // if a single body don't waste time because there aren't any primary constraints
4724         if ( sortedBodies.Num() == 1 ) {
4725                 return;
4726         }
4727
4728         invStep = 1.0f / timeStep;
4729
4730         // initialize right hand side
4731         for ( i = 0; i < sortedBodies.Num(); i++ ) {
4732                 body = sortedBodies[i];
4733
4734                 body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
4735                 body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep;
4736                 primaryConstraint = body->primaryConstraint;
4737                 if ( primaryConstraint ) {
4738                         // b = ( J * acc + c )
4739                         c = primaryConstraint;
4740                         c->s = c->J1 * c->body1->acceleration + c->J2 * c->body2->acceleration + invStep * ( c->c1 + c->c2 );
4741                         c->fl.isZero = false;
4742                 }
4743                 body->s.Zero();
4744                 body->fl.isZero = true;
4745         }
4746
4747         // solve for primary constraints
4748         Solve();
4749
4750         // calculate forces on bodies after applying primary constraints
4751         for ( i = 0; i < sortedBodies.Num(); i++ ) {
4752                 body = sortedBodies[i];
4753
4754                 // add forces of all primary constraints acting on this body
4755                 primaryConstraint = body->primaryConstraint;
4756                 if ( primaryConstraint ) {
4757                         primaryConstraint->J1.TransposeMultiplyAdd( body->totalForce, primaryConstraint->lm );
4758                 }
4759                 for ( j = 0; j < body->children.Num(); j++ ) {
4760                         child = body->children[j]->primaryConstraint;
4761                         child->J2.TransposeMultiplyAdd( body->totalForce, child->lm );
4762                 }
4763         }
4764 }
4765
4766 /*
4767 ================
4768 idAFTree::SetMaxSubTreeAuxiliaryIndex
4769 ================
4770 */
4771 void idAFTree::SetMaxSubTreeAuxiliaryIndex( void ) {
4772         int i, j;
4773         idAFBody *body, *child;
4774
4775         // from the leaves up towards the root
4776         for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
4777                 body = sortedBodies[i];
4778
4779                 body->maxSubTreeAuxiliaryIndex = body->maxAuxiliaryIndex;
4780                 for ( j = 0; j < body->children.Num(); j++ ) {
4781                         child = body->children[j];
4782                         if ( child->maxSubTreeAuxiliaryIndex > body->maxSubTreeAuxiliaryIndex ) {
4783                                 body->maxSubTreeAuxiliaryIndex = child->maxSubTreeAuxiliaryIndex;
4784                         }
4785                 }
4786         }
4787 }
4788
4789 /*
4790 ================
4791 idAFTree::SortBodies_r
4792 ================
4793 */
4794 void idAFTree::SortBodies_r( idList<idAFBody*>&sortedList, idAFBody *body ) {
4795         int i;
4796
4797         for ( i = 0; i < body->children.Num(); i++ ) {
4798                 sortedList.Append( body->children[i] );
4799         }
4800         for ( i = 0; i < body->children.Num(); i++ ) {
4801                 SortBodies_r( sortedList, body->children[i] );
4802         }
4803 }
4804
4805 /*
4806 ================
4807 idAFTree::SortBodies
4808
4809   sort body list to make sure parents come first
4810 ================
4811 */
4812 void idAFTree::SortBodies( void ) {
4813         int i;
4814         idAFBody *body;
4815
4816         // find the root
4817         for ( i = 0; i < sortedBodies.Num(); i++ ) {
4818                 if ( !sortedBodies[i]->parent ) {
4819                         break;
4820                 }
4821         }
4822
4823         if ( i >= sortedBodies.Num() ) {
4824                 gameLocal.Error( "Articulated figure tree has no root." );
4825         }
4826
4827         body = sortedBodies[i];
4828         sortedBodies.Clear();
4829         sortedBodies.Append( body );
4830         SortBodies_r( sortedBodies, body );
4831 }
4832
4833 /*
4834 ================
4835 idAFTree::DebugDraw
4836 ================
4837 */
4838 void idAFTree::DebugDraw( const idVec4 &color ) const {
4839         int i;
4840         idAFBody *body;
4841
4842         for ( i = 1; i < sortedBodies.Num(); i++ ) {
4843                 body = sortedBodies[i];
4844                 gameRenderWorld->DebugArrow( color, body->parent->current->worldOrigin, body->current->worldOrigin, 1 );
4845         }
4846 }
4847
4848
4849 //===============================================================
4850 //                                                        M
4851 //  idPhysics_AF                                         MrE
4852 //                                                        E
4853 //===============================================================
4854
4855 /*
4856 ================
4857 idPhysics_AF::EvaluateConstraints
4858 ================
4859 */
4860 void idPhysics_AF::EvaluateConstraints( float timeStep ) {
4861         int i;
4862         float invTimeStep;
4863         idAFBody *body;
4864         idAFConstraint *c;
4865
4866         invTimeStep = 1.0f / timeStep;
4867
4868         // setup the constraint equations for the current position and orientation of the bodies
4869         for ( i = 0; i < primaryConstraints.Num(); i++ ) {
4870                 c = primaryConstraints[i];
4871                 c->Evaluate( invTimeStep );
4872                 c->J = c->J2;
4873         }
4874         for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
4875                 auxiliaryConstraints[i]->Evaluate( invTimeStep );
4876         }
4877
4878         // add contact constraints to the list with frame constraints
4879         for ( i = 0; i < contactConstraints.Num(); i++ ) {
4880                 AddFrameConstraint( contactConstraints[i] );
4881         }
4882
4883         // setup body primary constraint matrix
4884         for ( i = 0; i < bodies.Num(); i++ ) {
4885                 body = bodies[i];
4886
4887                 if ( body->primaryConstraint ) {
4888                         body->J = body->primaryConstraint->J1.Transpose();
4889                 }
4890         }
4891 }
4892
4893 /*
4894 ================
4895 idPhysics_AF::EvaluateBodies
4896 ================
4897 */
4898 void idPhysics_AF::EvaluateBodies( float timeStep ) {
4899         int i;
4900         idAFBody *body;
4901         idMat3 axis;
4902
4903         for ( i = 0; i < bodies.Num(); i++ ) {
4904                 body = bodies[i];
4905
4906                 // we transpose the axis before using it because idMat3 is column-major
4907                 axis = body->current->worldAxis.Transpose();
4908
4909                 // if the center of mass is at the body point of reference
4910                 if ( body->centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) {
4911
4912                         // spatial inertia in world space
4913                         body->I.Set( body->mass * mat3_identity, mat3_zero,
4914                                                         mat3_zero, axis * body->inertiaTensor * axis.Transpose() );
4915
4916                         // inverse spatial inertia in world space
4917                         body->inverseWorldSpatialInertia.Set( body->invMass * mat3_identity, mat3_zero,
4918                                                                                         mat3_zero, axis * body->inverseInertiaTensor * axis.Transpose() );
4919
4920                         body->fl.spatialInertiaSparse = true;
4921                 }
4922                 else {
4923                         idMat3 massMoment = body->mass * SkewSymmetric( body->centerOfMass );
4924
4925                         // spatial inertia in world space
4926                         body->I.Set( body->mass * mat3_identity, massMoment,
4927                                                                 massMoment.Transpose(), axis * body->inertiaTensor * axis.Transpose() );
4928
4929                         // inverse spatial inertia in world space
4930                         body->inverseWorldSpatialInertia = body->I.InverseFast();
4931
4932                         body->fl.spatialInertiaSparse = false;
4933                 }
4934
4935                 // initialize auxiliary constraint force to zero
4936                 body->auxForce.Zero();
4937         }
4938 }
4939
4940 /*
4941 ================
4942 idPhysics_AF::AddFrameConstraints
4943 ================
4944 */
4945 void idPhysics_AF::AddFrameConstraints( void ) {
4946         int i;
4947
4948         // add frame constraints to auxiliary constraints
4949         for ( i = 0; i < frameConstraints.Num(); i++ ) {
4950                 auxiliaryConstraints.Append( frameConstraints[i] );
4951         }
4952 }
4953
4954 /*
4955 ================
4956 idPhysics_AF::RemoveFrameConstraints
4957 ================
4958 */
4959 void idPhysics_AF::RemoveFrameConstraints( void ) {
4960         // remove all the frame constraints from the auxiliary constraints
4961         auxiliaryConstraints.SetNum( auxiliaryConstraints.Num() - frameConstraints.Num(), false );
4962         frameConstraints.SetNum( 0, false );
4963 }
4964
4965 /*
4966 ================
4967 idPhysics_AF::ApplyFriction
4968 ================
4969 */
4970 void idPhysics_AF::ApplyFriction( float timeStep, float endTimeMSec ) {
4971         int i;
4972         float invTimeStep;
4973
4974         if ( af_skipFriction.GetBool() ) {
4975                 return;
4976         }
4977
4978         if ( jointFrictionDentStart < MS2SEC( endTimeMSec ) && jointFrictionDentEnd > MS2SEC( endTimeMSec ) ) {
4979                 float halfTime = ( jointFrictionDentEnd - jointFrictionDentStart ) * 0.5f;
4980                 if ( jointFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) {
4981                         jointFrictionDentScale = 1.0f - ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart ) / halfTime;
4982                 } else {
4983                         jointFrictionDentScale = jointFrictionDent + ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart - halfTime ) / halfTime;
4984                 }
4985         } else {
4986                 jointFrictionDentScale = 0.0f;
4987         }
4988
4989         if ( contactFrictionDentStart < MS2SEC( endTimeMSec ) && contactFrictionDentEnd > MS2SEC( endTimeMSec ) ) {
4990                 float halfTime = ( contactFrictionDentEnd - contactFrictionDentStart ) * 0.5f;
4991                 if ( contactFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) {
4992                         contactFrictionDentScale = 1.0f - ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart ) / halfTime;
4993                 } else {
4994                         contactFrictionDentScale = contactFrictionDent + ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart - halfTime ) / halfTime;
4995                 }
4996         } else {
4997                 contactFrictionDentScale = 0.0f;
4998         }
4999
5000         invTimeStep = 1.0f / timeStep;
5001
5002         for ( i = 0; i < primaryConstraints.Num(); i++ ) {
5003                 primaryConstraints[i]->ApplyFriction( invTimeStep );
5004         }
5005         for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5006                 auxiliaryConstraints[i]->ApplyFriction( invTimeStep );
5007         }
5008         for ( i = 0; i < frameConstraints.Num(); i++ ) {
5009                 frameConstraints[i]->ApplyFriction( invTimeStep );
5010         }
5011 }
5012
5013 /*
5014 ================
5015 idPhysics_AF::PrimaryFactor
5016 ================
5017 */
5018 void idPhysics_AF::PrimaryFactor( void ) {
5019         int i;
5020
5021         for ( i = 0; i < trees.Num(); i++ ) {
5022                 trees[i]->Factor();
5023         }
5024 }
5025
5026 /*
5027 ================
5028 idPhysics_AF::PrimaryForces
5029 ================
5030 */
5031 void idPhysics_AF::PrimaryForces( float timeStep ) {
5032         int i;
5033
5034         for ( i = 0; i < trees.Num(); i++ ) {
5035                 trees[i]->CalculateForces( timeStep );
5036         }
5037 }
5038
5039 /*
5040 ================
5041 idPhysics_AF::AuxiliaryForces
5042 ================
5043 */
5044 void idPhysics_AF::AuxiliaryForces( float timeStep ) {
5045         int i, j, k, l, n, m, s, numAuxConstraints, *index, *boxIndex;
5046         float *ptr, *j1, *j2, *dstPtr, *forcePtr;
5047         float invStep, u;
5048         idAFBody *body;
5049         idAFConstraint *constraint;
5050         idVecX tmp;
5051         idMatX jmk;
5052         idVecX rhs, w, lm, lo, hi;
5053
5054         // get the number of one dimensional auxiliary constraints
5055         for ( numAuxConstraints = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5056                 numAuxConstraints += auxiliaryConstraints[i]->J1.GetNumRows();
5057         }
5058
5059         if ( numAuxConstraints == 0 ) {
5060                 return;
5061         }
5062
5063         // allocate memory to store the body response to auxiliary constraint forces
5064         forcePtr = (float *) _alloca16( bodies.Num() * numAuxConstraints * 8 * sizeof( float ) );
5065         index = (int *) _alloca16( bodies.Num() * numAuxConstraints * sizeof( int ) );
5066         for ( i = 0; i < bodies.Num(); i++ ) {
5067                 body = bodies[i];
5068                 body->response = forcePtr;
5069                 body->responseIndex = index;
5070                 body->numResponses = 0;
5071                 body->maxAuxiliaryIndex = 0;
5072                 forcePtr += numAuxConstraints * 8;
5073                 index += numAuxConstraints;
5074         }
5075
5076         // set on each body the largest index of an auxiliary constraint constraining the body
5077         if ( af_useSymmetry.GetBool() ) {
5078                 for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5079                         constraint = auxiliaryConstraints[i];
5080                         for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5081                                 if ( k > constraint->body1->maxAuxiliaryIndex ) {
5082                                         constraint->body1->maxAuxiliaryIndex = k;
5083                                 }
5084                                 if ( constraint->body2 && k > constraint->body2->maxAuxiliaryIndex ) {
5085                                         constraint->body2->maxAuxiliaryIndex = k;
5086                                 }
5087                         }
5088                 }
5089                 for ( i = 0; i < trees.Num(); i++ ) {
5090                         trees[i]->SetMaxSubTreeAuxiliaryIndex();
5091                 }
5092         }
5093
5094         // calculate forces of primary constraints in response to the auxiliary constraint forces
5095         for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5096                 constraint = auxiliaryConstraints[i];
5097
5098                 for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5099
5100                         // calculate body forces in the tree in response to the constraint force
5101                         constraint->body1->tree->Response( constraint, j, k );
5102                         // if there is a second body which is part of a different tree
5103                         if ( constraint->body2 && constraint->body2->tree != constraint->body1->tree ) {
5104                                 // calculate body forces in the second tree in response to the constraint force
5105                                 constraint->body2->tree->Response( constraint, j, k );
5106                         }
5107                 }
5108         }
5109
5110         // NOTE: the rows are 16 byte padded
5111         jmk.SetData( numAuxConstraints, ((numAuxConstraints+3)&~3), MATX_ALLOCA( numAuxConstraints * ((numAuxConstraints+3)&~3) ) );
5112         tmp.SetData( 6, VECX_ALLOCA( 6 ) );
5113
5114         // create constraint matrix for auxiliary constraints using a mass matrix adjusted for the primary constraints
5115         for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5116                 constraint = auxiliaryConstraints[i];
5117
5118                 for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5119                         constraint->body1->InverseWorldSpatialInertiaMultiply( tmp, constraint->J1[j] );
5120                         j1 = tmp.ToFloatPtr();
5121                         ptr = constraint->body1->response;
5122                         index = constraint->body1->responseIndex;
5123                         dstPtr = jmk[k];
5124                         s = af_useSymmetry.GetBool() ? k + 1 : numAuxConstraints;
5125                         for ( l = n = 0, m = index[n]; n < constraint->body1->numResponses && m < s; n++, m = index[n] ) {
5126                                 while( l < m ) {
5127                                         dstPtr[l++] = 0.0f;
5128                                 }
5129                                 dstPtr[l++] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] +
5130                                                                 j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5];
5131                                 ptr += 8;
5132                         }
5133
5134                         while( l < s ) {
5135                                 dstPtr[l++] = 0.0f;
5136                         }
5137
5138                         if ( constraint->body2 ) {
5139                                 constraint->body2->InverseWorldSpatialInertiaMultiply( tmp, constraint->J2[j] );
5140                                 j2 = tmp.ToFloatPtr();
5141                                 ptr = constraint->body2->response;
5142                                 index = constraint->body2->responseIndex;
5143                                 for ( n = 0, m = index[n]; n < constraint->body2->numResponses && m < s; n++, m = index[n] ) {
5144                                         dstPtr[m] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] +
5145                                                                                 j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5];
5146                                         ptr += 8;
5147                                 }
5148                         }
5149                 }
5150         }
5151
5152         if ( af_useSymmetry.GetBool() ) {
5153                 n = jmk.GetNumColumns();
5154                 for ( i = 0; i < numAuxConstraints; i++ ) {
5155                         ptr = jmk.ToFloatPtr() + ( i + 1 ) * n + i;
5156                         dstPtr = jmk.ToFloatPtr() + i * n + i + 1;
5157                         for ( j = i+1; j < numAuxConstraints; j++ ) {
5158                                 *dstPtr++ = *ptr;
5159                                 ptr += n;
5160                         }
5161                 }
5162         }
5163
5164         invStep = 1.0f / timeStep;
5165
5166         // calculate body acceleration
5167         for ( i = 0; i < bodies.Num(); i++ ) {
5168                 body = bodies[i];
5169                 body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
5170                 body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep;
5171         }
5172
5173         rhs.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
5174         lo.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
5175         hi.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
5176         lm.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
5177         boxIndex = (int *) _alloca16( numAuxConstraints * sizeof( int ) );
5178
5179         // set first index for special box constrained variables
5180         for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5181                 auxiliaryConstraints[i]->firstIndex = k;
5182                 k += auxiliaryConstraints[i]->J1.GetNumRows();
5183         }
5184
5185         // initialize right hand side and low and high bounds for auxiliary constraints
5186         for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5187                 constraint = auxiliaryConstraints[i];
5188                 n = k;
5189
5190                 for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5191
5192                         j1 = constraint->J1[j];
5193                         ptr = constraint->body1->acceleration.ToFloatPtr();
5194                         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];
5195                         rhs[k] += constraint->c1[j] * invStep;
5196
5197                         if ( constraint->body2 ) {
5198                                 j2 = constraint->J2[j];
5199                                 ptr = constraint->body2->acceleration.ToFloatPtr();
5200                                 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];
5201                                 rhs[k] += constraint->c2[j] * invStep;
5202                         }
5203
5204                         rhs[k] = -rhs[k];
5205                         lo[k] = constraint->lo[j];
5206                         hi[k] = constraint->hi[j];
5207
5208                         if ( constraint->boxIndex[j] >= 0 ) {
5209                                 if ( constraint->boxConstraint->fl.isPrimary ) {
5210                                         gameLocal.Error( "cannot reference primary constraints for the box index" );
5211                                 }
5212                                 boxIndex[k] = constraint->boxConstraint->firstIndex + constraint->boxIndex[j];
5213                         }
5214                         else {
5215                                 boxIndex[k] = -1;
5216                         }
5217                         jmk[k][k] += constraint->e[j] * invStep;
5218                 }
5219         }
5220
5221 #ifdef AF_TIMINGS
5222         timer_lcp.Start();
5223 #endif
5224
5225         // calculate lagrange multipliers for auxiliary constraints
5226         if ( !lcp->Solve( jmk, lm, rhs, lo, hi, boxIndex ) ) {
5227                 return;         // bad monkey!
5228         }
5229
5230 #ifdef AF_TIMINGS
5231         timer_lcp.Stop();
5232 #endif
5233
5234         // calculate auxiliary constraint forces
5235         for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
5236                 constraint = auxiliaryConstraints[i];
5237
5238                 for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
5239                         constraint->lm[j] = u = lm[k];
5240
5241                         j1 = constraint->J1[j];
5242                         ptr = constraint->body1->auxForce.ToFloatPtr();
5243                         ptr[0] += j1[0] * u; ptr[1] += j1[1] * u; ptr[2] += j1[2] * u;
5244                         ptr[3] += j1[3] * u; ptr[4] += j1[4] * u; ptr[5] += j1[5] * u;
5245
5246                         if ( constraint->body2 ) {
5247                                 j2 = constraint->J2[j];
5248                                 ptr = constraint->body2->auxForce.ToFloatPtr();
5249                                 ptr[0] += j2[0] * u; ptr[1] += j2[1] * u; ptr[2] += j2[2] * u;
5250                                 ptr[3] += j2[3] * u; ptr[4] += j2[4] * u; ptr[5] += j2[5] * u;
5251                         }
5252                 }
5253         }
5254
5255         // recalculate primary constraint forces in response to auxiliary constraint forces
5256         PrimaryForces( timeStep );
5257
5258         // clear pointers pointing to stack space so tools don't get confused
5259         for ( i = 0; i < bodies.Num(); i++ ) {
5260                 body = bodies[i];
5261                 body->response = NULL;
5262                 body->responseIndex = NULL;
5263         }
5264 }
5265
5266 /*
5267 ================
5268 idPhysics_AF::VerifyContactConstraints
5269 ================
5270 */
5271 void idPhysics_AF::VerifyContactConstraints( void ) {
5272 #if 0
5273         int i;
5274         float impulseNumerator, impulseDenominator;
5275         idVec3 r, velocity, normalVelocity, normal, impulse;
5276         idAFBody *body;
5277
5278         for ( i = 0; i < contactConstraints.Num(); i++ ) {
5279                 body = contactConstraints[i]->body1;
5280                 const contactInfo_t &contact = contactConstraints[i]->GetContact();
5281
5282                 r = contact.point - body->GetCenterOfMass();
5283
5284                 // calculate velocity at contact point
5285                 velocity = body->GetLinearVelocity() + body->GetAngularVelocity().Cross( r );
5286
5287                 // velocity along normal vector
5288                 normalVelocity = ( velocity * contact.normal ) * contact.normal;
5289
5290                 // if moving towards the surface at the contact point
5291                 if ( normalVelocity * contact.normal < 0.0f ) {
5292                         // calculate impulse
5293                         normal = -normalVelocity;
5294                         impulseNumerator = normal.Normalize();
5295                         impulseDenominator = body->GetInverseMass() + ( ( body->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal );
5296                         impulse = (impulseNumerator / impulseDenominator) * normal * 1.0001f;
5297
5298                         // apply impulse
5299                         body->SetLinearVelocity( body->GetLinearVelocity() + impulse );
5300                         body->SetAngularVelocity( body->GetAngularVelocity() + r.Cross( impulse ) );
5301                 }
5302         }
5303 #else
5304         int i;
5305         idAFBody *body;
5306         idVec3 normal;
5307
5308         for ( i = 0; i < contactConstraints.Num(); i++ ) {
5309                 body = contactConstraints[i]->body1;
5310                 normal = contactConstraints[i]->GetContact().normal;
5311                 if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) {
5312                         body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal;
5313                 }
5314                 body = contactConstraints[i]->body2;
5315                 if ( !body ) {
5316                         continue;
5317                 }
5318                 normal = -normal;
5319                 if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) {
5320                         body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal;
5321                 }
5322         }
5323 #endif
5324 }
5325
5326 /*
5327 ================
5328 idPhysics_AF::Evolve
5329 ================
5330 */
5331 void idPhysics_AF::Evolve( float timeStep ) {
5332         int i;
5333         float angle;
5334         idVec3 vec;
5335         idAFBody *body;
5336         idVec6 force;
5337         idRotation rotation;
5338         float vSqr, maxLinearVelocity, maxAngularVelocity;
5339
5340         maxLinearVelocity = af_maxLinearVelocity.GetFloat() / timeStep;
5341         maxAngularVelocity = af_maxAngularVelocity.GetFloat() / timeStep;
5342
5343         for ( i = 0; i < bodies.Num(); i++ ) {
5344                 body = bodies[i];
5345
5346                 // calculate the spatial velocity for the next physics state
5347                 body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
5348                 body->next->spatialVelocity = body->current->spatialVelocity + timeStep * body->acceleration.SubVec6(0);
5349
5350                 if ( maxLinearVelocity > 0.0f ) {
5351                         // cap the linear velocity
5352                         vSqr = body->next->spatialVelocity.SubVec3(0).LengthSqr();
5353                         if ( vSqr > Square( maxLinearVelocity ) ) {
5354                                 body->next->spatialVelocity.SubVec3(0) *= idMath::InvSqrt( vSqr ) * maxLinearVelocity;
5355                         }
5356                 }
5357
5358                 if ( maxAngularVelocity > 0.0f ) {
5359                         // cap the angular velocity
5360                         vSqr = body->next->spatialVelocity.SubVec3(1).LengthSqr();
5361                         if ( vSqr > Square( maxAngularVelocity ) ) {
5362                                 body->next->spatialVelocity.SubVec3(1) *= idMath::InvSqrt( vSqr ) * maxAngularVelocity;
5363                         }
5364                 }
5365         }
5366
5367         // make absolutely sure all contact constraints are satisfied
5368         VerifyContactConstraints();
5369
5370         // calculate the position of the bodies for the next physics state
5371         for ( i = 0; i < bodies.Num(); i++ ) {
5372                 body = bodies[i];
5373
5374                 // translate world origin
5375                 body->next->worldOrigin = body->current->worldOrigin + timeStep * body->next->spatialVelocity.SubVec3( 0 );
5376
5377                 // convert angular velocity to a rotation matrix
5378                 vec = body->next->spatialVelocity.SubVec3( 1 );
5379                 angle = -timeStep * (float) RAD2DEG( vec.Normalize() );
5380                 rotation = idRotation( vec3_origin, vec, angle );
5381                 rotation.Normalize180();
5382
5383                 // rotate world axis
5384                 body->next->worldAxis = body->current->worldAxis * rotation.ToMat3();
5385                 body->next->worldAxis.OrthoNormalizeSelf();
5386
5387                 // linear and angular friction
5388                 body->next->spatialVelocity.SubVec3(0) -= body->linearFriction * body->next->spatialVelocity.SubVec3(0);
5389                 body->next->spatialVelocity.SubVec3(1) -= body->angularFriction * body->next->spatialVelocity.SubVec3(1);
5390         }
5391 }
5392
5393 /*
5394 ================
5395 idPhysics_AF::CollisionImpulse
5396
5397   apply impulse to the colliding bodies
5398   the current state of the body should be set to the moment of impact
5399   this is silly as it doesn't take the AF structure into account
5400 ================
5401 */
5402 bool idPhysics_AF::CollisionImpulse( float timeStep, idAFBody *body, trace_t &collision ) {
5403         idVec3 r, velocity, impulse;
5404         idMat3 inverseWorldInertiaTensor;
5405         float impulseNumerator, impulseDenominator;
5406         impactInfo_t info;
5407         idEntity *ent;
5408
5409         ent = gameLocal.entities[collision.c.entityNum];
5410         if ( ent == self ) {
5411                 return false;
5412         }
5413
5414         // get info from other entity involved
5415         ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info );
5416         // collision point relative to the body center of mass
5417         r = collision.c.point - (body->current->worldOrigin + body->centerOfMass * body->current->worldAxis);
5418         // the velocity at the collision point
5419         velocity = body->current->spatialVelocity.SubVec3(0) + body->current->spatialVelocity.SubVec3(1).Cross(r);
5420         // subtract velocity of other entity
5421         velocity -= info.velocity;
5422         // never stick
5423         if ( velocity * collision.c.normal > 0.0f ) {
5424                 velocity = collision.c.normal;
5425         }
5426         inverseWorldInertiaTensor = body->current->worldAxis.Transpose() * body->inverseInertiaTensor * body->current->worldAxis;
5427         impulseNumerator = -( 1.0f + body->bouncyness ) * ( velocity * collision.c.normal );
5428         impulseDenominator = body->invMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal );
5429         if ( info.invMass ) {
5430                 impulseDenominator += info.invMass + ( ( info.invInertiaTensor * info.position.Cross( collision.c.normal ) ).Cross( info.position ) * collision.c.normal );
5431         }
5432         impulse = (impulseNumerator / impulseDenominator) * collision.c.normal;
5433
5434         // apply impact to other entity
5435         ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse );
5436
5437         // callback to self to let the entity know about the impact
5438         return self->Collide( collision, velocity );
5439 }
5440
5441 /*
5442 ================
5443 idPhysics_AF::ApplyCollisions
5444 ================
5445 */
5446 bool idPhysics_AF::ApplyCollisions( float timeStep ) {
5447         int i;
5448
5449         for ( i = 0; i < collisions.Num(); i++ ) {
5450                 if ( CollisionImpulse( timeStep, collisions[i].body, collisions[i].trace ) ) {
5451                         return true;
5452                 }
5453         }
5454         return false;
5455 }
5456
5457 /*
5458 ================
5459 idPhysics_AF::SetupCollisionForBody
5460 ================
5461 */
5462 idEntity *idPhysics_AF::SetupCollisionForBody( idAFBody *body ) const {
5463         int i;
5464         idAFBody *b;
5465         idEntity *passEntity;
5466
5467         passEntity = NULL;
5468
5469         if ( !selfCollision || !body->fl.selfCollision || af_skipSelfCollision.GetBool() ) {
5470
5471                 // disable all bodies
5472                 for ( i = 0; i < bodies.Num(); i++ ) {
5473                         bodies[i]->clipModel->Disable();
5474                 }
5475
5476                 // don't collide with world collision model if attached to the world
5477                 for ( i = 0; i < body->constraints.Num(); i++ ) {
5478                         if ( !body->constraints[i]->fl.noCollision ) {
5479                                 continue;
5480                         }
5481                         // if this constraint attaches the body to the world
5482                         if ( body->constraints[i]->body2 == NULL ) {
5483                                 // don't collide with the world collision model
5484                                 passEntity = gameLocal.world;
5485                         }
5486                 }
5487
5488         } else {
5489
5490                 // enable all bodies that have self collision
5491                 for ( i = 0; i < bodies.Num(); i++ ) {
5492                         if ( bodies[i]->fl.selfCollision ) {
5493                                 bodies[i]->clipModel->Enable();
5494                         } else {
5495                                 bodies[i]->clipModel->Disable();
5496                         }
5497                 }
5498
5499                 // don't let the body collide with itself
5500                 body->clipModel->Disable();
5501
5502                 // disable any bodies attached with constraints
5503                 for ( i = 0; i < body->constraints.Num(); i++ ) {
5504                         if ( !body->constraints[i]->fl.noCollision ) {
5505                                 continue;
5506                         }
5507                         // if this constraint attaches the body to the world
5508                         if ( body->constraints[i]->body2 == NULL ) {
5509                                 // don't collide with the world collision model
5510                                 passEntity = gameLocal.world;
5511                         } else {
5512                                 if ( body->constraints[i]->body1 == body ) {
5513                                         b = body->constraints[i]->body2;
5514                                 } else if ( body->constraints[i]->body2 == body ) {
5515                                         b = body->constraints[i]->body1;
5516                                 } else {
5517                                         continue;
5518                                 }
5519                                 // don't collide with this body
5520                                 b->clipModel->Disable();
5521                         }
5522                 }
5523         }
5524
5525         return passEntity;
5526 }
5527
5528 /*
5529 ================
5530 idPhysics_AF::CheckForCollisions
5531
5532   check for collisions between the current and next state
5533   if there is a collision the next state is set to the state at the moment of impact
5534   assumes all bodies are linked for collision detection and relinks all bodies after moving them
5535 ================
5536 */
5537 void idPhysics_AF::CheckForCollisions( float timeStep ) {
5538 //      #define TEST_COLLISION_DETECTION
5539         int i, index;
5540         idAFBody *body;
5541         idMat3 axis;
5542         idRotation rotation;
5543         trace_t collision;
5544         idEntity *passEntity;
5545
5546         // clear list with collisions
5547         collisions.SetNum( 0, false );
5548
5549         if ( !enableCollision ) {
5550                 return;
5551         }
5552
5553         for ( i = 0; i < bodies.Num(); i++ ) {
5554                 body = bodies[i];
5555
5556                 if ( body->clipMask != 0 ) {
5557
5558                         passEntity = SetupCollisionForBody( body );
5559
5560 #ifdef TEST_COLLISION_DETECTION
5561                         bool startsolid = false;
5562                         if ( gameLocal.clip.Contents( body->current->worldOrigin, body->clipModel,
5563                                                                                                                         body->current->worldAxis, body->clipMask, passEntity ) ) {
5564                                 startsolid = true;
5565                         }
5566 #endif
5567
5568                         TransposeMultiply( body->current->worldAxis, body->next->worldAxis, axis );
5569                         rotation = axis.ToRotation();
5570                         rotation.SetOrigin( body->current->worldOrigin );
5571
5572                         // if there was a collision
5573                         if ( gameLocal.clip.Motion( collision, body->current->worldOrigin, body->next->worldOrigin, rotation,
5574                                                                                 body->clipModel, body->current->worldAxis, body->clipMask, passEntity ) ) {
5575
5576                                 // set the next state to the state at the moment of impact
5577                                 body->next->worldOrigin = collision.endpos;
5578                                 body->next->worldAxis = collision.endAxis;
5579
5580                                 // add collision to the list
5581                                 index = collisions.Num();
5582                                 collisions.SetNum( index + 1, false );
5583                                 collisions[index].trace = collision;
5584                                 collisions[index].body = body;
5585                         }
5586
5587 #ifdef TEST_COLLISION_DETECTION
5588                         if ( gameLocal.clip.Contents( body->next->worldOrigin, body->clipModel,
5589                                                                                                                 body->next->worldAxis, body->clipMask, passEntity ) ) {
5590                                 if ( !startsolid ) {
5591                                         int bah = 1;
5592                                 }
5593                         }
5594 #endif
5595                 }
5596
5597                 body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->next->worldOrigin, body->next->worldAxis );
5598         }
5599 }
5600
5601 /*
5602 ================
5603 idPhysics_AF::EvaluateContacts
5604 ================
5605 */
5606 bool idPhysics_AF::EvaluateContacts( void ) {
5607         int i, j, k, numContacts, numBodyContacts;
5608         idAFBody *body;
5609         contactInfo_t contactInfo[10];
5610         idEntity *passEntity;
5611         idVecX dir( 6, VECX_ALLOCA( 6 ) );
5612
5613         // evaluate bodies
5614         EvaluateBodies( current.lastTimeStep );
5615
5616         // remove all existing contacts
5617         ClearContacts();
5618
5619         contactBodies.SetNum( 0, false );
5620
5621         if ( !enableCollision ) {
5622                 return false;
5623         }
5624
5625         // find all the contacts
5626         for ( i = 0; i < bodies.Num(); i++ ) {
5627                 body = bodies[i];
5628
5629                 if ( body->clipMask == 0 ) {
5630                         continue;
5631                 }
5632
5633                 passEntity = SetupCollisionForBody( body );
5634
5635                 body->InverseWorldSpatialInertiaMultiply( dir, body->current->externalForce.ToFloatPtr() );
5636                 dir.SubVec6(0) = body->current->spatialVelocity + current.lastTimeStep * dir.SubVec6(0);
5637                 dir.SubVec3(0).Normalize();
5638                 dir.SubVec3(1).Normalize();
5639
5640                 numContacts = gameLocal.clip.Contacts( contactInfo, 10, body->current->worldOrigin, dir.SubVec6(0), 2.0f, //CONTACT_EPSILON,
5641                                                 body->clipModel, body->current->worldAxis, body->clipMask, passEntity );
5642
5643 #if 1
5644                 // merge nearby contacts between the same bodies
5645                 // and assure there are at most three planar contacts between any pair of bodies
5646                 for ( j = 0; j < numContacts; j++ ) {
5647
5648                         numBodyContacts = 0;
5649                         for ( k = 0; k < contacts.Num(); k++ ) {
5650                                 if ( contacts[k].entityNum == contactInfo[j].entityNum ) {
5651                                         if ( ( contacts[k].id == i && contactInfo[j].id == contactBodies[k] ) ||
5652                                                         ( contactBodies[k] == i && contacts[k].id == contactInfo[j].id ) ) {
5653
5654                                                 if ( ( contacts[k].point - contactInfo[j].point ).LengthSqr() < Square( 2.0f ) ) {
5655                                                         break;
5656                                                 }
5657                                                 if ( idMath::Fabs( contacts[k].normal * contactInfo[j].normal ) > 0.9f ) {
5658                                                         numBodyContacts++;
5659                                                 }
5660                                         }
5661                                 }
5662                         }
5663
5664                         if ( k >= contacts.Num() && numBodyContacts < 3 ) {
5665                                 contacts.Append( contactInfo[j] );
5666                                 contactBodies.Append( i );
5667                         }
5668                 }
5669
5670 #else
5671
5672                 for ( j = 0; j < numContacts; j++ ) {
5673                         contacts.Append( contactInfo[j] );
5674                         contactBodies.Append( i );
5675                 }
5676 #endif
5677
5678         }
5679
5680         AddContactEntitiesForContacts();
5681
5682         return ( contacts.Num() != 0 );
5683 }
5684
5685 /*
5686 ================
5687 idPhysics_AF::SetupContactConstraints
5688 ================
5689 */
5690 void idPhysics_AF::SetupContactConstraints( void ) {
5691         int i;
5692
5693         // make sure enough contact constraints are allocated
5694         contactConstraints.AssureSizeAlloc( contacts.Num(), idListNewElement<idAFConstraint_Contact> );
5695         contactConstraints.SetNum( contacts.Num(), false );
5696
5697         // setup contact constraints
5698         for ( i = 0; i < contacts.Num(); i++ ) {
5699                 // add contact constraint
5700                 contactConstraints[i]->physics = this;
5701                 if ( contacts[i].entityNum == self->entityNumber ) {
5702                         contactConstraints[i]->Setup( bodies[contactBodies[i]], bodies[ contacts[i].id ], contacts[i] );
5703                 }
5704                 else {
5705                         contactConstraints[i]->Setup( bodies[contactBodies[i]], NULL, contacts[i] );
5706                 }
5707         }
5708 }
5709
5710 /*
5711 ================
5712 idPhysics_AF::ApplyContactForces
5713 ================
5714 */
5715 void idPhysics_AF::ApplyContactForces( void ) {
5716 #if 0
5717         int i;
5718         idEntity *ent;
5719         idVec3 force;
5720
5721         for ( i = 0; i < contactConstraints.Num(); i++ ) {
5722                 if ( contactConstraints[i]->body2 != NULL ) {
5723                         continue;
5724                 }
5725                 const contactInfo_t &contact = contactConstraints[i]->GetContact();
5726                 ent = gameLocal.entities[contact.entityNum];
5727                 if ( !ent ) {
5728                         continue;
5729                 }
5730                 force.Zero();
5731                 ent->AddForce( self, contact.id, contact.point, force );
5732         }
5733 #endif
5734 }
5735
5736 /*
5737 ================
5738 idPhysics_AF::ClearExternalForce
5739 ================
5740 */
5741 void idPhysics_AF::ClearExternalForce( void ) {
5742         int i;
5743         idAFBody *body;
5744
5745         for ( i = 0; i < bodies.Num(); i++ ) {
5746                 body = bodies[i];
5747
5748                 // clear external force
5749                 body->current->externalForce.Zero();
5750                 body->next->externalForce.Zero();
5751         }
5752 }
5753
5754 /*
5755 ================
5756 idPhysics_AF::AddGravity
5757 ================
5758 */
5759 void idPhysics_AF::AddGravity( void ) {
5760         int i;
5761         idAFBody *body;
5762
5763         for ( i = 0; i < bodies.Num(); i++ ) {
5764                 body = bodies[i];
5765                 // add gravitational force
5766                 body->current->externalForce.SubVec3( 0 ) += body->mass * gravityVector;
5767         }
5768 }
5769
5770 /*
5771 ================
5772 idPhysics_AF::SwapStates
5773 ================
5774 */
5775 void idPhysics_AF::SwapStates( void ) {
5776         int i;
5777         idAFBody *body;
5778         AFBodyPState_t *swap;
5779
5780         for ( i = 0; i < bodies.Num(); i++ ) {
5781
5782                 body = bodies[i];
5783
5784                 // swap the current and next state for next simulation step
5785                 swap = body->current;
5786                 body->current = body->next;
5787                 body->next = swap;
5788         }
5789 }
5790
5791 /*
5792 ================
5793 idPhysics_AF::UpdateClipModels
5794 ================
5795 */
5796 void idPhysics_AF::UpdateClipModels( void ) {
5797         int i;
5798         idAFBody *body;
5799
5800         for ( i = 0; i < bodies.Num(); i++ ) {
5801                 body = bodies[i];
5802                 body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->current->worldOrigin, body->current->worldAxis );
5803         }
5804 }
5805
5806 /*
5807 ================
5808 idPhysics_AF::SetSuspendSpeed
5809 ================
5810 */
5811 void idPhysics_AF::SetSuspendSpeed( const idVec2 &velocity, const idVec2 &acceleration ) {
5812         this->suspendVelocity = velocity;
5813         this->suspendAcceleration = acceleration;
5814 }
5815
5816 /*
5817 ================
5818 idPhysics_AF::SetSuspendTime
5819 ================
5820 */
5821 void idPhysics_AF::SetSuspendTime( const float minTime, const float maxTime ) {
5822         this->minMoveTime = minTime;
5823         this->maxMoveTime = maxTime;
5824 }
5825
5826 /*
5827 ================
5828 idPhysics_AF::SetSuspendTolerance
5829 ================
5830 */
5831 void idPhysics_AF::SetSuspendTolerance( const float noMoveTime, const float noMoveTranslation, const float noMoveRotation ) {
5832         this->noMoveTime = noMoveTime;
5833         this->noMoveTranslation = noMoveTranslation;
5834         this->noMoveRotation = noMoveRotation;
5835 }
5836
5837 /*
5838 ================
5839 idPhysics_AF::SetTimeScaleRamp
5840 ================
5841 */
5842 void idPhysics_AF::SetTimeScaleRamp( const float start, const float end ) {
5843         timeScaleRampStart = start;
5844         timeScaleRampEnd = end;
5845 }
5846
5847 /*
5848 ================
5849 idPhysics_AF::SetJointFrictionDent
5850 ================
5851 */
5852 void idPhysics_AF::SetJointFrictionDent( const float dent, const float start, const float end ) {
5853         jointFrictionDent = dent;
5854         jointFrictionDentStart = start;
5855         jointFrictionDentEnd = end;
5856 }
5857
5858 /*
5859 ================
5860 idPhysics_AF::GetJointFrictionScale
5861 ================
5862 */
5863 float idPhysics_AF::GetJointFrictionScale( void ) const {
5864         if ( jointFrictionDentScale > 0.0f ) {
5865                 return jointFrictionDentScale;
5866         } else if ( jointFrictionScale > 0.0f ) {
5867                 return jointFrictionScale;
5868         } else if ( af_jointFrictionScale.GetFloat() > 0.0f ) {
5869                 return af_jointFrictionScale.GetFloat();
5870         }
5871         return 1.0f;
5872 }
5873
5874 /*
5875 ================
5876 idPhysics_AF::SetContactFrictionDent
5877 ================
5878 */
5879 void idPhysics_AF::SetContactFrictionDent( const float dent, const float start, const float end ) {
5880         contactFrictionDent = dent;
5881         contactFrictionDentStart = start;
5882         contactFrictionDentEnd = end;
5883 }
5884
5885 /*
5886 ================
5887 idPhysics_AF::GetContactFrictionScale
5888 ================
5889 */
5890 float idPhysics_AF::GetContactFrictionScale( void ) const {
5891         if ( contactFrictionDentScale > 0.0f ) {
5892                 return contactFrictionDentScale;
5893         } else if ( contactFrictionScale > 0.0f ) {
5894                 return contactFrictionScale;
5895         } else if ( af_contactFrictionScale.GetFloat() > 0.0f ) {
5896                 return af_contactFrictionScale.GetFloat();
5897         }
5898         return 1.0f;
5899 }
5900
5901 /*
5902 ================
5903 idPhysics_AF::TestIfAtRest
5904 ================
5905 */
5906 bool idPhysics_AF::TestIfAtRest( float timeStep ) {
5907         int i;
5908         float translationSqr, maxTranslationSqr, rotation, maxRotation;
5909         idAFBody *body;
5910
5911         if ( current.atRest >= 0 ) {
5912                 return true;
5913         }
5914
5915         current.activateTime += timeStep;
5916
5917         // if the simulation should never be suspended before a certaint amount of time passed
5918         if ( minMoveTime > 0.0f && current.activateTime < minMoveTime ) {
5919                 return false;
5920         }
5921
5922         // if the simulation should always be suspended after a certain amount time passed
5923         if ( maxMoveTime > 0.0f && current.activateTime > maxMoveTime ) {
5924                 return true;
5925         }
5926
5927         // test if all bodies hardly moved over a period of time
5928         if ( current.noMoveTime == 0.0f ) {
5929                 for ( i = 0; i < bodies.Num(); i++ ) {
5930                         body = bodies[i];
5931                         body->atRestOrigin = body->current->worldOrigin;
5932                         body->atRestAxis = body->current->worldAxis;
5933                 }
5934                 current.noMoveTime += timeStep;
5935         }
5936         else if ( current.noMoveTime > noMoveTime ) {
5937                 current.noMoveTime = 0.0f;
5938                 maxTranslationSqr = 0.0f;
5939                 maxRotation = 0.0f;
5940                 for ( i = 0; i < bodies.Num(); i++ ) {
5941                         body = bodies[i];
5942
5943                         translationSqr = ( body->current->worldOrigin - body->atRestOrigin ).LengthSqr();
5944                         if ( translationSqr > maxTranslationSqr ) {
5945                                 maxTranslationSqr = translationSqr;
5946                         }
5947                         rotation = ( body->atRestAxis.Transpose() * body->current->worldAxis ).ToRotation().GetAngle();
5948                         if ( rotation > maxRotation ) {
5949                                 maxRotation = rotation;
5950                         }
5951                 }
5952
5953                 if ( maxTranslationSqr < Square( noMoveTranslation ) && maxRotation < noMoveRotation ) {
5954                         // hardly moved over a period of time so the articulated figure may come to rest
5955                         return true;
5956                 }
5957         } else {
5958                 current.noMoveTime += timeStep;
5959         }
5960
5961         // test if the velocity or acceleration of any body is still too large to come to rest
5962         for ( i = 0; i < bodies.Num(); i++ ) {
5963                 body = bodies[i];
5964
5965                 if ( body->current->spatialVelocity.SubVec3(0).LengthSqr() > Square( suspendVelocity[0] ) ) {
5966                         return false;
5967                 }
5968                 if ( body->current->spatialVelocity.SubVec3(1).LengthSqr() > Square( suspendVelocity[1] ) ) {
5969                         return false;
5970                 }
5971                 if ( body->acceleration.SubVec3(0).LengthSqr() > Square( suspendAcceleration[0] ) ) {
5972                         return false;
5973                 }
5974                 if ( body->acceleration.SubVec3(1).LengthSqr() > Square( suspendAcceleration[1] ) ) {
5975                         return false;
5976                 }
5977         }
5978
5979         // all bodies have a velocity and acceleration small enough to come to rest
5980         return true;
5981 }
5982
5983 /*
5984 ================
5985 idPhysics_AF::Rest
5986 ================
5987 */
5988 void idPhysics_AF::Rest( void ) {
5989         int i;
5990
5991         current.atRest = gameLocal.time;
5992
5993         for ( i = 0; i < bodies.Num(); i++ ) {
5994                 bodies[i]->current->spatialVelocity.Zero();
5995                 bodies[i]->current->externalForce.Zero();
5996         }
5997
5998         self->BecomeInactive( TH_PHYSICS );
5999 }
6000
6001 /*
6002 ================
6003 idPhysics_AF::Activate
6004 ================
6005 */
6006 void idPhysics_AF::Activate( void ) {
6007         // if the articulated figure was at rest
6008         if ( current.atRest >= 0 ) {
6009                 // normally gravity is added at the end of a simulation frame
6010                 // if the figure was at rest add gravity here so it is applied this simulation frame
6011                 AddGravity();
6012                 // reset the active time for the max move time
6013                 current.activateTime = 0.0f;
6014         }
6015         current.atRest = -1;
6016         current.noMoveTime = 0.0f;
6017         self->BecomeActive( TH_PHYSICS );
6018 }
6019
6020 /*
6021 ================
6022 idPhysics_AF::PutToRest
6023
6024   put to rest untill something collides with this physics object
6025 ================
6026 */
6027 void idPhysics_AF::PutToRest( void ) {
6028         Rest();
6029 }
6030
6031 /*
6032 ================
6033 idPhysics_AF::EnableImpact
6034 ================
6035 */
6036 void idPhysics_AF::EnableImpact( void ) {
6037         noImpact = false;
6038 }
6039
6040 /*
6041 ================
6042 idPhysics_AF::DisableImpact
6043 ================
6044 */
6045 void idPhysics_AF::DisableImpact( void ) {
6046         noImpact = true;
6047 }
6048
6049 /*
6050 ================
6051 idPhysics_AF::AddPushVelocity
6052 ================
6053 */
6054 void idPhysics_AF::AddPushVelocity( const idVec6 &pushVelocity ) {
6055         int i;
6056
6057         if ( pushVelocity != vec6_origin ) {
6058                 for ( i = 0; i < bodies.Num(); i++ ) {
6059                         bodies[i]->current->spatialVelocity += pushVelocity;
6060                 }
6061         }
6062 }
6063
6064 /*
6065 ================
6066 idPhysics_AF::SetClipModel
6067 ================
6068 */
6069 void idPhysics_AF::SetClipModel( idClipModel *model, float density, int id, bool freeOld ) {
6070 }
6071
6072 /*
6073 ================
6074 idPhysics_AF::GetClipModel
6075 ================
6076 */
6077 idClipModel *idPhysics_AF::GetClipModel( int id ) const {
6078         if ( id >= 0 && id < bodies.Num() ) {
6079                 return bodies[id]->GetClipModel();
6080         }
6081         return NULL;
6082 }
6083
6084 /*
6085 ================
6086 idPhysics_AF::GetNumClipModels
6087 ================
6088 */
6089 int idPhysics_AF::GetNumClipModels( void ) const {
6090         return bodies.Num();
6091 }
6092
6093 /*
6094 ================
6095 idPhysics_AF::SetMass
6096 ================
6097 */
6098 void idPhysics_AF::SetMass( float mass, int id ) {
6099         if ( id >= 0 && id < bodies.Num() ) {
6100         }
6101         else {
6102                 forceTotalMass = mass;
6103         }
6104         SetChanged();
6105 }
6106
6107 /*
6108 ================
6109 idPhysics_AF::GetMass
6110 ================
6111 */
6112 float idPhysics_AF::GetMass( int id ) const {
6113         if ( id >= 0 && id < bodies.Num() ) {
6114                 return bodies[id]->mass;
6115         }
6116         return totalMass;
6117 }
6118
6119 /*
6120 ================
6121 idPhysics_AF::SetContents
6122 ================
6123 */
6124 void idPhysics_AF::SetContents( int contents, int id ) {
6125         int i;
6126
6127         if ( id >= 0 && id < bodies.Num() ) {
6128                 bodies[id]->GetClipModel()->SetContents( contents );
6129         }
6130         else {
6131                 for ( i = 0; i < bodies.Num(); i++ ) {
6132                         bodies[i]->GetClipModel()->SetContents( contents );
6133                 }
6134         }
6135 }
6136
6137 /*
6138 ================
6139 idPhysics_AF::GetContents
6140 ================
6141 */
6142 int idPhysics_AF::GetContents( int id ) const {
6143         int i, contents;
6144
6145         if ( id >= 0 && id < bodies.Num() ) {
6146                 return bodies[id]->GetClipModel()->GetContents();
6147         }
6148         else {
6149                 contents = 0;
6150                 for ( i = 0; i < bodies.Num(); i++ ) {
6151                         contents |= bodies[i]->GetClipModel()->GetContents();
6152                 }
6153                 return contents;
6154         }
6155 }
6156
6157 /*
6158 ================
6159 idPhysics_AF::GetBounds
6160 ================
6161 */
6162 const idBounds &idPhysics_AF::GetBounds( int id ) const {
6163         int i;
6164         static idBounds relBounds;
6165
6166         if ( id >= 0 && id < bodies.Num() ) {
6167                 return bodies[id]->GetClipModel()->GetBounds();
6168         }
6169         else if ( !bodies.Num() ) {
6170                 relBounds.Zero();
6171                 return relBounds;
6172         }
6173         else {
6174                 relBounds = bodies[0]->GetClipModel()->GetBounds();
6175                 for ( i = 1; i < bodies.Num(); i++ ) {
6176                         idBounds bounds;
6177                         idVec3 origin = ( bodies[i]->GetWorldOrigin() - bodies[0]->GetWorldOrigin() ) * bodies[0]->GetWorldAxis().Transpose();
6178                         idMat3 axis = bodies[i]->GetWorldAxis() * bodies[0]->GetWorldAxis().Transpose();
6179                         bounds.FromTransformedBounds( bodies[i]->GetClipModel()->GetBounds(), origin, axis );
6180                         relBounds += bounds;
6181                 }
6182                 return relBounds;
6183         }
6184 }
6185
6186 /*
6187 ================
6188 idPhysics_AF::GetAbsBounds
6189 ================
6190 */
6191 const idBounds &idPhysics_AF::GetAbsBounds( int id ) const {
6192         int i;
6193         static idBounds absBounds;
6194
6195         if ( id >= 0 && id < bodies.Num() ) {
6196                 return bodies[id]->GetClipModel()->GetAbsBounds();
6197         }
6198         else if ( !bodies.Num() ) {
6199                 absBounds.Zero();
6200                 return absBounds;
6201         }
6202         else {
6203                 absBounds = bodies[0]->GetClipModel()->GetAbsBounds();
6204                 for ( i = 1; i < bodies.Num(); i++ ) {
6205                         absBounds += bodies[i]->GetClipModel()->GetAbsBounds();
6206                 }
6207                 return absBounds;
6208         }
6209 }
6210
6211 /*
6212 ================
6213 idPhysics_AF::Evaluate
6214 ================
6215 */
6216 bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) {
6217         float timeStep;
6218
6219         if ( timeScaleRampStart < MS2SEC( endTimeMSec ) && timeScaleRampEnd > MS2SEC( endTimeMSec ) ) {
6220                 timeStep = MS2SEC( timeStepMSec ) * ( MS2SEC( endTimeMSec ) - timeScaleRampStart ) / ( timeScaleRampEnd - timeScaleRampStart );
6221         } else if ( af_timeScale.GetFloat() != 1.0f ) {
6222                 timeStep = MS2SEC( timeStepMSec ) * af_timeScale.GetFloat();
6223         } else {
6224                 timeStep = MS2SEC( timeStepMSec ) * timeScale;
6225         }
6226         current.lastTimeStep = timeStep;
6227
6228
6229         // if the articulated figure changed
6230         if ( changedAF || ( linearTime != af_useLinearTime.GetBool() ) ) {
6231                 BuildTrees();
6232                 changedAF = false;
6233                 linearTime = af_useLinearTime.GetBool();
6234         }
6235
6236         // get the new master position
6237         if ( masterBody ) {
6238                 idVec3 masterOrigin;
6239                 idMat3 masterAxis;
6240                 self->GetMasterPosition( masterOrigin, masterAxis );
6241                 if ( current.atRest >= 0 && ( masterBody->current->worldOrigin != masterOrigin || masterBody->current->worldAxis != masterAxis ) ) {
6242                         Activate();
6243                 }
6244                 masterBody->current->worldOrigin = masterOrigin;
6245                 masterBody->current->worldAxis = masterAxis;
6246         }
6247
6248         // if the simulation is suspended because the figure is at rest
6249         if ( current.atRest >= 0 || timeStep <= 0.0f ) {
6250                 DebugDraw();
6251                 return false;
6252         }
6253
6254         // move the af velocity into the frame of a pusher
6255         AddPushVelocity( -current.pushVelocity );
6256
6257 #ifdef AF_TIMINGS
6258         timer_total.Start();
6259 #endif
6260
6261 #ifdef AF_TIMINGS
6262         timer_collision.Start();
6263 #endif
6264
6265         // evaluate contacts
6266         EvaluateContacts();
6267
6268         // setup contact constraints
6269         SetupContactConstraints();
6270
6271 #ifdef AF_TIMINGS
6272         timer_collision.Stop();
6273 #endif
6274
6275         // evaluate constraint equations
6276         EvaluateConstraints( timeStep );
6277
6278         // apply friction
6279         ApplyFriction( timeStep, endTimeMSec );
6280
6281         // add frame constraints
6282         AddFrameConstraints();
6283
6284 #ifdef AF_TIMINGS
6285         int i, numPrimary = 0, numAuxiliary = 0;
6286         for ( i = 0; i < primaryConstraints.Num(); i++ ) {
6287                 numPrimary += primaryConstraints[i]->J1.GetNumRows();
6288         }
6289         for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
6290                 numAuxiliary += auxiliaryConstraints[i]->J1.GetNumRows();
6291         }
6292         timer_pc.Start();
6293 #endif
6294
6295         // factor matrices for primary constraints
6296         PrimaryFactor();
6297
6298         // calculate forces on bodies after applying primary constraints
6299         PrimaryForces( timeStep );
6300
6301 #ifdef AF_TIMINGS
6302         timer_pc.Stop();
6303         timer_ac.Start();
6304 #endif
6305
6306         // calculate and apply auxiliary constraint forces
6307         AuxiliaryForces( timeStep );
6308
6309 #ifdef AF_TIMINGS
6310         timer_ac.Stop();
6311 #endif
6312
6313         // evolve current state to next state
6314         Evolve( timeStep );
6315
6316         // debug graphics
6317         DebugDraw();
6318
6319         // clear external forces on all bodies
6320         ClearExternalForce();
6321
6322         // apply contact force to other entities
6323         ApplyContactForces();
6324
6325         // remove all frame constraints
6326         RemoveFrameConstraints();
6327
6328 #ifdef AF_TIMINGS
6329         timer_collision.Start();
6330 #endif
6331
6332         // check for collisions between current and next state
6333         CheckForCollisions( timeStep );
6334
6335 #ifdef AF_TIMINGS
6336         timer_collision.Stop();
6337 #endif
6338
6339         // swap the current and next state
6340         SwapStates();
6341
6342         // make sure all clip models are disabled in case they were enabled for self collision
6343         if ( selfCollision && !af_skipSelfCollision.GetBool() ) {
6344                 DisableClip();
6345         }
6346
6347         // apply collision impulses
6348         if ( ApplyCollisions( timeStep ) ) {
6349                 current.atRest = gameLocal.time;
6350                 comeToRest = true;
6351         }
6352
6353         // test if the simulation can be suspended because the whole figure is at rest
6354         if ( comeToRest && TestIfAtRest( timeStep ) ) {
6355                 Rest();
6356         } else {
6357                 ActivateContactEntities();
6358         }
6359
6360         // add gravitational force
6361         AddGravity();
6362
6363         // move the af velocity back into the world frame
6364         AddPushVelocity( current.pushVelocity );
6365         current.pushVelocity.Zero();
6366
6367         if ( IsOutsideWorld() ) {
6368                 gameLocal.Warning( "articulated figure moved outside world bounds for entity '%s' type '%s' at (%s)",
6369                                                         self->name.c_str(), self->GetType()->classname, bodies[0]->current->worldOrigin.ToString(0) );
6370                 Rest();
6371         }
6372
6373 #ifdef AF_TIMINGS
6374         timer_total.Stop();
6375
6376         if ( af_showTimings.GetInteger() == 1 ) {
6377                 gameLocal.Printf( "%12s: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n",
6378                                                 self->name.c_str(),
6379                                                 timer_total.Milliseconds(),
6380                                                 numPrimary, timer_pc.Milliseconds(),
6381                                                 numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(),
6382                                                 timer_lcp.Milliseconds(), timer_collision.Milliseconds() );
6383         }
6384         else if ( af_showTimings.GetInteger() == 2 ) {
6385                 numArticulatedFigures++;
6386                 if ( endTimeMSec > lastTimerReset ) {
6387                         gameLocal.Printf( "af %d: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n",
6388                                                         numArticulatedFigures,
6389                                                         timer_total.Milliseconds(),
6390                                                         numPrimary, timer_pc.Milliseconds(),
6391                                                         numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(),
6392                                                         timer_lcp.Milliseconds(), timer_collision.Milliseconds() );
6393                 }
6394         }
6395
6396         if ( endTimeMSec > lastTimerReset ) {
6397                 lastTimerReset = endTimeMSec;
6398                 numArticulatedFigures = 0;
6399                 timer_total.Clear();
6400                 timer_pc.Clear();
6401                 timer_ac.Clear();
6402                 timer_collision.Clear();
6403                 timer_lcp.Clear();
6404         }
6405 #endif
6406
6407         return true;
6408 }
6409
6410 /*
6411 ================
6412 idPhysics_AF::UpdateTime
6413 ================
6414 */
6415 void idPhysics_AF::UpdateTime( int endTimeMSec ) {
6416 }
6417
6418 /*
6419 ================
6420 idPhysics_AF::GetTime
6421 ================
6422 */
6423 int idPhysics_AF::GetTime( void ) const {
6424         return gameLocal.time;
6425 }
6426
6427 /*
6428 ================
6429 DrawTraceModelSilhouette
6430 ================
6431 */
6432 void DrawTraceModelSilhouette( const idVec3 &projectionOrigin, const idClipModel *clipModel ) {
6433         int i, numSilEdges;
6434         int silEdges[MAX_TRACEMODEL_EDGES];
6435         idVec3 v1, v2;
6436         const idTraceModel *trm = clipModel->GetTraceModel();
6437         const idVec3 &origin = clipModel->GetOrigin();
6438         const idMat3 &axis = clipModel->GetAxis();
6439
6440         numSilEdges = trm->GetProjectionSilhouetteEdges( ( projectionOrigin - origin ) * axis.Transpose(), silEdges );
6441         for ( i = 0; i < numSilEdges; i++ ) {
6442                 v1 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITSET( silEdges[i] ) ] ];
6443                 v2 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITNOTSET( silEdges[i] ) ] ];
6444                 gameRenderWorld->DebugArrow( colorRed, origin + v1 * axis, origin + v2 * axis, 1 );
6445         }
6446 }
6447
6448 /*
6449 ================
6450 idPhysics_AF::DebugDraw
6451 ================
6452 */
6453 void idPhysics_AF::DebugDraw( void ) {
6454         int i;
6455         idAFBody *body, *highlightBody = NULL, *constrainedBody1 = NULL, *constrainedBody2 = NULL;
6456         idAFConstraint *constraint;
6457         idVec3 center;
6458         idMat3 axis;
6459
6460         if ( af_highlightConstraint.GetString()[0] ) {
6461                 constraint = GetConstraint( af_highlightConstraint.GetString() );
6462                 if ( constraint ) {
6463                         constraint->GetCenter( center );
6464                         axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3();
6465                         gameRenderWorld->DebugCone( colorYellow, center, (axis[2] - axis[1]) * 4.0f, 0.0f, 1.0f, 0 );
6466
6467                         if ( af_showConstrainedBodies.GetBool() ) {
6468                                 cvarSystem->SetCVarString( "cm_drawColor", colorCyan.ToString( 0 ) );
6469                                 constrainedBody1 = constraint->body1;
6470                                 if ( constrainedBody1 ) {
6471                                         collisionModelManager->DrawModel( constrainedBody1->clipModel->Handle(), constrainedBody1->clipModel->GetOrigin(),
6472                                                                                         constrainedBody1->clipModel->GetAxis(), vec3_origin, 0.0f );
6473                                 }
6474                                 cvarSystem->SetCVarString( "cm_drawColor", colorBlue.ToString( 0 ) );
6475                                 constrainedBody2 = constraint->body2;
6476                                 if ( constrainedBody2 ) {
6477                                         collisionModelManager->DrawModel( constrainedBody2->clipModel->Handle(), constrainedBody2->clipModel->GetOrigin(),
6478                                                                                         constrainedBody2->clipModel->GetAxis(), vec3_origin, 0.0f );
6479                                 }
6480                                 cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) );
6481                         }
6482                 }
6483         }
6484
6485         if ( af_highlightBody.GetString()[0] ) {
6486                 highlightBody = GetBody( af_highlightBody.GetString() );
6487                 if ( highlightBody ) {
6488                         cvarSystem->SetCVarString( "cm_drawColor", colorYellow.ToString( 0 ) );
6489                         collisionModelManager->DrawModel( highlightBody->clipModel->Handle(), highlightBody->clipModel->GetOrigin(),
6490                                                                         highlightBody->clipModel->GetAxis(), vec3_origin, 0.0f );
6491                         cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) );
6492                 }
6493         }
6494
6495         if ( af_showBodies.GetBool() ) {
6496                 for ( i = 0; i < bodies.Num(); i++ ) {
6497                         body = bodies[i];
6498                         if ( body == constrainedBody1 || body == constrainedBody2 ) {
6499                                 continue;
6500                         }
6501                         if ( body == highlightBody ) {
6502                                 continue;
6503                         }
6504                         collisionModelManager->DrawModel( body->clipModel->Handle(), body->clipModel->GetOrigin(),
6505                                                                                 body->clipModel->GetAxis(), vec3_origin, 0.0f );
6506                         //DrawTraceModelSilhouette( gameLocal.GetLocalPlayer()->GetEyePosition(), body->clipModel );
6507                 }
6508         }
6509
6510         if ( af_showBodyNames.GetBool() ) {
6511                 for ( i = 0; i < bodies.Num(); i++ ) {
6512                         body = bodies[i];
6513                         gameRenderWorld->DrawText( body->GetName().c_str(), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6514                 }
6515         }
6516
6517         if ( af_showMass.GetBool() ) {
6518                 for ( i = 0; i < bodies.Num(); i++ ) {
6519                         body = bodies[i];
6520                         gameRenderWorld->DrawText( va( "\n%1.2f", 1.0f / body->GetInverseMass() ), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6521                 }
6522         }
6523
6524         if ( af_showTotalMass.GetBool() ) {
6525                 axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3();
6526                 gameRenderWorld->DrawText( va( "\n%1.2f", totalMass ), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1 );
6527         }
6528
6529         if ( af_showInertia.GetBool() ) {
6530                 for ( i = 0; i < bodies.Num(); i++ ) {
6531                         body = bodies[i];
6532                         idMat3 &I = body->inertiaTensor;
6533                         gameRenderWorld->DrawText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )",
6534                                                                                 I[0].x, I[0].y, I[0].z,
6535                                                                                 I[1].x, I[1].y, I[1].z,
6536                                                                                 I[2].x, I[2].y, I[2].z ),
6537                                                                                 body->GetWorldOrigin(), 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6538                 }
6539         }
6540
6541         if ( af_showVelocity.GetBool() ) {
6542                 for ( i = 0; i < bodies.Num(); i++ ) {
6543                         DrawVelocity( bodies[i]->clipModel->GetId(), 0.1f, 4.0f );
6544                 }
6545         }
6546
6547         if ( af_showConstraints.GetBool() ) {
6548                 for ( i = 0; i < primaryConstraints.Num(); i++ ) {
6549                         constraint = primaryConstraints[i];
6550                         constraint->DebugDraw();
6551                 }
6552                 if ( !af_showPrimaryOnly.GetBool() ) {
6553                         for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
6554                                 constraint = auxiliaryConstraints[i];
6555                                 constraint->DebugDraw();
6556                         }
6557                 }
6558         }
6559
6560         if ( af_showConstraintNames.GetBool() ) {
6561                 for ( i = 0; i < primaryConstraints.Num(); i++ ) {
6562                         constraint = primaryConstraints[i];
6563                         constraint->GetCenter( center );
6564                         gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6565                 }
6566                 if ( !af_showPrimaryOnly.GetBool() ) {
6567                         for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
6568                                 constraint = auxiliaryConstraints[i];
6569                                 constraint->GetCenter( center );
6570                                 gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
6571                         }
6572                 }
6573         }
6574
6575         if ( af_showTrees.GetBool() || ( af_showActive.GetBool() && current.atRest < 0 ) ) {
6576                 for ( i = 0; i < trees.Num(); i++ ) {
6577                         trees[i]->DebugDraw( idStr::ColorForIndex( i+3 ) );
6578                 }
6579         }
6580 }
6581
6582 /*
6583 ================
6584 idPhysics_AF::idPhysics_AF
6585 ================
6586 */
6587 idPhysics_AF::idPhysics_AF( void ) {
6588         trees.Clear();
6589         bodies.Clear();
6590         constraints.Clear();
6591         primaryConstraints.Clear();
6592         auxiliaryConstraints.Clear();
6593         frameConstraints.Clear();
6594         contacts.Clear();
6595         collisions.Clear();
6596         changedAF = true;
6597         masterBody = NULL;
6598
6599         lcp = idLCP::AllocSymmetric();
6600
6601         memset( &current, 0, sizeof( current ) );
6602         current.atRest = -1;
6603         current.lastTimeStep = USERCMD_MSEC;
6604         saved = current;
6605
6606         linearFriction = 0.005f;
6607         angularFriction = 0.005f;
6608         contactFriction = 0.8f;
6609         bouncyness = 0.4f;
6610         totalMass = 0.0f;
6611         forceTotalMass = -1.0f;
6612
6613         suspendVelocity.Set( SUSPEND_LINEAR_VELOCITY, SUSPEND_ANGULAR_VELOCITY );
6614         suspendAcceleration.Set( SUSPEND_LINEAR_ACCELERATION, SUSPEND_LINEAR_ACCELERATION );
6615         noMoveTime = NO_MOVE_TIME;
6616         noMoveTranslation = NO_MOVE_TRANSLATION_TOLERANCE;
6617         noMoveRotation = NO_MOVE_ROTATION_TOLERANCE;
6618         minMoveTime = MIN_MOVE_TIME;
6619         maxMoveTime = MAX_MOVE_TIME;
6620         impulseThreshold = IMPULSE_THRESHOLD;
6621
6622         timeScale = 1.0f;
6623         timeScaleRampStart = 0.0f;
6624         timeScaleRampEnd = 0.0f;
6625
6626         jointFrictionScale = 0.0f;
6627         jointFrictionDent = 0.0f;
6628         jointFrictionDentStart = 0.0f;
6629         jointFrictionDentEnd = 0.0f;
6630         jointFrictionDentScale = 0.0f;
6631
6632         contactFrictionScale = 0.0f;
6633         contactFrictionDent = 0.0f;
6634         contactFrictionDentStart = 0.0f;
6635         contactFrictionDentEnd = 0.0f;
6636         contactFrictionDentScale = 0.0f;
6637
6638         enableCollision = true;
6639         selfCollision = true;
6640         comeToRest = true;
6641         linearTime = true;
6642         noImpact = false;
6643         worldConstraintsLocked = false;
6644         forcePushable = false;
6645
6646 #ifdef AF_TIMINGS
6647         lastTimerReset = 0;
6648 #endif
6649 }
6650
6651 /*
6652 ================
6653 idPhysics_AF::~idPhysics_AF
6654 ================
6655 */
6656 idPhysics_AF::~idPhysics_AF( void ) {
6657         int i;
6658
6659         trees.DeleteContents( true );
6660
6661         for ( i = 0; i < bodies.Num(); i++ ) {
6662                 delete bodies[i];
6663         }
6664
6665         for ( i = 0; i < constraints.Num(); i++ ) {
6666                 delete constraints[i];
6667         }
6668
6669         contactConstraints.SetNum( contactConstraints.NumAllocated(), false );
6670         for ( i = 0; i < contactConstraints.NumAllocated(); i++ ) {
6671                 delete contactConstraints[i];
6672         }
6673
6674         delete lcp;
6675
6676         if ( masterBody ) {
6677                 delete masterBody;
6678         }
6679 }
6680
6681 /*
6682 ================
6683 idPhysics_AF_SavePState
6684 ================
6685 */
6686 void idPhysics_AF_SavePState( idSaveGame *saveFile, const AFPState_t &state ) {
6687         saveFile->WriteInt( state.atRest );
6688         saveFile->WriteFloat( state.noMoveTime );
6689         saveFile->WriteFloat( state.activateTime );
6690         saveFile->WriteFloat( state.lastTimeStep );
6691         saveFile->WriteVec6( state.pushVelocity );
6692 }
6693
6694 /*
6695 ================
6696 idPhysics_AF_RestorePState
6697 ================
6698 */
6699 void idPhysics_AF_RestorePState( idRestoreGame *saveFile, AFPState_t &state ) {
6700         saveFile->ReadInt( state.atRest );
6701         saveFile->ReadFloat( state.noMoveTime );
6702         saveFile->ReadFloat( state.activateTime );
6703         saveFile->ReadFloat( state.lastTimeStep );
6704         saveFile->ReadVec6( state.pushVelocity );
6705 }
6706
6707 /*
6708 ================
6709 idPhysics_AF::Save
6710 ================
6711 */
6712 void idPhysics_AF::Save( idSaveGame *saveFile ) const {
6713         int i;
6714
6715         // the articulated figure structure is handled by the owner
6716
6717         idPhysics_AF_SavePState( saveFile, current );
6718         idPhysics_AF_SavePState( saveFile, saved );
6719
6720         saveFile->WriteInt( bodies.Num() );
6721         for ( i = 0; i < bodies.Num(); i++ ) {
6722                 bodies[i]->Save( saveFile );
6723         }
6724         if ( masterBody ) {
6725                 saveFile->WriteBool( true );
6726                 masterBody->Save( saveFile );
6727         } else {
6728                 saveFile->WriteBool( false );
6729         }
6730
6731         saveFile->WriteInt( constraints.Num() );
6732         for ( i = 0; i < constraints.Num(); i++ ) {
6733                 constraints[i]->Save( saveFile );
6734         }
6735
6736         saveFile->WriteBool( changedAF );
6737
6738         saveFile->WriteFloat( linearFriction );
6739         saveFile->WriteFloat( angularFriction );
6740         saveFile->WriteFloat( contactFriction );
6741         saveFile->WriteFloat( bouncyness );
6742         saveFile->WriteFloat( totalMass );
6743         saveFile->WriteFloat( forceTotalMass );
6744
6745         saveFile->WriteVec2( suspendVelocity );
6746         saveFile->WriteVec2( suspendAcceleration );
6747         saveFile->WriteFloat( noMoveTime );
6748         saveFile->WriteFloat( noMoveTranslation );
6749         saveFile->WriteFloat( noMoveRotation );
6750         saveFile->WriteFloat( minMoveTime );
6751         saveFile->WriteFloat( maxMoveTime );
6752         saveFile->WriteFloat( impulseThreshold );
6753
6754         saveFile->WriteFloat( timeScale );
6755         saveFile->WriteFloat( timeScaleRampStart );
6756         saveFile->WriteFloat( timeScaleRampEnd );
6757
6758         saveFile->WriteFloat( jointFrictionScale );
6759         saveFile->WriteFloat( jointFrictionDent );
6760         saveFile->WriteFloat( jointFrictionDentStart );
6761         saveFile->WriteFloat( jointFrictionDentEnd );
6762         saveFile->WriteFloat( jointFrictionDentScale );
6763
6764         saveFile->WriteFloat( contactFrictionScale );
6765         saveFile->WriteFloat( contactFrictionDent );
6766         saveFile->WriteFloat( contactFrictionDentStart );
6767         saveFile->WriteFloat( contactFrictionDentEnd );
6768         saveFile->WriteFloat( contactFrictionDentScale );
6769
6770         saveFile->WriteBool( enableCollision );
6771         saveFile->WriteBool( selfCollision );
6772         saveFile->WriteBool( comeToRest );
6773         saveFile->WriteBool( linearTime );
6774         saveFile->WriteBool( noImpact );
6775         saveFile->WriteBool( worldConstraintsLocked );
6776         saveFile->WriteBool( forcePushable );
6777 }
6778
6779 /*
6780 ================
6781 idPhysics_AF::Restore
6782 ================
6783 */
6784 void idPhysics_AF::Restore( idRestoreGame *saveFile ) {
6785         int i, num;
6786         bool hasMaster;
6787
6788         // the articulated figure structure should have already been restored
6789
6790         idPhysics_AF_RestorePState( saveFile, current );
6791         idPhysics_AF_RestorePState( saveFile, saved );
6792
6793         saveFile->ReadInt( num );
6794         assert( num == bodies.Num() );
6795         for ( i = 0; i < bodies.Num(); i++ ) {
6796                 bodies[i]->Restore( saveFile );
6797         }
6798         saveFile->ReadBool( hasMaster );
6799         if ( hasMaster ) {
6800                 masterBody = new idAFBody();
6801                 masterBody->Restore( saveFile );
6802         }
6803
6804         saveFile->ReadInt( num );
6805         assert( num == constraints.Num() );
6806         for ( i = 0; i < constraints.Num(); i++ ) {
6807                 constraints[i]->Restore( saveFile );
6808         }
6809
6810         saveFile->ReadBool( changedAF );
6811
6812         saveFile->ReadFloat( linearFriction );
6813         saveFile->ReadFloat( angularFriction );
6814         saveFile->ReadFloat( contactFriction );
6815         saveFile->ReadFloat( bouncyness );
6816         saveFile->ReadFloat( totalMass );
6817         saveFile->ReadFloat( forceTotalMass );
6818
6819         saveFile->ReadVec2( suspendVelocity );
6820         saveFile->ReadVec2( suspendAcceleration );
6821         saveFile->ReadFloat( noMoveTime );
6822         saveFile->ReadFloat( noMoveTranslation );
6823         saveFile->ReadFloat( noMoveRotation );
6824         saveFile->ReadFloat( minMoveTime );
6825         saveFile->ReadFloat( maxMoveTime );
6826         saveFile->ReadFloat( impulseThreshold );
6827
6828         saveFile->ReadFloat( timeScale );
6829         saveFile->ReadFloat( timeScaleRampStart );
6830         saveFile->ReadFloat( timeScaleRampEnd );
6831
6832         saveFile->ReadFloat( jointFrictionScale );
6833         saveFile->ReadFloat( jointFrictionDent );
6834         saveFile->ReadFloat( jointFrictionDentStart );
6835         saveFile->ReadFloat( jointFrictionDentEnd );
6836         saveFile->ReadFloat( jointFrictionDentScale );
6837
6838         saveFile->ReadFloat( contactFrictionScale );
6839         saveFile->ReadFloat( contactFrictionDent );
6840         saveFile->ReadFloat( contactFrictionDentStart );
6841         saveFile->ReadFloat( contactFrictionDentEnd );
6842         saveFile->ReadFloat( contactFrictionDentScale );
6843
6844         saveFile->ReadBool( enableCollision );
6845         saveFile->ReadBool( selfCollision );
6846         saveFile->ReadBool( comeToRest );
6847         saveFile->ReadBool( linearTime );
6848         saveFile->ReadBool( noImpact );
6849         saveFile->ReadBool( worldConstraintsLocked );
6850         saveFile->ReadBool( forcePushable );
6851
6852         changedAF = true;
6853
6854         UpdateClipModels();
6855 }
6856
6857 /*
6858 ================
6859 idPhysics_AF::IsClosedLoop
6860 ================
6861 */
6862 bool idPhysics_AF::IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) const {
6863         const idAFBody *b1, *b2;
6864
6865         for ( b1 = body1; b1->parent; b1 = b1->parent ) {
6866         }
6867         for ( b2 = body2; b2->parent; b2 = b2->parent ) {
6868         }
6869         return ( b1 == b2 );
6870 }
6871
6872 /*
6873 ================
6874 idPhysics_AF::BuildTrees
6875 ================
6876 */
6877 void idPhysics_AF::BuildTrees( void ) {
6878         int i;
6879         float scale;
6880         idAFBody *b;
6881         idAFConstraint *c;
6882         idAFTree *tree;
6883
6884         primaryConstraints.Clear();
6885         auxiliaryConstraints.Clear();
6886         trees.DeleteContents( true );
6887
6888         totalMass = 0.0f;
6889         for ( i = 0; i < bodies.Num(); i++ ) {
6890                 b = bodies[i];
6891                 b->parent = NULL;
6892                 b->primaryConstraint = NULL;
6893                 b->constraints.SetNum( 0, false );
6894                 b->children.Clear();
6895                 b->tree = NULL;
6896                 totalMass += b->mass;
6897         }
6898
6899         if ( forceTotalMass > 0.0f ) {
6900                 scale = forceTotalMass / totalMass;
6901                 for ( i = 0; i < bodies.Num(); i++ ) {
6902                         b = bodies[i];
6903                         b->mass *= scale;
6904                         b->invMass = 1.0f / b->mass;
6905                         b->inertiaTensor *= scale;
6906                         b->inverseInertiaTensor = b->inertiaTensor.Inverse();
6907                 }
6908                 totalMass = forceTotalMass;
6909         }
6910
6911         if ( af_useLinearTime.GetBool() ) {
6912
6913                 for ( i = 0; i < constraints.Num(); i++ ) {
6914                         c = constraints[i];
6915
6916                         c->body1->constraints.Append( c );
6917                         if ( c->body2 ) {
6918                                 c->body2->constraints.Append( c );
6919                         }
6920
6921                         // only bilateral constraints between two non-world bodies that do not
6922                         // create loops can be used as primary constraints
6923                         if ( !c->body1->primaryConstraint && c->fl.allowPrimary && c->body2 != NULL && !IsClosedLoop( c->body1, c->body2 ) ) {
6924                                 c->body1->primaryConstraint = c;
6925                                 c->body1->parent = c->body2;
6926                                 c->body2->children.Append( c->body1 );
6927                                 c->fl.isPrimary = true;
6928                                 c->firstIndex = 0;
6929                                 primaryConstraints.Append( c );
6930                         } else {
6931                                 c->fl.isPrimary = false;
6932                                 auxiliaryConstraints.Append( c );
6933                         }
6934                 }
6935
6936                 // create trees for all parent bodies
6937                 for ( i = 0; i < bodies.Num(); i++ ) {
6938                         if ( !bodies[i]->parent ) {
6939                                 tree = new idAFTree();
6940                                 tree->sortedBodies.Clear();
6941                                 tree->sortedBodies.Append( bodies[i] );
6942                                 bodies[i]->tree = tree;
6943                                 trees.Append( tree );
6944                         }
6945                 }
6946
6947                 // add each child body to the appropriate tree
6948                 for ( i = 0; i < bodies.Num(); i++ ) {
6949                         if ( bodies[i]->parent ) {
6950                                 for ( b = bodies[i]->parent; !b->tree; b = b->parent ) {
6951                                 }
6952                                 b->tree->sortedBodies.Append( bodies[i] );
6953                                 bodies[i]->tree = b->tree;
6954                         }
6955                 }
6956
6957                 if ( trees.Num() > 1 ) {
6958                         gameLocal.Warning( "Articulated figure has multiple seperate tree structures for entity '%s' type '%s'.",
6959                                                                 self->name.c_str(), self->GetType()->classname );
6960                 }
6961
6962                 // sort bodies in each tree to make sure parents come first
6963                 for ( i = 0; i < trees.Num(); i++ ) {
6964                         trees[i]->SortBodies();
6965                 }
6966
6967         } else {
6968
6969                 // create a tree for each body
6970                 for ( i = 0; i < bodies.Num(); i++ ) {
6971                         tree = new idAFTree();
6972                         tree->sortedBodies.Clear();
6973                         tree->sortedBodies.Append( bodies[i] );
6974                         bodies[i]->tree = tree;
6975                         trees.Append( tree );
6976                 }
6977
6978                 for ( i = 0; i < constraints.Num(); i++ ) {
6979                         c = constraints[i];
6980
6981                         c->body1->constraints.Append( c );
6982                         if ( c->body2 ) {
6983                                 c->body2->constraints.Append( c );
6984                         }
6985
6986                         c->fl.isPrimary = false;
6987                         auxiliaryConstraints.Append( c );
6988                 }
6989         }
6990 }
6991
6992 /*
6993 ================
6994 idPhysics_AF::AddBody
6995
6996   bodies get an id in the order they are added starting at zero
6997   as such the first body added will get id zero
6998 ================
6999 */
7000 int idPhysics_AF::AddBody( idAFBody *body ) {
7001         int id = 0;
7002
7003         if ( !body->clipModel ) {
7004                 gameLocal.Error( "idPhysics_AF::AddBody: body '%s' has no clip model.", body->name.c_str() );
7005         }
7006
7007         if ( bodies.Find( body ) ) {
7008                 gameLocal.Error( "idPhysics_AF::AddBody: body '%s' added twice.", body->name.c_str() );
7009         }
7010
7011         if ( GetBody( body->name ) ) {
7012                 gameLocal.Error( "idPhysics_AF::AddBody: a body with the name '%s' already exists.", body->name.c_str() );
7013         }
7014
7015         id = bodies.Num();
7016         body->clipModel->SetId( id );
7017         if ( body->linearFriction < 0.0f ) {
7018                 body->linearFriction = linearFriction;
7019                 body->angularFriction = angularFriction;
7020                 body->contactFriction = contactFriction;
7021         }
7022         if ( body->bouncyness < 0.0f ) {
7023                 body->bouncyness = bouncyness;
7024         }
7025         if ( !body->fl.clipMaskSet ) {
7026                 body->clipMask = clipMask;
7027         }
7028
7029         bodies.Append( body );
7030
7031         changedAF = true;
7032
7033         return id;
7034 }
7035
7036 /*
7037 ================
7038 idPhysics_AF::AddConstraint
7039 ================
7040 */
7041 void idPhysics_AF::AddConstraint( idAFConstraint *constraint ) {
7042
7043         if ( constraints.Find( constraint ) ) {
7044                 gameLocal.Error( "idPhysics_AF::AddConstraint: constraint '%s' added twice.", constraint->name.c_str() );
7045         }
7046         if ( GetConstraint( constraint->name ) ) {
7047                 gameLocal.Error( "idPhysics_AF::AddConstraint: a constraint with the name '%s' already exists.", constraint->name.c_str() );
7048         }
7049         if ( !constraint->body1 ) {
7050                 gameLocal.Error( "idPhysics_AF::AddConstraint: body1 == NULL on constraint '%s'.", constraint->name.c_str() );
7051         }
7052         if ( !bodies.Find( constraint->body1 ) ) {
7053                 gameLocal.Error( "idPhysics_AF::AddConstraint: body1 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() );
7054         }
7055         if ( constraint->body2 && !bodies.Find( constraint->body2 ) ) {
7056                 gameLocal.Error( "idPhysics_AF::AddConstraint: body2 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() );
7057         }
7058         if ( constraint->body1 == constraint->body2 ) {
7059                 gameLocal.Error( "idPhysics_AF::AddConstraint: body1 and body2 of constraint '%s' are the same.", constraint->name.c_str() );
7060         }
7061
7062         constraints.Append( constraint );
7063         constraint->physics = this;
7064
7065         changedAF = true;
7066 }
7067
7068 /*
7069 ================
7070 idPhysics_AF::AddFrameConstraint
7071 ================
7072 */
7073 void idPhysics_AF::AddFrameConstraint( idAFConstraint *constraint ) {
7074         frameConstraints.Append( constraint );
7075         constraint->physics = this;
7076 }
7077
7078 /*
7079 ================
7080 idPhysics_AF::ForceBodyId
7081 ================
7082 */
7083 void idPhysics_AF::ForceBodyId( idAFBody *body, int newId ) {
7084         int id;
7085
7086         id = bodies.FindIndex( body );
7087         if ( id == -1 ) {
7088                 gameLocal.Error( "ForceBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() );
7089         }
7090         if ( id != newId ) {
7091                 idAFBody *b = bodies[newId];
7092                 bodies[newId] = bodies[id];
7093                 bodies[id] = b;
7094                 changedAF = true;
7095         }
7096 }
7097
7098 /*
7099 ================
7100 idPhysics_AF::GetBodyId
7101 ================
7102 */
7103 int idPhysics_AF::GetBodyId( idAFBody *body ) const {
7104         int id;
7105
7106         id = bodies.FindIndex( body );
7107         if ( id == -1 && body ) {
7108                 gameLocal.Error( "GetBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() );
7109         }
7110         return id;
7111 }
7112
7113 /*
7114 ================
7115 idPhysics_AF::GetBodyId
7116 ================
7117 */
7118 int idPhysics_AF::GetBodyId( const char *bodyName ) const {
7119         int i;
7120
7121         for ( i = 0; i < bodies.Num(); i++ ) {
7122                 if ( !bodies[i]->name.Icmp( bodyName ) ) {
7123                         return i;
7124                 }
7125         }
7126         gameLocal.Error( "GetBodyId: no body with the name '%s' is not part of the articulated figure.\n", bodyName );
7127         return 0;
7128 }
7129
7130 /*
7131 ================
7132 idPhysics_AF::GetConstraintId
7133 ================
7134 */
7135 int idPhysics_AF::GetConstraintId( idAFConstraint *constraint ) const {
7136         int id;
7137
7138         id = constraints.FindIndex( constraint );
7139         if ( id == -1 && constraint ) {
7140                 gameLocal.Error( "GetConstraintId: constraint '%s' is not part of the articulated figure.\n", constraint->name.c_str() );
7141         }
7142         return id;
7143 }
7144
7145 /*
7146 ================
7147 idPhysics_AF::GetConstraintId
7148 ================
7149 */
7150 int idPhysics_AF::GetConstraintId( const char *constraintName ) const {
7151         int i;
7152
7153         for ( i = 0; i < constraints.Num(); i++ ) {
7154                 if ( constraints[i]->name.Icmp( constraintName ) == 0 ) {
7155                         return i;
7156                 }
7157         }
7158         gameLocal.Error( "GetConstraintId: no constraint with the name '%s' is not part of the articulated figure.\n", constraintName );
7159         return 0;
7160 }
7161
7162 /*
7163 ================
7164 idPhysics_AF::GetNumBodies
7165 ================
7166 */
7167 int idPhysics_AF::GetNumBodies( void ) const {
7168         return bodies.Num();
7169 }
7170
7171 /*
7172 ================
7173 idPhysics_AF::GetNumConstraints
7174 ================
7175 */
7176 int idPhysics_AF::GetNumConstraints( void ) const {
7177         return constraints.Num();
7178 }
7179
7180 /*
7181 ================
7182 idPhysics_AF::GetBody
7183 ================
7184 */
7185 idAFBody *idPhysics_AF::GetBody( const char *bodyName ) const {
7186         int i;
7187
7188         for ( i = 0; i < bodies.Num(); i++ ) {
7189                 if ( !bodies[i]->name.Icmp( bodyName ) ) {
7190                         return bodies[i];
7191                 }
7192         }
7193
7194         return NULL;
7195 }
7196
7197 /*
7198 ================
7199 idPhysics_AF::GetBody
7200 ================
7201 */
7202 idAFBody *idPhysics_AF::GetBody( const int id ) const {
7203         if ( id < 0 || id >= bodies.Num() ) {
7204                 gameLocal.Error( "GetBody: no body with id %d exists\n", id );
7205                 return NULL;
7206         }
7207         return bodies[id];
7208 }
7209
7210 /*
7211 ================
7212 idPhysics_AF::GetConstraint
7213 ================
7214 */
7215 idAFConstraint *idPhysics_AF::GetConstraint( const char *constraintName ) const {
7216         int i;
7217
7218         for ( i = 0; i < constraints.Num(); i++ ) {
7219                 if ( constraints[i]->name.Icmp( constraintName ) == 0 ) {
7220                         return constraints[i];
7221                 }
7222         }
7223
7224         return NULL;
7225 }
7226
7227 /*
7228 ================
7229 idPhysics_AF::GetConstraint
7230 ================
7231 */
7232 idAFConstraint *idPhysics_AF::GetConstraint( const int id ) const {
7233         if ( id < 0 || id >= constraints.Num() ) {
7234                 gameLocal.Error( "GetConstraint: no constraint with id %d exists\n", id );
7235                 return NULL;
7236         }
7237         return constraints[id];
7238 }
7239
7240 /*
7241 ================
7242 idPhysics_AF::DeleteBody
7243 ================
7244 */
7245 void idPhysics_AF::DeleteBody( const char *bodyName ) {
7246         int i;
7247
7248         // find the body with the given name
7249         for ( i = 0; i < bodies.Num(); i++ ) {
7250                 if ( !bodies[i]->name.Icmp( bodyName ) ) {
7251                         break;
7252                 }
7253         }
7254
7255         if ( i >= bodies.Num() ) {
7256                 gameLocal.Warning( "DeleteBody: no body found in the articulated figure with the name '%s' for entity '%s' type '%s'.",
7257                                                         bodyName, self->name.c_str(), self->GetType()->classname );
7258                 return;
7259         }
7260
7261         DeleteBody( i );
7262 }
7263
7264 /*
7265 ================
7266 idPhysics_AF::DeleteBody
7267 ================
7268 */
7269 void idPhysics_AF::DeleteBody( const int id ) {
7270         int j;
7271
7272         if ( id < 0 || id > bodies.Num() ) {
7273                 gameLocal.Error( "DeleteBody: no body with id %d.", id );
7274                 return;
7275         }
7276
7277         // remove any constraints attached to this body
7278         for ( j = 0; j < constraints.Num(); j++ ) {
7279                 if ( constraints[j]->body1 == bodies[id] || constraints[j]->body2 == bodies[id] ) {
7280                         delete constraints[j];
7281                         constraints.RemoveIndex( j );
7282                         j--;
7283                 }
7284         }
7285
7286         // remove the body
7287         delete bodies[id];
7288         bodies.RemoveIndex( id );
7289
7290         // set new body ids
7291         for ( j = 0; j < bodies.Num(); j++ ) {
7292                 bodies[j]->clipModel->SetId( j );
7293         }
7294
7295         changedAF = true;
7296 }
7297
7298 /*
7299 ================
7300 idPhysics_AF::DeleteConstraint
7301 ================
7302 */
7303 void idPhysics_AF::DeleteConstraint( const char *constraintName ) {
7304         int i;
7305
7306         // find the constraint with the given name
7307         for ( i = 0; i < constraints.Num(); i++ ) {
7308                 if ( !constraints[i]->name.Icmp( constraintName ) ) {
7309                         break;
7310                 }
7311         }
7312
7313         if ( i >= constraints.Num() ) {
7314                 gameLocal.Warning( "DeleteConstraint: no constriant found in the articulated figure with the name '%s' for entity '%s' type '%s'.",
7315                                                         constraintName, self->name.c_str(), self->GetType()->classname );
7316                 return;
7317         }
7318
7319         DeleteConstraint( i );
7320 }
7321
7322 /*
7323 ================
7324 idPhysics_AF::DeleteConstraint
7325 ================
7326 */
7327 void idPhysics_AF::DeleteConstraint( const int id ) {
7328
7329         if ( id < 0 || id >= constraints.Num() ) {
7330                 gameLocal.Error( "DeleteConstraint: no constraint with id %d.", id );
7331                 return;
7332         }
7333
7334         // remove the constraint
7335         delete constraints[id];
7336         constraints.RemoveIndex( id );
7337
7338         changedAF = true;
7339 }
7340
7341 /*
7342 ================
7343 idPhysics_AF::GetBodyContactConstraints
7344 ================
7345 */
7346 int idPhysics_AF::GetBodyContactConstraints( const int id, idAFConstraint_Contact *contacts[], int maxContacts ) const {
7347         int i, numContacts;
7348         idAFBody *body;
7349         idAFConstraint_Contact *contact;
7350
7351         if ( id < 0 || id >= bodies.Num() || maxContacts <= 0 ) {
7352                 return 0;
7353         }
7354
7355         numContacts = 0;
7356         body = bodies[id];
7357         for ( i = 0; i < contactConstraints.Num(); i++ ) {
7358                 contact = contactConstraints[i];
7359                 if ( contact->body1 == body || contact->body2 == body ) {
7360                         contacts[numContacts++] = contact;
7361                         if ( numContacts >= maxContacts ) {
7362                                 return numContacts;
7363                         }
7364                 }
7365         }
7366         return numContacts;
7367 }
7368
7369 /*
7370 ================
7371 idPhysics_AF::SetDefaultFriction
7372 ================
7373 */
7374 void idPhysics_AF::SetDefaultFriction( float linear, float angular, float contact ) {
7375         if (    linear < 0.0f || linear > 1.0f ||
7376                         angular < 0.0f || angular > 1.0f ||
7377                         contact < 0.0f || contact > 1.0f ) {
7378                 return;
7379         }
7380         linearFriction = linear;
7381         angularFriction = angular;
7382         contactFriction = contact;
7383 }
7384
7385 /*
7386 ================
7387 idPhysics_AF::GetImpactInfo
7388 ================
7389 */
7390 void idPhysics_AF::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const {
7391         if ( id < 0 || id >= bodies.Num() ) {
7392                 memset( info, 0, sizeof( *info ) );
7393                 return;
7394         }
7395         info->invMass = 1.0f / bodies[id]->mass;
7396         info->invInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis;
7397         info->position = point - bodies[id]->current->worldOrigin;
7398         info->velocity = bodies[id]->current->spatialVelocity.SubVec3(0) + bodies[id]->current->spatialVelocity.SubVec3(1).Cross( info->position );
7399 }
7400
7401 /*
7402 ================
7403 idPhysics_AF::ApplyImpulse
7404 ================
7405 */
7406 void idPhysics_AF::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) {
7407         if ( id < 0 || id >= bodies.Num() ) {
7408                 return;
7409         }
7410         if ( noImpact || impulse.LengthSqr() < Square( impulseThreshold ) ) {
7411                 return;
7412         }
7413         idMat3 invWorldInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis;
7414         bodies[id]->current->spatialVelocity.SubVec3(0) += bodies[id]->invMass * impulse;
7415         bodies[id]->current->spatialVelocity.SubVec3(1) += invWorldInertiaTensor * (point - bodies[id]->current->worldOrigin).Cross( impulse );
7416         Activate();
7417 }
7418
7419 /*
7420 ================
7421 idPhysics_AF::AddForce
7422 ================
7423 */
7424 void idPhysics_AF::AddForce( const int id, const idVec3 &point, const idVec3 &force ) {
7425         if ( noImpact ) {
7426                 return;
7427         }
7428         if ( id < 0 || id >= bodies.Num() ) {
7429                 return;
7430         }
7431         bodies[id]->current->externalForce.SubVec3( 0 ) += force;
7432         bodies[id]->current->externalForce.SubVec3( 1 ) += (point - bodies[id]->current->worldOrigin).Cross( force );
7433         Activate();
7434 }
7435
7436 /*
7437 ================
7438 idPhysics_AF::IsAtRest
7439 ================
7440 */
7441 bool idPhysics_AF::IsAtRest( void ) const {
7442         return current.atRest >= 0;
7443 }
7444
7445 /*
7446 ================
7447 idPhysics_AF::GetRestStartTime
7448 ================
7449 */
7450 int idPhysics_AF::GetRestStartTime( void ) const {
7451         return current.atRest;
7452 }
7453
7454 /*
7455 ================
7456 idPhysics_AF::IsPushable
7457 ================
7458 */
7459 bool idPhysics_AF::IsPushable( void ) const {
7460         return ( !noImpact && ( masterBody == NULL || forcePushable ) );
7461 }
7462
7463 /*
7464 ================
7465 idPhysics_AF::SaveState
7466 ================
7467 */
7468 void idPhysics_AF::SaveState( void ) {
7469         int i;
7470
7471         saved = current;
7472
7473         for ( i = 0; i < bodies.Num(); i++ ) {
7474                 memcpy( &bodies[i]->saved, bodies[i]->current, sizeof( AFBodyPState_t ) );
7475         }
7476 }
7477
7478 /*
7479 ================
7480 idPhysics_AF::RestoreState
7481 ================
7482 */
7483 void idPhysics_AF::RestoreState( void ) {
7484         int i;
7485
7486         current = saved;
7487
7488         for ( i = 0; i < bodies.Num(); i++ ) {
7489                 *(bodies[i]->current) = bodies[i]->saved;
7490         }
7491
7492         EvaluateContacts();
7493 }
7494
7495 /*
7496 ================
7497 idPhysics_AF::SetOrigin
7498 ================
7499 */
7500 void idPhysics_AF::SetOrigin( const idVec3 &newOrigin, int id ) {
7501         if ( masterBody ) {
7502                 Translate( masterBody->current->worldOrigin + masterBody->current->worldAxis * newOrigin - bodies[0]->current->worldOrigin );
7503         } else {
7504                 Translate( newOrigin - bodies[0]->current->worldOrigin );
7505         }
7506 }
7507
7508 /*
7509 ================
7510 idPhysics_AF::SetAxis
7511 ================
7512 */
7513 void idPhysics_AF::SetAxis( const idMat3 &newAxis, int id ) {
7514         idMat3 axis;
7515         idRotation rotation;
7516
7517         if ( masterBody ) {
7518                 axis = bodies[0]->current->worldAxis.Transpose() * ( newAxis * masterBody->current->worldAxis );
7519         } else {
7520                 axis = bodies[0]->current->worldAxis.Transpose() * newAxis;
7521         }
7522         rotation = axis.ToRotation();
7523         rotation.SetOrigin( bodies[0]->current->worldOrigin );
7524
7525         Rotate( rotation );
7526 }
7527
7528 /*
7529 ================
7530 idPhysics_AF::Translate
7531 ================
7532 */
7533 void idPhysics_AF::Translate( const idVec3 &translation, int id ) {
7534         int i;
7535         idAFBody *body;
7536
7537         if ( !worldConstraintsLocked ) {
7538                 // translate constraints attached to the world
7539                 for ( i = 0; i < constraints.Num(); i++ ) {
7540                         constraints[i]->Translate( translation );
7541                 }
7542         }
7543
7544         // translate all the bodies
7545         for ( i = 0; i < bodies.Num(); i++ ) {
7546
7547                 body = bodies[i];
7548                 body->current->worldOrigin += translation;
7549         }
7550
7551         Activate();
7552
7553         UpdateClipModels();
7554 }
7555
7556 /*
7557 ================
7558 idPhysics_AF::Rotate
7559 ================
7560 */
7561 void idPhysics_AF::Rotate( const idRotation &rotation, int id ) {
7562         int i;
7563         idAFBody *body;
7564
7565         if ( !worldConstraintsLocked ) {
7566                 // rotate constraints attached to the world
7567                 for ( i = 0; i < constraints.Num(); i++ ) {
7568                         constraints[i]->Rotate( rotation );
7569                 }
7570         }
7571
7572         // rotate all the bodies
7573         for ( i = 0; i < bodies.Num(); i++ ) {
7574                 body = bodies[i];
7575
7576                 body->current->worldOrigin *= rotation;
7577                 body->current->worldAxis *= rotation.ToMat3();
7578         }
7579
7580         Activate();
7581
7582         UpdateClipModels();
7583 }
7584
7585 /*
7586 ================
7587 idPhysics_AF::GetOrigin
7588 ================
7589 */
7590 const idVec3 &idPhysics_AF::GetOrigin( int id ) const {
7591         if ( id < 0 || id >= bodies.Num() ) {
7592                 return vec3_origin;
7593         }
7594         else {
7595                 return bodies[id]->current->worldOrigin;
7596         }
7597 }
7598
7599 /*
7600 ================
7601 idPhysics_AF::GetAxis
7602 ================
7603 */
7604 const idMat3 &idPhysics_AF::GetAxis( int id ) const {
7605         if ( id < 0 || id >= bodies.Num() ) {
7606                 return mat3_identity;
7607         }
7608         else {
7609                 return bodies[id]->current->worldAxis;
7610         }
7611 }
7612
7613 /*
7614 ================
7615 idPhysics_AF::SetLinearVelocity
7616 ================
7617 */
7618 void idPhysics_AF::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) {
7619         if ( id < 0 || id >= bodies.Num() ) {
7620                 return;
7621         }
7622         bodies[id]->current->spatialVelocity.SubVec3( 0 ) = newLinearVelocity;
7623         Activate();
7624 }
7625
7626 /*
7627 ================
7628 idPhysics_AF::SetAngularVelocity
7629 ================
7630 */
7631 void idPhysics_AF::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) {
7632         if ( id < 0 || id >= bodies.Num() ) {
7633                 return;
7634         }
7635         bodies[id]->current->spatialVelocity.SubVec3( 1 ) = newAngularVelocity;
7636         Activate();
7637 }
7638
7639 /*
7640 ================
7641 idPhysics_AF::GetLinearVelocity
7642 ================
7643 */
7644 const idVec3 &idPhysics_AF::GetLinearVelocity( int id ) const {
7645         if ( id < 0 || id >= bodies.Num() ) {
7646                 return vec3_origin;
7647         }
7648         else {
7649                 return bodies[id]->current->spatialVelocity.SubVec3( 0 );
7650         }
7651 }
7652
7653 /*
7654 ================
7655 idPhysics_AF::GetAngularVelocity
7656 ================
7657 */
7658 const idVec3 &idPhysics_AF::GetAngularVelocity( int id ) const {
7659         if ( id < 0 || id >= bodies.Num() ) {
7660                 return vec3_origin;
7661         }
7662         else {
7663                 return bodies[id]->current->spatialVelocity.SubVec3( 1 );
7664         }
7665 }
7666
7667 /*
7668 ================
7669 idPhysics_AF::ClipTranslation
7670 ================
7671 */
7672 void idPhysics_AF::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const {
7673         int i;
7674         idAFBody *body;
7675         trace_t bodyResults;
7676
7677         results.fraction = 1.0f;
7678
7679         for ( i = 0; i < bodies.Num(); i++ ) {
7680                 body = bodies[i];
7681
7682                 if ( body->clipModel->IsTraceModel() ) {
7683                         if ( model ) {
7684                                 gameLocal.clip.TranslationModel( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation,
7685                                                                         body->clipModel, body->current->worldAxis, body->clipMask,
7686                                                                                 model->Handle(), model->GetOrigin(), model->GetAxis() );
7687                         }
7688                         else {
7689                                 gameLocal.clip.Translation( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation,
7690                                                                         body->clipModel, body->current->worldAxis, body->clipMask, self );
7691                         }
7692                         if ( bodyResults.fraction < results.fraction ) {
7693                                 results = bodyResults;
7694                         }
7695                 }
7696         }
7697
7698         results.endpos = bodies[0]->current->worldOrigin + results.fraction * translation;
7699         results.endAxis = bodies[0]->current->worldAxis;
7700 }
7701
7702 /*
7703 ================
7704 idPhysics_AF::ClipRotation
7705 ================
7706 */
7707 void idPhysics_AF::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const {
7708         int i;
7709         idAFBody *body;
7710         trace_t bodyResults;
7711         idRotation partialRotation;
7712
7713         results.fraction = 1.0f;
7714
7715         for ( i = 0; i < bodies.Num(); i++ ) {
7716                 body = bodies[i];
7717
7718                 if ( body->clipModel->IsTraceModel() ) {
7719                         if ( model ) {
7720                                 gameLocal.clip.RotationModel( bodyResults, body->current->worldOrigin, rotation,
7721                                                                         body->clipModel, body->current->worldAxis, body->clipMask,
7722                                                                                 model->Handle(), model->GetOrigin(), model->GetAxis() );
7723                         }
7724                         else {
7725                                 gameLocal.clip.Rotation( bodyResults, body->current->worldOrigin, rotation,
7726                                                                         body->clipModel, body->current->worldAxis, body->clipMask, self );
7727                         }
7728                         if ( bodyResults.fraction < results.fraction ) {
7729                                 results = bodyResults;
7730                         }
7731                 }
7732         }
7733
7734         partialRotation = rotation * results.fraction;
7735         results.endpos = bodies[0]->current->worldOrigin * partialRotation;
7736         results.endAxis = bodies[0]->current->worldAxis * partialRotation.ToMat3();
7737 }
7738
7739 /*
7740 ================
7741 idPhysics_AF::ClipContents
7742 ================
7743 */
7744 int idPhysics_AF::ClipContents( const idClipModel *model ) const {
7745         int i, contents;
7746         idAFBody *body;
7747
7748         contents = 0;
7749
7750         for ( i = 0; i < bodies.Num(); i++ ) {
7751                 body = bodies[i];
7752
7753                 if ( body->clipModel->IsTraceModel() ) {
7754                         if ( model ) {
7755                                 contents |= gameLocal.clip.ContentsModel( body->current->worldOrigin,
7756                                                                         body->clipModel, body->current->worldAxis, -1,
7757                                                                                 model->Handle(), model->GetOrigin(), model->GetAxis() );
7758                         }
7759                         else {
7760                                 contents |= gameLocal.clip.Contents( body->current->worldOrigin,
7761                                                                         body->clipModel, body->current->worldAxis, -1, NULL );
7762                         }
7763                 }
7764         }
7765
7766         return contents;
7767 }
7768
7769 /*
7770 ================
7771 idPhysics_AF::DisableClip
7772 ================
7773 */
7774 void idPhysics_AF::DisableClip( void ) {
7775         int i;
7776
7777         for ( i = 0; i < bodies.Num(); i++ ) {
7778                 bodies[i]->clipModel->Disable();
7779         }
7780 }
7781
7782 /*
7783 ================
7784 idPhysics_AF::EnableClip
7785 ================
7786 */
7787 void idPhysics_AF::EnableClip( void ) {
7788         int i;
7789
7790         for ( i = 0; i < bodies.Num(); i++ ) {
7791                 bodies[i]->clipModel->Enable();
7792         }
7793 }
7794
7795 /*
7796 ================
7797 idPhysics_AF::UnlinkClip
7798 ================
7799 */
7800 void idPhysics_AF::UnlinkClip( void ) {
7801         int i;
7802
7803         for ( i = 0; i < bodies.Num(); i++ ) {
7804                 bodies[i]->clipModel->Unlink();
7805         }
7806 }
7807
7808 /*
7809 ================
7810 idPhysics_AF::LinkClip
7811 ================
7812 */
7813 void idPhysics_AF::LinkClip( void ) {
7814         UpdateClipModels();
7815 }
7816
7817 /*
7818 ================
7819 idPhysics_AF::SetPushed
7820 ================
7821 */
7822 void idPhysics_AF::SetPushed( int deltaTime ) {
7823         idAFBody *body;
7824         idRotation rotation;
7825
7826         if ( bodies.Num() ) {
7827                 body = bodies[0];
7828                 rotation = ( body->saved.worldAxis.Transpose() * body->current->worldAxis ).ToRotation();
7829
7830                 // velocity with which the af is pushed
7831                 current.pushVelocity.SubVec3(0) += ( body->current->worldOrigin - body->saved.worldOrigin ) / ( deltaTime * idMath::M_MS2SEC );
7832                 current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( deltaTime * idMath::M_MS2SEC );
7833         }
7834 }
7835
7836 /*
7837 ================
7838 idPhysics_AF::GetPushedLinearVelocity
7839 ================
7840 */
7841 const idVec3 &idPhysics_AF::GetPushedLinearVelocity( const int id ) const {
7842         return current.pushVelocity.SubVec3(0);
7843 }
7844
7845 /*
7846 ================
7847 idPhysics_AF::GetPushedAngularVelocity
7848 ================
7849 */
7850 const idVec3 &idPhysics_AF::GetPushedAngularVelocity( const int id ) const {
7851         return current.pushVelocity.SubVec3(1);
7852 }
7853
7854 /*
7855 ================
7856 idPhysics_AF::SetMaster
7857
7858    the binding is orientated based on the constraints being used
7859 ================
7860 */
7861 void idPhysics_AF::SetMaster( idEntity *master, const bool orientated ) {
7862         int i;
7863         idVec3 masterOrigin;
7864         idMat3 masterAxis;
7865         idRotation rotation;
7866
7867         if ( master ) {
7868                 self->GetMasterPosition( masterOrigin, masterAxis );
7869                 if ( !masterBody ) {
7870                         masterBody = new idAFBody();
7871                         // translate and rotate all the constraints with body2 == NULL from world space to master space
7872                         rotation = masterAxis.Transpose().ToRotation();
7873                         for ( i = 0; i < constraints.Num(); i++ ) {
7874                                 if ( constraints[i]->GetBody2() == NULL ) {
7875                                         constraints[i]->Translate( -masterOrigin );
7876                                         constraints[i]->Rotate( rotation );
7877                                 }
7878                         }
7879                         Activate();
7880                 }
7881                 masterBody->current->worldOrigin = masterOrigin;
7882                 masterBody->current->worldAxis = masterAxis;
7883         }
7884         else {
7885                 if ( masterBody ) {
7886                         // translate and rotate all the constraints with body2 == NULL from master space to world space
7887                         rotation = masterBody->current->worldAxis.ToRotation();
7888                         for ( i = 0; i < constraints.Num(); i++ ) {
7889                                 if ( constraints[i]->GetBody2() == NULL ) {
7890                                         constraints[i]->Rotate( rotation );
7891                                         constraints[i]->Translate( masterBody->current->worldOrigin );
7892                                 }
7893                         }
7894                         delete masterBody;
7895                         masterBody = NULL;
7896                         Activate();
7897                 }
7898         }
7899 }
7900
7901
7902 const float     AF_VELOCITY_MAX                         = 16000;
7903 const int       AF_VELOCITY_TOTAL_BITS          = 16;
7904 const int       AF_VELOCITY_EXPONENT_BITS       = idMath::BitsForInteger( idMath::BitsForFloat( AF_VELOCITY_MAX ) ) + 1;
7905 const int       AF_VELOCITY_MANTISSA_BITS       = AF_VELOCITY_TOTAL_BITS - 1 - AF_VELOCITY_EXPONENT_BITS;
7906 const float     AF_FORCE_MAX                            = 1e20f;
7907 const int       AF_FORCE_TOTAL_BITS                     = 16;
7908 const int       AF_FORCE_EXPONENT_BITS          = idMath::BitsForInteger( idMath::BitsForFloat( AF_FORCE_MAX ) ) + 1;
7909 const int       AF_FORCE_MANTISSA_BITS          = AF_FORCE_TOTAL_BITS - 1 - AF_FORCE_EXPONENT_BITS;
7910
7911 /*
7912 ================
7913 idPhysics_AF::WriteToSnapshot
7914 ================
7915 */
7916 void idPhysics_AF::WriteToSnapshot( idBitMsgDelta &msg ) const {
7917         int i;
7918         idCQuat quat;
7919
7920         msg.WriteLong( current.atRest );
7921         msg.WriteFloat( current.noMoveTime );
7922         msg.WriteFloat( current.activateTime );
7923         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7924         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7925         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7926         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7927         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7928         msg.WriteDeltaFloat( 0.0f, current.pushVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7929
7930         msg.WriteByte( bodies.Num() );
7931
7932         for ( i = 0; i < bodies.Num(); i++ ) {
7933                 AFBodyPState_t *state = bodies[i]->current;
7934                 quat = state->worldAxis.ToCQuat();
7935
7936                 msg.WriteFloat( state->worldOrigin[0] );
7937                 msg.WriteFloat( state->worldOrigin[1] );
7938                 msg.WriteFloat( state->worldOrigin[2] );
7939                 msg.WriteFloat( quat.x );
7940                 msg.WriteFloat( quat.y );
7941                 msg.WriteFloat( quat.z );
7942                 msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7943                 msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7944                 msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7945                 msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7946                 msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7947                 msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7948 /*              msg.WriteDeltaFloat( 0.0f, state->externalForce[0], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7949                 msg.WriteDeltaFloat( 0.0f, state->externalForce[1], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7950                 msg.WriteDeltaFloat( 0.0f, state->externalForce[2], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7951                 msg.WriteDeltaFloat( 0.0f, state->externalForce[3], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7952                 msg.WriteDeltaFloat( 0.0f, state->externalForce[4], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7953                 msg.WriteDeltaFloat( 0.0f, state->externalForce[5], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7954 */
7955         }
7956 }
7957
7958 /*
7959 ================
7960 idPhysics_AF::ReadFromSnapshot
7961 ================
7962 */
7963 void idPhysics_AF::ReadFromSnapshot( const idBitMsgDelta &msg ) {
7964         int i, num;
7965         idCQuat quat;
7966
7967         current.atRest = msg.ReadLong();
7968         current.noMoveTime = msg.ReadFloat();
7969         current.activateTime = msg.ReadFloat();
7970         current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7971         current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7972         current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7973         current.pushVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7974         current.pushVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7975         current.pushVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7976
7977         num = msg.ReadByte();
7978         assert( num == bodies.Num() );
7979
7980         for ( i = 0; i < bodies.Num(); i++ ) {
7981                 AFBodyPState_t *state = bodies[i]->current;
7982
7983                 state->worldOrigin[0] = msg.ReadFloat();
7984                 state->worldOrigin[1] = msg.ReadFloat();
7985                 state->worldOrigin[2] = msg.ReadFloat();
7986                 quat.x = msg.ReadFloat();
7987                 quat.y = msg.ReadFloat();
7988                 quat.z = msg.ReadFloat();
7989                 state->spatialVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7990                 state->spatialVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7991                 state->spatialVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7992                 state->spatialVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7993                 state->spatialVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7994                 state->spatialVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
7995 /*              state->externalForce[0] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7996                 state->externalForce[1] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7997                 state->externalForce[2] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7998                 state->externalForce[3] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
7999                 state->externalForce[4] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
8000                 state->externalForce[5] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
8001 */
8002                 state->worldAxis = quat.ToMat3();
8003         }
8004
8005         UpdateClipModels();
8006 }