/// <summary> /// Perform mapping in local bone space. /// </summary> private static void MapLocal(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, Quaternion rotationBToA, Quaternion rotationAToB) { var boneTransform = skeletonA.GetBoneTransform(boneIndexA); // Remove any scaling. boneTransform.Scale = Vector3.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 = Vector3.Zero; } // Apply new bone transform to B. skeletonB.SetBoneTransform(boneIndexB, boneTransform); }
/// <summary> /// Sets the bone rotation of a bone so that it matches the given rotation in model space. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <param name="rotation">The rotation in model space.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static void SetBoneRotationAbsolute(this SkeletonPose skeletonPose, int boneIndex, Quaternion rotation) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeleton = skeletonPose.Skeleton; var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex); var parentIndex = skeleton.GetParent(boneIndex); var parentBonePoseAbsolute = (parentIndex >= 0) ? skeletonPose.GetBonePoseAbsolute(parentIndex) : SrtTransform.Identity; // Solving this equation (using only rotations): // rotation = parentBonePoseAbsolute * bindPoseRelative * rotationRelative; // rotationRelative = boneTransform. var rotationRelative = bindPoseRelative.Rotation.Conjugated * parentBonePoseAbsolute.Rotation.Conjugated * rotation; rotationRelative.Normalize(); var boneTransform = skeletonPose.GetBoneTransform(boneIndex); boneTransform.Rotation = rotationRelative; skeletonPose.SetBoneTransform(boneIndex, boneTransform); }
//public static void RotateBoneWorld(this SkeletonPose SkeletonPose, int boneIndex, Quaternion rotation, Matrix world) //{ // Quaternion worldRotation = Quaternion.CreateFromRotationMatrix(world.Minor); // RotateBoneAbsolute(SkeletonPose, boneIndex, worldRotation.Conjugated * rotation); //} // TODO: This method should really be called RotateBoneLocalAnimated? ///// <summary> ///// Rotates bone where the rotation is given in the bone space. ///// </summary> ///// <param name="skeletonPose">The skeleton pose.</param> ///// <param name="boneIndex">The index of the bone.</param> ///// <param name="rotation">The rotation in bone space.</param> ///// <exception cref="ArgumentNullException"> ///// <paramref name="skeletonPose" /> is <see langword="null"/>. ///// </exception> //public static void RotateBoneLocal(this SkeletonPose skeletonPose, int boneIndex, Quaternion rotation) //{ // if (skeletonPose == null) // throw new ArgumentNullException("skeletonPose"); // var boneTransform = skeletonPose.GetBoneTransform(boneIndex); // boneTransform.Rotation = boneTransform.Rotation * rotation; // skeletonPose.SetBoneTransform(boneIndex, boneTransform); //} /// <summary> /// Rotates a bone where the rotation is given in model space. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <param name="rotation">The rotation in model space.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static void RotateBoneAbsolute(this SkeletonPose skeletonPose, int boneIndex, Quaternion rotation) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeleton = skeletonPose.Skeleton; var boneTransform = skeletonPose.GetBoneTransform(boneIndex); var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex); var parentIndex = skeleton.GetParent(boneIndex); var parentBonePoseAbsolute = skeletonPose.GetBonePoseAbsolute(parentIndex); // Solving these equations (using only the rotations): // rotation * bonePoseAbsolute = bonePoseAbsoluteNew // bonePoseAbsolute = parentBonePoseAbsolute * bindPoseRelative * boneTransform. // ... // Rotation relative to bone bind pose space (using similarity transformation). var rotationRelative = bindPoseRelative.Rotation.Conjugated * parentBonePoseAbsolute.Rotation.Conjugated * rotation * parentBonePoseAbsolute.Rotation * bindPoseRelative.Rotation; // The final rotation is: First rotate into bone bind pose space, then apply rotation. boneTransform.Rotation = rotationRelative * boneTransform.Rotation; // So many multiplications, numerical errors adds up quickly in iterative IK algorithms... boneTransform.Rotation.Normalize(); skeletonPose.SetBoneTransform(boneIndex, boneTransform); }
/// <summary> /// Sets the bone transform to create a desired pose in model space. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <param name="bonePoseAbsolute">The bone pose in model space.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static void SetBonePoseAbsolute(this SkeletonPose skeletonPose, int boneIndex, SrtTransform bonePoseAbsolute) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeleton = skeletonPose.Skeleton; var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex); var parentIndex = skeleton.GetParent(boneIndex); var parentBonePoseAbsolute = (parentIndex >= 0) ? skeletonPose.GetBonePoseAbsolute(parentIndex) : SrtTransform.Identity; // Solving this equation: // bonePoseAbsolute = parentBonePoseAbsolute * bindPoseRelative * BoneTransform; var boneTransform = bindPoseRelative.Inverse * parentBonePoseAbsolute.Inverse * bonePoseAbsolute; boneTransform.Rotation.Normalize(); skeletonPose.SetBoneTransform(boneIndex, boneTransform); }
/// <overloads> /// <summary> /// Resets bone transforms. /// </summary> /// </overloads> /// /// <summary> /// Resets the bone transforms of all bones in a bone chain. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="startBoneIndex">Index of the start bone (root of the chain). Can be -1.</param> /// <param name="endBoneIndex">Index of the end bone (tip of the chain). Must not be -1.</param> /// <returns>The number of bones in the chain; or 0 if the chain is invalid.</returns> /// <remarks> /// If a bone transform is reset, it is set to the <see cref="SrtTransform.Identity"/> /// transform. If all bone transforms of a skeleton are reset, then the skeleton is in its /// bind pose. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static void ResetBoneTransforms(this SkeletonPose skeletonPose, int startBoneIndex, int endBoneIndex) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeleton = skeletonPose.Skeleton; int boneIndex = endBoneIndex; while (boneIndex >= 0) { skeletonPose.SetBoneTransform(boneIndex, SrtTransform.Identity); if (boneIndex == startBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } }
/// <summary> /// Resets the bone transform components (scale, rotation or translation) of all bones in a /// bone chain. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="startBoneIndex">Index of the start bone (root of the chain). Can be -1.</param> /// <param name="endBoneIndex">Index of the end bone (tip of the chain). Must not be -1.</param> /// <param name="resetScale">If set to <see langword="true"/>, the scale is reset.</param> /// <param name="resetRotation">If set to <see langword="true"/>, the rotation is reset.</param> /// <param name="resetTranslation">If set to <see langword="true"/>, the translation is reset.</param> /// <remarks> /// If a bone transform is reset, it is set to the <see cref="SrtTransform.Identity"/> /// transform. If all bone transforms of a skeleton are reset, then the skeleton is in its /// bind pose. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose"/> is <see langword="null"/>. /// </exception> public static void ResetBoneTransforms(this SkeletonPose skeletonPose, int startBoneIndex, int endBoneIndex, bool resetScale, bool resetRotation, bool resetTranslation) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeleton = skeletonPose.Skeleton; int boneIndex = endBoneIndex; while (boneIndex >= 0) { var boneTransform = skeletonPose.GetBoneTransform(boneIndex); if (resetScale) { boneTransform.Scale = Vector3.One; } if (resetRotation) { boneTransform.Rotation = Quaternion.Identity; } if (resetTranslation) { boneTransform.Translation = Vector3.Zero; } skeletonPose.SetBoneTransform(boneIndex, boneTransform); if (boneIndex == startBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } }
/// <summary> /// Resets the bone transform components (scale, rotation or translation) of all bones in a /// bone subtree. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex"> /// Index of the root bone of the subtree. Must not be negative. /// </param> /// <param name="resetScale">If set to <see langword="true"/>, the scale is reset.</param> /// <param name="resetRotation">If set to <see langword="true"/>, the rotation is reset.</param> /// <param name="resetTranslation">If set to <see langword="true"/>, the translation is reset.</param> /// <remarks> /// If a bone transform is reset, it is set to the <see cref="SrtTransform.Identity"/> /// transform. If all bone transforms of a skeleton are reset, then the skeleton is in its /// bind pose. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose"/> is <see langword="null"/>. /// </exception> /// <exception cref="ArgumentOutOfRangeException"> /// <paramref name="boneIndex"/> is negative.</exception> public static void ResetBoneTransformsInSubtree(this SkeletonPose skeletonPose, int boneIndex, bool resetScale, bool resetRotation, bool resetTranslation) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } if (boneIndex < 0) { throw new ArgumentOutOfRangeException("boneIndex"); } var boneTransform = skeletonPose.GetBoneTransform(boneIndex); if (resetScale) { boneTransform.Scale = Vector3.One; } if (resetRotation) { boneTransform.Rotation = Quaternion.Identity; } if (resetTranslation) { boneTransform.Translation = Vector3.Zero; } skeletonPose.SetBoneTransform(boneIndex, boneTransform); var skeleton = skeletonPose.Skeleton; for (int i = 0; i < skeleton.GetNumberOfChildren(boneIndex); i++) { ResetBoneTransformsInSubtree(skeletonPose, skeleton.GetChild(boneIndex, i), resetScale, resetRotation, resetTranslation); } }
/// <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 (_isDirty) { // Validate bone chain. if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, HingeBoneIndex)) { throw new ArgumentException("The RootBoneIndex and the HingeBoneIndex do not form a valid bone chain."); } if (!SkeletonPose.IsAncestorOrSelf(HingeBoneIndex, TipBoneIndex)) { throw new ArgumentException("The HingeBoneIndex and the TipBoneIndex do not form a valid bone chain."); } } _isDirty = false; if (MinHingeAngle > MaxHingeAngle) { throw new AnimationException("The MinHingeAngle must be less than or equal to the MaxHingeAngle"); } // Remember original bone transforms for interpolation at the end. var originalRootBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex); var originalHingeBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex); var originalTipBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex); var rootBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(RootBoneIndex); var hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex); var tipBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex); // Get tip position in model space. Vector3 tipAbsolute; if (TipBoneOrientation != null) { // If the user has specified an absolute tip rotation, then we consider this rotation and // use the tip bone origin as tip. tipAbsolute = tipBonePoseAbsolute.Translation; Target -= tipBonePoseAbsolute.ToParentDirection(tipBonePoseAbsolute.Scale * TipOffset); } else { // The user hasn't specified a desired tip rotation. Therefore we do not modify the // tip rotation and use the offset position in the tip bone as tip. tipAbsolute = tipBonePoseAbsolute.ToParentPosition(TipOffset); } // Abort if we already touch the target. if (Vector3.AreNumericallyEqual(tipAbsolute, Target)) { return; } // Root to target vector. var rootToTarget = Target - rootBonePoseAbsolute.Translation; var rootToTargetLength = rootToTarget.Length; if (Numeric.IsZero(rootToTargetLength)) { return; } rootToTarget /= rootToTargetLength; // ----- Align chain with target. // Align the root to target vector with the root to tip vector. var rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation; var rootToTipLength = rootToTip.Length; if (!Numeric.IsZero(rootToTipLength)) { rootToTip /= rootToTipLength; var rotation = Quaternion.CreateFromRotationMatrix(rootToTip, rootToTarget); if (rotation.Angle > Numeric.EpsilonF) { // Apply rotation to root bone. rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation; SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation); hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex); // Compute new tip absolute tip position from the known quantities. tipAbsolute = rootBonePoseAbsolute.Translation + rootToTarget * rootToTipLength; } } // ----- Compute ideal angle. var rootToHinge = hingeBonePoseAbsolute.Translation - rootBonePoseAbsolute.Translation; var hingeToTip = tipAbsolute - hingeBonePoseAbsolute.Translation; var hingeAxis = hingeBonePoseAbsolute.ToParentDirection(HingeAxis); // Project vectors to hinge plane. Everything should be in a plane for the following // computations. rootToHinge -= hingeAxis * Vector3.Dot(rootToHinge, hingeAxis); hingeToTip -= hingeAxis * Vector3.Dot(hingeToTip, hingeAxis); // Get lengths. float rootToHingeLength = rootToHinge.Length; if (Numeric.IsZero(rootToHingeLength)) { return; } rootToHinge /= rootToHingeLength; float hingeToTipLength = hingeToTip.Length; if (Numeric.IsZero(hingeToTipLength)) { return; } hingeToTip /= hingeToTipLength; // Compute current hinge angle (angle between root bone and hinge bone). float currentHingeAngle = (float)Math.Acos(MathHelper.Clamp(Vector3.Dot(rootToHinge, hingeToTip), -1, 1)); // Make sure the computed angle is about the hingeAxis and not about -hingeAxis. if (Vector3.Dot(Vector3.Cross(rootToHinge, hingeToTip), hingeAxis) < 0) { currentHingeAngle = -currentHingeAngle; } // Using law of cosines to compute the desired hinge angle using the triangle lengths. float cosDesiredHingeAngle = (rootToHingeLength * rootToHingeLength + hingeToTipLength * hingeToTipLength - rootToTargetLength * rootToTargetLength) / (2 * rootToHingeLength * hingeToTipLength); float desiredHingeAngle = ConstantsF.Pi - (float)Math.Acos(MathHelper.Clamp(cosDesiredHingeAngle, -1, 1)); // Apply hinge limits. if (desiredHingeAngle < MinHingeAngle) { desiredHingeAngle = MinHingeAngle; } else if (desiredHingeAngle > MaxHingeAngle) { desiredHingeAngle = MaxHingeAngle; } // Compute delta rotation between current and desired angle. float deltaAngle = desiredHingeAngle - currentHingeAngle; var hingeRotation = Quaternion.CreateFromRotationMatrix(hingeAxis, deltaAngle); hingeBonePoseAbsolute.Rotation = hingeRotation * hingeBonePoseAbsolute.Rotation; // Update tip position. tipAbsolute = hingeBonePoseAbsolute.Translation + hingeRotation.Rotate(tipAbsolute - hingeBonePoseAbsolute.Translation); // ----- Align chain with target. // If we hit a hinge limit, then we can move the tip closer to the target by aligning // the whole chain again. rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation; rootToTipLength = rootToTip.Length; if (!Numeric.IsZero(rootToTipLength)) { rootToTip /= rootToTipLength; var rotation = Quaternion.CreateFromRotationMatrix(rootToTip, rootToTarget); rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation; hingeBonePoseAbsolute.Rotation = rotation * hingeBonePoseAbsolute.Rotation; } // ----- Set results. SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation); SkeletonPose.SetBoneRotationAbsolute(HingeBoneIndex, hingeBonePoseAbsolute.Rotation); if (TipBoneOrientation != null) { SkeletonPose.SetBoneRotationAbsolute(TipBoneIndex, TipBoneOrientation.Value); } // ----- Apply weight, velocity limit and set results. bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { var targetBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(RootBoneIndex, targetBoneTransform); targetBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(HingeBoneIndex, targetBoneTransform); targetBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(TipBoneIndex, targetBoneTransform); } }
/// <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) { // Get cosine of limit or -1 if unlimited. float cosLimits = -1; if (0 <= Limit && Limit < ConstantsF.PiOver2) { cosLimits = (float)Math.Cos(Limit); } var skeleton = SkeletonPose.Skeleton; int parentIndex = skeleton.GetParent(BoneIndex); // Bone pose without a bone transform. var unanimatedBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(parentIndex) * skeleton.GetBindPoseRelative(BoneIndex); // Target position and direction in bone space. var targetPositionLocal = unanimatedBonePoseAbsolute.ToLocalPosition(Target); var targetDirection = targetPositionLocal - EyeOffset; if (!targetDirection.TryNormalize()) { return; } // The axes of the view space (where forward is -z, relative to bone space). Vector3 forward = Forward; Vector3 up = Up; Vector3 side = Vector3.Cross(up, -forward); // This matrix converts from view space to bone space (in other words, it // rotates the -z direction into the view direction). var boneFromView = new Matrix(side.X, up.X, -forward.X, side.Y, up.Y, -forward.Y, side.Z, up.Z, -forward.Z); // Get the components of the target direction relative to the view space axes. float targetUp = Vector3.Dot(targetDirection, up); float targetSide = Vector3.Dot(targetDirection, side); float targetForward = Vector3.Dot(targetDirection, forward); // Limit rotations of the desired up and side vector. // The target forward direction is inverted if necessary. (If limited the bone // does not never rotate back.) if (cosLimits > 0) { cosLimits = (float)Math.Sqrt(1 - cosLimits * cosLimits); if (targetUp > 0 && targetUp > cosLimits) { targetUp = cosLimits; } else if (targetUp < 0 && -targetUp > cosLimits) { targetUp = -cosLimits; } if (targetSide > 0 && targetSide > cosLimits) { targetSide = cosLimits; } else if (targetSide < 0 && -targetSide > cosLimits) { targetSide = -cosLimits; } targetForward = Math.Abs(targetForward); } // Make new target direction vector that conforms to the limits. targetDirection = Math.Sign(targetForward) * forward * (float)Math.Sqrt(Math.Max(0, 1 - targetUp * targetUp - targetSide * targetSide)) + side * targetSide + up * targetUp; Debug.Assert(targetDirection.IsNumericallyNormalized); // Make axes of desired view space. forward = targetDirection; side = Vector3.Cross(up, -forward); if (!side.TryNormalize()) { return; } up = Vector3.Cross(side, forward); Debug.Assert(up.IsNumericallyNormalized); // Create new view space matrix. var boneFromNewView = new Matrix( side.X, up.X, -forward.X, side.Y, up.Y, -forward.Y, side.Z, up.Z, -forward.Z); // Apply a bone transform that rotates the rest view space to the desired view space. Quaternion boneTransform = Quaternion.CreateFromRotationMatrix(boneFromNewView * boneFromView.Transposed); var startTransform = SkeletonPose.GetBoneTransform(BoneIndex); var lookAtTransform = new SrtTransform(startTransform.Scale, boneTransform, startTransform.Translation); // Apply weight. if (RequiresBlending()) { BlendBoneTransform(ref startTransform, ref lookAtTransform); } // Apply angular velocity limit. float maxRotationAngle; if (RequiresLimiting(deltaTime, out maxRotationAngle)) { LimitBoneTransform(ref startTransform, ref lookAtTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(BoneIndex, lookAtTransform); }
/// <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) { UpdateDerivedValues(); if (_totalChainLength == 0) { return; } var skeleton = SkeletonPose.Skeleton; // Make a local copy of the absolute bone poses. The following operations will be performed // on the data in _bone and not on the skeleton pose. var numberOfBones = _boneIndices.Count; for (int i = 0; i < numberOfBones; i++) { _bones[i] = SkeletonPose.GetBonePoseAbsolute(_boneIndices[i]); } // Calculate the position at the tip of the last bone. // If TipOffset is not 0, then we can rotate the last bone. // If TipOffset is 0, then the last bone defines the tip but is not rotated. // --> numberOfBones is set to the number of affected bones. Vector3 tipAbsolute; if (TipOffset.IsNumericallyZero) { numberOfBones--; tipAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).Translation; } else { tipAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).ToParentPosition(TipOffset); } // The root bone rotation that aligns the whole chain with the target. Quaternion chainRotation = Quaternion.Identity; Vector3 boneToTarget, boneToTip; float remainingChainLength = _totalChainLength; // Apply the soft limit to the distance to the IK goal //vecToIkGoal = Target - _bones[0].Translation; //distToIkGoal = vecToIkGoal.Length; //// Limit the extension to 98% and ramp it up over 5% of the chains length //vecToIkGoal *= (LimitValue(distToIkGoal, _totalChainLength * 0.98f, _totalChainLength * 0.08f)) / distToIkGoal; //Vector3 goalPosition = _bones[0].Translation + vecToIkGoal; var targetAbsolute = Target; // This algorithms iterates once over all bones from root to tip. for (int i = 0; i < numberOfBones; i++) { if (i > 0) { // Transform the bone position by the overall chain offset. var translation = _bones[0].Translation + chainRotation.Rotate(_bones[i].Translation - _bones[0].Translation); _bones[i] = new SrtTransform(_bones[i].Scale, _bones[i].Rotation, translation); } // The bone to tip vector of the aligned chain (without other IK rotations!). boneToTip = tipAbsolute - _bones[i].Translation; float boneToTipLength = boneToTip.Length; boneToTip /= boneToTipLength; // TODO: Check for division by 0? if (i > 0) { // Calculate the new absolute bone position. var translation = _bones[i - 1].ToParentPosition(skeleton.GetBindPoseRelative(_boneIndices[i]).Translation); _bones[i] = new SrtTransform(_bones[i].Scale, _bones[i].Rotation, translation); } // The bone to target vector of the new chain configuration. boneToTarget = targetAbsolute - _bones[i].Translation; float boneToTargetLength = boneToTarget.Length; boneToTarget /= boneToTargetLength; if (i == 0) { // This is the first bone: Compute rotation that aligns the whole initial chain with // the target. chainRotation = Quaternion.CreateFromRotationMatrix(boneToTip, boneToTarget); // Update tip. tipAbsolute = _bones[i].Translation + (boneToTarget * boneToTipLength); // Apply chainRotation to root bone. _bones[i] = new SrtTransform(_bones[i].Scale, chainRotation * _bones[i].Rotation, _bones[i].Translation); } else { // Apply the chain alignment rotation. Also the parent bones have changed, so we apply // an additional rotation that accounts for the ancestor rotations. This additional // rotation aligns the last bone with the target. // TODO: Find an explanation/derivation of this additional rotation. _bones[i] = new SrtTransform( _bones[i].Scale, Quaternion.CreateFromRotationMatrix(boneToTip, boneToTarget) * chainRotation * _bones[i].Rotation, _bones[i].Translation); } // Now, solve the bone using trigonometry. // The last bone was already aligned with the target. For the second last bone we use // the law of cosines. For all other bones we use the complicated steps described in the // GPG article. if (i <= numberOfBones - 2) { // Length of chain after this bone. remainingChainLength -= _boneLengths[i]; // The direction of the current bone. For the tip bone we use the TipOffset. Vector3 boneDirection; if (i != TipBoneIndex) { boneDirection = _bones[i].Rotation.Rotate(skeleton.GetBindPoseRelative(_boneIndices[i + 1]).Translation); } else { boneDirection = _bones[i].Rotation.Rotate(TipOffset); } if (!boneDirection.TryNormalize()) { continue; } // The bone rotates around an axis normal to the bone to target direction and the bone // vector. Vector3 rotationAxis = Vector3.Cross(boneToTarget, boneDirection); if (!rotationAxis.TryNormalize()) { continue; // TODO: If this happens, can we choose a useful direction? } // The current angle between bone direction and bone to target vector. float currentAngle = (float)Math.Acos(MathHelper.Clamp(Vector3.Dot(boneDirection, boneToTarget), -1, 1)); // Side lengths of the involved triangles. var a = _boneLengths[i]; var b = boneToTargetLength; var c = remainingChainLength; var d = boneToTipLength; float desiredAngle; if (i == numberOfBones - 2) { // Use trigonometry (law of cosines) to determine the desired angle. desiredAngle = (float)Math.Acos(MathHelper.Clamp((a * a + b * b - c * c) / (2 * a * b), -1.0f, 1.0f)); } else { // The maximal angle that this bone can have where the chain still reaches the tip. float maxTipAngle; if (boneToTipLength > remainingChainLength) { maxTipAngle = (float)Math.Acos(MathHelper.Clamp((a * a + d * d - c * c) / (2 * a * d), -1.0f, 1.0f)); } else { // Tip is very near and this bone can bend more than 180�. Add additional chain length // in radians. maxTipAngle = (float)Math.Acos(MathHelper.Clamp((a * 0.5f) / remainingChainLength, 0.0f, 1.0f)); maxTipAngle += ((c - d) / a); } // The maximal angle that this bone can have where the chain still reaches the target. float maxTargetAngle; if (boneToTargetLength > remainingChainLength) { maxTargetAngle = (float)Math.Acos(MathHelper.Clamp((a * a + b * b - c * c) / (2 * a * b), -1.0f, 1.0f)); } else { // Target is very near and this bone can bend more than 180�. Add additional chain // length in radians. maxTargetAngle = (float)Math.Acos(MathHelper.Clamp((a * 0.5f) / remainingChainLength, 0.0f, 1.0f)); maxTargetAngle += ((c - b) / a); } // If we set the desired angle to maxTargetAngle, the remain bones must be all // stretched. We want to keep the chain appearance, therefore, we set a smaller angle. // The new angle relative to the final remaining chain should have the same ratio as the // current angle to the current remaining chain. if (!Numeric.IsZero(maxTipAngle)) { desiredAngle = maxTargetAngle * (currentAngle / maxTipAngle); } else { desiredAngle = maxTargetAngle; // Avoiding divide by zero. } } // The rotation angle that we have to apply. float deltaAngle = desiredAngle - currentAngle; // Apply the rotation to the current bones _bones[i] = new SrtTransform( _bones[i].Scale, (Quaternion.CreateFromRotationMatrix(rotationAxis, deltaAngle) * _bones[i].Rotation).Normalized, _bones[i].Translation); } } bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { // We have to blend the computed results with the original bone transforms. // Get original bone transforms. for (int i = 0; i < numberOfBones; i++) { _originalBoneTransforms.Add(SkeletonPose.GetBoneTransform(_boneIndices[i])); } for (int i = 0; i < numberOfBones; i++) { int boneIndex = _boneIndices[i]; var originalBoneTransform = _originalBoneTransforms[i]; // Set absolute bone pose and let the skeleton compute the bone transform for us. SkeletonPose.SetBoneRotationAbsolute(boneIndex, _bones[i].Rotation); var targetBoneTransform = SkeletonPose.GetBoneTransform(boneIndex); // Apply weight. if (requiresBlending) { BlendBoneTransform(ref originalBoneTransform, ref targetBoneTransform); } // Apply angular velocity limit. if (requiresLimiting) { LimitBoneTransform(ref originalBoneTransform, ref targetBoneTransform, maxRotationAngle); } // Set final bone transform. SkeletonPose.SetBoneTransform(boneIndex, targetBoneTransform); } _originalBoneTransforms.Clear(); } else { // Weight is 1 and angular velocity limit is not active. // --> Just copy the compute rotations. for (int i = 0; i < numberOfBones; i++) { int boneIndex = _boneIndices[i]; SkeletonPose.SetBoneRotationAbsolute(boneIndex, _bones[i].Rotation); } } }
/// <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(); }
/// <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; } // Validate new chains. if (_isDirty && !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); } } // We iterate NumberOfIteration times or until we are within the allowed deviation. // In each iteration we move each bone once. float toleranceSquared = AllowedDeviation * AllowedDeviation; bool targetReached = false; for (int i = 0; i < NumberOfIterations && !targetReached; i++) { // Iterate bones from tip to root. int boneIndex = TipBoneIndex; while (true) { // Get current tip position in local bone space. var bonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(boneIndex); var targetPositionLocal = bonePoseAbsolute.ToLocalPosition(Target); Vector3 tipLocal; if (boneIndex == TipBoneIndex) { tipLocal = TipOffset; } else { var tipBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex); var tipAbsolute = tipBonePoseAbsolute.ToParentPosition(TipOffset); tipLocal = bonePoseAbsolute.ToLocalPosition(tipAbsolute); } if ((tipLocal - targetPositionLocal).LengthSquared() < toleranceSquared) { // Target reached! If this is the first iteration and the first bone, then we // didn't have to do anything and can abort. Otherwise we just leave the loops. if (i == 0 && boneIndex == TipBoneIndex) { return; } targetReached = true; break; } // Rotate bone so that it points to the target. if (tipLocal.TryNormalize() && targetPositionLocal.TryNormalize()) { var rotation = Quaternion.CreateFromRotationMatrix(tipLocal, targetPositionLocal); var angle = rotation.Angle; // If the bone gain is less than 1, then we make a smaller correction. We will need // more iterations but the change is more evenly distributed over the chain. if (BoneGain < 1) { angle = angle * BoneGain; rotation.Angle = angle; } // Apply rotation to bone transform. if (Numeric.IsGreater(angle, 0)) { var boneTransform = SkeletonPose.GetBoneTransform(boneIndex); boneTransform.Rotation = boneTransform.Rotation * rotation; SkeletonPose.SetBoneTransform(boneIndex, boneTransform); // Call delegate that enforces bone limits. if (LimitBoneTransforms != null) { LimitBoneTransforms(boneIndex); } } } if (boneIndex == RootBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } } 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(); }