public static Vector3 RotatePoint(ref Quaternion q, ref Vector3 v)
{
Vector3 outv;
Quaternion inv = new Quaternion();
inv.X = -q.X;
inv.Y = -q.Y;
inv.Z = -q.Z;
inv.W = q.W;
Quaternion norminv = Quaternion.Normalize(inv);
Quaternion m = MultVec(ref q, ref v);
Quaternion qm = MathExt.Mult(ref m, ref norminv);
outv.X = qm.Xyz.X;
outv.Y = qm.Xyz.Y;
outv.Z = qm.Xyz.Z;
return(outv);
}
/// <summary>
/// luo luuranko.
/// </summary>
/// <param name="jointInfos"></param>
/// <param name="baseFrame"></param>
/// <param name="animFrameData"></param>
/// <param name="frameIndex"></param>
/// <param name="num_joints"></param>
/// <param name="md5anim"></param>
void BuildFrameSkeleton(ref MD5JointInfo[] jointInfos,
ref MD5BaseFrameJoint[] baseFrame,
ref float[] animFrameData,
int frameIndex,
int num_joints, ref Animation md5anim)
{
int i;
for (i = 0; i < num_joints; ++i)
{
MD5BaseFrameJoint baseJoint = baseFrame[i];
Vector3 animatedPos;
Quaternion animatedOrient;
int j = 0;
animatedPos = baseJoint.pos;
animatedOrient = baseJoint.orient;
if ((jointInfos[i].flags & 1) > 0) /* Tx */
{
animatedPos.X = animFrameData[jointInfos[i].startIndex + j];
++j;
}
if ((jointInfos[i].flags & 2) > 0) /* Ty */
{
animatedPos.Y = animFrameData[jointInfos[i].startIndex + j];
++j;
}
if ((jointInfos[i].flags & 4) > 0) /* Tz */
{
animatedPos.Z = animFrameData[jointInfos[i].startIndex + j];
++j;
}
if ((jointInfos[i].flags & 8) > 0) /* Qx */
{
animatedOrient.X = animFrameData[jointInfos[i].startIndex + j];
++j;
}
if ((jointInfos[i].flags & 16) > 0) /* Qy */
{
animatedOrient.Y = animFrameData[jointInfos[i].startIndex + j];
++j;
}
if ((jointInfos[i].flags & 32) > 0) /* Qz */
{
animatedOrient.Z = animFrameData[jointInfos[i].startIndex + j];
++j;
}
/* Compute orient quaternion's w value */
MathExt.ComputeW(ref animatedOrient);
int parent = jointInfos[i].parent;
md5anim.skelFrames[frameIndex, i].parent = parent;
md5anim.skelFrames[frameIndex, i].name = jointInfos[i].name;
/* Has parent? */
if (md5anim.skelFrames[frameIndex, i].parent < 0)
{
md5anim.skelFrames[frameIndex, i].pos = animatedPos;
md5anim.skelFrames[frameIndex, i].orient = animatedOrient;
}
else
{
MD5Joint parentJoint = md5anim.skelFrames[frameIndex, parent];
Vector3 rpos; /* Rotated Position */
/* Add positions */
rpos = MathExt.RotatePoint(ref parentJoint.orient, ref animatedPos);
md5anim.skelFrames[frameIndex, i].pos.X = rpos.X + parentJoint.pos.X;
md5anim.skelFrames[frameIndex, i].pos.Y = rpos.Y + parentJoint.pos.Y;
md5anim.skelFrames[frameIndex, i].pos.Z = rpos.Z + parentJoint.pos.Z;
/* Concatenate rotations */
md5anim.skelFrames[frameIndex, i].orient = MathExt.Mult(ref parentJoint.orient, ref animatedOrient);
md5anim.skelFrames[frameIndex, i].orient = MathExt.Normalize(ref md5anim.skelFrames[frameIndex, i].orient);
}
}
}