// Copies the quaternions of all bones to the specified array.
private void SkeletonPoseToArray(float[] values)
{
var numberOfBones = SkeletonPose.Skeleton.NumberOfBones;
for (int i = 0; i < numberOfBones; i++)
{
QuaternionF quaternion = SkeletonPose.GetBoneTransform(i).Rotation;
values[i * 4 + 0] = quaternion.W;
values[i * 4 + 1] = quaternion.X;
values[i * 4 + 2] = quaternion.Y;
values[i * 4 + 3] = quaternion.Z;
}
}
// Initializes the bone rotations using the quaternions of the specified array.
private void ArrayToSkeletonPose(float[] values)
{
var numberOfBones = SkeletonPose.Skeleton.NumberOfBones;
for (int i = 0; i < numberOfBones; i++)
{
QuaternionF quaternion = new QuaternionF(
values[i * 4 + 0],
values[i * 4 + 1],
values[i * 4 + 2],
values[i * 4 + 3]);
// The quaternions were filtered using component-wise linear interpolation. This
// is only an approximation which denormalizes the quaternions.
// --> Renormalize the quaternions.
quaternion.TryNormalize();
// Exchange the rotation in the bone transform.
var boneTransform = SkeletonPose.GetBoneTransform(i);
boneTransform.Rotation = quaternion;
SkeletonPose.SetBoneTransform(i, boneTransform);
}
}
/// <summary>
/// Perform mapping in local bone space.
/// </summary>
private static void MapLocal(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, QuaternionF rotationBToA, QuaternionF rotationAToB)
{
var boneTransform = skeletonA.GetBoneTransform(boneIndexA);
// Remove any scaling.
boneTransform.Scale = Vector3F.One;
// Rotation: Using similarity transformation: (Read from right to left.)
// Rotate from bone B space to bone A space.
// Apply the bone transform rotation in bone A space
// Rotate back from bone A space to bone B space.
boneTransform.Rotation = rotationAToB * boneTransform.Rotation * rotationBToA;
// If we should map translations, then we scale the translation and rotate it from
// bone A space to bone B space.
if (mapTranslations)
boneTransform.Translation = rotationAToB.Rotate(boneTransform.Translation * scaleAToB);
else
boneTransform.Translation = Vector3F.Zero;
// Apply new bone transform to B.
skeletonB.SetBoneTransform(boneIndexB, boneTransform);
}
