private static void DoWork(Quaternion skeletonOffset, SkeletonPose skeletonA, SkeletonPose skeletonB, int boneIndexA, int neckBoneIndexA, int leftShoulderBoneIndexA, int rightShoulderBoneIndexA, int boneIndexB, int neckBoneIndexB, int leftShoulderBoneIndexB, int rightShoulderBoneIndexB)
{
// Reset root bone.
skeletonB.ResetBoneTransforms(boneIndexB, boneIndexB, false, true, false);
// Get absolute positions all bones.
var boneA = skeletonA.GetBonePoseAbsolute(boneIndexA).Translation;
var neckA = skeletonA.GetBonePoseAbsolute(neckBoneIndexA).Translation;
var leftShoulderA = skeletonA.GetBonePoseAbsolute(leftShoulderBoneIndexA).Translation;
var rightShoulderA = skeletonA.GetBonePoseAbsolute(rightShoulderBoneIndexA).Translation;
var boneB = skeletonB.GetBonePoseAbsolute(boneIndexB).Translation;
var neckB = skeletonB.GetBonePoseAbsolute(neckBoneIndexB).Translation;
var leftShoulderB = skeletonB.GetBonePoseAbsolute(leftShoulderBoneIndexB).Translation;
var rightShoulderB = skeletonB.GetBonePoseAbsolute(rightShoulderBoneIndexB).Translation;
// Abort if any bone to bone distance is 0.
if (Vector3.AreNumericallyEqual(boneA, neckA) ||
Vector3.AreNumericallyEqual(boneA, rightShoulderA) ||
Vector3.AreNumericallyEqual(leftShoulderA, rightShoulderA) ||
Vector3.AreNumericallyEqual(boneB, neckB) ||
Vector3.AreNumericallyEqual(boneB, rightShoulderB) ||
Vector3.AreNumericallyEqual(leftShoulderB, rightShoulderB))
{
return;
}
// Get shoulder axis vectors in model B space.
var shoulderAxisA = rightShoulderA - leftShoulderA;
shoulderAxisA = skeletonOffset.Rotate(shoulderAxisA);
var shoulderAxisB = rightShoulderB - leftShoulderB;
// Create a twist rotation from the shoulder vectors.
var shoulderRotation = Quaternion.CreateFromRotationMatrix(shoulderAxisB, shoulderAxisA);
// Apply this twist to the spine. (Modifies the neckB position.)
neckB = boneB + shoulderRotation.Rotate(neckB - boneB);
// Get spine vectors in model B space.
var spineAxisA = neckA - boneA;
spineAxisA = skeletonOffset.Rotate(spineAxisA);
var spineAxisB = neckB - boneB;
// Create swing rotation from spine vectors.
var spineRotation = Quaternion.CreateFromRotationMatrix(spineAxisB, spineAxisA);
// Apply the shoulder twist rotation followed by the spine swing rotation.
skeletonB.RotateBoneAbsolute(boneIndexB, spineRotation * shoulderRotation);
}