2 ===========================================================================
5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
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.
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.
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/>.
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.
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.
26 ===========================================================================
29 #include "../idlib/precompiled.h"
32 #include "Game_local.h"
35 ===============================================================================
39 ===============================================================================
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 );
82 void idIK::Restore( idRestoreGame *savefile ) {
85 savefile->ReadBool( initialized );
86 savefile->ReadBool( ik_activate );
87 savefile->ReadObject( reinterpret_cast<idClass *&>( self ) );
88 savefile->ReadString( anim );
89 savefile->ReadVec3( modelOffset );
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) );
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) );
113 bool idIK::IsInitialized( void ) const {
114 return initialized && ik_enable.GetBool();
122 bool idIK::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
123 idRenderModel *model;
125 if ( self == NULL ) {
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) );
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) );
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) );
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) );
155 this->modelOffset = modelOffset;
165 void idIK::Evaluate( void ) {
173 void idIK::ClearJointMods( void ) {
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;
186 vec0 = endPos - startPos;
187 lengthSqr = vec0.LengthSqr();
188 lengthInv = idMath::InvSqrt( lengthSqr );
189 length = lengthInv * lengthSqr;
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;
198 vec1 = dir - vec0 * dir * vec0;
201 x = ( length * length + len0 * len0 - len1 * len1 ) * ( 0.5f * lengthInv );
202 y = idMath::Sqrt( len0 * len0 - x * x );
204 jointPos = startPos + x * vec0 + y * vec1;
214 float idIK::GetBoneAxis( const idVec3 &startPos, const idVec3 &endPos, const idVec3 &dir, idMat3 &axis ) {
216 axis[0] = endPos - startPos;
217 length = axis[0].Normalize();
218 axis[1] = dir - axis[0] * dir * axis[0];
220 axis[2].Cross( axis[1], axis[0] );
226 ===============================================================================
230 ===============================================================================
238 idIK_Walk::idIK_Walk() {
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;
259 waistJoint = INVALID_JOINT;
262 waistSmoothing = 0.5f;
265 minWaistFloorDist = 0.0f;
266 minWaistAnkleDist = 0.0f;
268 footDownTrace = 32.0f;
276 oldHeightsValid = false;
277 oldWaistHeight = 0.0f;
283 idIK_Walk::~idIK_Walk
286 idIK_Walk::~idIK_Walk() {
297 void idIK_Walk::Save( idSaveGame *savefile ) const {
300 idIK::Save( savefile );
302 savefile->WriteClipModel( footModel );
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 );
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] );
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] );
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] );
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 );
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] );
352 savefile->WriteVec3( waistOffset );
360 void idIK_Walk::Restore( idRestoreGame *savefile ) {
363 idIK::Restore( savefile );
365 savefile->ReadClipModel( footModel );
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 );
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] );
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] );
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] );
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 );
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] );
415 savefile->ReadVec3( waistOffset );
423 bool idIK_Walk::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
428 const char *jointName;
429 idVec3 dir, ankleOrigin, kneeOrigin, hipOrigin, dirOrigin;
430 idMat3 axis, ankleAxis, kneeAxis, hipAxis;
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 )
443 numLegs = Min( self->spawnArgs.GetInt( "ik_numLegs", "0" ), MAX_LEGS );
444 if ( numLegs == 0 ) {
448 if ( !idIK::Init( self, anim, modelOffset ) ) {
452 int numJoints = animator->NumJoints();
453 idJointMat *joints = ( idJointMat * )_alloca16( numJoints * sizeof( joints[0] ) );
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() );
460 // get all the joints
461 for ( i = 0; i < numLegs; i++ ) {
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 );
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 );
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 );
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 );
487 jointName = self->spawnArgs.GetString( va( "ik_dir%d", i+1 ) );
488 dirJoints[i] = animator->GetJointHandle( jointName );
490 enabledLegs |= 1 << i;
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 );
499 // get the leg bone lengths and rotation matrices
500 for ( i = 0; i < numLegs; i++ ) {
501 oldAnkleHeights[i] = 0.0f;
503 ankleAxis = joints[ ankleJoints[ i ] ].ToMat3();
504 ankleOrigin = joints[ ankleJoints[ i ] ].ToVec3();
506 kneeAxis = joints[ kneeJoints[ i ] ].ToMat3();
507 kneeOrigin = joints[ kneeJoints[ i ] ].ToVec3();
509 hipAxis = joints[ hipJoints[ i ] ].ToMat3();
510 hipOrigin = joints[ hipJoints[ i ] ].ToVec3();
512 // get the IK direction
513 if ( dirJoints[i] != INVALID_JOINT ) {
514 dirOrigin = joints[ dirJoints[ i ] ].ToVec3();
515 dir = dirOrigin - kneeOrigin;
517 dir.Set( 1.0f, 0.0f, 0.0f );
520 hipForward[i] = dir * hipAxis.Transpose();
521 kneeForward[i] = dir * kneeAxis.Transpose();
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();
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();
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" );
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;
549 trm.SetupPolygon( verts, 4 );
550 footModel = new idClipModel( trm );
563 void idIK_Walk::Evaluate( void ) {
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];
573 if ( !self || !gameLocal.isNewFrame ) {
577 // if no IK enabled on any legs
578 if ( !enabledLegs ) {
582 normal = - self->GetPhysics()->GetGravityNormal();
583 modelOrigin = self->GetPhysics()->GetOrigin();
584 modelAxis = self->GetRenderEntity()->axis;
585 modelHeight = modelOrigin * normal;
587 modelOrigin += modelOffset * modelAxis;
589 // create frame without joint mods
590 animator->CreateFrame( gameLocal.time, false );
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;
606 newPivotYaw = modelAxis[0].ToYaw();
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;
616 // keep pivot foot in place
617 jointOrigins[pivotFoot] = pivotPos;
620 // get the floor heights for the feet
621 for ( i = 0; i < numLegs; i++ ) {
623 if ( !( enabledLegs & ( 1 << i ) ) ) {
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;
632 if ( ik_debug.GetBool() && footModel ) {
634 for ( int j = 0; j < footModel->GetTraceModel()->numVerts; j++ ) {
635 w += footModel->GetTraceModel()->verts[j];
637 gameRenderWorld->DebugWinding( colorRed, w, results.endpos, results.endAxis );
641 const idPhysics *phys = self->GetPhysics();
643 // test whether or not the character standing on the ground
644 bool onGround = phys->HasGroundContacts();
646 // test whether or not the character is standing on a plat
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 ) ) {
656 // adjust heights of the ankles
657 smallestShift = idMath::INFINITY;
658 largestAnkleHeight = -idMath::INFINITY;
659 for ( i = 0; i < numLegs; i++ ) {
661 if ( onGround && ( enabledLegs & ( 1 << i ) ) ) {
662 shift = floorHeights[i] - modelHeight + footShift;
667 if ( shift < smallestShift ) {
668 smallestShift = shift;
671 animator->GetJointTransform( ankleJoints[i], gameLocal.time, ankleOrigin, ankleAxis[i] );
672 jointOrigins[i] = modelOrigin + ankleOrigin * modelAxis;
674 height = jointOrigins[i] * normal;
676 if ( oldHeightsValid && !onPlat ) {
677 step = height + shift - oldAnkleHeights[i];
678 shift -= smoothing * step;
681 newHeight = height + shift;
682 if ( newHeight > largestAnkleHeight ) {
683 largestAnkleHeight = newHeight;
686 oldAnkleHeights[i] = newHeight;
688 jointOrigins[i] += shift * normal;
691 animator->GetJointTransform( waistJoint, gameLocal.time, waistOrigin, waistAxis );
692 waistOrigin = modelOrigin + waistOrigin * modelAxis;
694 // adjust position of the waist
695 waistOffset = ( smallestShift + waistShift ) * normal;
697 // if the waist should be at least a certain distance above the floor
698 if ( minWaistFloorDist > 0.0f && waistOffset * normal < 0.0f ) {
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;
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;
716 if ( oldHeightsValid ) {
717 // smoothly adjust height of waist
718 newHeight = ( waistOrigin + waistOffset ) * normal;
719 step = newHeight - oldWaistHeight;
720 waistOffset -= waistSmoothing * step * normal;
723 // save height of waist for smoothing
724 oldWaistHeight = ( waistOrigin + waistOffset ) * normal;
726 if ( !oldHeightsValid ) {
727 oldHeightsValid = true;
732 for ( i = 0; i < numLegs; i++ ) {
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;
739 // get the IK bend direction
740 animator->GetJointTransform( kneeJoints[i], gameLocal.time, kneeOrigin, axis );
741 kneeDir = kneeForward[i] * axis * modelAxis;
743 // solve IK and calculate knee position
744 SolveTwoBones( hipOrigin, jointOrigins[i], kneeDir, upperLegLength[i], lowerLegLength[i], kneeOrigin );
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 );
753 // get the axis for the hip joint
754 GetBoneAxis( hipOrigin, kneeOrigin, hipDir, axis );
755 hipAxis[i] = upperLegToHipJoint[i] * ( axis * modelAxis.Transpose() );
757 // get the axis for the knee joint
758 GetBoneAxis( kneeOrigin, jointOrigins[i], kneeDir, axis );
759 kneeAxis[i] = lowerLegToKneeJoint[i] * ( axis * modelAxis.Transpose() );
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] );
776 idIK_Walk::ClearJointMods
779 void idIK_Walk::ClearJointMods( void ) {
782 if ( !self || !ik_activate ) {
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 );
802 void idIK_Walk::EnableAll( void ) {
803 enabledLegs = ( 1 << numLegs ) - 1;
804 oldHeightsValid = false;
809 idIK_Walk::DisableAll
812 void idIK_Walk::DisableAll( void ) {
814 oldHeightsValid = false;
822 void idIK_Walk::EnableLeg( int num ) {
823 enabledLegs |= 1 << num;
828 idIK_Walk::DisableLeg
831 void idIK_Walk::DisableLeg( int num ) {
832 enabledLegs &= ~( 1 << num );
837 ===============================================================================
841 ===============================================================================
846 idIK_Reach::idIK_Reach
849 idIK_Reach::idIK_Reach() {
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();
871 idIK_Reach::~idIK_Reach
874 idIK_Reach::~idIK_Reach() {
882 void idIK_Reach::Save( idSaveGame *savefile ) const {
884 idIK::Save( savefile );
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] );
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] );
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] );
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] );
918 void idIK_Reach::Restore( idRestoreGame *savefile ) {
920 idIK::Restore( savefile );
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] );
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] );
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] );
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] );
954 bool idIK_Reach::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
956 const char *jointName;
958 idVec3 dir, handOrigin, elbowOrigin, shoulderOrigin, dirOrigin;
959 idMat3 axis, handAxis, elbowAxis, shoulderAxis;
965 numArms = Min( self->spawnArgs.GetInt( "ik_numArms", "0" ), MAX_ARMS );
966 if ( numArms == 0 ) {
970 if ( !idIK::Init( self, anim, modelOffset ) ) {
974 int numJoints = animator->NumJoints();
975 idJointMat *joints = ( idJointMat * )_alloca16( numJoints * sizeof( joints[0] ) );
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() );
982 // get all the joints
983 for ( i = 0; i < numArms; i++ ) {
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 );
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 );
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 );
1003 jointName = self->spawnArgs.GetString( va( "ik_elbowDir%d", i+1 ) );
1004 dirJoints[i] = animator->GetJointHandle( jointName );
1006 enabledArms |= 1 << i;
1009 // get the arm bone lengths and rotation matrices
1010 for ( i = 0; i < numArms; i++ ) {
1012 handAxis = joints[ handJoints[ i ] ].ToMat3();
1013 handOrigin = joints[ handJoints[ i ] ].ToVec3();
1015 elbowAxis = joints[ elbowJoints[ i ] ].ToMat3();
1016 elbowOrigin = joints[ elbowJoints[ i ] ].ToVec3();
1018 shoulderAxis = joints[ shoulderJoints[ i ] ].ToMat3();
1019 shoulderOrigin = joints[ shoulderJoints[ i ] ].ToVec3();
1021 // get the IK direction
1022 if ( dirJoints[i] != INVALID_JOINT ) {
1023 dirOrigin = joints[ dirJoints[ i ] ].ToVec3();
1024 dir = dirOrigin - elbowOrigin;
1026 dir.Set( -1.0f, 0.0f, 0.0f );
1029 shoulderForward[i] = dir * shoulderAxis.Transpose();
1030 elbowForward[i] = dir * elbowAxis.Transpose();
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();
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();
1048 idIK_Reach::Evaluate
1051 void idIK_Reach::Evaluate( void ) {
1053 idVec3 modelOrigin, shoulderOrigin, elbowOrigin, handOrigin, shoulderDir, elbowDir;
1054 idMat3 modelAxis, axis;
1055 idMat3 shoulderAxis[MAX_ARMS], elbowAxis[MAX_ARMS];
1058 modelOrigin = self->GetRenderEntity()->origin;
1059 modelAxis = self->GetRenderEntity()->axis;
1062 for ( i = 0; i < numArms; i++ ) {
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;
1069 // get the position of the hand in world space
1070 animator->GetJointTransform( handJoints[i], gameLocal.time, handOrigin, axis );
1071 handOrigin = modelOrigin + handOrigin * modelAxis;
1073 // get first collision going from shoulder to hand
1074 gameLocal.clip.TracePoint( trace, shoulderOrigin, handOrigin, CONTENTS_SOLID, self );
1075 handOrigin = trace.endpos;
1077 // get the IK bend direction
1078 animator->GetJointTransform( elbowJoints[i], gameLocal.time, elbowOrigin, axis );
1079 elbowDir = elbowForward[i] * axis * modelAxis;
1081 // solve IK and calculate elbow position
1082 SolveTwoBones( shoulderOrigin, handOrigin, elbowDir, upperArmLength[i], lowerArmLength[i], elbowOrigin );
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 );
1091 // get the axis for the shoulder joint
1092 GetBoneAxis( shoulderOrigin, elbowOrigin, shoulderDir, axis );
1093 shoulderAxis[i] = upperArmToShoulderJoint[i] * ( axis * modelAxis.Transpose() );
1095 // get the axis for the elbow joint
1096 GetBoneAxis( elbowOrigin, handOrigin, elbowDir, axis );
1097 elbowAxis[i] = lowerArmToElbowJoint[i] * ( axis * modelAxis.Transpose() );
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] );
1110 idIK_Reach::ClearJointMods
1113 void idIK_Reach::ClearJointMods( void ) {
1116 if ( !self || !ik_activate ) {
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 );
1126 ik_activate = false;