neo/game/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
  35 /*
  36 ===============================================================================
  37
  38   Articulated figure controller.
  39
  40 ===============================================================================
  41 */
  42 #define ARTICULATED_FIGURE_ANIM         "af_pose"
  43 #define POSE_BOUNDS_EXPANSION           5.0f
  44
  45 /*
  46 ================
  47 idAF::idAF
  48 ================
  49 */
  50 idAF::idAF( void ) {
  51         self = NULL;
  52         animator = NULL;
  53         modifiedAnim = 0;
  54         baseOrigin.Zero();
  55         baseAxis.Identity();
  56         poseTime = -1;
  57         restStartTime = -1;
  58         isLoaded = false;
  59         isActive = false;
  60         hasBindConstraints = false;
  61 }
  62
  63 /*
  64 ================
  65 idAF::~idAF
  66 ================
  67 */
  68 idAF::~idAF( void ) {
  69 }
  70
  71 /*
  72 ================
  73 idAF::Save
  74 ================
  75 */
  76 void idAF::Save( idSaveGame *savefile ) const {
  77         savefile->WriteObject( self );
  78         savefile->WriteString( GetName() );
  79         savefile->WriteBool( hasBindConstraints );
  80         savefile->WriteVec3( baseOrigin );
  81         savefile->WriteMat3( baseAxis );
  82         savefile->WriteInt( poseTime );
  83         savefile->WriteInt( restStartTime );
  84         savefile->WriteBool( isLoaded );
  85         savefile->WriteBool( isActive );
  86         savefile->WriteStaticObject( physicsObj );
  87 }
  88
  89 /*
  90 ================
  91 idAF::Restore
  92 ================
  93 */
  94 void idAF::Restore( idRestoreGame *savefile ) {
  95         savefile->ReadObject( reinterpret_cast<idClass *&>( self ) );
  96         savefile->ReadString( name );
  97         savefile->ReadBool( hasBindConstraints );
  98         savefile->ReadVec3( baseOrigin );
  99         savefile->ReadMat3( baseAxis );
 100         savefile->ReadInt( poseTime );
 101         savefile->ReadInt( restStartTime );
 102         savefile->ReadBool( isLoaded );
 103         savefile->ReadBool( isActive );
 104
 105         animator = NULL;
 106         modifiedAnim = 0;
 107
 108         if ( self ) {
 109                 SetAnimator( self->GetAnimator() );
 110                 Load( self, name );
 111                 if ( hasBindConstraints ) {
 112                         AddBindConstraints();
 113                 }
 114         }
 115
 116         savefile->ReadStaticObject( physicsObj );
 117
 118         if ( self ) {
 119                 if ( isActive ) {
 120                         // clear all animations
 121                         animator->ClearAllAnims( gameLocal.time, 0 );
 122                         animator->ClearAllJoints();
 123
 124                         // switch to articulated figure physics
 125                         self->RestorePhysics( &physicsObj );
 126                         physicsObj.EnableClip();
 127                 }
 128                 UpdateAnimation();
 129         }
 130 }
 131
 132 /*
 133 ================
 134 idAF::UpdateAnimation
 135 ================
 136 */
 137 bool idAF::UpdateAnimation( void ) {
 138         int i;
 139         idVec3 origin, renderOrigin, bodyOrigin;
 140         idMat3 axis, renderAxis, bodyAxis;
 141         renderEntity_t *renderEntity;
 142
 143         if ( !IsLoaded() ) {
 144                 return false;
 145         }
 146
 147         if ( !IsActive() ) {
 148                 return false;
 149         }
 150
 151         renderEntity = self->GetRenderEntity();
 152         if ( !renderEntity ) {
 153                 return false;
 154         }
 155
 156         if ( physicsObj.IsAtRest() ) {
 157                 if ( restStartTime == physicsObj.GetRestStartTime() ) {
 158                         return false;
 159                 }
 160                 restStartTime = physicsObj.GetRestStartTime();
 161         }
 162
 163         // get the render position
 164         origin = physicsObj.GetOrigin( 0 );
 165         axis = physicsObj.GetAxis( 0 );
 166         renderAxis = baseAxis.Transpose() * axis;
 167         renderOrigin = origin - baseOrigin * renderAxis;
 168
 169         // create an animation frame which reflects the current pose of the articulated figure
 170         animator->InitAFPose();
 171         for ( i = 0; i < jointMods.Num(); i++ ) {
 172                 // check for the origin joint
 173                 if ( jointMods[i].jointHandle == 0 ) {
 174                         continue;
 175                 }
 176                 bodyOrigin = physicsObj.GetOrigin( jointMods[i].bodyId );
 177                 bodyAxis = physicsObj.GetAxis( jointMods[i].bodyId );
 178                 axis = jointMods[i].jointBodyAxis.Transpose() * ( bodyAxis * renderAxis.Transpose() );
 179                 origin = ( bodyOrigin - jointMods[i].jointBodyOrigin * axis - renderOrigin ) * renderAxis.Transpose();
 180                 animator->SetAFPoseJointMod( jointMods[i].jointHandle, jointMods[i].jointMod, axis, origin );
 181         }
 182         animator->FinishAFPose( modifiedAnim, GetBounds().Expand( POSE_BOUNDS_EXPANSION ), gameLocal.time );
 183         animator->SetAFPoseBlendWeight( 1.0f );
 184
 185         return true;
 186 }
 187
 188 /*
 189 ================
 190 idAF::GetBounds
 191
 192   returns bounds for the current pose
 193 ================
 194 */
 195 idBounds idAF::GetBounds( void ) const {
 196         int i;
 197         idAFBody *body;
 198         idVec3 origin, entityOrigin;
 199         idMat3 axis, entityAxis;
 200         idBounds bounds, b;
 201
 202         bounds.Clear();
 203
 204         // get model base transform
 205         origin = physicsObj.GetOrigin( 0 );
 206         axis = physicsObj.GetAxis( 0 );
 207
 208         entityAxis = baseAxis.Transpose() * axis;
 209         entityOrigin = origin - baseOrigin * entityAxis;
 210
 211         // get bounds relative to base
 212         for ( i = 0; i < jointMods.Num(); i++ ) {
 213                 body = physicsObj.GetBody( jointMods[i].bodyId );
 214                 origin = ( body->GetWorldOrigin() - entityOrigin ) * entityAxis.Transpose();
 215                 axis = body->GetWorldAxis() * entityAxis.Transpose();
 216                 b.FromTransformedBounds( body->GetClipModel()->GetBounds(), origin, axis );
 217
 218                 bounds += b;
 219         }
 220
 221         return bounds;
 222 }
 223
 224 /*
 225 ================
 226 idAF::SetupPose
 227
 228   Transforms the articulated figure to match the current animation pose of the given entity.
 229 ================
 230 */
 231 void idAF::SetupPose( idEntity *ent, int time ) {
 232         int i;
 233         idAFBody *body;
 234         idVec3 origin;
 235         idMat3 axis;
 236         idAnimator *animatorPtr;
 237         renderEntity_t *renderEntity;
 238
 239         if ( !IsLoaded() || !ent ) {
 240                 return;
 241         }
 242
 243         animatorPtr = ent->GetAnimator();
 244         if ( !animatorPtr ) {
 245                 return;
 246         }
 247
 248         renderEntity = ent->GetRenderEntity();
 249         if ( !renderEntity ) {
 250                 return;
 251         }
 252
 253         // if the animation is driven by the physics
 254         if ( self->GetPhysics() == &physicsObj ) {
 255                 return;
 256         }
 257
 258         // if the pose was already updated this frame
 259         if ( poseTime == time ) {
 260                 return;
 261         }
 262         poseTime = time;
 263
 264         for ( i = 0; i < jointMods.Num(); i++ ) {
 265                 body = physicsObj.GetBody( jointMods[i].bodyId );
 266                 animatorPtr->GetJointTransform( jointMods[i].jointHandle, time, origin, axis );
 267                 body->SetWorldOrigin( renderEntity->origin + ( origin + jointMods[i].jointBodyOrigin * axis ) * renderEntity->axis );
 268                 body->SetWorldAxis( jointMods[i].jointBodyAxis * axis * renderEntity->axis );
 269         }
 270
 271         if ( isActive ) {
 272                 physicsObj.UpdateClipModels();
 273         }
 274 }
 275
 276 /*
 277 ================
 278 idAF::ChangePose
 279
 280    Change the articulated figure to match the current animation pose of the given entity
 281    and set the velocity relative to the previous pose.
 282 ================
 283 */
 284 void idAF::ChangePose( idEntity *ent, int time ) {
 285         int i;
 286         float invDelta;
 287         idAFBody *body;
 288         idVec3 origin, lastOrigin;
 289         idMat3 axis;
 290         idAnimator *animatorPtr;
 291         renderEntity_t *renderEntity;
 292
 293         if ( !IsLoaded() || !ent ) {
 294                 return;
 295         }
 296
 297         animatorPtr = ent->GetAnimator();
 298         if ( !animatorPtr ) {
 299                 return;
 300         }
 301
 302         renderEntity = ent->GetRenderEntity();
 303         if ( !renderEntity ) {
 304                 return;
 305         }
 306
 307         // if the animation is driven by the physics
 308         if ( self->GetPhysics() == &physicsObj ) {
 309                 return;
 310         }
 311
 312         // if the pose was already updated this frame
 313         if ( poseTime == time ) {
 314                 return;
 315         }
 316         invDelta = 1.0f / MS2SEC( time - poseTime );
 317         poseTime = time;
 318
 319         for ( i = 0; i < jointMods.Num(); i++ ) {
 320                 body = physicsObj.GetBody( jointMods[i].bodyId );
 321                 animatorPtr->GetJointTransform( jointMods[i].jointHandle, time, origin, axis );
 322                 lastOrigin = body->GetWorldOrigin();
 323                 body->SetWorldOrigin( renderEntity->origin + ( origin + jointMods[i].jointBodyOrigin * axis ) * renderEntity->axis );
 324                 body->SetWorldAxis( jointMods[i].jointBodyAxis * axis * renderEntity->axis );
 325                 body->SetLinearVelocity( ( body->GetWorldOrigin() - lastOrigin ) * invDelta );
 326         }
 327
 328         physicsObj.UpdateClipModels();
 329 }
 330
 331 /*
 332 ================
 333 idAF::EntitiesTouchingAF
 334 ================
 335 */
 336 int idAF::EntitiesTouchingAF( afTouch_t touchList[ MAX_GENTITIES ] ) const {
 337         int i, j, numClipModels;
 338         idAFBody *body;
 339         idClipModel *cm;
 340         idClipModel *clipModels[ MAX_GENTITIES ];
 341         int numTouching;
 342
 343         if ( !IsLoaded() ) {
 344                 return 0;
 345         }
 346
 347         numTouching = 0;
 348         numClipModels = gameLocal.clip.ClipModelsTouchingBounds( physicsObj.GetAbsBounds(), -1, clipModels, MAX_GENTITIES );
 349
 350         for ( i = 0; i < jointMods.Num(); i++ ) {
 351                 body = physicsObj.GetBody( jointMods[i].bodyId );
 352
 353                 for ( j = 0; j < numClipModels; j++ ) {
 354                         cm = clipModels[j];
 355
 356                         if ( !cm || cm->GetEntity() == self ) {
 357                                 continue;
 358                         }
 359
 360                         if ( !cm->IsTraceModel() ) {
 361                                 continue;
 362                         }
 363
 364                         if ( !body->GetClipModel()->GetAbsBounds().IntersectsBounds( cm->GetAbsBounds() ) ) {
 365                                 continue;
 366                         }
 367
 368                         if ( gameLocal.clip.ContentsModel( body->GetWorldOrigin(), body->GetClipModel(), body->GetWorldAxis(), -1, cm->Handle(), cm->GetOrigin(), cm->GetAxis() ) ) {
 369                                 touchList[ numTouching ].touchedByBody = body;
 370                                 touchList[ numTouching ].touchedClipModel = cm;
 371                                 touchList[ numTouching ].touchedEnt  = cm->GetEntity();
 372                                 numTouching++;
 373                                 clipModels[j] = NULL;
 374                         }
 375                 }
 376         }
 377
 378         return numTouching;
 379 }
 380
 381 /*
 382 ================
 383 idAF::BodyForClipModelId
 384 ================
 385 */
 386 int idAF::BodyForClipModelId( int id ) const {
 387         if ( id >= 0 ) {
 388                 return id;
 389         } else {
 390                 id = CLIPMODEL_ID_TO_JOINT_HANDLE( id );
 391                 if ( id < jointBody.Num() ) {
 392                         return jointBody[id];
 393                 } else {
 394                         return 0;
 395                 }
 396         }
 397 }
 398
 399 /*
 400 ================
 401 idAF::GetPhysicsToVisualTransform
 402 ================
 403 */
 404 void idAF::GetPhysicsToVisualTransform( idVec3 &origin, idMat3 &axis ) const {
 405         origin = - baseOrigin;
 406         axis = baseAxis.Transpose();
 407 }
 408
 409 /*
 410 ================
 411 idAF::GetImpactInfo
 412 ================
 413 */
 414 void idAF::GetImpactInfo( idEntity *ent, int id, const idVec3 &point, impactInfo_t *info ) {
 415         SetupPose( self, gameLocal.time );
 416         physicsObj.GetImpactInfo( BodyForClipModelId( id ), point, info );
 417 }
 418
 419 /*
 420 ================
 421 idAF::ApplyImpulse
 422 ================
 423 */
 424 void idAF::ApplyImpulse( idEntity *ent, int id, const idVec3 &point, const idVec3 &impulse ) {
 425         SetupPose( self, gameLocal.time );
 426         physicsObj.ApplyImpulse( BodyForClipModelId( id ), point, impulse );
 427 }
 428
 429 /*
 430 ================
 431 idAF::AddForce
 432 ================
 433 */
 434 void idAF::AddForce( idEntity *ent, int id, const idVec3 &point, const idVec3 &force ) {
 435         SetupPose( self, gameLocal.time );
 436         physicsObj.AddForce( BodyForClipModelId( id ), point, force );
 437 }
 438
 439 /*
 440 ================
 441 idAF::AddBody
 442
 443   Adds a body.
 444 ================
 445 */
 446 void idAF::AddBody( idAFBody *body, const idJointMat *joints, const char *jointName, const AFJointModType_t mod ) {
 447         int index;
 448         jointHandle_t handle;
 449         idVec3 origin;
 450         idMat3 axis;
 451
 452         handle = animator->GetJointHandle( jointName );
 453         if ( handle == INVALID_JOINT ) {
 454                 gameLocal.Error( "idAF for entity '%s' at (%s) modifies unknown joint '%s'", self->name.c_str(), self->GetPhysics()->GetOrigin().ToString(0), jointName );
 455         }
 456
 457         assert( handle < animator->NumJoints() );
 458         origin = joints[ handle ].ToVec3();
 459         axis = joints[ handle ].ToMat3();
 460
 461         index = jointMods.Num();
 462         jointMods.SetNum( index + 1, false );
 463         jointMods[index].bodyId = physicsObj.GetBodyId( body );
 464         jointMods[index].jointHandle = handle;
 465         jointMods[index].jointMod = mod;
 466         jointMods[index].jointBodyOrigin = ( body->GetWorldOrigin() - origin ) * axis.Transpose();
 467         jointMods[index].jointBodyAxis = body->GetWorldAxis() * axis.Transpose();
 468 }
 469
 470 /*
 471 ================
 472 idAF::SetBase
 473
 474   Sets the base body.
 475 ================
 476 */
 477 void idAF::SetBase( idAFBody *body, const idJointMat *joints ) {
 478         physicsObj.ForceBodyId( body, 0 );
 479         baseOrigin = body->GetWorldOrigin();
 480         baseAxis = body->GetWorldAxis();
 481         AddBody( body, joints, animator->GetJointName( animator->GetFirstChild( "origin" ) ), AF_JOINTMOD_AXIS );
 482 }
 483
 484 /*
 485 ================
 486 idAF::LoadBody
 487 ================
 488 */
 489 bool idAF::LoadBody( const idDeclAF_Body *fb, const idJointMat *joints ) {
 490         int id, i;
 491         float length, mass;
 492         idTraceModel trm;
 493         idClipModel *clip;
 494         idAFBody *body;
 495         idMat3 axis, inertiaTensor;
 496         idVec3 centerOfMass, origin;
 497         idBounds bounds;
 498         idList<jointHandle_t> jointList;
 499
 500         origin = fb->origin.ToVec3();
 501         axis = fb->angles.ToMat3();
 502         bounds[0] = fb->v1.ToVec3();
 503         bounds[1] = fb->v2.ToVec3();
 504
 505         switch( fb->modelType ) {
 506                 case TRM_BOX: {
 507                         trm.SetupBox( bounds );
 508                         break;
 509                 }
 510                 case TRM_OCTAHEDRON: {
 511                         trm.SetupOctahedron( bounds );
 512                         break;
 513                 }
 514                 case TRM_DODECAHEDRON: {
 515                         trm.SetupDodecahedron( bounds );
 516                         break;
 517                 }
 518                 case TRM_CYLINDER: {
 519                         trm.SetupCylinder( bounds, fb->numSides );
 520                         break;
 521                 }
 522                 case TRM_CONE: {
 523                         // place the apex at the origin
 524                         bounds[0].z -= bounds[1].z;
 525                         bounds[1].z = 0.0f;
 526                         trm.SetupCone( bounds, fb->numSides );
 527                         break;
 528                 }
 529                 case TRM_BONE: {
 530                         // direction of bone
 531                         axis[2] = fb->v2.ToVec3() - fb->v1.ToVec3();
 532                         length = axis[2].Normalize();
 533                         // axis of bone trace model
 534                         axis[2].NormalVectors( axis[0], axis[1] );
 535                         axis[1] = -axis[1];
 536                         // create bone trace model
 537                         trm.SetupBone( length, fb->width );
 538                         break;
 539                 }
 540                 default:
 541                         assert( 0 );
 542                         break;
 543         }
 544         trm.GetMassProperties( 1.0f, mass, centerOfMass, inertiaTensor );
 545         trm.Translate( -centerOfMass );
 546         origin += centerOfMass * axis;
 547
 548         body = physicsObj.GetBody( fb->name );
 549         if ( body ) {
 550                 clip = body->GetClipModel();
 551                 if ( !clip->IsEqual( trm ) ) {
 552                         clip = new idClipModel( trm );
 553                         clip->SetContents( fb->contents );
 554                         clip->Link( gameLocal.clip, self, 0, origin, axis );
 555                         body->SetClipModel( clip );
 556                 }
 557                 clip->SetContents( fb->contents );
 558                 body->SetDensity( fb->density, fb->inertiaScale );
 559                 body->SetWorldOrigin( origin );
 560                 body->SetWorldAxis( axis );
 561                 id = physicsObj.GetBodyId( body );
 562         }
 563         else {
 564                 clip = new idClipModel( trm );
 565                 clip->SetContents( fb->contents );
 566                 clip->Link( gameLocal.clip, self, 0, origin, axis );
 567                 body = new idAFBody( fb->name, clip, fb->density );
 568                 if ( fb->inertiaScale != mat3_identity ) {
 569                         body->SetDensity( fb->density, fb->inertiaScale );
 570                 }
 571                 id = physicsObj.AddBody( body );
 572         }
 573         if ( fb->linearFriction != -1.0f ) {
 574                 body->SetFriction( fb->linearFriction, fb->angularFriction, fb->contactFriction );
 575         }
 576         body->SetClipMask( fb->clipMask );
 577         body->SetSelfCollision( fb->selfCollision );
 578
 579         if ( fb->jointName == "origin" ) {
 580                 SetBase( body, joints );
 581         } else {
 582                 AFJointModType_t mod;
 583                 if ( fb->jointMod == DECLAF_JOINTMOD_AXIS ) {
 584                         mod = AF_JOINTMOD_AXIS;
 585                 } else if ( fb->jointMod == DECLAF_JOINTMOD_ORIGIN ) {
 586                         mod = AF_JOINTMOD_ORIGIN;
 587                 } else if ( fb->jointMod == DECLAF_JOINTMOD_BOTH ) {
 588                         mod = AF_JOINTMOD_BOTH;
 589                 } else {
 590                         mod = AF_JOINTMOD_AXIS;
 591                 }
 592                 AddBody( body, joints, fb->jointName, mod );
 593         }
 594
 595         if ( fb->frictionDirection.ToVec3() != vec3_origin ) {
 596                 body->SetFrictionDirection( fb->frictionDirection.ToVec3() );
 597         }
 598         if ( fb->contactMotorDirection.ToVec3() != vec3_origin ) {
 599                 body->SetContactMotorDirection( fb->contactMotorDirection.ToVec3() );
 600         }
 601
 602         // update table to find the nearest articulated figure body for a joint of the skeletal model
 603         animator->GetJointList( fb->containedJoints, jointList );
 604         for( i = 0; i < jointList.Num(); i++ ) {
 605                 if ( jointBody[ jointList[ i ] ] != -1 ) {
 606                         gameLocal.Warning( "%s: joint '%s' is already contained by body '%s'",
 607                                                 name.c_str(), animator->GetJointName( (jointHandle_t)jointList[i] ),
 608                                                         physicsObj.GetBody( jointBody[ jointList[ i ] ] )->GetName().c_str() );
 609                 }
 610                 jointBody[ jointList[ i ] ] = id;
 611         }
 612
 613         return true;
 614 }
 615
 616 /*
 617 ================
 618 idAF::LoadConstraint
 619 ================
 620 */
 621 bool idAF::LoadConstraint( const idDeclAF_Constraint *fc ) {
 622         idAFBody *body1, *body2;
 623         idAngles angles;
 624         idMat3 axis;
 625
 626         body1 = physicsObj.GetBody( fc->body1 );
 627         body2 = physicsObj.GetBody( fc->body2 );
 628
 629         switch( fc->type ) {
 630                 case DECLAF_CONSTRAINT_FIXED: {
 631                         idAFConstraint_Fixed *c;
 632                         c = static_cast<idAFConstraint_Fixed *>(physicsObj.GetConstraint( fc->name ));
 633                         if ( c ) {
 634                                 c->SetBody1( body1 );
 635                                 c->SetBody2( body2 );
 636                         }
 637                         else {
 638                                 c = new idAFConstraint_Fixed( fc->name, body1, body2 );
 639                                 physicsObj.AddConstraint( c );
 640                         }
 641                         break;
 642                 }
 643                 case DECLAF_CONSTRAINT_BALLANDSOCKETJOINT: {
 644                         idAFConstraint_BallAndSocketJoint *c;
 645                         c = static_cast<idAFConstraint_BallAndSocketJoint *>(physicsObj.GetConstraint( fc->name ));
 646                         if ( c ) {
 647                                 c->SetBody1( body1 );
 648                                 c->SetBody2( body2 );
 649                         }
 650                         else {
 651                                 c = new idAFConstraint_BallAndSocketJoint( fc->name, body1, body2 );
 652                                 physicsObj.AddConstraint( c );
 653                         }
 654                         c->SetAnchor( fc->anchor.ToVec3() );
 655                         c->SetFriction( fc->friction );
 656                         switch( fc->limit ) {
 657                                 case idDeclAF_Constraint::LIMIT_CONE: {
 658                                         c->SetConeLimit( fc->limitAxis.ToVec3(), fc->limitAngles[0], fc->shaft[0].ToVec3() );
 659                                         break;
 660                                 }
 661                                 case idDeclAF_Constraint::LIMIT_PYRAMID: {
 662                                         angles = fc->limitAxis.ToVec3().ToAngles();
 663                                         angles.roll = fc->limitAngles[2];
 664                                         axis = angles.ToMat3();
 665                                         c->SetPyramidLimit( axis[0], axis[1], fc->limitAngles[0], fc->limitAngles[1], fc->shaft[0].ToVec3() );
 666                                         break;
 667                                 }
 668                                 default: {
 669                                         c->SetNoLimit();
 670                                         break;
 671                                 }
 672                         }
 673                         break;
 674                 }
 675                 case DECLAF_CONSTRAINT_UNIVERSALJOINT: {
 676                         idAFConstraint_UniversalJoint *c;
 677                         c = static_cast<idAFConstraint_UniversalJoint *>(physicsObj.GetConstraint( fc->name ));
 678                         if ( c ) {
 679                                 c->SetBody1( body1 );
 680                                 c->SetBody2( body2 );
 681                         }
 682                         else {
 683                                 c = new idAFConstraint_UniversalJoint( fc->name, body1, body2 );
 684                                 physicsObj.AddConstraint( c );
 685                         }
 686                         c->SetAnchor( fc->anchor.ToVec3() );
 687                         c->SetShafts( fc->shaft[0].ToVec3(), fc->shaft[1].ToVec3() );
 688                         c->SetFriction( fc->friction );
 689                         switch( fc->limit ) {
 690                                 case idDeclAF_Constraint::LIMIT_CONE: {
 691                                         c->SetConeLimit( fc->limitAxis.ToVec3(), fc->limitAngles[0] );
 692                                         break;
 693                                 }
 694                                 case idDeclAF_Constraint::LIMIT_PYRAMID: {
 695                                         angles = fc->limitAxis.ToVec3().ToAngles();
 696                                         angles.roll = fc->limitAngles[2];
 697                                         axis = angles.ToMat3();
 698                                         c->SetPyramidLimit( axis[0], axis[1], fc->limitAngles[0], fc->limitAngles[1] );
 699                                         break;
 700                                 }
 701                                 default: {
 702                                         c->SetNoLimit();
 703                                         break;
 704                                 }
 705                         }
 706                         break;
 707                 }
 708                 case DECLAF_CONSTRAINT_HINGE: {
 709                         idAFConstraint_Hinge *c;
 710                         c = static_cast<idAFConstraint_Hinge *>(physicsObj.GetConstraint( fc->name ));
 711                         if ( c ) {
 712                                 c->SetBody1( body1 );
 713                                 c->SetBody2( body2 );
 714                         }
 715                         else {
 716                                 c = new idAFConstraint_Hinge( fc->name, body1, body2 );
 717                                 physicsObj.AddConstraint( c );
 718                         }
 719                         c->SetAnchor( fc->anchor.ToVec3() );
 720                         c->SetAxis( fc->axis.ToVec3() );
 721                         c->SetFriction( fc->friction );
 722                         switch( fc->limit ) {
 723                                 case idDeclAF_Constraint::LIMIT_CONE: {
 724                                         idVec3 left, up, axis, shaft;
 725                                         fc->axis.ToVec3().OrthogonalBasis( left, up );
 726                                         axis = left * idRotation( vec3_origin, fc->axis.ToVec3(), fc->limitAngles[0] );
 727                                         shaft = left * idRotation( vec3_origin, fc->axis.ToVec3(), fc->limitAngles[2] );
 728                                         c->SetLimit( axis, fc->limitAngles[1], shaft );
 729                                         break;
 730                                 }
 731                                 default: {
 732                                         c->SetNoLimit();
 733                                         break;
 734                                 }
 735                         }
 736                         break;
 737                 }
 738                 case DECLAF_CONSTRAINT_SLIDER: {
 739                         idAFConstraint_Slider *c;
 740                         c = static_cast<idAFConstraint_Slider *>(physicsObj.GetConstraint( fc->name ));
 741                         if ( c ) {
 742                                 c->SetBody1( body1 );
 743                                 c->SetBody2( body2 );
 744                         }
 745                         else {
 746                                 c = new idAFConstraint_Slider( fc->name, body1, body2 );
 747                                 physicsObj.AddConstraint( c );
 748                         }
 749                         c->SetAxis( fc->axis.ToVec3() );
 750                         break;
 751                 }
 752                 case DECLAF_CONSTRAINT_SPRING: {
 753                         idAFConstraint_Spring *c;
 754                         c = static_cast<idAFConstraint_Spring *>(physicsObj.GetConstraint( fc->name ));
 755                         if ( c ) {
 756                                 c->SetBody1( body1 );
 757                                 c->SetBody2( body2 );
 758                         }
 759                         else {
 760                                 c = new idAFConstraint_Spring( fc->name, body1, body2 );
 761                                 physicsObj.AddConstraint( c );
 762                         }
 763                         c->SetAnchor( fc->anchor.ToVec3(), fc->anchor2.ToVec3() );
 764                         c->SetSpring( fc->stretch, fc->compress, fc->damping, fc->restLength );
 765                         c->SetLimit( fc->minLength, fc->maxLength );
 766                         break;
 767                 }
 768         }
 769         return true;
 770 }
 771
 772 /*
 773 ================
 774 GetJointTransform
 775 ================
 776 */
 777 static bool GetJointTransform( void *model, const idJointMat *frame, const char *jointName, idVec3 &origin, idMat3 &axis ) {
 778         jointHandle_t   joint;
 779
 780         joint = reinterpret_cast<idAnimator *>(model)->GetJointHandle( jointName );
 781         if ( ( joint >= 0 ) && ( joint < reinterpret_cast<idAnimator *>(model)->NumJoints() ) ) {
 782                 origin = frame[ joint ].ToVec3();
 783                 axis = frame[ joint ].ToMat3();
 784                 return true;
 785         } else {
 786                 return false;
 787         }
 788 }
 789
 790 /*
 791 ================
 792 idAF::Load
 793 ================
 794 */
 795 bool idAF::Load( idEntity *ent, const char *fileName ) {
 796         int i, j;
 797         const idDeclAF *file;
 798         const idDeclModelDef *modelDef;
 799         idRenderModel *model;
 800         int numJoints;
 801         idJointMat *joints;
 802
 803         assert( ent );
 804
 805         self = ent;
 806         physicsObj.SetSelf( self );
 807
 808         if ( animator == NULL ) {
 809                 gameLocal.Warning( "Couldn't load af '%s' for entity '%s' at (%s): NULL animator\n", name.c_str(), ent->name.c_str(), ent->GetPhysics()->GetOrigin().ToString(0) );
 810                 return false;
 811         }
 812
 813         name = fileName;
 814         name.StripFileExtension();
 815
 816         file = static_cast<const idDeclAF *>( declManager->FindType( DECL_AF, name ) );
 817         if ( !file ) {
 818                 gameLocal.Warning( "Couldn't load af '%s' for entity '%s' at (%s)\n", name.c_str(), ent->name.c_str(), ent->GetPhysics()->GetOrigin().ToString(0) );
 819                 return false;
 820         }
 821
 822         if ( file->bodies.Num() == 0 || file->bodies[0]->jointName != "origin" ) {
 823                 gameLocal.Warning( "idAF::Load: articulated figure '%s' for entity '%s' at (%s) has no body which modifies the origin joint.",
 824                                                         name.c_str(), ent->name.c_str(), ent->GetPhysics()->GetOrigin().ToString(0) );
 825                 return false;
 826         }
 827
 828         modelDef = animator->ModelDef();
 829         if ( modelDef == NULL || modelDef->GetState() == DS_DEFAULTED ) {
 830                 gameLocal.Warning( "idAF::Load: articulated figure '%s' for entity '%s' at (%s) has no or defaulted modelDef '%s'",
 831                                                         name.c_str(), ent->name.c_str(), ent->GetPhysics()->GetOrigin().ToString(0), modelDef ? modelDef->GetName() : "" );
 832                 return false;
 833         }
 834
 835         model = animator->ModelHandle();
 836         if ( model == NULL || model->IsDefaultModel() ) {
 837                 gameLocal.Warning( "idAF::Load: articulated figure '%s' for entity '%s' at (%s) has no or defaulted model '%s'",
 838                                                         name.c_str(), ent->name.c_str(), ent->GetPhysics()->GetOrigin().ToString(0), model ? model->Name() : "" );
 839                 return false;
 840         }
 841
 842         // get the modified animation
 843         modifiedAnim = animator->GetAnim( ARTICULATED_FIGURE_ANIM );
 844         if ( !modifiedAnim ) {
 845                 gameLocal.Warning( "idAF::Load: articulated figure '%s' for entity '%s' at (%s) has no modified animation '%s'",
 846                                                         name.c_str(), ent->name.c_str(), ent->GetPhysics()->GetOrigin().ToString(0), ARTICULATED_FIGURE_ANIM );
 847                 return false;
 848         }
 849
 850         // create the animation frame used to setup the articulated figure
 851         numJoints = animator->NumJoints();
 852         joints = ( idJointMat * )_alloca16( numJoints * sizeof( joints[0] ) );
 853         gameEdit->ANIM_CreateAnimFrame( model, animator->GetAnim( modifiedAnim )->MD5Anim( 0 ), numJoints, joints, 1, animator->ModelDef()->GetVisualOffset(), animator->RemoveOrigin() );
 854
 855         // set all vector positions from model joints
 856         file->Finish( GetJointTransform, joints, animator );
 857
 858         // initialize articulated figure physics
 859         physicsObj.SetGravity( gameLocal.GetGravity() );
 860         physicsObj.SetClipMask( file->clipMask );
 861         physicsObj.SetDefaultFriction( file->defaultLinearFriction, file->defaultAngularFriction, file->defaultContactFriction );
 862         physicsObj.SetSuspendSpeed( file->suspendVelocity, file->suspendAcceleration );
 863         physicsObj.SetSuspendTolerance( file->noMoveTime, file->noMoveTranslation, file->noMoveRotation );
 864         physicsObj.SetSuspendTime( file->minMoveTime, file->maxMoveTime );
 865         physicsObj.SetSelfCollision( file->selfCollision );
 866
 867         // clear the list with transforms from joints to bodies
 868         jointMods.SetNum( 0, false );
 869
 870         // clear the joint to body conversion list
 871         jointBody.AssureSize( animator->NumJoints() );
 872         for ( i = 0; i < jointBody.Num(); i++ ) {
 873                 jointBody[i] = -1;
 874         }
 875
 876         // delete any bodies in the physicsObj that are no longer in the idDeclAF
 877         for ( i = 0; i < physicsObj.GetNumBodies(); i++ ) {
 878                 idAFBody *body = physicsObj.GetBody( i );
 879                 for ( j = 0; j < file->bodies.Num(); j++ ) {
 880                         if ( file->bodies[j]->name.Icmp( body->GetName() ) == 0 ) {
 881                                 break;
 882                         }
 883                 }
 884                 if ( j >= file->bodies.Num() ) {
 885                         physicsObj.DeleteBody( i );
 886                         i--;
 887                 }
 888         }
 889
 890         // delete any constraints in the physicsObj that are no longer in the idDeclAF
 891         for ( i = 0; i < physicsObj.GetNumConstraints(); i++ ) {
 892                 idAFConstraint *constraint = physicsObj.GetConstraint( i );
 893                 for ( j = 0; j < file->constraints.Num(); j++ ) {
 894                         if ( file->constraints[j]->name.Icmp( constraint->GetName() ) == 0 &&
 895                                         file->constraints[j]->type == constraint->GetType() ) {
 896                                 break;
 897                         }
 898                 }
 899                 if ( j >= file->constraints.Num() ) {
 900                         physicsObj.DeleteConstraint( i );
 901                         i--;
 902                 }
 903         }
 904
 905         // load bodies from the file
 906         for ( i = 0; i < file->bodies.Num(); i++ ) {
 907                 LoadBody( file->bodies[i], joints );
 908         }
 909
 910         // load constraints from the file
 911         for ( i = 0; i < file->constraints.Num(); i++ ) {
 912                 LoadConstraint( file->constraints[i] );
 913         }
 914
 915         physicsObj.UpdateClipModels();
 916
 917         // check if each joint is contained by a body
 918         for( i = 0; i < animator->NumJoints(); i++ ) {
 919                 if ( jointBody[i] == -1 ) {
 920                         gameLocal.Warning( "idAF::Load: articulated figure '%s' for entity '%s' at (%s) joint '%s' is not contained by a body",
 921                                 name.c_str(), self->name.c_str(), self->GetPhysics()->GetOrigin().ToString(0), animator->GetJointName( (jointHandle_t)i ) );
 922                 }
 923         }
 924
 925         physicsObj.SetMass( file->totalMass );
 926         physicsObj.SetChanged();
 927
 928         // disable the articulated figure for collision detection until activated
 929         physicsObj.DisableClip();
 930
 931         isLoaded = true;
 932
 933         return true;
 934 }
 935
 936 /*
 937 ================
 938 idAF::Start
 939 ================
 940 */
 941 void idAF::Start( void ) {
 942         if ( !IsLoaded() ) {
 943                 return;
 944         }
 945         // clear all animations
 946         animator->ClearAllAnims( gameLocal.time, 0 );
 947         animator->ClearAllJoints();
 948         // switch to articulated figure physics
 949         self->SetPhysics( &physicsObj );
 950         // start the articulated figure physics simulation
 951         physicsObj.EnableClip();
 952         physicsObj.Activate();
 953         isActive = true;
 954 }
 955
 956 /*
 957 ================
 958 idAF::TestSolid
 959 ================
 960 */
 961 bool idAF::TestSolid( void ) const {
 962         int i;
 963         idAFBody *body;
 964         trace_t trace;
 965         idStr str;
 966         bool solid;
 967
 968         if ( !IsLoaded() ) {
 969                 return false;
 970         }
 971
 972         if ( !af_testSolid.GetBool() ) {
 973                 return false;
 974         }
 975
 976         solid = false;
 977
 978         for ( i = 0; i < physicsObj.GetNumBodies(); i++ ) {
 979                 body = physicsObj.GetBody( i );
 980                 if ( gameLocal.clip.Translation( trace, body->GetWorldOrigin(), body->GetWorldOrigin(), body->GetClipModel(), body->GetWorldAxis(), body->GetClipMask(), self ) ) {
 981                         float depth = idMath::Fabs( trace.c.point * trace.c.normal - trace.c.dist );
 982
 983                         body->SetWorldOrigin( body->GetWorldOrigin() + trace.c.normal * ( depth + 8.0f ) );
 984
 985                         gameLocal.DWarning( "%s: body '%s' stuck in %d (normal = %.2f %.2f %.2f, depth = %.2f)", self->name.c_str(),
 986                                                 body->GetName().c_str(), trace.c.contents, trace.c.normal.x, trace.c.normal.y, trace.c.normal.z, depth );
 987                         solid = true;
 988
 989                 }
 990         }
 991         return solid;
 992 }
 993
 994 /*
 995 ================
 996 idAF::StartFromCurrentPose
 997 ================
 998 */
 999 void idAF::StartFromCurrentPose( int inheritVelocityTime ) {
1000
1001         if ( !IsLoaded() ) {
1002                 return;
1003         }
1004
1005         // if the ragdoll should inherit velocity from the animation
1006         if ( inheritVelocityTime > 0 ) {
1007
1008                 // make sure the ragdoll is at rest
1009                 physicsObj.PutToRest();
1010
1011                 // set the pose for some time back
1012                 SetupPose( self, gameLocal.time - inheritVelocityTime );
1013
1014                 // change the pose for the current time and set velocities
1015                 ChangePose( self, gameLocal.time );
1016         }
1017         else {
1018                 // transform the articulated figure to reflect the current animation pose
1019                 SetupPose( self, gameLocal.time );
1020         }
1021
1022         physicsObj.UpdateClipModels();
1023
1024         TestSolid();
1025
1026         Start();
1027
1028         UpdateAnimation();
1029
1030         // update the render entity origin and axis
1031         self->UpdateModel();
1032
1033         // make sure the renderer gets the updated origin and axis
1034         self->Present();
1035 }
1036
1037 /*
1038 ================
1039 idAF::Stop
1040 ================
1041 */
1042 void idAF::Stop( void ) {
1043         // disable the articulated figure for collision detection
1044         physicsObj.UnlinkClip();
1045         isActive = false;
1046 }
1047
1048 /*
1049 ================
1050 idAF::Rest
1051 ================
1052 */
1053 void idAF::Rest( void ) {
1054         physicsObj.PutToRest();
1055 }
1056
1057 /*
1058 ================
1059 idAF::SetConstraintPosition
1060
1061   Only moves constraints that bind the entity to another entity.
1062 ================
1063 */
1064 void idAF::SetConstraintPosition( const char *name, const idVec3 &pos ) {
1065         idAFConstraint *constraint;
1066
1067         constraint = GetPhysics()->GetConstraint( name );
1068
1069         if ( !constraint ) {
1070                 gameLocal.Warning( "can't find a constraint with the name '%s'", name );
1071                 return;
1072         }
1073
1074         if ( constraint->GetBody2() != NULL ) {
1075                 gameLocal.Warning( "constraint '%s' does not bind to another entity", name );
1076                 return;
1077         }
1078
1079         switch( constraint->GetType() ) {
1080                 case CONSTRAINT_BALLANDSOCKETJOINT: {
1081                         idAFConstraint_BallAndSocketJoint *bs = static_cast<idAFConstraint_BallAndSocketJoint *>(constraint);
1082                         bs->Translate( pos - bs->GetAnchor() );
1083                         break;
1084                 }
1085                 case CONSTRAINT_UNIVERSALJOINT: {
1086                         idAFConstraint_UniversalJoint *uj = static_cast<idAFConstraint_UniversalJoint *>(constraint);
1087                         uj->Translate( pos - uj->GetAnchor() );
1088                         break;
1089                 }
1090                 case CONSTRAINT_HINGE: {
1091                         idAFConstraint_Hinge *hinge = static_cast<idAFConstraint_Hinge *>(constraint);
1092                         hinge->Translate( pos - hinge->GetAnchor() );
1093                         break;
1094                 }
1095                 default: {
1096                         gameLocal.Warning( "cannot set the constraint position for '%s'", name );
1097                         break;
1098                 }
1099         }
1100 }
1101
1102 /*
1103 ================
1104 idAF::SaveState
1105 ================
1106 */
1107 void idAF::SaveState( idDict &args ) const {
1108         int i;
1109         idAFBody *body;
1110         idStr key, value;
1111
1112         for ( i = 0; i < jointMods.Num(); i++ ) {
1113                 body = physicsObj.GetBody( jointMods[i].bodyId );
1114
1115                 key = "body " + body->GetName();
1116                 value = body->GetWorldOrigin().ToString( 8 );
1117                 value += " ";
1118                 value += body->GetWorldAxis().ToAngles().ToString( 8 );
1119                 args.Set( key, value );
1120         }
1121 }
1122
1123 /*
1124 ================
1125 idAF::LoadState
1126 ================
1127 */
1128 void idAF::LoadState( const idDict &args ) {
1129         const idKeyValue *kv;
1130         idStr name;
1131         idAFBody *body;
1132         idVec3 origin;
1133         idAngles angles;
1134
1135         kv = args.MatchPrefix( "body ", NULL );
1136         while ( kv ) {
1137
1138                 name = kv->GetKey();
1139                 name.Strip( "body " );
1140                 body = physicsObj.GetBody( name );
1141                 if ( body ) {
1142                         sscanf( kv->GetValue(), "%f %f %f %f %f %f", &origin.x, &origin.y, &origin.z, &angles.pitch, &angles.yaw, &angles.roll );
1143                         body->SetWorldOrigin( origin );
1144                         body->SetWorldAxis( angles.ToMat3() );
1145                 } else {
1146                         gameLocal.Warning("Unknown body part %s in articulated figure %s", name.c_str(), this->name.c_str());
1147                 }
1148
1149                 kv = args.MatchPrefix( "body ", kv );
1150         }
1151
1152         physicsObj.UpdateClipModels();
1153 }
1154
1155 /*
1156 ================
1157 idAF::AddBindConstraints
1158 ================
1159 */
1160 void idAF::AddBindConstraints( void ) {
1161         const idKeyValue *kv;
1162         idStr name;
1163         idAFBody *body;
1164         idLexer lexer;
1165         idToken type, bodyName, jointName;
1166         idVec3 origin, renderOrigin;
1167         idMat3 axis, renderAxis;
1168
1169         if ( !IsLoaded() ) {
1170                 return;
1171         }
1172
1173         const idDict &args = self->spawnArgs;
1174
1175         // get the render position
1176         origin = physicsObj.GetOrigin( 0 );
1177         axis = physicsObj.GetAxis( 0 );
1178         renderAxis = baseAxis.Transpose() * axis;
1179         renderOrigin = origin - baseOrigin * renderAxis;
1180
1181         // parse all the bind constraints
1182         for ( kv = args.MatchPrefix( "bindConstraint ", NULL ); kv; kv = args.MatchPrefix( "bindConstraint ", kv ) ) {
1183                 name = kv->GetKey();
1184                 name.Strip( "bindConstraint " );
1185
1186                 lexer.LoadMemory( kv->GetValue(), kv->GetValue().Length(), kv->GetKey() );
1187                 lexer.ReadToken( &type );
1188
1189                 lexer.ReadToken( &bodyName );
1190                 body = physicsObj.GetBody( bodyName );
1191                 if ( !body ) {
1192                         gameLocal.Warning( "idAF::AddBindConstraints: body '%s' not found on entity '%s'", bodyName.c_str(), self->name.c_str() );
1193                         lexer.FreeSource();
1194                         continue;
1195                 }
1196
1197                 if ( type.Icmp( "fixed" ) == 0 ) {
1198                         idAFConstraint_Fixed *c;
1199
1200                         c = new idAFConstraint_Fixed( name, body, NULL );
1201                         physicsObj.AddConstraint( c );
1202                 }
1203                 else if ( type.Icmp( "ballAndSocket" ) == 0 ) {
1204                         idAFConstraint_BallAndSocketJoint *c;
1205
1206                         c = new idAFConstraint_BallAndSocketJoint( name, body, NULL );
1207                         physicsObj.AddConstraint( c );
1208                         lexer.ReadToken( &jointName );
1209
1210                         jointHandle_t joint = animator->GetJointHandle( jointName );
1211                         if ( joint == INVALID_JOINT ) {
1212                                 gameLocal.Warning( "idAF::AddBindConstraints: joint '%s' not found", jointName.c_str() );
1213                         }
1214
1215                         animator->GetJointTransform( joint, gameLocal.time, origin, axis );
1216                         c->SetAnchor( renderOrigin + origin * renderAxis );
1217                 }
1218                 else if ( type.Icmp( "universal" ) == 0 ) {
1219                         idAFConstraint_UniversalJoint *c;
1220
1221                         c = new idAFConstraint_UniversalJoint( name, body, NULL );
1222                         physicsObj.AddConstraint( c );
1223                         lexer.ReadToken( &jointName );
1224
1225                         jointHandle_t joint = animator->GetJointHandle( jointName );
1226                         if ( joint == INVALID_JOINT ) {
1227                                 gameLocal.Warning( "idAF::AddBindConstraints: joint '%s' not found", jointName.c_str() );
1228                         }
1229                         animator->GetJointTransform( joint, gameLocal.time, origin, axis );
1230                         c->SetAnchor( renderOrigin + origin * renderAxis );
1231                         c->SetShafts( idVec3( 0, 0, 1 ), idVec3( 0, 0, -1 ) );
1232                 }
1233                 else {
1234                         gameLocal.Warning( "idAF::AddBindConstraints: unknown constraint type '%s' on entity '%s'", type.c_str(), self->name.c_str() );
1235                 }
1236
1237                 lexer.FreeSource();
1238         }
1239
1240         hasBindConstraints = true;
1241 }
1242
1243 /*
1244 ================
1245 idAF::RemoveBindConstraints
1246 ================
1247 */
1248 void idAF::RemoveBindConstraints( void ) {
1249         const idKeyValue *kv;
1250
1251         if ( !IsLoaded() ) {
1252                 return;
1253         }
1254
1255         const idDict &args = self->spawnArgs;
1256         idStr name;
1257
1258         kv = args.MatchPrefix( "bindConstraint ", NULL );
1259         while ( kv ) {
1260                 name = kv->GetKey();
1261                 name.Strip( "bindConstraint " );
1262
1263                 if ( physicsObj.GetConstraint( name ) ) {
1264             physicsObj.DeleteConstraint( name );
1265                 }
1266
1267                 kv = args.MatchPrefix( "bindConstraint ", kv );
1268         }
1269
1270         hasBindConstraints = false;
1271 }