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); } } }