neo/game/IK.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   idIK
  38
  39 ===============================================================================
  40 */
  41
  42 /*
  43 ================
  44 idIK::idIK
  45 ================
  46 */
  47 idIK::idIK( void ) {
  48         ik_activate = false;
  49         initialized = false;
  50         self = NULL;
  51         animator = NULL;
  52         modifiedAnim = 0;
  53         modelOffset.Zero();
  54 }
  55
  56 /*
  57 ================
  58 idIK::~idIK
  59 ================
  60 */
  61 idIK::~idIK( void ) {
  62 }
  63
  64 /*
  65 ================
  66 idIK::Save
  67 ================
  68 */
  69 void idIK::Save( idSaveGame *savefile ) const {
  70         savefile->WriteBool( initialized );
  71         savefile->WriteBool( ik_activate );
  72         savefile->WriteObject( self );
  73         savefile->WriteString( animator != NULL && animator->GetAnim( modifiedAnim ) ? animator->GetAnim( modifiedAnim )->Name() : "" );
  74         savefile->WriteVec3( modelOffset );
  75 }
  76
  77 /*
  78 ================
  79 idIK::Restore
  80 ================
  81 */
  82 void idIK::Restore( idRestoreGame *savefile ) {
  83         idStr anim;
  84
  85         savefile->ReadBool( initialized );
  86         savefile->ReadBool( ik_activate );
  87         savefile->ReadObject( reinterpret_cast<idClass *&>( self ) );
  88         savefile->ReadString( anim );
  89         savefile->ReadVec3( modelOffset );
  90
  91         if ( self ) {
  92                 animator = self->GetAnimator();
  93                 if ( animator == NULL || animator->ModelDef() == NULL ) {
  94                         gameLocal.Warning( "idIK::Restore: IK for entity '%s' at (%s) has no model set.",
  95                                                                 self->name.c_str(), self->GetPhysics()->GetOrigin().ToString(0) );
  96                 }
  97                 modifiedAnim = animator->GetAnim( anim );
  98                 if ( modifiedAnim == 0 ) {
  99                         gameLocal.Warning( "idIK::Restore: IK for entity '%s' at (%s) has no modified animation.",
 100                                                                         self->name.c_str(), self->GetPhysics()->GetOrigin().ToString(0) );
 101                 }
 102         } else {
 103                 animator = NULL;
 104                 modifiedAnim = 0;
 105         }
 106 }
 107
 108 /*
 109 ================
 110 idIK::IsInitialized
 111 ================
 112 */
 113 bool idIK::IsInitialized( void ) const {
 114         return initialized && ik_enable.GetBool();
 115 }
 116
 117 /*
 118 ================
 119 idIK::Init
 120 ================
 121 */
 122 bool idIK::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
 123         idRenderModel *model;
 124
 125         if ( self == NULL ) {
 126                 return false;
 127         }
 128
 129         this->self = self;
 130
 131         animator = self->GetAnimator();
 132         if ( animator == NULL || animator->ModelDef() == NULL ) {
 133                 gameLocal.Warning( "idIK::Init: IK for entity '%s' at (%s) has no model set.",
 134                                                         self->name.c_str(), self->GetPhysics()->GetOrigin().ToString(0) );
 135                 return false;
 136         }
 137         if ( animator->ModelDef()->ModelHandle() == NULL ) {
 138                 gameLocal.Warning( "idIK::Init: IK for entity '%s' at (%s) uses default model.",
 139                                                         self->name.c_str(), self->GetPhysics()->GetOrigin().ToString(0) );
 140                 return false;
 141         }
 142         model = animator->ModelHandle();
 143         if ( model == NULL ) {
 144                 gameLocal.Warning( "idIK::Init: IK for entity '%s' at (%s) has no model set.",
 145                                                         self->name.c_str(), self->GetPhysics()->GetOrigin().ToString(0) );
 146                 return false;
 147         }
 148         modifiedAnim = animator->GetAnim( anim );
 149         if ( modifiedAnim == 0 ) {
 150                 gameLocal.Warning( "idIK::Init: IK for entity '%s' at (%s) has no modified animation.",
 151                                                                 self->name.c_str(), self->GetPhysics()->GetOrigin().ToString(0) );
 152                 return false;
 153         }
 154
 155         this->modelOffset = modelOffset;
 156
 157         return true;
 158 }
 159
 160 /*
 161 ================
 162 idIK::Evaluate
 163 ================
 164 */
 165 void idIK::Evaluate( void ) {
 166 }
 167
 168 /*
 169 ================
 170 idIK::ClearJointMods
 171 ================
 172 */
 173 void idIK::ClearJointMods( void ) {
 174         ik_activate = false;
 175 }
 176
 177 /*
 178 ================
 179 idIK::SolveTwoBones
 180 ================
 181 */
 182 bool idIK::SolveTwoBones( const idVec3 &startPos, const idVec3 &endPos, const idVec3 &dir, float len0, float len1, idVec3 &jointPos ) {
 183         float length, lengthSqr, lengthInv, x, y;
 184         idVec3 vec0, vec1;
 185
 186         vec0 = endPos - startPos;
 187         lengthSqr = vec0.LengthSqr();
 188         lengthInv = idMath::InvSqrt( lengthSqr );
 189         length = lengthInv * lengthSqr;
 190
 191         // if the start and end position are too far out or too close to each other
 192         if ( length > len0 + len1 || length < idMath::Fabs( len0 - len1 ) ) {
 193                 jointPos = startPos + 0.5f * vec0;
 194                 return false;
 195         }
 196
 197         vec0 *= lengthInv;
 198         vec1 = dir - vec0 * dir * vec0;
 199         vec1.Normalize();
 200
 201         x = ( length * length + len0 * len0 - len1 * len1 ) * ( 0.5f * lengthInv );
 202         y = idMath::Sqrt( len0 * len0 - x * x );
 203
 204         jointPos = startPos + x * vec0 + y * vec1;
 205
 206         return true;
 207 }
 208
 209 /*
 210 ================
 211 idIK::GetBoneAxis
 212 ================
 213 */
 214 float idIK::GetBoneAxis( const idVec3 &startPos, const idVec3 &endPos, const idVec3 &dir, idMat3 &axis ) {
 215         float length;
 216         axis[0] = endPos - startPos;
 217         length = axis[0].Normalize();
 218         axis[1] = dir - axis[0] * dir * axis[0];
 219         axis[1].Normalize();
 220         axis[2].Cross( axis[1], axis[0] );
 221         return length;
 222 }
 223
 224
 225 /*
 226 ===============================================================================
 227
 228   idIK_Walk
 229
 230 ===============================================================================
 231 */
 232
 233 /*
 234 ================
 235 idIK_Walk::idIK_Walk
 236 ================
 237 */
 238 idIK_Walk::idIK_Walk() {
 239         int i;
 240
 241         initialized = false;
 242         footModel = NULL;
 243         numLegs = 0;
 244         enabledLegs = 0;
 245         for ( i = 0; i < MAX_LEGS; i++ ) {
 246                 footJoints[i] = INVALID_JOINT;
 247                 ankleJoints[i] = INVALID_JOINT;
 248                 kneeJoints[i] = INVALID_JOINT;
 249                 hipJoints[i] = INVALID_JOINT;
 250                 dirJoints[i] = INVALID_JOINT;
 251                 hipForward[i].Zero();
 252                 kneeForward[i].Zero();
 253                 upperLegLength[i] = 0.0f;
 254                 lowerLegLength[i] = 0.0f;
 255                 upperLegToHipJoint[i].Identity();
 256                 lowerLegToKneeJoint[i].Identity();
 257                 oldAnkleHeights[i] = 0.0f;
 258         }
 259         waistJoint = INVALID_JOINT;
 260
 261         smoothing = 0.75f;
 262         waistSmoothing = 0.5f;
 263         footShift = 0.0f;
 264         waistShift = 0.0f;
 265         minWaistFloorDist = 0.0f;
 266         minWaistAnkleDist = 0.0f;
 267         footUpTrace = 32.0f;
 268         footDownTrace = 32.0f;
 269         tiltWaist = false;
 270         usePivot = false;
 271
 272         pivotFoot = -1;
 273         pivotYaw = 0.0f;
 274         pivotPos.Zero();
 275
 276         oldHeightsValid = false;
 277         oldWaistHeight = 0.0f;
 278         waistOffset.Zero();
 279 }
 280
 281 /*
 282 ================
 283 idIK_Walk::~idIK_Walk
 284 ================
 285 */
 286 idIK_Walk::~idIK_Walk() {
 287         if ( footModel ) {
 288                 delete footModel;
 289         }
 290 }
 291
 292 /*
 293 ================
 294 idIK_Walk::Save
 295 ================
 296 */
 297 void idIK_Walk::Save( idSaveGame *savefile ) const {
 298         int i;
 299
 300         idIK::Save( savefile );
 301
 302         savefile->WriteClipModel( footModel );
 303
 304         savefile->WriteInt( numLegs );
 305         savefile->WriteInt( enabledLegs );
 306         for ( i = 0; i < MAX_LEGS; i++ )
 307                 savefile->WriteInt( footJoints[i] );
 308         for ( i = 0; i < MAX_LEGS; i++ )
 309                 savefile->WriteInt( ankleJoints[i] );
 310         for ( i = 0; i < MAX_LEGS; i++ )
 311                 savefile->WriteInt( kneeJoints[i] );
 312         for ( i = 0; i < MAX_LEGS; i++ )
 313                 savefile->WriteInt( hipJoints[i] );
 314         for ( i = 0; i < MAX_LEGS; i++ )
 315                 savefile->WriteInt( dirJoints[i] );
 316         savefile->WriteInt( waistJoint );
 317
 318         for ( i = 0; i < MAX_LEGS; i++ )
 319                 savefile->WriteVec3( hipForward[i] );
 320         for ( i = 0; i < MAX_LEGS; i++ )
 321                 savefile->WriteVec3( kneeForward[i] );
 322
 323         for ( i = 0; i < MAX_LEGS; i++ )
 324                 savefile->WriteFloat( upperLegLength[i] );
 325         for ( i = 0; i < MAX_LEGS; i++ )
 326                 savefile->WriteFloat( lowerLegLength[i] );
 327
 328         for ( i = 0; i < MAX_LEGS; i++ )
 329                 savefile->WriteMat3( upperLegToHipJoint[i] );
 330         for ( i = 0; i < MAX_LEGS; i++ )
 331                 savefile->WriteMat3( lowerLegToKneeJoint[i] );
 332
 333         savefile->WriteFloat( smoothing );
 334         savefile->WriteFloat( waistSmoothing );
 335         savefile->WriteFloat( footShift );
 336         savefile->WriteFloat( waistShift );
 337         savefile->WriteFloat( minWaistFloorDist );
 338         savefile->WriteFloat( minWaistAnkleDist );
 339         savefile->WriteFloat( footUpTrace );
 340         savefile->WriteFloat( footDownTrace );
 341         savefile->WriteBool( tiltWaist );
 342         savefile->WriteBool( usePivot );
 343
 344         savefile->WriteInt( pivotFoot );
 345         savefile->WriteFloat( pivotYaw );
 346         savefile->WriteVec3( pivotPos );
 347         savefile->WriteBool( oldHeightsValid );
 348         savefile->WriteFloat( oldWaistHeight );
 349         for ( i = 0; i < MAX_LEGS; i++ ) {
 350                 savefile->WriteFloat( oldAnkleHeights[i] );
 351         }
 352         savefile->WriteVec3( waistOffset );
 353 }
 354
 355 /*
 356 ================
 357 idIK_Walk::Restore
 358 ================
 359 */
 360 void idIK_Walk::Restore( idRestoreGame *savefile ) {
 361         int i;
 362
 363         idIK::Restore( savefile );
 364
 365         savefile->ReadClipModel( footModel );
 366
 367         savefile->ReadInt( numLegs );
 368         savefile->ReadInt( enabledLegs );
 369         for ( i = 0; i < MAX_LEGS; i++ )
 370                 savefile->ReadInt( (int&)footJoints[i] );
 371         for ( i = 0; i < MAX_LEGS; i++ )
 372                 savefile->ReadInt( (int&)ankleJoints[i] );
 373         for ( i = 0; i < MAX_LEGS; i++ )
 374                 savefile->ReadInt( (int&)kneeJoints[i] );
 375         for ( i = 0; i < MAX_LEGS; i++ )
 376                 savefile->ReadInt( (int&)hipJoints[i] );
 377         for ( i = 0; i < MAX_LEGS; i++ )
 378                 savefile->ReadInt( (int&)dirJoints[i] );
 379         savefile->ReadInt( (int&)waistJoint );
 380
 381         for ( i = 0; i < MAX_LEGS; i++ )
 382                 savefile->ReadVec3( hipForward[i] );
 383         for ( i = 0; i < MAX_LEGS; i++ )
 384                 savefile->ReadVec3( kneeForward[i] );
 385
 386         for ( i = 0; i < MAX_LEGS; i++ )
 387                 savefile->ReadFloat( upperLegLength[i] );
 388         for ( i = 0; i < MAX_LEGS; i++ )
 389                 savefile->ReadFloat( lowerLegLength[i] );
 390
 391         for ( i = 0; i < MAX_LEGS; i++ )
 392                 savefile->ReadMat3( upperLegToHipJoint[i] );
 393         for ( i = 0; i < MAX_LEGS; i++ )
 394                 savefile->ReadMat3( lowerLegToKneeJoint[i] );
 395
 396         savefile->ReadFloat( smoothing );
 397         savefile->ReadFloat( waistSmoothing );
 398         savefile->ReadFloat( footShift );
 399         savefile->ReadFloat( waistShift );
 400         savefile->ReadFloat( minWaistFloorDist );
 401         savefile->ReadFloat( minWaistAnkleDist );
 402         savefile->ReadFloat( footUpTrace );
 403         savefile->ReadFloat( footDownTrace );
 404         savefile->ReadBool( tiltWaist );
 405         savefile->ReadBool( usePivot );
 406
 407         savefile->ReadInt( pivotFoot );
 408         savefile->ReadFloat( pivotYaw );
 409         savefile->ReadVec3( pivotPos );
 410         savefile->ReadBool( oldHeightsValid );
 411         savefile->ReadFloat( oldWaistHeight );
 412         for ( i = 0; i < MAX_LEGS; i++ ) {
 413                 savefile->ReadFloat( oldAnkleHeights[i] );
 414         }
 415         savefile->ReadVec3( waistOffset );
 416 }
 417
 418 /*
 419 ================
 420 idIK_Walk::Init
 421 ================
 422 */
 423 bool idIK_Walk::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
 424         int i;
 425         float footSize;
 426         idVec3 verts[4];
 427         idTraceModel trm;
 428         const char *jointName;
 429         idVec3 dir, ankleOrigin, kneeOrigin, hipOrigin, dirOrigin;
 430         idMat3 axis, ankleAxis, kneeAxis, hipAxis;
 431
 432         static idVec3 footWinding[4] = {
 433                 idVec3(  1.0f,  1.0f, 0.0f ),
 434                 idVec3( -1.0f,  1.0f, 0.0f ),
 435                 idVec3( -1.0f, -1.0f, 0.0f ),
 436                 idVec3(  1.0f, -1.0f, 0.0f )
 437         };
 438
 439         if ( !self ) {
 440                 return false;
 441         }
 442
 443         numLegs = Min( self->spawnArgs.GetInt( "ik_numLegs", "0" ), MAX_LEGS );
 444         if ( numLegs == 0 ) {
 445                 return true;
 446         }
 447
 448         if ( !idIK::Init( self, anim, modelOffset ) ) {
 449                 return false;
 450         }
 451
 452         int numJoints = animator->NumJoints();
 453         idJointMat *joints = ( idJointMat * )_alloca16( numJoints * sizeof( joints[0] ) );
 454
 455         // create the animation frame used to setup the IK
 456         gameEdit->ANIM_CreateAnimFrame( animator->ModelHandle(), animator->GetAnim( modifiedAnim )->MD5Anim( 0 ), numJoints, joints, 1, animator->ModelDef()->GetVisualOffset() + modelOffset, animator->RemoveOrigin() );
 457
 458         enabledLegs = 0;
 459
 460         // get all the joints
 461         for ( i = 0; i < numLegs; i++ ) {
 462
 463                 jointName = self->spawnArgs.GetString( va( "ik_foot%d", i+1 ) );
 464                 footJoints[i] = animator->GetJointHandle( jointName );
 465                 if ( footJoints[i] == INVALID_JOINT ) {
 466                         gameLocal.Error( "idIK_Walk::Init: invalid foot joint '%s'", jointName );
 467                 }
 468
 469                 jointName = self->spawnArgs.GetString( va( "ik_ankle%d", i+1 ) );
 470                 ankleJoints[i] = animator->GetJointHandle( jointName );
 471                 if ( ankleJoints[i] == INVALID_JOINT ) {
 472                         gameLocal.Error( "idIK_Walk::Init: invalid ankle joint '%s'", jointName );
 473                 }
 474
 475                 jointName = self->spawnArgs.GetString( va( "ik_knee%d", i+1 ) );
 476                 kneeJoints[i] = animator->GetJointHandle( jointName );
 477                 if ( kneeJoints[i] == INVALID_JOINT ) {
 478                         gameLocal.Error( "idIK_Walk::Init: invalid knee joint '%s'\n", jointName );
 479                 }
 480
 481                 jointName = self->spawnArgs.GetString( va( "ik_hip%d", i+1 ) );
 482                 hipJoints[i] = animator->GetJointHandle( jointName );
 483                 if ( hipJoints[i] == INVALID_JOINT ) {
 484                         gameLocal.Error( "idIK_Walk::Init: invalid hip joint '%s'\n", jointName );
 485                 }
 486
 487                 jointName = self->spawnArgs.GetString( va( "ik_dir%d", i+1 ) );
 488                 dirJoints[i] = animator->GetJointHandle( jointName );
 489
 490                 enabledLegs |= 1 << i;
 491         }
 492
 493         jointName = self->spawnArgs.GetString( "ik_waist" );
 494         waistJoint = animator->GetJointHandle( jointName );
 495         if ( waistJoint == INVALID_JOINT ) {
 496                 gameLocal.Error( "idIK_Walk::Init: invalid waist joint '%s'\n", jointName );
 497         }
 498
 499         // get the leg bone lengths and rotation matrices
 500         for ( i = 0; i < numLegs; i++ ) {
 501                 oldAnkleHeights[i] = 0.0f;
 502
 503                 ankleAxis = joints[ ankleJoints[ i ] ].ToMat3();
 504                 ankleOrigin = joints[ ankleJoints[ i ] ].ToVec3();
 505
 506                 kneeAxis = joints[ kneeJoints[ i ] ].ToMat3();
 507                 kneeOrigin = joints[ kneeJoints[ i ] ].ToVec3();
 508
 509                 hipAxis = joints[ hipJoints[ i ] ].ToMat3();
 510                 hipOrigin = joints[ hipJoints[ i ] ].ToVec3();
 511
 512                 // get the IK direction
 513                 if ( dirJoints[i] != INVALID_JOINT ) {
 514                         dirOrigin = joints[ dirJoints[ i ] ].ToVec3();
 515                         dir = dirOrigin - kneeOrigin;
 516                 } else {
 517                         dir.Set( 1.0f, 0.0f, 0.0f );
 518                 }
 519
 520                 hipForward[i] = dir * hipAxis.Transpose();
 521                 kneeForward[i] = dir * kneeAxis.Transpose();
 522
 523                 // conversion from upper leg bone axis to hip joint axis
 524                 upperLegLength[i] = GetBoneAxis( hipOrigin, kneeOrigin, dir, axis );
 525                 upperLegToHipJoint[i] = hipAxis * axis.Transpose();
 526
 527                 // conversion from lower leg bone axis to knee joint axis
 528                 lowerLegLength[i] = GetBoneAxis( kneeOrigin, ankleOrigin, dir, axis );
 529                 lowerLegToKneeJoint[i] = kneeAxis * axis.Transpose();
 530         }
 531
 532         smoothing = self->spawnArgs.GetFloat( "ik_smoothing", "0.75" );
 533         waistSmoothing = self->spawnArgs.GetFloat( "ik_waistSmoothing", "0.75" );
 534         footShift = self->spawnArgs.GetFloat( "ik_footShift", "0" );
 535         waistShift = self->spawnArgs.GetFloat( "ik_waistShift", "0" );
 536         minWaistFloorDist = self->spawnArgs.GetFloat( "ik_minWaistFloorDist", "0" );
 537         minWaistAnkleDist = self->spawnArgs.GetFloat( "ik_minWaistAnkleDist", "0" );
 538         footUpTrace = self->spawnArgs.GetFloat( "ik_footUpTrace", "32" );
 539         footDownTrace = self->spawnArgs.GetFloat( "ik_footDownTrace", "32" );
 540         tiltWaist = self->spawnArgs.GetBool( "ik_tiltWaist", "0" );
 541         usePivot = self->spawnArgs.GetBool( "ik_usePivot", "0" );
 542
 543         // setup a clip model for the feet
 544         footSize = self->spawnArgs.GetFloat( "ik_footSize", "4" ) * 0.5f;
 545         if ( footSize > 0.0f ) {
 546                 for ( i = 0; i < 4; i++ ) {
 547                         verts[i] = footWinding[i] * footSize;
 548                 }
 549                 trm.SetupPolygon( verts, 4 );
 550                 footModel = new idClipModel( trm );
 551         }
 552
 553         initialized = true;
 554
 555         return true;
 556 }
 557
 558 /*
 559 ================
 560 idIK_Walk::Evaluate
 561 ================
 562 */
 563 void idIK_Walk::Evaluate( void ) {
 564         int i, newPivotFoot;
 565         float modelHeight, jointHeight, lowestHeight, floorHeights[MAX_LEGS];
 566         float shift, smallestShift, newHeight, step, newPivotYaw, height, largestAnkleHeight;
 567         idVec3 modelOrigin, normal, hipDir, kneeDir, start, end, jointOrigins[MAX_LEGS];
 568         idVec3 footOrigin, ankleOrigin, kneeOrigin, hipOrigin, waistOrigin;
 569         idMat3 modelAxis, waistAxis, axis;
 570         idMat3 hipAxis[MAX_LEGS], kneeAxis[MAX_LEGS], ankleAxis[MAX_LEGS];
 571         trace_t results;
 572
 573         if ( !self || !gameLocal.isNewFrame ) {
 574                 return;
 575         }
 576
 577         // if no IK enabled on any legs
 578         if ( !enabledLegs ) {
 579                 return;
 580         }
 581
 582         normal = - self->GetPhysics()->GetGravityNormal();
 583         modelOrigin = self->GetPhysics()->GetOrigin();
 584         modelAxis = self->GetRenderEntity()->axis;
 585         modelHeight = modelOrigin * normal;
 586
 587         modelOrigin += modelOffset * modelAxis;
 588
 589         // create frame without joint mods
 590         animator->CreateFrame( gameLocal.time, false );
 591
 592         // get the joint positions for the feet
 593         lowestHeight = idMath::INFINITY;
 594         for ( i = 0; i < numLegs; i++ ) {
 595                 animator->GetJointTransform( footJoints[i], gameLocal.time, footOrigin, axis );
 596                 jointOrigins[i] = modelOrigin + footOrigin * modelAxis;
 597                 jointHeight = jointOrigins[i] * normal;
 598                 if ( jointHeight < lowestHeight ) {
 599                         lowestHeight = jointHeight;
 600                         newPivotFoot = i;
 601                 }
 602         }
 603
 604         if ( usePivot ) {
 605
 606                 newPivotYaw = modelAxis[0].ToYaw();
 607
 608                 // change pivot foot
 609                 if ( newPivotFoot != pivotFoot || idMath::Fabs( idMath::AngleNormalize180( newPivotYaw - pivotYaw ) ) > 30.0f ) {
 610                         pivotFoot = newPivotFoot;
 611                         pivotYaw = newPivotYaw;
 612                         animator->GetJointTransform( footJoints[pivotFoot], gameLocal.time, footOrigin, axis );
 613                         pivotPos = modelOrigin + footOrigin * modelAxis;
 614                 }
 615
 616                 // keep pivot foot in place
 617                 jointOrigins[pivotFoot] = pivotPos;
 618         }
 619
 620         // get the floor heights for the feet
 621         for ( i = 0; i < numLegs; i++ ) {
 622
 623                 if ( !( enabledLegs & ( 1 << i ) ) ) {
 624                         continue;
 625                 }
 626
 627                 start = jointOrigins[i] + normal * footUpTrace;
 628                 end = jointOrigins[i] - normal * footDownTrace;
 629                 gameLocal.clip.Translation( results, start, end, footModel, mat3_identity, CONTENTS_SOLID|CONTENTS_IKCLIP, self );
 630                 floorHeights[i] = results.endpos * normal;
 631
 632                 if ( ik_debug.GetBool() && footModel ) {
 633                         idFixedWinding w;
 634                         for ( int j = 0; j < footModel->GetTraceModel()->numVerts; j++ ) {
 635                                 w += footModel->GetTraceModel()->verts[j];
 636                         }
 637                         gameRenderWorld->DebugWinding( colorRed, w, results.endpos, results.endAxis );
 638                 }
 639         }
 640
 641         const idPhysics *phys = self->GetPhysics();
 642
 643         // test whether or not the character standing on the ground
 644         bool onGround = phys->HasGroundContacts();
 645
 646         // test whether or not the character is standing on a plat
 647         bool onPlat = false;
 648         for ( i = 0; i < phys->GetNumContacts(); i++ ) {
 649                 idEntity *ent = gameLocal.entities[ phys->GetContact( i ).entityNum ];
 650                 if ( ent != NULL && ent->IsType( idPlat::Type ) ) {
 651                         onPlat = true;
 652                         break;
 653                 }
 654         }
 655
 656         // adjust heights of the ankles
 657         smallestShift = idMath::INFINITY;
 658         largestAnkleHeight = -idMath::INFINITY;
 659         for ( i = 0; i < numLegs; i++ ) {
 660
 661                 if ( onGround && ( enabledLegs & ( 1 << i ) ) ) {
 662                         shift = floorHeights[i] - modelHeight + footShift;
 663                 } else {
 664                         shift = 0.0f;
 665                 }
 666
 667                 if ( shift < smallestShift ) {
 668                         smallestShift = shift;
 669                 }
 670
 671                 animator->GetJointTransform( ankleJoints[i], gameLocal.time, ankleOrigin, ankleAxis[i] );
 672                 jointOrigins[i] = modelOrigin + ankleOrigin * modelAxis;
 673
 674                 height = jointOrigins[i] * normal;
 675
 676                 if ( oldHeightsValid && !onPlat ) {
 677                         step = height + shift - oldAnkleHeights[i];
 678                         shift -= smoothing * step;
 679                 }
 680
 681                 newHeight = height + shift;
 682                 if ( newHeight > largestAnkleHeight ) {
 683                         largestAnkleHeight = newHeight;
 684                 }
 685
 686                 oldAnkleHeights[i] = newHeight;
 687
 688                 jointOrigins[i] += shift * normal;
 689         }
 690
 691         animator->GetJointTransform( waistJoint, gameLocal.time, waistOrigin, waistAxis );
 692         waistOrigin = modelOrigin + waistOrigin * modelAxis;
 693
 694         // adjust position of the waist
 695         waistOffset = ( smallestShift + waistShift ) * normal;
 696
 697         // if the waist should be at least a certain distance above the floor
 698         if ( minWaistFloorDist > 0.0f && waistOffset * normal < 0.0f ) {
 699                 start = waistOrigin;
 700                 end = waistOrigin + waistOffset - normal * minWaistFloorDist;
 701                 gameLocal.clip.Translation( results, start, end, footModel, modelAxis, CONTENTS_SOLID|CONTENTS_IKCLIP, self );
 702                 height = ( waistOrigin + waistOffset - results.endpos ) * normal;
 703                 if ( height < minWaistFloorDist ) {
 704                         waistOffset += ( minWaistFloorDist - height ) * normal;
 705                 }
 706         }
 707
 708         // if the waist should be at least a certain distance above the ankles
 709         if ( minWaistAnkleDist > 0.0f ) {
 710                 height = ( waistOrigin + waistOffset ) * normal;
 711                 if ( height - largestAnkleHeight < minWaistAnkleDist ) {
 712                         waistOffset += ( minWaistAnkleDist - ( height - largestAnkleHeight ) ) * normal;
 713                 }
 714         }
 715
 716         if ( oldHeightsValid ) {
 717                 // smoothly adjust height of waist
 718                 newHeight = ( waistOrigin + waistOffset ) * normal;
 719                 step = newHeight - oldWaistHeight;
 720                 waistOffset -= waistSmoothing * step * normal;
 721         }
 722
 723         // save height of waist for smoothing
 724         oldWaistHeight = ( waistOrigin + waistOffset ) * normal;
 725
 726         if ( !oldHeightsValid ) {
 727                 oldHeightsValid = true;
 728                 return;
 729         }
 730
 731         // solve IK
 732         for ( i = 0; i < numLegs; i++ ) {
 733
 734                 // get the position of the hip in world space
 735                 animator->GetJointTransform( hipJoints[i], gameLocal.time, hipOrigin, axis );
 736                 hipOrigin = modelOrigin + waistOffset + hipOrigin * modelAxis;
 737                 hipDir = hipForward[i] * axis * modelAxis;
 738
 739                 // get the IK bend direction
 740                 animator->GetJointTransform( kneeJoints[i], gameLocal.time, kneeOrigin, axis );
 741                 kneeDir = kneeForward[i] * axis * modelAxis;
 742
 743                 // solve IK and calculate knee position
 744                 SolveTwoBones( hipOrigin, jointOrigins[i], kneeDir, upperLegLength[i], lowerLegLength[i], kneeOrigin );
 745
 746                 if ( ik_debug.GetBool() ) {
 747                         gameRenderWorld->DebugLine( colorCyan, hipOrigin, kneeOrigin );
 748                         gameRenderWorld->DebugLine( colorRed, kneeOrigin, jointOrigins[i] );
 749                         gameRenderWorld->DebugLine( colorYellow, kneeOrigin, kneeOrigin + hipDir );
 750                         gameRenderWorld->DebugLine( colorGreen, kneeOrigin, kneeOrigin + kneeDir );
 751                 }
 752
 753                 // get the axis for the hip joint
 754                 GetBoneAxis( hipOrigin, kneeOrigin, hipDir, axis );
 755                 hipAxis[i] = upperLegToHipJoint[i] * ( axis * modelAxis.Transpose() );
 756
 757                 // get the axis for the knee joint
 758                 GetBoneAxis( kneeOrigin, jointOrigins[i], kneeDir, axis );
 759                 kneeAxis[i] = lowerLegToKneeJoint[i] * ( axis * modelAxis.Transpose() );
 760         }
 761
 762         // set the joint mods
 763         animator->SetJointAxis( waistJoint, JOINTMOD_WORLD_OVERRIDE, waistAxis );
 764         animator->SetJointPos( waistJoint, JOINTMOD_WORLD_OVERRIDE, ( waistOrigin + waistOffset - modelOrigin ) * modelAxis.Transpose() );
 765         for ( i = 0; i < numLegs; i++ ) {
 766                 animator->SetJointAxis( hipJoints[i], JOINTMOD_WORLD_OVERRIDE, hipAxis[i] );
 767                 animator->SetJointAxis( kneeJoints[i], JOINTMOD_WORLD_OVERRIDE, kneeAxis[i] );
 768                 animator->SetJointAxis( ankleJoints[i], JOINTMOD_WORLD_OVERRIDE, ankleAxis[i] );
 769         }
 770
 771         ik_activate = true;
 772 }
 773
 774 /*
 775 ================
 776 idIK_Walk::ClearJointMods
 777 ================
 778 */
 779 void idIK_Walk::ClearJointMods( void ) {
 780         int i;
 781
 782         if ( !self || !ik_activate ) {
 783                 return;
 784         }
 785
 786         animator->SetJointAxis( waistJoint, JOINTMOD_NONE, mat3_identity );
 787         animator->SetJointPos( waistJoint, JOINTMOD_NONE, vec3_origin );
 788         for ( i = 0; i < numLegs; i++ ) {
 789                 animator->SetJointAxis( hipJoints[i], JOINTMOD_NONE, mat3_identity );
 790                 animator->SetJointAxis( kneeJoints[i], JOINTMOD_NONE, mat3_identity );
 791                 animator->SetJointAxis( ankleJoints[i], JOINTMOD_NONE, mat3_identity );
 792         }
 793
 794         ik_activate = false;
 795 }
 796
 797 /*
 798 ================
 799 idIK_Walk::EnableAll
 800 ================
 801 */
 802 void idIK_Walk::EnableAll( void ) {
 803         enabledLegs = ( 1 << numLegs ) - 1;
 804         oldHeightsValid = false;
 805 }
 806
 807 /*
 808 ================
 809 idIK_Walk::DisableAll
 810 ================
 811 */
 812 void idIK_Walk::DisableAll( void ) {
 813         enabledLegs = 0;
 814         oldHeightsValid = false;
 815 }
 816
 817 /*
 818 ================
 819 idIK_Walk::EnableLeg
 820 ================
 821 */
 822 void idIK_Walk::EnableLeg( int num ) {
 823         enabledLegs |= 1 << num;
 824 }
 825
 826 /*
 827 ================
 828 idIK_Walk::DisableLeg
 829 ================
 830 */
 831 void idIK_Walk::DisableLeg( int num ) {
 832         enabledLegs &= ~( 1 << num );
 833 }
 834
 835
 836 /*
 837 ===============================================================================
 838
 839   idIK_Reach
 840
 841 ===============================================================================
 842 */
 843
 844 /*
 845 ================
 846 idIK_Reach::idIK_Reach
 847 ================
 848 */
 849 idIK_Reach::idIK_Reach() {
 850         int i;
 851
 852         initialized = false;
 853         numArms = 0;
 854         enabledArms = 0;
 855         for ( i = 0; i < MAX_ARMS; i++ ) {
 856                 handJoints[i] = INVALID_JOINT;
 857                 elbowJoints[i] = INVALID_JOINT;
 858                 shoulderJoints[i] = INVALID_JOINT;
 859                 dirJoints[i] = INVALID_JOINT;
 860                 shoulderForward[i].Zero();
 861                 elbowForward[i].Zero();
 862                 upperArmLength[i] = 0.0f;
 863                 lowerArmLength[i] = 0.0f;
 864                 upperArmToShoulderJoint[i].Identity();
 865                 lowerArmToElbowJoint[i].Identity();
 866         }
 867 }
 868
 869 /*
 870 ================
 871 idIK_Reach::~idIK_Reach
 872 ================
 873 */
 874 idIK_Reach::~idIK_Reach() {
 875 }
 876
 877 /*
 878 ================
 879 idIK_Reach::Save
 880 ================
 881 */
 882 void idIK_Reach::Save( idSaveGame *savefile ) const {
 883         int i;
 884         idIK::Save( savefile );
 885
 886         savefile->WriteInt( numArms );
 887         savefile->WriteInt( enabledArms );
 888         for ( i = 0; i <  MAX_ARMS; i++ )
 889                 savefile->WriteInt( handJoints[i] );
 890         for ( i = 0; i <  MAX_ARMS; i++ )
 891                 savefile->WriteInt( elbowJoints[i] );
 892         for ( i = 0; i <  MAX_ARMS; i++ )
 893                 savefile->WriteInt( shoulderJoints[i] );
 894         for ( i = 0; i <  MAX_ARMS; i++ )
 895                 savefile->WriteInt( dirJoints[i] );
 896
 897         for ( i = 0; i <  MAX_ARMS; i++ )
 898                 savefile->WriteVec3( shoulderForward[i] );
 899         for ( i = 0; i <  MAX_ARMS; i++ )
 900                 savefile->WriteVec3( elbowForward[i] );
 901
 902         for ( i = 0; i <  MAX_ARMS; i++ )
 903                 savefile->WriteFloat( upperArmLength[i] );
 904         for ( i = 0; i <  MAX_ARMS; i++ )
 905                 savefile->WriteFloat( lowerArmLength[i] );
 906
 907         for ( i = 0; i <  MAX_ARMS; i++ )
 908                 savefile->WriteMat3( upperArmToShoulderJoint[i] );
 909         for ( i = 0; i <  MAX_ARMS; i++ )
 910                 savefile->WriteMat3( lowerArmToElbowJoint[i] );
 911 }
 912
 913 /*
 914 ================
 915 idIK_Reach::Restore
 916 ================
 917 */
 918 void idIK_Reach::Restore( idRestoreGame *savefile ) {
 919         int i;
 920         idIK::Restore( savefile );
 921
 922         savefile->ReadInt( numArms );
 923         savefile->ReadInt( enabledArms );
 924         for ( i = 0; i <  MAX_ARMS; i++ )
 925                 savefile->ReadInt( (int&)handJoints[i] );
 926         for ( i = 0; i <  MAX_ARMS; i++ )
 927                 savefile->ReadInt( (int&)elbowJoints[i] );
 928         for ( i = 0; i <  MAX_ARMS; i++ )
 929                 savefile->ReadInt( (int&)shoulderJoints[i] );
 930         for ( i = 0; i <  MAX_ARMS; i++ )
 931                 savefile->ReadInt( (int&)dirJoints[i] );
 932
 933         for ( i = 0; i <  MAX_ARMS; i++ )
 934                 savefile->ReadVec3( shoulderForward[i] );
 935         for ( i = 0; i <  MAX_ARMS; i++ )
 936                 savefile->ReadVec3( elbowForward[i] );
 937
 938         for ( i = 0; i <  MAX_ARMS; i++ )
 939                 savefile->ReadFloat( upperArmLength[i] );
 940         for ( i = 0; i <  MAX_ARMS; i++ )
 941                 savefile->ReadFloat( lowerArmLength[i] );
 942
 943         for ( i = 0; i <  MAX_ARMS; i++ )
 944                 savefile->ReadMat3( upperArmToShoulderJoint[i] );
 945         for ( i = 0; i <  MAX_ARMS; i++ )
 946                 savefile->ReadMat3( lowerArmToElbowJoint[i] );
 947 }
 948
 949 /*
 950 ================
 951 idIK_Reach::Init
 952 ================
 953 */
 954 bool idIK_Reach::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
 955         int i;
 956         const char *jointName;
 957         idTraceModel trm;
 958         idVec3 dir, handOrigin, elbowOrigin, shoulderOrigin, dirOrigin;
 959         idMat3 axis, handAxis, elbowAxis, shoulderAxis;
 960
 961         if ( !self ) {
 962                 return false;
 963         }
 964
 965         numArms = Min( self->spawnArgs.GetInt( "ik_numArms", "0" ), MAX_ARMS );
 966         if ( numArms == 0 ) {
 967                 return true;
 968         }
 969
 970         if ( !idIK::Init( self, anim, modelOffset ) ) {
 971                 return false;
 972         }
 973
 974         int numJoints = animator->NumJoints();
 975         idJointMat *joints = ( idJointMat * )_alloca16( numJoints * sizeof( joints[0] ) );
 976
 977         // create the animation frame used to setup the IK
 978         gameEdit->ANIM_CreateAnimFrame( animator->ModelHandle(), animator->GetAnim( modifiedAnim )->MD5Anim( 0 ), numJoints, joints, 1, animator->ModelDef()->GetVisualOffset() + modelOffset, animator->RemoveOrigin() );
 979
 980         enabledArms = 0;
 981
 982         // get all the joints
 983         for ( i = 0; i < numArms; i++ ) {
 984
 985                 jointName = self->spawnArgs.GetString( va( "ik_hand%d", i+1 ) );
 986                 handJoints[i] = animator->GetJointHandle( jointName );
 987                 if ( handJoints[i] == INVALID_JOINT ) {
 988                         gameLocal.Error( "idIK_Reach::Init: invalid hand joint '%s'", jointName );
 989                 }
 990
 991                 jointName = self->spawnArgs.GetString( va( "ik_elbow%d", i+1 ) );
 992                 elbowJoints[i] = animator->GetJointHandle( jointName );
 993                 if ( elbowJoints[i] == INVALID_JOINT ) {
 994                         gameLocal.Error( "idIK_Reach::Init: invalid elbow joint '%s'\n", jointName );
 995                 }
 996
 997                 jointName = self->spawnArgs.GetString( va( "ik_shoulder%d", i+1 ) );
 998                 shoulderJoints[i] = animator->GetJointHandle( jointName );
 999                 if ( shoulderJoints[i] == INVALID_JOINT ) {
1000                         gameLocal.Error( "idIK_Reach::Init: invalid shoulder joint '%s'\n", jointName );
1001                 }
1002
1003                 jointName = self->spawnArgs.GetString( va( "ik_elbowDir%d", i+1 ) );
1004                 dirJoints[i] = animator->GetJointHandle( jointName );
1005
1006                 enabledArms |= 1 << i;
1007         }
1008
1009         // get the arm bone lengths and rotation matrices
1010         for ( i = 0; i < numArms; i++ ) {
1011
1012                 handAxis = joints[ handJoints[ i ] ].ToMat3();
1013                 handOrigin = joints[ handJoints[ i ] ].ToVec3();
1014
1015                 elbowAxis = joints[ elbowJoints[ i ] ].ToMat3();
1016                 elbowOrigin = joints[ elbowJoints[ i ] ].ToVec3();
1017
1018                 shoulderAxis = joints[ shoulderJoints[ i ] ].ToMat3();
1019                 shoulderOrigin = joints[ shoulderJoints[ i ] ].ToVec3();
1020
1021                 // get the IK direction
1022                 if ( dirJoints[i] != INVALID_JOINT ) {
1023                         dirOrigin = joints[ dirJoints[ i ] ].ToVec3();
1024                         dir = dirOrigin - elbowOrigin;
1025                 } else {
1026                         dir.Set( -1.0f, 0.0f, 0.0f );
1027                 }
1028
1029                 shoulderForward[i] = dir * shoulderAxis.Transpose();
1030                 elbowForward[i] = dir * elbowAxis.Transpose();
1031
1032                 // conversion from upper arm bone axis to should joint axis
1033                 upperArmLength[i] = GetBoneAxis( shoulderOrigin, elbowOrigin, dir, axis );
1034                 upperArmToShoulderJoint[i] = shoulderAxis * axis.Transpose();
1035
1036                 // conversion from lower arm bone axis to elbow joint axis
1037                 lowerArmLength[i] = GetBoneAxis( elbowOrigin, handOrigin, dir, axis );
1038                 lowerArmToElbowJoint[i] = elbowAxis * axis.Transpose();
1039         }
1040
1041         initialized = true;
1042
1043         return true;
1044 }
1045
1046 /*
1047 ================
1048 idIK_Reach::Evaluate
1049 ================
1050 */
1051 void idIK_Reach::Evaluate( void ) {
1052         int i;
1053         idVec3 modelOrigin, shoulderOrigin, elbowOrigin, handOrigin, shoulderDir, elbowDir;
1054         idMat3 modelAxis, axis;
1055         idMat3 shoulderAxis[MAX_ARMS], elbowAxis[MAX_ARMS];
1056         trace_t trace;
1057
1058         modelOrigin = self->GetRenderEntity()->origin;
1059         modelAxis = self->GetRenderEntity()->axis;
1060
1061         // solve IK
1062         for ( i = 0; i < numArms; i++ ) {
1063
1064                 // get the position of the shoulder in world space
1065                 animator->GetJointTransform( shoulderJoints[i], gameLocal.time, shoulderOrigin, axis );
1066                 shoulderOrigin = modelOrigin + shoulderOrigin * modelAxis;
1067                 shoulderDir = shoulderForward[i] * axis * modelAxis;
1068
1069                 // get the position of the hand in world space
1070                 animator->GetJointTransform( handJoints[i], gameLocal.time, handOrigin, axis );
1071                 handOrigin = modelOrigin + handOrigin * modelAxis;
1072
1073                 // get first collision going from shoulder to hand
1074                 gameLocal.clip.TracePoint( trace, shoulderOrigin, handOrigin, CONTENTS_SOLID, self );
1075                 handOrigin = trace.endpos;
1076
1077                 // get the IK bend direction
1078                 animator->GetJointTransform( elbowJoints[i], gameLocal.time, elbowOrigin, axis );
1079                 elbowDir = elbowForward[i] * axis * modelAxis;
1080
1081                 // solve IK and calculate elbow position
1082                 SolveTwoBones( shoulderOrigin, handOrigin, elbowDir, upperArmLength[i], lowerArmLength[i], elbowOrigin );
1083
1084                 if ( ik_debug.GetBool() ) {
1085                         gameRenderWorld->DebugLine( colorCyan, shoulderOrigin, elbowOrigin );
1086                         gameRenderWorld->DebugLine( colorRed, elbowOrigin, handOrigin );
1087                         gameRenderWorld->DebugLine( colorYellow, elbowOrigin, elbowOrigin + elbowDir );
1088                         gameRenderWorld->DebugLine( colorGreen, elbowOrigin, elbowOrigin + shoulderDir );
1089                 }
1090
1091                 // get the axis for the shoulder joint
1092                 GetBoneAxis( shoulderOrigin, elbowOrigin, shoulderDir, axis );
1093                 shoulderAxis[i] = upperArmToShoulderJoint[i] * ( axis * modelAxis.Transpose() );
1094
1095                 // get the axis for the elbow joint
1096                 GetBoneAxis( elbowOrigin, handOrigin, elbowDir, axis );
1097                 elbowAxis[i] = lowerArmToElbowJoint[i] * ( axis * modelAxis.Transpose() );
1098         }
1099
1100         for ( i = 0; i < numArms; i++ ) {
1101                 animator->SetJointAxis( shoulderJoints[i], JOINTMOD_WORLD_OVERRIDE, shoulderAxis[i] );
1102                 animator->SetJointAxis( elbowJoints[i], JOINTMOD_WORLD_OVERRIDE, elbowAxis[i] );
1103         }
1104
1105         ik_activate = true;
1106 }
1107
1108 /*
1109 ================
1110 idIK_Reach::ClearJointMods
1111 ================
1112 */
1113 void idIK_Reach::ClearJointMods( void ) {
1114         int i;
1115
1116         if ( !self || !ik_activate ) {
1117                 return;
1118         }
1119
1120         for ( i = 0; i < numArms; i++ ) {
1121                 animator->SetJointAxis( shoulderJoints[i], JOINTMOD_NONE, mat3_identity );
1122                 animator->SetJointAxis( elbowJoints[i], JOINTMOD_NONE, mat3_identity );
1123                 animator->SetJointAxis( handJoints[i], JOINTMOD_NONE, mat3_identity );
1124         }
1125
1126         ik_activate = false;
1127 }