void StudioModel::SlerpBones( vec4_t q1[], vec3_t pos1[], vec4_t q2[], vec3_t pos2[], float s )
{
int i;
vec4_t q3;
float s1;
if (s < 0) s = 0;
else if (s > 1.0) s = 1.0;
s1 = 1.0 - s;
for (i = 0; i < m_pstudiohdr->numbones; i++)
{
QuaternionSlerp( q1[i], q2[i], s, q3 );
q1[i][0] = q3[0];
q1[i][1] = q3[1];
q1[i][2] = q3[2];
q1[i][3] = q3[3];
pos1[i][0] = pos1[i][0] * s1 + pos2[i][0] * s;
pos1[i][1] = pos1[i][1] * s1 + pos2[i][1] * s;
pos1[i][2] = pos1[i][2] * s1 + pos2[i][2] * s;
}
}
/* <830a3> ../engine/r_studio.c:639 */
void R_StudioSlerpBones(vec4_t *q1, vec3_t *pos1, vec4_t *q2, vec3_t *pos2, float s)
{
int i;
vec4_t q3;
float s1;
if (s < 0) s = 0;
else if (s > 1.0) s = 1.0;
s1 = 1.0f - s;
for (i = 0; i < pstudiohdr->numbones; i++)
{
QuaternionSlerp(q1[i], q2[i], s, q3);
q1[i][0] = q3[0];
q1[i][1] = q3[1];
q1[i][2] = q3[2];
q1[i][3] = q3[3];
pos1[i][0] = pos1[i][0] * s1 + pos2[i][0] * s;
pos1[i][1] = pos1[i][1] * s1 + pos2[i][1] * s;
pos1[i][2] = pos1[i][2] * s1 + pos2[i][2] * s;
}
}
//-----------------------------------------------------------------------------
// Purpose: Causes the turret to face its desired angles
// Returns distance current and goal angles the angles in degrees.
//-----------------------------------------------------------------------------
float CNPC_RocketTurret::UpdateFacing( void )
{
Quaternion qtCurrent ( m_vecCurrentAngles.Get() );
Quaternion qtGoal ( m_vecGoalAngles );
Quaternion qtOut;
float flDiff = QuaternionAngleDiff( qtCurrent, qtGoal );
// 1/10th degree is all the granularity we need, gives rocket player hit box width accuracy at 18k game units.
if ( flDiff < 0.1 )
return flDiff;
// Slerp 5% of the way to goal (distance dependant speed, but torque minimial and no euler wrapping issues).
QuaternionSlerp( qtCurrent, qtGoal, 0.05, qtOut );
QAngle vNewAngles;
QuaternionAngles( qtOut, vNewAngles );
m_vecCurrentAngles = vNewAngles;
SyncPoseToAimAngles();
return flDiff;
}
/*
====================
StudioCalcBoneQuaterion
====================
*/
void CStudioModelRenderer::StudioCalcBoneQuaterion( int frame, float s, mstudiobone_t *pbone, mstudioanim_t *panim, float *adj, float *q )
{
int j, k;
vec4_t q1, q2;
vec3_t angle1, angle2;
mstudioanimvalue_t *panimvalue;
for (j = 0; j < 3; j++)
{
if (panim->offset[j+3] == 0)
{
angle2[j] = angle1[j] = pbone->value[j+3]; // default;
}
else
{
panimvalue = (mstudioanimvalue_t *)((byte *)panim + panim->offset[j+3]);
k = frame;
// DEBUG
if (panimvalue->num.total < panimvalue->num.valid)
k = 0;
while (panimvalue->num.total <= k)
{
k -= panimvalue->num.total;
panimvalue += panimvalue->num.valid + 1;
// DEBUG
if (panimvalue->num.total < panimvalue->num.valid)
k = 0;
}
// Bah, missing blend!
if (panimvalue->num.valid > k)
{
angle1[j] = panimvalue[k+1].value;
if (panimvalue->num.valid > k + 1)
{
angle2[j] = panimvalue[k+2].value;
}
else
{
if (panimvalue->num.total > k + 1)
angle2[j] = angle1[j];
else
angle2[j] = panimvalue[panimvalue->num.valid+2].value;
}
}
else
{
angle1[j] = panimvalue[panimvalue->num.valid].value;
if (panimvalue->num.total > k + 1)
{
angle2[j] = angle1[j];
}
else
{
angle2[j] = panimvalue[panimvalue->num.valid + 2].value;
}
}
angle1[j] = pbone->value[j+3] + angle1[j] * pbone->scale[j+3];
angle2[j] = pbone->value[j+3] + angle2[j] * pbone->scale[j+3];
}
if (pbone->bonecontroller[j+3] != -1)
{
angle1[j] += adj[pbone->bonecontroller[j+3]];
angle2[j] += adj[pbone->bonecontroller[j+3]];
}
}
if (!VectorCompare( angle1, angle2 ))
{
AngleQuaternion( angle1, q1 );
AngleQuaternion( angle2, q2 );
QuaternionSlerp( q1, q2, s, q );
}
else
{
AngleQuaternion( angle1, q );
}
}
void msModel::EvaluateJoint(int index, float frame)
{
ms3d_joint_t *joint = &m_joints[index];
//
// calculate joint animation matrix, this matrix will animate matLocalSkeleton
//
vec3_t pos = { 0.0f, 0.0f, 0.0f };
int numPositionKeys = (int) joint->positionKeys.size();
if (numPositionKeys > 0)
{
int i1 = -1;
int i2 = -1;
// find the two keys, where "frame" is in between for the position channel
for (int i = 0; i < (numPositionKeys - 1); i++)
{
if (frame >= joint->positionKeys[i].time && frame < joint->positionKeys[i + 1].time)
{
i1 = i;
i2 = i + 1;
break;
}
}
// if there are no such keys
if (i1 == -1 || i2 == -1)
{
// either take the first
if (frame < joint->positionKeys[0].time)
{
pos[0] = joint->positionKeys[0].key[0];
pos[1] = joint->positionKeys[0].key[1];
pos[2] = joint->positionKeys[0].key[2];
}
// or the last key
else if (frame >= joint->positionKeys[numPositionKeys - 1].time)
{
pos[0] = joint->positionKeys[numPositionKeys - 1].key[0];
pos[1] = joint->positionKeys[numPositionKeys - 1].key[1];
pos[2] = joint->positionKeys[numPositionKeys - 1].key[2];
}
}
// there are such keys, so interpolate using hermite interpolation
else
{
ms3d_keyframe_t *p0 = &joint->positionKeys[i1];
ms3d_keyframe_t *p1 = &joint->positionKeys[i2];
ms3d_tangent_t *m0 = &joint->tangents[i1];
ms3d_tangent_t *m1 = &joint->tangents[i2];
// normalize the time between the keys into [0..1]
float t = (frame - joint->positionKeys[i1].time) / (joint->positionKeys[i2].time - joint->positionKeys[i1].time);
float t2 = t * t;
float t3 = t2 * t;
// calculate hermite basis
float h1 = 2.0f * t3 - 3.0f * t2 + 1.0f;
float h2 = -2.0f * t3 + 3.0f * t2;
float h3 = t3 - 2.0f * t2 + t;
float h4 = t3 - t2;
// do hermite interpolation
pos[0] = h1 * p0->key[0] + h3 * m0->tangentOut[0] + h2 * p1->key[0] + h4 * m1->tangentIn[0];
pos[1] = h1 * p0->key[1] + h3 * m0->tangentOut[1] + h2 * p1->key[1] + h4 * m1->tangentIn[1];
pos[2] = h1 * p0->key[2] + h3 * m0->tangentOut[2] + h2 * p1->key[2] + h4 * m1->tangentIn[2];
}
}
vec4_t quat = { 0.0f, 0.0f, 0.0f, 1.0f };
int numRotationKeys = (int) joint->rotationKeys.size();
if (numRotationKeys > 0)
{
int i1 = -1;
int i2 = -1;
// find the two keys, where "frame" is in between for the rotation channel
for (int i = 0; i < (numRotationKeys - 1); i++)
{
if (frame >= joint->rotationKeys[i].time && frame < joint->rotationKeys[i + 1].time)
{
i1 = i;
i2 = i + 1;
break;
}
}
// if there are no such keys
if (i1 == -1 || i2 == -1)
{
// either take the first key
if (frame < joint->rotationKeys[0].time)
{
AngleQuaternion(joint->rotationKeys[0].key, quat);
}
// or the last key
else if (frame >= joint->rotationKeys[numRotationKeys - 1].time)
{
AngleQuaternion(joint->rotationKeys[numRotationKeys - 1].key, quat);
}
}
// there are such keys, so do the quaternion slerp interpolation
else
{
float t = (frame - joint->rotationKeys[i1].time) / (joint->rotationKeys[i2].time - joint->rotationKeys[i1].time);
vec4_t q1;
AngleQuaternion(joint->rotationKeys[i1].key, q1);
vec4_t q2;
AngleQuaternion(joint->rotationKeys[i2].key, q2);
QuaternionSlerp(q1, q2, t, quat);
}
}
// make a matrix from pos/quat
float matAnimate[3][4];
QuaternionMatrix(quat, matAnimate);
matAnimate[0][3] = pos[0];
matAnimate[1][3] = pos[1];
matAnimate[2][3] = pos[2];
// animate the local joint matrix using: matLocal = matLocalSkeleton * matAnimate
R_ConcatTransforms(joint->matLocalSkeleton, matAnimate, joint->matLocal);
// build up the hierarchy if joints
// matGlobal = matGlobal(parent) * matLocal
if (joint->parentIndex == -1)
{
memcpy(joint->matGlobal, joint->matLocal, sizeof(joint->matGlobal));
}
else
{
ms3d_joint_t *parentJoint = &m_joints[joint->parentIndex];
R_ConcatTransforms(parentJoint->matGlobal, joint->matLocal, joint->matGlobal);
}
}
void RotationInterpolatorFunc_Linear( float time, Quaternion &outRot )
{
// basic 4D spherical linear interpolation
QuaternionSlerp( g_KeyFramePtr[0].qRot, g_KeyFramePtr[1].qRot, time, outRot );
}
void Interpolator_CurveInterpolate_NonNormalized( int interpolationType,
const Quaternion &vPre,
const Quaternion &vStart,
const Quaternion &vEnd,
const Quaternion &vNext,
float f,
Quaternion &vOut )
{
vOut.Init();
switch ( interpolationType )
{
default:
Warning( "Unknown interpolation type %d\n",
(int)interpolationType );
// break; // Fall through and use catmull_rom as default
case INTERPOLATE_CATMULL_ROM_NORMALIZEX:
case INTERPOLATE_DEFAULT:
case INTERPOLATE_CATMULL_ROM:
case INTERPOLATE_CATMULL_ROM_NORMALIZE:
case INTERPOLATE_CATMULL_ROM_TANGENT:
case INTERPOLATE_KOCHANEK_BARTELS:
case INTERPOLATE_KOCHANEK_BARTELS_EARLY:
case INTERPOLATE_KOCHANEK_BARTELS_LATE:
case INTERPOLATE_SIMPLE_CUBIC:
case INTERPOLATE_BSPLINE:
// FIXME, since this ignores vPre and vNext we could omit computing them aove
QuaternionSlerp( vStart, vEnd, f, vOut );
break;
case INTERPOLATE_EASE_IN:
{
f = sin( M_PI * f * 0.5f );
// Fixme, since this ignores vPre and vNext we could omit computing them aove
QuaternionSlerp( vStart, vEnd, f, vOut );
}
break;
case INTERPOLATE_EASE_OUT:
{
f = 1.0f - sin( M_PI * f * 0.5f + 0.5f * M_PI );
// Fixme, since this ignores vPre and vNext we could omit computing them aove
QuaternionSlerp( vStart, vEnd, f, vOut );
}
break;
case INTERPOLATE_EASE_INOUT:
{
f = SimpleSpline( f );
// Fixme, since this ignores vPre and vNext we could omit computing them aove
QuaternionSlerp( vStart, vEnd, f, vOut );
}
break;
case INTERPOLATE_LINEAR_INTERP:
// Fixme, since this ignores vPre and vNext we could omit computing them aove
QuaternionSlerp( vStart, vEnd, f, vOut );
break;
case INTERPOLATE_EXPONENTIAL_DECAY:
vOut.Init();
break;
case INTERPOLATE_HOLD:
{
vOut = vStart;
}
break;
}
}
void PmxBonePerformTransform(PMX_BONE* bone)
{
float rotation[4] = IDENTITY_QUATERNION;
float position[3] = {0};
if((bone->flags & PMX_BONE_FLAG_HAS_INHERENT_ROTATION) != 0)
{
PMX_BONE *parent_bone = bone->parent_inherent_bone;
if(parent_bone != NULL)
{
if((parent_bone->flags & PMX_BONE_FLAG_HAS_INHERENT_ROTATION) != 0)
{
MultiQuaternion(rotation, parent_bone->local_inherent_rotation);
}
else
{
float rotate_value[4];
COPY_VECTOR4(rotate_value, parent_bone->local_rotation);
MultiQuaternion(rotate_value, parent_bone->local_morph_rotation);
MultiQuaternion(rotation, rotate_value);
//MultiQuaternion(rotation, parent_bone->local_morph_rotation);
//MultiQuaternion(rotation, parent_bone->local_rotation);
}
}
if(FuzzyZero(bone->coefficient - 1.0f) == 0)
{
float set_value[4] = IDENTITY_QUATERNION;
QuaternionSlerp(set_value, set_value, rotation, bone->coefficient);
COPY_VECTOR4(rotation, set_value);
}
if(parent_bone != NULL && (parent_bone->flags & PMX_BONE_FLAG_HAS_INVERSE_KINEMATICS) != 0)
{
MultiQuaternion(rotation, parent_bone->joint_rotation);
}
COPY_VECTOR4(bone->local_inherent_rotation, rotation);
MultiQuaternion(bone->local_inherent_rotation, bone->local_rotation);
MultiQuaternion(bone->local_inherent_rotation, bone->local_morph_rotation);
QuaternionNormalize(bone->local_inherent_rotation);
}
MultiQuaternion(rotation, bone->local_rotation);
MultiQuaternion(rotation, bone->local_morph_rotation);
MultiQuaternion(rotation, bone->joint_rotation);
QuaternionNormalize(rotation);
if((bone->flags & PMX_BONE_FLAG_HAS_INHERENT_TRANSLATION) != 0)
{
PMX_BONE *parent_bone = bone->parent_inherent_bone;
if(parent_bone != NULL)
{
if((parent_bone->flags & PMX_BONE_FLAG_HAS_INHERENT_TRANSLATION) != 0)
{
position[0] += parent_bone->local_inherent_translation[0];
position[1] += parent_bone->local_inherent_translation[1];
position[2] += parent_bone->local_inherent_translation[2];
}
else
{
position[0] += parent_bone->local_translation[0] + parent_bone->local_morph_translation[0];
position[1] += parent_bone->local_translation[1] + parent_bone->local_morph_translation[1];
position[2] += parent_bone->local_translation[2] + parent_bone->local_morph_translation[2];
}
}
if(FuzzyZero(bone->coefficient - 1) == 0)
{
position[0] *= bone->coefficient;
position[1] *= bone->coefficient;
position[2] *= bone->coefficient;
}
COPY_VECTOR3(bone->local_inherent_translation, position);
}
position[0] += bone->local_translation[0] + bone->local_morph_translation[0];
position[1] += bone->local_translation[1] + bone->local_morph_translation[1];
position[2] += bone->local_translation[2] + bone->local_morph_translation[2];
PmxBoneUpdateWorldTransform(bone, position, rotation);
}
