public static Assimp.Quaternion ToQuaternion(this SlimDX.Quaternion q)
{
Assimp.Quaternion ret = new Assimp.Quaternion();
ret.W = q.W;
ret.X = q.X;
ret.Y = q.Y;
ret.Z = q.Z;
return(ret);
}
/// <summary>
/// Find the rotation quaternion at the specified animation time for the specified animation sequence
/// </summary>
/// <param name="animationTime"> The number of ticks in the animation time domain </param>
/// <param name="animationNode"> The node to extract the translation vector from </param>
/// <returns> The linearly interpolated rotation quaternion that represents the rotation at specified time </returns>
private Quaternion CalcInterpolateRotation(double animationTime, ClientAnimationNode animationNode)
{
if (animationNode.Rotations.Count == 1)
{
return(animationNode.Rotations[0]);
}
if (animationNode.RotationTime[0] > animationTime)
{
return(animationNode.Rotations[0]);
}
// find the key frame before or at the current frame
int rotationIndex = -1;
for (int i = 0; i < animationNode.Rotations.Count; i++)
{
if (animationNode.RotationTime[i] > animationTime)
{
rotationIndex = i - 1;
break;
}
}
if (rotationIndex == -1)
{
return(animationNode.Rotations[animationNode.Rotations.Count - 1]);
}
int nextRotationIndex = rotationIndex + 1;
double frameDuration = animationNode.RotationTime[nextRotationIndex] -
animationNode.RotationTime[rotationIndex];
double factor = (animationTime - animationNode.RotationTime[rotationIndex]) / frameDuration;
if (factor <= 0.0)
{
return(animationNode.Rotations[rotationIndex]);
}
if (factor >= 1.0)
{
return(animationNode.Rotations[nextRotationIndex]);
}
return(Quaternion.Lerp(animationNode.Rotations[rotationIndex],
animationNode.Rotations[nextRotationIndex], (float)factor));
}
/// <summary>
///
/// </summary>
/// <param name="pos1">回転対象</param>
/// <param name="pos2">目標ボーン</param>
/// <param name="skeleton"></param>
/// <param name="type1"></param>
/// <param name="type2"></param>
/// <returns></returns>
private void getRotation(PMXBone b1, PMXBone b2, Skeleton skeleton, SkeletonJoint skel1,
SkeletonJoint skel2)
{
SkeletonJoint targetBone = skel1;
SkeletonJoint baseBone = skel2;
if (targetBone.PositionConfidence < 0.5 || baseBone.PositionConfidence < 0.5)
{
return;
}
Vector3 t2b = Vector3.Normalize(ToVector3(targetBone.Position) - ToVector3(baseBone.Position));
Vector3 it2b = Vector3.Normalize(Vector3.TransformCoordinate(b2.Position, b2.GlobalPose) - Vector3.TransformCoordinate(b1.Position, b1.GlobalPose));
Vector3 axis = Vector3.Cross(t2b, it2b);
float angle = (float)-Math.Acos(Vector3.Dot(t2b, it2b));
b1.Rotation *= Quaternion.RotationAxis(axis, angle);
b1.UpdateGrobalPose();
}
public void Read(out SlimDX.Quaternion data)
{
int length = Marshal.SizeOf(typeof(SlimDX.Quaternion));
if (CurPtr() + length > mSize)
{
data = SlimDX.Quaternion.Identity;
OnReadError();
return;
}
unsafe
{
fixed(SlimDX.Quaternion *pValue = &data)
{
Marshal.Copy(mHandle, mPos, (IntPtr)pValue, length);
}
}
mPos += length;
}
/// <summary>
/// Set the local tranformation matrices of the bone tree
/// </summary>
/// <param name="animationTime"> The time, in ticks, in the animation time domain </param>
/// <param name="animationIndex"> The index of the animation sequence </param>
private void Evaluate(double animationTime, int animationIndex)
{
Dictionary <string, ClientAnimationNode> currentChannels = _animationNodes[animationIndex];
foreach (var chpair in currentChannels)
{
if (!_allBoneLookup.ContainsKey(chpair.Key))
{
Console.WriteLine("Did not find the bone node " + chpair.Key);
continue;
}
Vector3 vPosition = CalcInterpolateTranslation(animationTime, chpair.Value);
Quaternion vQuaternion = CalcInterpolateRotation(animationTime, chpair.Value);
Vector3 Scaling = CalcInterpolateScaling(animationTime, chpair.Value);
Matrix r_mat = Matrix.RotationQuaternion(vQuaternion);
Matrix s_mat = Matrix.Scaling(Scaling);
Matrix t_mat = Matrix.Translation(vPosition);
_allBoneLookup[chpair.Key].LocalTransform = s_mat * r_mat * t_mat;
}
}
public static PMath.Quaternion ToPhx(this SDX.Quaternion q)
{
return(new PMath.Quaternion(q.X, q.Y, q.Z, q.W));
}
/// <summary>
/// ITransformUpdaterのメンバーの実装
/// </summary>
public bool UpdateTransform()
{
UserTrackerFrameRef usrFrameRef = device.CurrentUserTrackerFrameRef;
if (CurrentTrackUserId >= 0)
{
UserData[] usrs =
(from sk in usrFrameRef.Users where sk.UserId == CurrentTrackUserId select sk).ToArray();
if (usrs.Length != 1)
{
return(true);
}
Skeleton skeleton = usrs[0].Skeleton;
if (skeleton.State == Skeleton.SkeletonState.Tracked)
{
trackTarget = skeleton;
if (TrackingUser != null)
{
TrackingUser(this, usrs[0]);
}
PMXBone head = getBone("頭");
PMXBone neck = getBone("首");
PMXBone torso = getBone("上半身");
PMXBone r_shoulder = getBone("右腕");
PMXBone r_elbow = getBone("右ひじ");
PMXBone r_hand = getBone("右手首");
PMXBone r_hip = getBone("右足");
PMXBone r_knee = getBone("右ひざ");
PMXBone r_foot = getBone("右足首");
PMXBone l_shoulder = getBone("左腕");
PMXBone l_elbow = getBone("左ひじ");
PMXBone l_hand = getBone("左手首");
PMXBone l_hip = getBone("左足");
PMXBone l_knee = getBone("左ひざ");
PMXBone l_foot = getBone("左足首");
SkeletonJoint sj_head = skeleton.GetJoint(SkeletonJoint.JointType.Head);
SkeletonJoint sj_neck = skeleton.GetJoint(SkeletonJoint.JointType.Neck);
SkeletonJoint sj_rShoulder = skeleton.GetJoint(SkeletonJoint.JointType.RightShoulder);
SkeletonJoint sj_lShoulder = skeleton.GetJoint(SkeletonJoint.JointType.LeftShoulder);
SkeletonJoint sj_rElbow = skeleton.GetJoint(SkeletonJoint.JointType.RightElbow);
SkeletonJoint sj_lElbow = skeleton.GetJoint(SkeletonJoint.JointType.LeftElbow);
SkeletonJoint sj_r_hand = skeleton.GetJoint(SkeletonJoint.JointType.RightHand);
SkeletonJoint sj_l_hand = skeleton.GetJoint(SkeletonJoint.JointType.LeftHand);
SkeletonJoint sj_torso = skeleton.GetJoint(SkeletonJoint.JointType.Torso);
SkeletonJoint sj_rHip = skeleton.GetJoint(SkeletonJoint.JointType.RightHip);
SkeletonJoint sj_lHip = skeleton.GetJoint(SkeletonJoint.JointType.LeftHip);
SkeletonJoint sj_rKnee = skeleton.GetJoint(SkeletonJoint.JointType.RightKnee);
SkeletonJoint sj_lKnee = skeleton.GetJoint(SkeletonJoint.JointType.LeftKnee);
SkeletonJoint sj_rFoot = skeleton.GetJoint(SkeletonJoint.JointType.RightFoot);
SkeletonJoint sj_lFoot = skeleton.GetJoint(SkeletonJoint.JointType.LeftFoot);
//腰のひねり
Vector3 shoulder_l2r =
Vector3.Normalize(
ToVector3(sj_rShoulder.Position) -
ToVector3(sj_lShoulder.Position));
Vector3 hip_l2r =
Vector3.Normalize(
ToVector3(sj_rHip.Position) -
ToVector3(sj_lHip.Position));
//shoulder_l2r.Normalize();hip_l2r.Normalize();
float angle = (float)Math.Acos(Math.Min(Vector3.Dot(shoulder_l2r, hip_l2r), 1f));
torso.Rotation *= Quaternion.RotationAxis(new Vector3(0, 1, 0), angle);
torso.UpdateGrobalPose();
getRotation(neck, head, skeleton, sj_neck,
sj_head);
getRotation(r_shoulder, r_elbow, skeleton,
sj_rShoulder, sj_rElbow);
getRotation(r_elbow, r_hand, skeleton,
sj_rElbow,
sj_r_hand);
getRotation(l_shoulder, l_elbow, skeleton,
sj_lShoulder, sj_lElbow);
getRotation(l_elbow, l_hand, skeleton,
sj_lElbow, sj_l_hand);
getRotation(torso, neck, skeleton, sj_torso, sj_neck);
getRotation(r_hip, r_knee, skeleton,
sj_rHip, sj_rKnee);
getRotation(r_knee, r_foot, skeleton,
sj_rKnee, sj_rFoot);
getRotation(l_hip, l_knee, skeleton,
sj_lHip, sj_lKnee);
getRotation(l_knee, l_foot, skeleton,
sj_lKnee, sj_lFoot);
}
}
return(true);
}
public static float[] ToArray(this Quaternion q)
{
return(new[] { q.X, q.Y, q.Z, q.W });
}