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); }
/// <summary> /// Called when <see cref="IKSolver.Solve"/> is called. /// </summary> /// <param name="deltaTime">The current time step (in seconds).</param> protected override void OnSolve(float deltaTime) { if (NumberOfIterations <= 0) { return; } if (_isDirty) { if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, TipBoneIndex)) { throw new ArgumentException("The RootBoneIndex and the TipBoneIndex do not form a valid bone chain."); } } _isDirty = false; var skeleton = SkeletonPose.Skeleton; bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { // Remember original bone transforms for interpolation with the result at the end. // Transforms are stored from tip to root (reverse order!). _originalTransforms.Clear(); int boneIndex = TipBoneIndex; while (true) { _originalTransforms.Add(SkeletonPose.GetBoneTransform(boneIndex)); if (boneIndex == RootBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } } int numberOfBones = SkeletonPose.GetNumberOfBones(RootBoneIndex, TipBoneIndex); // The transposed jacobian matrix. var jacobianTransposed = new MatrixF(numberOfBones, 6); // The force vector (3 linear and 3 angular (torque) entries). VectorF force = new VectorF(6); // The rotation axes of the bones. Vector3[] axes = new Vector3[numberOfBones]; float toleranceSquared = AllowedDeviation * AllowedDeviation; // In each iteration we compute the jacobian matrix, compute the bone velocities // an make an euler integration step. for (int iteration = 0; iteration < NumberOfIterations; iteration++) { var tipBoneAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex); var tipAbsolute = tipBoneAbsolute.ToParentPosition(TipOffset); var targetToTip = tipAbsolute - Target; if (targetToTip.LengthSquared() < toleranceSquared) { if (iteration == 0) { return; } break; } // Loop from tip to root and fill Jacobian. // (See description of Jacobian Transpose method to see how the rotation axes and // Jacobian entries must look like). var currentBoneIndex = TipBoneIndex; int exitBoneIndex = RootBoneIndex >= 0 ? skeleton.GetParent(RootBoneIndex) : -1; int i = numberOfBones; do { i--; // Compute rotation axis. Vector3 currentJointAbsolute = SkeletonPose.GetBonePoseAbsolute(currentBoneIndex).Translation; Vector3 jointToTarget = Target - currentJointAbsolute; Vector3 jointToTip = tipAbsolute - currentJointAbsolute; axes[i] = Vector3.Cross(jointToTarget, jointToTip); if (!axes[i].TryNormalize()) { axes[i] = Vector3.UnitX; // TODO: What should we really do in this case? } Vector3 jacobianColumnUpperPart = Vector3.Cross(jointToTip, axes[i]); // Fill J. jacobianTransposed[i, 0] = jacobianColumnUpperPart.X; jacobianTransposed[i, 1] = jacobianColumnUpperPart.Y; jacobianTransposed[i, 2] = jacobianColumnUpperPart.Z; jacobianTransposed[i, 3] = axes[i].X; jacobianTransposed[i, 4] = axes[i].Y; jacobianTransposed[i, 5] = axes[i].Z; currentBoneIndex = skeleton.GetParent(currentBoneIndex); } while (currentBoneIndex != exitBoneIndex && currentBoneIndex >= 0); Debug.Assert(i == 0); // Set the force. force[0] = targetToTip.X; force[1] = targetToTip.Y; force[2] = targetToTip.Z; force[3] = 0; force[4] = 0; force[5] = 0; // Compute pseudo velocities. VectorF velocities = jacobianTransposed * force; // TODO: Garbage! // Euler integration step. currentBoneIndex = TipBoneIndex; i = numberOfBones; do { i--; // Rotation axis for this bone. Vector3 axis = axes[i]; // Angle is computed using Euler integration with an arbitrary step size. float angle = velocities[i] * StepSize; // Apply rotation. Quaternion rotationChange = Quaternion.CreateFromRotationMatrix(axis, angle); SkeletonPose.RotateBoneAbsolute(currentBoneIndex, rotationChange); currentBoneIndex = skeleton.GetParent(currentBoneIndex); } while (currentBoneIndex != exitBoneIndex && currentBoneIndex >= 0); // Call delegate that checks bone limits. if (LimitBoneTransforms != null) { LimitBoneTransforms(); } } if (requiresBlending || requiresLimiting) { // Apply weight and the angular velocity limit. int boneIndex = TipBoneIndex; int i = 0; while (true) { var originalTransform = _originalTransforms[i]; var targetTransform = SkeletonPose.GetBoneTransform(boneIndex); // Apply weight. if (requiresBlending) { BlendBoneTransform(ref originalTransform, ref targetTransform); } // Apply angular velocity limit. if (requiresLimiting) { LimitBoneTransform(ref originalTransform, ref targetTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(boneIndex, targetTransform); if (boneIndex == RootBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); i++; } } _originalTransforms.Clear(); }