/*
================
idIK_Reach::Evaluate
================
*/
void idIK_Reach::Evaluate( void )
{
int i;
idVec3 modelOrigin, shoulderOrigin, elbowOrigin, handOrigin, shoulderDir, elbowDir;
idMat3 modelAxis, axis;
idMat3 shoulderAxis[MAX_ARMS], elbowAxis[MAX_ARMS];
trace_t trace;
modelOrigin = self->GetRenderEntity()->origin;
modelAxis = self->GetRenderEntity()->axis;
// solve IK
for( i = 0; i < numArms; i++ )
{
// get the position of the shoulder in world space
animator->GetJointTransform( shoulderJoints[i], gameLocal.time, shoulderOrigin, axis );
shoulderOrigin = modelOrigin + shoulderOrigin * modelAxis;
shoulderDir = shoulderForward[i] * axis * modelAxis;
// get the position of the hand in world space
animator->GetJointTransform( handJoints[i], gameLocal.time, handOrigin, axis );
handOrigin = modelOrigin + handOrigin * modelAxis;
// get first collision going from shoulder to hand
gameLocal.clip.TracePoint( trace, shoulderOrigin, handOrigin, CONTENTS_SOLID, self );
handOrigin = trace.endpos;
// get the IK bend direction
animator->GetJointTransform( elbowJoints[i], gameLocal.time, elbowOrigin, axis );
elbowDir = elbowForward[i] * axis * modelAxis;
// solve IK and calculate elbow position
SolveTwoBones( shoulderOrigin, handOrigin, elbowDir, upperArmLength[i], lowerArmLength[i], elbowOrigin );
if( ik_debug.GetBool() )
{
gameRenderWorld->DebugLine( colorCyan, shoulderOrigin, elbowOrigin );
gameRenderWorld->DebugLine( colorRed, elbowOrigin, handOrigin );
gameRenderWorld->DebugLine( colorYellow, elbowOrigin, elbowOrigin + elbowDir );
gameRenderWorld->DebugLine( colorGreen, elbowOrigin, elbowOrigin + shoulderDir );
}
// get the axis for the shoulder joint
GetBoneAxis( shoulderOrigin, elbowOrigin, shoulderDir, axis );
shoulderAxis[i] = upperArmToShoulderJoint[i] * ( axis * modelAxis.Transpose() );
// get the axis for the elbow joint
GetBoneAxis( elbowOrigin, handOrigin, elbowDir, axis );
elbowAxis[i] = lowerArmToElbowJoint[i] * ( axis * modelAxis.Transpose() );
}
for( i = 0; i < numArms; i++ )
{
animator->SetJointAxis( shoulderJoints[i], JOINTMOD_WORLD_OVERRIDE, shoulderAxis[i] );
animator->SetJointAxis( elbowJoints[i], JOINTMOD_WORLD_OVERRIDE, elbowAxis[i] );
}
ik_activate = true;
}
/*
================
idIK_Reach::Init
================
*/
bool idIK_Reach::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
int i;
const char *jointName;
idTraceModel trm;
idVec3 dir, handOrigin, elbowOrigin, shoulderOrigin, dirOrigin;
idMat3 axis, handAxis, elbowAxis, shoulderAxis;
if ( !self ) {
return false;
}
numArms = Min( self->spawnArgs.GetInt( "ik_numArms", "0" ), MAX_ARMS );
if ( numArms == 0 ) {
return true;
}
if ( !idIK::Init( self, anim, modelOffset ) ) {
return false;
}
int numJoints = animator->NumJoints();
idJointMat *joints = ( idJointMat * )_alloca16( numJoints * sizeof( joints[0] ) );
// create the animation frame used to setup the IK
gameEdit->ANIM_CreateAnimFrame( animator->ModelHandle(), animator->GetAnim( modifiedAnim )->MD5Anim( 0 ), numJoints, joints, 1, animator->ModelDef()->GetVisualOffset() + modelOffset, animator->RemoveOrigin() );
enabledArms = 0;
// get all the joints
for ( i = 0; i < numArms; i++ ) {
jointName = self->spawnArgs.GetString( va( "ik_hand%d", i+1 ) );
handJoints[i] = animator->GetJointHandle( jointName );
if ( handJoints[i] == INVALID_JOINT ) {
gameLocal.Error( "idIK_Reach::Init: invalid hand joint '%s'", jointName );
}
jointName = self->spawnArgs.GetString( va( "ik_elbow%d", i+1 ) );
elbowJoints[i] = animator->GetJointHandle( jointName );
if ( elbowJoints[i] == INVALID_JOINT ) {
gameLocal.Error( "idIK_Reach::Init: invalid elbow joint '%s'\n", jointName );
}
jointName = self->spawnArgs.GetString( va( "ik_shoulder%d", i+1 ) );
shoulderJoints[i] = animator->GetJointHandle( jointName );
if ( shoulderJoints[i] == INVALID_JOINT ) {
gameLocal.Error( "idIK_Reach::Init: invalid shoulder joint '%s'\n", jointName );
}
jointName = self->spawnArgs.GetString( va( "ik_elbowDir%d", i+1 ) );
dirJoints[i] = animator->GetJointHandle( jointName );
enabledArms |= 1 << i;
}
// get the arm bone lengths and rotation matrices
for ( i = 0; i < numArms; i++ ) {
handAxis = joints[ handJoints[ i ] ].ToMat3();
handOrigin = joints[ handJoints[ i ] ].ToVec3();
elbowAxis = joints[ elbowJoints[ i ] ].ToMat3();
elbowOrigin = joints[ elbowJoints[ i ] ].ToVec3();
shoulderAxis = joints[ shoulderJoints[ i ] ].ToMat3();
shoulderOrigin = joints[ shoulderJoints[ i ] ].ToVec3();
// get the IK direction
if ( dirJoints[i] != INVALID_JOINT ) {
dirOrigin = joints[ dirJoints[ i ] ].ToVec3();
dir = dirOrigin - elbowOrigin;
} else {
dir.Set( -1.0f, 0.0f, 0.0f );
}
shoulderForward[i] = dir * shoulderAxis.Transpose();
elbowForward[i] = dir * elbowAxis.Transpose();
// conversion from upper arm bone axis to should joint axis
upperArmLength[i] = GetBoneAxis( shoulderOrigin, elbowOrigin, dir, axis );
upperArmToShoulderJoint[i] = shoulderAxis * axis.Transpose();
// conversion from lower arm bone axis to elbow joint axis
lowerArmLength[i] = GetBoneAxis( elbowOrigin, handOrigin, dir, axis );
lowerArmToElbowJoint[i] = elbowAxis * axis.Transpose();
}
initialized = true;
return true;
}
/*
================
idIK_Walk::Evaluate
================
*/
void idIK_Walk::Evaluate() {
int i, newPivotFoot = -1;
float modelHeight, jointHeight, lowestHeight, floorHeights[MAX_LEGS];
float shift, smallestShift, newHeight, step, newPivotYaw, height, largestAnkleHeight;
idVec3 modelOrigin, normal, hipDir, kneeDir, start, end, jointOrigins[MAX_LEGS];
idVec3 footOrigin, ankleOrigin, kneeOrigin, hipOrigin, waistOrigin;
idMat3 modelAxis, waistAxis, axis;
idMat3 hipAxis[MAX_LEGS], kneeAxis[MAX_LEGS], ankleAxis[MAX_LEGS];
trace_t results;
if ( !self || !gameLocal.isNewFrame ) {
return;
}
// if no IK enabled on any legs
if ( !enabledLegs ) {
return;
}
normal = - self->GetPhysics()->GetGravityNormal();
modelOrigin = self->GetPhysics()->GetOrigin();
modelAxis = self->GetRenderEntity()->axis;
modelHeight = modelOrigin * normal;
modelOrigin += modelOffset * modelAxis;
// create frame without joint mods
animator->CreateFrame( gameLocal.time, false );
// get the joint positions for the feet
lowestHeight = idMath::INFINITY;
for ( i = 0; i < numLegs; i++ ) {
animator->GetJointTransform( footJoints[i], gameLocal.time, footOrigin, axis );
jointOrigins[i] = modelOrigin + footOrigin * modelAxis;
jointHeight = jointOrigins[i] * normal;
if ( jointHeight < lowestHeight ) {
lowestHeight = jointHeight;
newPivotFoot = i;
}
}
if ( usePivot ) {
newPivotYaw = modelAxis[0].ToYaw();
// change pivot foot
if ( newPivotFoot != pivotFoot || idMath::Fabs( idMath::AngleNormalize180( newPivotYaw - pivotYaw ) ) > 30.0f ) {
pivotFoot = newPivotFoot;
pivotYaw = newPivotYaw;
animator->GetJointTransform( footJoints[pivotFoot], gameLocal.time, footOrigin, axis );
pivotPos = modelOrigin + footOrigin * modelAxis;
}
// keep pivot foot in place
jointOrigins[pivotFoot] = pivotPos;
}
// get the floor heights for the feet
for ( i = 0; i < numLegs; i++ ) {
if ( !( enabledLegs & ( 1 << i ) ) ) {
continue;
}
start = jointOrigins[i] + normal * footUpTrace;
end = jointOrigins[i] - normal * footDownTrace;
gameLocal.clip.Translation( results, start, end, footModel, mat3_identity, CONTENTS_SOLID|CONTENTS_IKCLIP, self );
floorHeights[i] = results.endpos * normal;
if ( ik_debug.GetBool() && footModel ) {
idFixedWinding w;
for ( int j = 0; j < footModel->GetTraceModel()->numVerts; j++ ) {
w += footModel->GetTraceModel()->verts[j];
}
gameRenderWorld->DebugWinding( colorRed, w, results.endpos, results.endAxis );
}
}
const idPhysics *phys = self->GetPhysics();
// test whether or not the character standing on the ground
bool onGround = phys->HasGroundContacts();
// test whether or not the character is standing on a plat
bool onPlat = false;
for ( i = 0; i < phys->GetNumContacts(); i++ ) {
idEntity *ent = gameLocal.entities[ phys->GetContact( i ).entityNum ];
if ( ent != NULL && ent->IsType( idPlat::Type ) ) {
onPlat = true;
break;
}
}
// adjust heights of the ankles
smallestShift = idMath::INFINITY;
largestAnkleHeight = -idMath::INFINITY;
for ( i = 0; i < numLegs; i++ ) {
if ( onGround && ( enabledLegs & ( 1 << i ) ) ) {
shift = floorHeights[i] - modelHeight + footShift;
} else {
shift = 0.0f;
}
if ( shift < smallestShift ) {
smallestShift = shift;
}
animator->GetJointTransform( ankleJoints[i], gameLocal.time, ankleOrigin, ankleAxis[i] );
jointOrigins[i] = modelOrigin + ankleOrigin * modelAxis;
height = jointOrigins[i] * normal;
if ( oldHeightsValid && !onPlat ) {
step = height + shift - oldAnkleHeights[i];
shift -= smoothing * step;
}
newHeight = height + shift;
if ( newHeight > largestAnkleHeight ) {
largestAnkleHeight = newHeight;
}
oldAnkleHeights[i] = newHeight;
jointOrigins[i] += shift * normal;
}
animator->GetJointTransform( waistJoint, gameLocal.time, waistOrigin, waistAxis );
waistOrigin = modelOrigin + waistOrigin * modelAxis;
// adjust position of the waist
waistOffset = ( smallestShift + waistShift ) * normal;
// if the waist should be at least a certain distance above the floor
if ( minWaistFloorDist > 0.0f && waistOffset * normal < 0.0f ) {
start = waistOrigin;
end = waistOrigin + waistOffset - normal * minWaistFloorDist;
gameLocal.clip.Translation( results, start, end, footModel, modelAxis, CONTENTS_SOLID|CONTENTS_IKCLIP, self );
height = ( waistOrigin + waistOffset - results.endpos ) * normal;
if ( height < minWaistFloorDist ) {
waistOffset += ( minWaistFloorDist - height ) * normal;
}
}
// if the waist should be at least a certain distance above the ankles
if ( minWaistAnkleDist > 0.0f ) {
height = ( waistOrigin + waistOffset ) * normal;
if ( height - largestAnkleHeight < minWaistAnkleDist ) {
waistOffset += ( minWaistAnkleDist - ( height - largestAnkleHeight ) ) * normal;
}
}
if ( oldHeightsValid ) {
// smoothly adjust height of waist
newHeight = ( waistOrigin + waistOffset ) * normal;
step = newHeight - oldWaistHeight;
waistOffset -= waistSmoothing * step * normal;
}
// save height of waist for smoothing
oldWaistHeight = ( waistOrigin + waistOffset ) * normal;
if ( !oldHeightsValid ) {
oldHeightsValid = true;
return;
}
// solve IK
for ( i = 0; i < numLegs; i++ ) {
// get the position of the hip in world space
animator->GetJointTransform( hipJoints[i], gameLocal.time, hipOrigin, axis );
hipOrigin = modelOrigin + waistOffset + hipOrigin * modelAxis;
hipDir = hipForward[i] * axis * modelAxis;
// get the IK bend direction
animator->GetJointTransform( kneeJoints[i], gameLocal.time, kneeOrigin, axis );
kneeDir = kneeForward[i] * axis * modelAxis;
// solve IK and calculate knee position
SolveTwoBones( hipOrigin, jointOrigins[i], kneeDir, upperLegLength[i], lowerLegLength[i], kneeOrigin );
if ( ik_debug.GetBool() ) {
gameRenderWorld->DebugLine( colorCyan, hipOrigin, kneeOrigin );
gameRenderWorld->DebugLine( colorRed, kneeOrigin, jointOrigins[i] );
gameRenderWorld->DebugLine( colorYellow, kneeOrigin, kneeOrigin + hipDir );
gameRenderWorld->DebugLine( colorGreen, kneeOrigin, kneeOrigin + kneeDir );
}
// get the axis for the hip joint
GetBoneAxis( hipOrigin, kneeOrigin, hipDir, axis );
hipAxis[i] = upperLegToHipJoint[i] * ( axis * modelAxis.Transpose() );
// get the axis for the knee joint
GetBoneAxis( kneeOrigin, jointOrigins[i], kneeDir, axis );
kneeAxis[i] = lowerLegToKneeJoint[i] * ( axis * modelAxis.Transpose() );
}
// set the joint mods
animator->SetJointAxis( waistJoint, JOINTMOD_WORLD_OVERRIDE, waistAxis );
animator->SetJointPos( waistJoint, JOINTMOD_WORLD_OVERRIDE, ( waistOrigin + waistOffset - modelOrigin ) * modelAxis.Transpose() );
for ( i = 0; i < numLegs; i++ ) {
animator->SetJointAxis( hipJoints[i], JOINTMOD_WORLD_OVERRIDE, hipAxis[i] );
animator->SetJointAxis( kneeJoints[i], JOINTMOD_WORLD_OVERRIDE, kneeAxis[i] );
animator->SetJointAxis( ankleJoints[i], JOINTMOD_WORLD_OVERRIDE, ankleAxis[i] );
}
ik_activate = true;
}
/*
================
idIK_Walk::Init
================
*/
bool idIK_Walk::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
int i;
float footSize;
idVec3 verts[4];
idTraceModel trm;
const char *jointName;
idVec3 dir, ankleOrigin, kneeOrigin, hipOrigin, dirOrigin;
idMat3 axis, ankleAxis, kneeAxis, hipAxis;
static idVec3 footWinding[4] = {
idVec3( 1.0f, 1.0f, 0.0f ),
idVec3( -1.0f, 1.0f, 0.0f ),
idVec3( -1.0f, -1.0f, 0.0f ),
idVec3( 1.0f, -1.0f, 0.0f )
};
if ( !self ) {
return false;
}
numLegs = Min( self->spawnArgs.GetInt( "ik_numLegs", "0" ), MAX_LEGS );
if ( numLegs == 0 ) {
return true;
}
if ( !idIK::Init( self, anim, modelOffset ) ) {
return false;
}
int numJoints = animator->NumJoints();
idJointMat *joints = ( idJointMat * )_alloca16( numJoints * sizeof( joints[0] ) );
// create the animation frame used to setup the IK
gameEdit->ANIM_CreateAnimFrame( animator->ModelHandle(), animator->GetAnim( modifiedAnim )->MD5Anim( 0 ), numJoints, joints, 1, animator->ModelDef()->GetVisualOffset() + modelOffset, animator->RemoveOrigin() );
enabledLegs = 0;
// get all the joints
for ( i = 0; i < numLegs; i++ ) {
jointName = self->spawnArgs.GetString( va( "ik_foot%d", i+1 ) );
footJoints[i] = animator->GetJointHandle( jointName );
if ( footJoints[i] == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid foot joint '%s'", jointName );
}
jointName = self->spawnArgs.GetString( va( "ik_ankle%d", i+1 ) );
ankleJoints[i] = animator->GetJointHandle( jointName );
if ( ankleJoints[i] == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid ankle joint '%s'", jointName );
}
jointName = self->spawnArgs.GetString( va( "ik_knee%d", i+1 ) );
kneeJoints[i] = animator->GetJointHandle( jointName );
if ( kneeJoints[i] == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid knee joint '%s'\n", jointName );
}
jointName = self->spawnArgs.GetString( va( "ik_hip%d", i+1 ) );
hipJoints[i] = animator->GetJointHandle( jointName );
if ( hipJoints[i] == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid hip joint '%s'\n", jointName );
}
jointName = self->spawnArgs.GetString( va( "ik_dir%d", i+1 ) );
dirJoints[i] = animator->GetJointHandle( jointName );
enabledLegs |= 1 << i;
}
jointName = self->spawnArgs.GetString( "ik_waist" );
waistJoint = animator->GetJointHandle( jointName );
if ( waistJoint == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid waist joint '%s'\n", jointName );
}
// get the leg bone lengths and rotation matrices
for ( i = 0; i < numLegs; i++ ) {
oldAnkleHeights[i] = 0.0f;
ankleAxis = joints[ ankleJoints[ i ] ].ToMat3();
ankleOrigin = joints[ ankleJoints[ i ] ].ToVec3();
kneeAxis = joints[ kneeJoints[ i ] ].ToMat3();
kneeOrigin = joints[ kneeJoints[ i ] ].ToVec3();
hipAxis = joints[ hipJoints[ i ] ].ToMat3();
hipOrigin = joints[ hipJoints[ i ] ].ToVec3();
// get the IK direction
if ( dirJoints[i] != INVALID_JOINT ) {
dirOrigin = joints[ dirJoints[ i ] ].ToVec3();
dir = dirOrigin - kneeOrigin;
} else {
dir.Set( 1.0f, 0.0f, 0.0f );
}
hipForward[i] = dir * hipAxis.Transpose();
kneeForward[i] = dir * kneeAxis.Transpose();
// conversion from upper leg bone axis to hip joint axis
upperLegLength[i] = GetBoneAxis( hipOrigin, kneeOrigin, dir, axis );
upperLegToHipJoint[i] = hipAxis * axis.Transpose();
// conversion from lower leg bone axis to knee joint axis
lowerLegLength[i] = GetBoneAxis( kneeOrigin, ankleOrigin, dir, axis );
lowerLegToKneeJoint[i] = kneeAxis * axis.Transpose();
}
smoothing = self->spawnArgs.GetFloat( "ik_smoothing", "0.75" );
waistSmoothing = self->spawnArgs.GetFloat( "ik_waistSmoothing", "0.75" );
footShift = self->spawnArgs.GetFloat( "ik_footShift", "0" );
waistShift = self->spawnArgs.GetFloat( "ik_waistShift", "0" );
minWaistFloorDist = self->spawnArgs.GetFloat( "ik_minWaistFloorDist", "0" );
minWaistAnkleDist = self->spawnArgs.GetFloat( "ik_minWaistAnkleDist", "0" );
footUpTrace = self->spawnArgs.GetFloat( "ik_footUpTrace", "32" );
footDownTrace = self->spawnArgs.GetFloat( "ik_footDownTrace", "32" );
tiltWaist = self->spawnArgs.GetBool( "ik_tiltWaist", "0" );
usePivot = self->spawnArgs.GetBool( "ik_usePivot", "0" );
// setup a clip model for the feet
footSize = self->spawnArgs.GetFloat( "ik_footSize", "4" ) * 0.5f;
if ( footSize > 0.0f ) {
for ( i = 0; i < 4; i++ ) {
verts[i] = footWinding[i] * footSize;
}
trm.SetupPolygon( verts, 4 );
footModel = new (TAG_PHYSICS_CLIP) idClipModel( trm );
}
initialized = true;
return true;
}
/*
================
sdIK_Walker::Evaluate
================
*/
bool sdIK_Walker::Evaluate( void ) {
trace_t results;
// clear joint mods
ClearJointMods();
animator->CreateFrame( gameLocal.time, true );
const renderEntity_t* renderEnt = self->GetRenderEntity();
idVec3 normal = -self->GetPhysics()->GetGravityNormal();
const idVec3& modelOrigin = renderEnt->origin;
const idMat3& modelAxis = renderEnt->axis;
float modelHeight = modelOrigin * normal;
isStable = true;
// get the joint positions for the feet
for ( int i = 0; i < legs.Num(); i++ ) {
leg_t& leg = legs[ i ];
idVec3 footOrigin;
animator->GetJointTransform( leg.footJoint, gameLocal.time, footOrigin );
leg.current.jointWorldOrigin = modelOrigin + footOrigin * modelAxis;
idVec3 start = leg.current.jointWorldOrigin + ( normal * footUpTrace );
idVec3 end = leg.current.jointWorldOrigin - ( normal * footDownTrace );
// gameLocal.clip.Translation( results, start, end, footModel, modelAxis, CONTENTS_SOLID, self );
if ( !gameLocal.clip.TracePoint( CLIP_DEBUG_PARMS results, start, end, MASK_VEHICLESOLID | CONTENTS_MONSTER, self ) ) {
isStable = false;
}
leg.current.floorHeight = results.endpos * normal;
idMat3 newAxes;
if ( results.fraction != 1.f ) {
idVec3 normal = results.c.normal * modelAxis.Transpose();
idVec3 vec3_forward( 1.f, 0.f, 0.f );
newAxes[ 0 ] = vec3_forward - ( normal * ( vec3_forward * normal ) );
newAxes[ 0 ].Normalize();
newAxes[ 2 ] = normal;
newAxes[ 1 ] = newAxes[ 2 ].Cross( newAxes[ 0 ] );
} else {
newAxes.Identity();
}
idQuat newQuat;
newQuat.Slerp( leg.lastAnkleAxes.ToQuat(), newAxes.ToQuat(), 0.1f );
leg.lastAnkleAxes = newQuat.ToMat3();
leg.lastAnkleAxes.FixDenormals();
if ( ik_debug.GetBool() && footModel ) {
idFixedWinding w;
for ( int j = 0; j < footModel->GetTraceModel()->numVerts; j++ ) {
w += footModel->GetTraceModel()->verts[j];
}
gameRenderWorld->DebugWinding( colorRed, w, results.endpos, results.endAxis );
}
}
// adjust heights of the ankles
float smallestShift = idMath::INFINITY;
float largestAnkleHeight = -idMath::INFINITY;
for ( int i = 0; i < legs.Num(); i++ ) {
leg_t& leg = legs[ i ];
idVec3 ankleOrigin;
idVec3 footOrigin;
animator->GetJointTransform( leg.ankleJoint, gameLocal.time, ankleOrigin, leg.current.ankleAxis );
animator->GetJointTransform( leg.footJoint, gameLocal.time, footOrigin );
float shift = leg.current.floorHeight - modelHeight + footShift;
if ( shift < smallestShift ) {
smallestShift = shift;
}
leg.current.jointWorldOrigin = modelOrigin + ankleOrigin * modelAxis;
float height = leg.current.jointWorldOrigin * normal;
if ( oldHeightsValid ) {
float step = height + shift - leg.oldAnkleHeight;
if ( step < 0 ) {
shift -= smoothing * step;
} else {
shift -= downsmoothing * step;
}
}
float newHeight = height + shift;
if ( newHeight > largestAnkleHeight ) {
largestAnkleHeight = newHeight;
}
leg.oldAnkleHeight = newHeight;
leg.current.jointWorldOrigin += shift * normal;
}
idVec3 waistOrigin;
animator->GetJointTransform( waistJoint, gameLocal.time, waistOrigin );
waistOrigin = modelOrigin + waistOrigin * modelAxis;
// adjust position of the waist
waistOffset = ( smallestShift + waistShift ) * normal;
// if the waist should be at least a certain distance above the floor
if ( minWaistFloorDist > 0.0f && waistOffset * normal < 0.0f ) {
idVec3 start = waistOrigin;
idVec3 end = waistOrigin + waistOffset - normal * minWaistFloorDist;
gameLocal.clip.Translation( CLIP_DEBUG_PARMS results, start, end, footModel, modelAxis, CONTENTS_SOLID, self );
float height = ( waistOrigin + waistOffset - results.endpos ) * normal;
if ( height < minWaistFloorDist ) {
waistOffset += ( minWaistFloorDist - height ) * normal;
}
}
// if the waist should be at least a certain distance above the ankles
if ( minWaistAnkleDist > 0.0f ) {
float height = ( waistOrigin + waistOffset ) * normal;
if ( height - largestAnkleHeight < minWaistAnkleDist ) {
waistOffset += ( minWaistAnkleDist - ( height - largestAnkleHeight ) ) * normal;
}
}
if ( oldHeightsValid ) {
// smoothly adjust height of waist
float newHeight = ( waistOrigin + waistOffset ) * normal;
float step = newHeight - oldWaistHeight;
waistOffset -= waistSmoothing * step * normal;
}
// save height of waist for smoothing
oldWaistHeight = ( waistOrigin + waistOffset ) * normal;
if ( !oldHeightsValid ) {
oldHeightsValid = true;
return false;
}
// solve IK
for ( int i = 0; i < legs.Num(); i++ ) {
leg_t& leg = legs[ i ];
idVec3 hipOrigin;
idMat3 axis;
// get the position of the hip in world space
animator->GetJointTransform( leg.hipJoint, gameLocal.time, hipOrigin, axis );
hipOrigin = modelOrigin + waistOffset + hipOrigin * modelAxis;
// DebugAxis( hipOrigin, axis * modelAxis );
idVec3 hipDir = leg.hipForward * axis * modelAxis;
idVec3 midOrigin;
// get the IK bend direction
animator->GetJointTransform( leg.midJoint, gameLocal.time, midOrigin, axis );
midOrigin = modelOrigin + waistOffset + midOrigin * modelAxis;
// DebugAxis( midOrigin, axis * modelAxis );
idVec3 midDir = leg.midForward * axis * modelAxis;
idVec3 midSideDir = leg.midSide * axis * modelAxis;
idVec3 kneeOrigin;
// get the IK bend direction
animator->GetJointTransform( leg.kneeJoint, gameLocal.time, kneeOrigin, axis );
kneeOrigin = modelOrigin + waistOffset + kneeOrigin * modelAxis;
// DebugAxis( kneeOrigin, axis * modelAxis );
idVec3 kneeDir = leg.kneeForward * axis * modelAxis;
idVec3 ankleOrigin;
animator->GetJointTransform( leg.ankleJoint, gameLocal.time, ankleOrigin, axis );
ankleOrigin = modelOrigin + waistOffset + ankleOrigin * modelAxis;
float len1 = leg.upperLegLength;
float minLen2 = fabs( leg.midLegLength - leg.lowerLegLength );
float maxLen2 = leg.midLegLength + leg.lowerLegLength;
float wantLen = ( hipOrigin - leg.current.jointWorldOrigin ).Length();
float constrainedMaxLen2, constrainedMinLen2;
float lenTotal = 0;
if( minLen2 < fabs( wantLen - len1 ) ) {
minLen2 = fabs( wantLen - len1 );
}
float minTotal = FindSmallest( len1, minLen2, maxLen2, constrainedMaxLen2, constrainedMinLen2 );
float maxTotal = len1 + maxLen2;
if ( maxTotal < wantLen ) {
lenTotal = maxTotal;
} else if ( minTotal > wantLen ) {
lenTotal = constrainedMaxLen2;
} else {
float scale = compressionInterpolate.GetCurrentValue( MS2SEC( gameLocal.time ) );
lenTotal = Lerp( minLen2, maxLen2, scale );
}
// solve IK and calculate upper knee position
SolveTwoBones( hipOrigin, leg.current.jointWorldOrigin, midDir, leg.upperLegLength, lenTotal, midOrigin );
float d1 = ( hipOrigin - midOrigin ).Length();
idVec3 kneeDirTest = kneeDir;
kneeDirTest.z += invertedLegDirOffset;
kneeDirTest.NormalizeFast();
// solve IK and calculate lower knee position, using -kneeDir, as lower leg is inverted
SolveTwoBones( midOrigin, leg.current.jointWorldOrigin, -kneeDirTest, leg.midLegLength, leg.lowerLegLength, kneeOrigin );
float d2 = ( midOrigin - kneeOrigin ).Length();
if ( ik_debug.GetBool() ) {
gameRenderWorld->DebugLine( colorCyan, hipOrigin, midOrigin );
gameRenderWorld->DebugLine( colorRed, midOrigin, kneeOrigin );
gameRenderWorld->DebugLine( colorBlue, kneeOrigin, leg.current.jointWorldOrigin );
gameRenderWorld->DebugLine( colorYellow, midOrigin, midOrigin + ( midSideDir * 32 ) );
gameRenderWorld->DebugLine( colorGreen, hipOrigin, hipOrigin + ( hipDir * 32 ) );
gameRenderWorld->DebugLine( colorGreen, midOrigin, midOrigin + ( midDir * 32 ) );
gameRenderWorld->DebugLine( colorGreen, kneeOrigin, kneeOrigin + ( -kneeDirTest * 32 ) );
}
// get the axis for the hip joint
GetBoneAxis( hipOrigin, midOrigin, hipDir, axis );
leg.current.hipAxis = leg.upperLegToHipJoint * ( axis * modelAxis.Transpose() );
// get the axis for the knee joint
GetBoneAxis( midOrigin, kneeOrigin, midSideDir, axis );
leg.current.midAxis = leg.midToUpperLegJoint * ( axis * modelAxis.Transpose() );
// get the axis for the knee joint
GetBoneAxis( kneeOrigin, leg.current.jointWorldOrigin, kneeDir, axis );
leg.current.kneeAxis = leg.lowerLegToKneeJoint * ( axis * modelAxis.Transpose() );
}
// set the joint mods
animator->SetJointPos( waistJoint, JOINTMOD_WORLD_OVERRIDE, ( waistOrigin + waistOffset - modelOrigin ) * modelAxis.Transpose() );
for ( int i = 0; i < legs.Num(); i++ ) {
leg_t& leg = legs[ i ];
animator->SetJointAxis( leg.hipJoint, JOINTMOD_WORLD_OVERRIDE, leg.current.hipAxis );
animator->SetJointAxis( leg.midJoint, JOINTMOD_WORLD_OVERRIDE, leg.current.midAxis );
animator->SetJointAxis( leg.kneeJoint, JOINTMOD_WORLD_OVERRIDE, leg.current.kneeAxis );
animator->SetJointAxis( leg.ankleJoint, JOINTMOD_WORLD_OVERRIDE, leg.current.ankleAxis * leg.lastAnkleAxes );
}
return true;
}
/*
================
sdIK_Walker::Init
================
*/
bool sdIK_Walker::Init( idEntity *self, const char *anim, const idVec3 &modelOffset ) {
float footSize;
idVec3 verts[ 4 ];
idTraceModel trm;
static idVec3 footWinding[ 4 ] = {
idVec3( 1.0f, 1.0f, 0.0f ),
idVec3( -1.0f, 1.0f, 0.0f ),
idVec3( -1.0f, -1.0f, 0.0f ),
idVec3( 1.0f, -1.0f, 0.0f )
};
waistOffset.Zero();
oldWaistHeight = 0;
oldHeightsValid = false;
isStable = true;
assert( self );
const idDict& spawnArgs = self->spawnArgs;
int numLegs = spawnArgs.GetInt( "ik_numLegs" );
if ( numLegs == 0 ) {
return true;
}
if ( !idIK::Init( self, anim, modelOffset ) ) {
return false;
}
float tweakFactor = spawnArgs.GetFloat( "ik_tweakFactor", "0.5" );
compressionInterpolate.Init( MS2SEC( gameLocal.time ), 1.f, tweakFactor, tweakFactor );
// get all the joints
for ( int i = 0; i < numLegs; i++ ) {
leg_t& leg = legs.Alloc();
const char* jointName;
jointName = spawnArgs.GetString( va( "ik_foot%d", i + 1 ) );
leg.footJoint = animator->GetJointHandle( jointName );
if ( leg.footJoint == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid foot joint '%s'", jointName );
}
jointName = spawnArgs.GetString( va( "ik_ankle%d", i + 1 ) );
leg.ankleJoint = animator->GetJointHandle( jointName );
if ( leg.ankleJoint == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid ankle joint '%s'", jointName );
}
jointName = spawnArgs.GetString( va( "ik_knee%d", i + 1 ) );
leg.kneeJoint = animator->GetJointHandle( jointName );
if ( leg.kneeJoint == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid knee joint '%s'", jointName );
}
jointName = spawnArgs.GetString( va( "ik_mid%d", i + 1 ) );
leg.midJoint = animator->GetJointHandle( jointName );
if ( leg.midJoint == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid mid joint '%s'", jointName );
}
jointName = spawnArgs.GetString( va( "ik_hip%d", i + 1 ) );
leg.hipJoint = animator->GetJointHandle( jointName );
if ( leg.hipJoint == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid hip joint '%s'", jointName );
}
}
const char* jointName = spawnArgs.GetString( "ik_waist" );
waistJoint = animator->GetJointHandle( jointName );
if ( waistJoint == INVALID_JOINT ) {
gameLocal.Error( "idIK_Walk::Init: invalid waist joint '%s'", jointName );
}
// get the leg bone lengths and rotation matrices
for ( int i = 0; i < legs.Num(); i++ ) {
leg_t& leg = legs[ i ];
idVec3 dir, ankleOrigin, kneeOrigin, midOrigin, hipOrigin, footOrigin, dirOrigin;
idVec3 dir2;
idMat3 axis, ankleAxis, kneeAxis, midAxis, hipAxis, footAxis;
leg.oldAnkleHeight = 0.0f;
animator->GetJointTransform( leg.ankleJoint, gameLocal.time, ankleOrigin, ankleAxis );
animator->GetJointTransform( leg.kneeJoint, gameLocal.time, kneeOrigin, kneeAxis );
animator->GetJointTransform( leg.midJoint, gameLocal.time, midOrigin, midAxis );
animator->GetJointTransform( leg.hipJoint, gameLocal.time, hipOrigin, hipAxis );
animator->GetJointTransform( leg.footJoint, gameLocal.time, footOrigin, footAxis );
// get the IK direction
dir = spawnArgs.GetVector( "ik_direction", "1 0 0" );
dir.Normalize();
dir2 = spawnArgs.GetVector( "ik_direction2", "0 1 0" );
dir2.Normalize();
leg.hipForward = dir * hipAxis.Transpose();
leg.midForward = dir * midAxis.Transpose();
leg.kneeForward = dir * kneeAxis.Transpose();
leg.midSide = dir2 * midAxis.Transpose();
// conversion from upper leg bone axis to hip joint axis
leg.upperLegLength = GetBoneAxis( hipOrigin, midOrigin, dir, axis );
leg.upperLegToHipJoint = hipAxis * axis.Transpose();
// conversion from upper leg bone axis to hip joint axis
leg.midLegLength = GetBoneAxis( midOrigin, kneeOrigin, dir2, axis );
leg.midToUpperLegJoint = midAxis * axis.Transpose();
// conversion from lower leg bone axis to knee joint axis
leg.lowerLegLength = GetBoneAxis( kneeOrigin, ankleOrigin, dir, axis );
leg.lowerLegToKneeJoint = kneeAxis * axis.Transpose();
leg.lastAnkleAxes.Identity();
}
smoothing = spawnArgs.GetFloat( "ik_smoothing", "0.75" );
downsmoothing = spawnArgs.GetFloat( "ik_downsmoothing", "0.25" );
waistSmoothing = spawnArgs.GetFloat( "ik_waistSmoothing", "0.75" );
footShift = spawnArgs.GetFloat( "ik_footShift", "0" );
waistShift = spawnArgs.GetFloat( "ik_waistShift", "0" );
minWaistFloorDist = spawnArgs.GetFloat( "ik_minWaistFloorDist", "0" );
minWaistAnkleDist = spawnArgs.GetFloat( "ik_minWaistAnkleDist", "0" );
footUpTrace = spawnArgs.GetFloat( "ik_footUpTrace", "32" );
footDownTrace = spawnArgs.GetFloat( "ik_footDownTrace", "32" );
tiltWaist = spawnArgs.GetBool( "ik_tiltWaist", "0" );
usePivot = spawnArgs.GetBool( "ik_usePivot", "0" );
invertedLegDirOffset= spawnArgs.GetFloat( "ik_invertlegoffset", "-2" ); // Gordon: Hack to tweak the inverted dir for the back section of the legs
// setup a clip model for the feet
footSize = spawnArgs.GetFloat( "ik_footSize", "4" ) * 0.5f;
if ( footSize > 0.0f ) {
for ( int i = 0; i < 4; i++ ) {
verts[ i ] = footWinding[ i ] * footSize;
}
trm.SetupPolygon( verts, 4 );
footModel = new idClipModel( trm, false );
}
initialized = true;
return true;
}
