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