/*
================
sdPlayerArmIK::Update
================
*/
void sdPlayerArmIK::Update( idPlayer* player, idEntity* target ) {
struct arm_t {
idMat3 targetShoulderAxis;
idMat3 targetElbowAxis;
idMat3 targetHandAxis;
};
arm_t arms[ NUM_ARMS ];
ClearJointMods( player );
idAnimator* animator = player->GetAnimator();
idMat3 tempAxis;
idVec3 modelOrigin = player->GetRenderEntity()->origin;
idMat3 modelAxis = player->GetRenderEntity()->axis;
idMat3 transposeModelAxis = modelAxis.Transpose();
// get the arm bone lengths and rotation matrices
for ( int i = 0; i < NUM_ARMS; i++ ) {
if ( armTargets[ i ].joint == INVALID_JOINT ) {
continue;
}
idMat3 handAxis;
idVec3 handOrigin;
animator->GetJointTransform( player->GetHandJoint( i ), gameLocal.time, handOrigin, handAxis );
idMat3 elbowAxis;
idVec3 elbowOrigin;
animator->GetJointTransform( player->GetElbowJoint( i ), gameLocal.time, elbowOrigin, elbowAxis );
idMat3 shoulderAxis;
idVec3 shoulderOrigin;
animator->GetJointTransform( player->GetShoulderJoint( i ), gameLocal.time, shoulderOrigin, shoulderAxis );
idVec3 t1 = ( elbowOrigin - shoulderOrigin );
// t1.Normalize();
idVec3 t2 = ( elbowOrigin - handOrigin );
// t2.Normalize();
idVec3 dir = t1 + t2;
dir.Normalize();
// conversion from upper arm bone axis to should joint axis
float upperArmLength = idIK::GetBoneAxis( shoulderOrigin, elbowOrigin, dir, tempAxis );
idMat3 upperArmToShoulderJoint = shoulderAxis * tempAxis.Transpose();
// conversion from lower arm bone axis to elbow joint axis
float lowerArmLength = idIK::GetBoneAxis( elbowOrigin, handOrigin, dir, tempAxis );
idMat3 lowerArmToElbowJoint = elbowAxis * tempAxis.Transpose();
// get target
idVec3 targetOrigin;
target->GetWorldOriginAxis( armTargets[ i ].joint, targetOrigin, arms[ i ].targetHandAxis );
idVec3 targetOriginLocal = targetOrigin - modelOrigin;
targetOriginLocal *= transposeModelAxis;
arms[ i ].targetHandAxis *= transposeModelAxis;
// solve IK and calculate elbow position
idIK::SolveTwoBones( shoulderOrigin, targetOriginLocal, dir, upperArmLength, lowerArmLength, elbowOrigin );
if ( ik_debug.GetBool() ) {
idVec3 shoulderWorld = ( shoulderOrigin * modelAxis ) + modelOrigin;
idVec3 elbowWorld = ( elbowOrigin * modelAxis ) + modelOrigin;
gameRenderWorld->DebugLine( colorCyan, shoulderWorld, elbowWorld );
gameRenderWorld->DebugLine( colorRed, elbowWorld, targetOrigin );
gameRenderWorld->DebugBox( colorYellow, idBox( targetOrigin, idVec3( 2, 2, 2 ), mat3_identity ) );
gameRenderWorld->DebugLine( colorGreen, elbowWorld, elbowWorld + ( ( dir * modelAxis ) * 8 ) );
}
// get the axis for the shoulder joint
idIK::GetBoneAxis( shoulderOrigin, elbowOrigin, dir, tempAxis );
arms[ i ].targetShoulderAxis = upperArmToShoulderJoint * tempAxis;
// get the axis for the elbow joint
idIK::GetBoneAxis( elbowOrigin, targetOriginLocal, dir, tempAxis );
arms[ i ].targetElbowAxis = lowerArmToElbowJoint * tempAxis;
}
for ( int i = 0; i < NUM_ARMS; i++ ) {
if ( armTargets[ i ].joint == INVALID_JOINT ) {
continue;
}
animator->SetJointAxis( player->GetShoulderJoint( i ), JOINTMOD_WORLD_OVERRIDE, arms[ i ].targetShoulderAxis );
animator->SetJointAxis( player->GetElbowJoint( i ), JOINTMOD_WORLD_OVERRIDE, arms[ i ].targetElbowAxis );
animator->SetJointAxis( player->GetHandJoint( i ), JOINTMOD_WORLD_OVERRIDE, arms[ i ].targetHandAxis );
}
}
/*
================
sdScriptedEntityHelper_LegIk::Update
================
*/
void sdScriptedEntityHelper_LegIk::Update( bool postThink ) {
if ( !gameLocal.isNewFrame || postThink ) {
return;
}
if ( gameLocal.time - startTime > lifetime ) {
delete this;
return;
}
idMat3 upperJointAxis, middleJointAxis, lowerJointAxis;
idVec3 upperJointOrg, middleJointOrg, lowerJointOrg;
ClearJointMods();
idAnimator* animator = _owner->GetAnimator();
if ( !animator->GetJointTransform( upperLegJoint, gameLocal.time, upperJointOrg, upperJointAxis ) ) {
delete this;
return;
}
if ( !animator->GetJointTransform( middleLegJoint, gameLocal.time, middleJointOrg, middleJointAxis ) ) {
delete this;
return;
}
if ( animator->GetJointTransform( lowerLegJoint, gameLocal.time, lowerJointOrg, lowerJointAxis ) ) {
delete this;
return;
}
{
idVec3 traceOrg = _owner->GetRenderEntity()->origin + ( lowerJointOrg * _owner->GetRenderEntity()->axis );
trace_t tr;
gameLocal.clip.TracePoint( CLIP_DEBUG_PARMS tr, traceOrg + idVec3( 0, 0, maxUpTrace ), traceOrg - idVec3( 0, 0, maxDownTrace ), CONTENTS_SOLID, _owner );
/* gameRenderWorld->DebugCircle( colorGreen, tr.endpos, idVec3( 0, 0, 1 ), 8, 16 );
gameRenderWorld->DebugLine( colorRed, tr.endpos, tr.endpos + idVec3( 0.f, 0.f, currentGroundOffset ) );
gameRenderWorld->DebugLine( colorBlue, tr.endpos, tr.endpos + idVec3( 0.f, 0.f, lowerJointOrg.z ) );*/
currentGroundOffset = Lerp( currentGroundOffset, lowerJointOrg.z, blendRate * MS2SEC( gameLocal.msec ) );
idVec3 jointPos = ( ( tr.endpos + idVec3( 0.f, 0.f, currentGroundOffset ) ) - _owner->GetRenderEntity()->origin ) * _owner->GetRenderEntity()->axis.Transpose();
idVec3 endPos;
idIK::SolveTwoBones( upperJointOrg, jointPos, -upDir, upperLength, lowerLength, endPos );
idMat3 axis;
idIK::GetBoneAxis( upperJointOrg, endPos, upDir, axis );
idMat3 upperAxis = midToUpperJoint * axis;
idIK::GetBoneAxis( endPos, jointPos, upDir, axis );
idMat3 middleAxis = lowerToMidJoint * axis;
animator->SetJointAxis( upperLegJoint, JOINTMOD_WORLD_OVERRIDE, upperAxis );
animator->SetJointAxis( middleLegJoint, JOINTMOD_WORLD_OVERRIDE, middleAxis );
/* idVec3 worldOrg;
_owner->GetWorldOrigin( upperLegJoint, worldOrg );
gameRenderWorld->DebugLine( colorGreen, worldOrg, worldOrg + ( ( side * _owner->GetRenderEntity()->axis ) * 16 ) );
gameRenderWorld->DebugLine( colorBlue, worldOrg, worldOrg + ( ( up * _owner->GetRenderEntity()->axis ) * 16 ) );
gameRenderWorld->DebugLine( colorRed, worldOrg, worldOrg + ( ( temp * _owner->GetRenderEntity()->axis ) * 16 ) );*/
}
}
/*
================
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;
}