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