/// <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); }
/// <summary> /// Gets the bone indices of 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="boneIndices"> /// A list where the bone indices should be stored. Must not be <see langword="null"/>. /// The list is cleared before the new bones are added. /// </param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose"/> or <paramref name="boneIndices"/> is <see langword="null"/>. /// </exception> /// <exception cref="ArgumentException"> /// <paramref name="startBoneIndex"/> and <paramref name="endBoneIndex"/> do not form a valid /// bone chain. /// </exception> public static void GetChain(this SkeletonPose skeletonPose, int startBoneIndex, int endBoneIndex, List <int> boneIndices) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } if (boneIndices == null) { throw new ArgumentNullException("boneIndices"); } if (!IsAncestorOrSelf(skeletonPose, startBoneIndex, endBoneIndex)) { throw new ArgumentException("startBoneIndex and endBoneIndex do not form a valid bone chain."); } boneIndices.Clear(); var skeleton = skeletonPose.Skeleton; int boneIndex = endBoneIndex; while (boneIndex >= 0) { boneIndices.Add(boneIndex); if (boneIndex == startBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } boneIndices.Reverse(); }
/// <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, QuaternionF 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); }
/// <summary> /// Recycles this instance of the <see cref="SkeletonBoneAccessor"/> class. /// </summary> /// <remarks> /// <para> /// This method resets this instance and returns it to a resource pool if resource pooling is /// enabled (see <see cref="ResourcePool.Enabled">ResourcePool.Enabled</see>). /// </para> /// </remarks> public void Recycle() { var pool = _skeletonPose.Skeleton.SkeletonBoneAccessorPool; _skeletonPose = null; pool.Recycle(this); }
//public static void RotateBoneWorld(this SkeletonPose SkeletonPose, int boneIndex, QuaternionF rotation, Matrix44F world) //{ // QuaternionF worldRotation = QuaternionF.CreateRotation(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, QuaternionF 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, QuaternionF 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> /// Counts the number of 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> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static int GetNumberOfBones(this SkeletonPose skeletonPose, int startBoneIndex, int endBoneIndex) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } if (!IsAncestorOrSelf(skeletonPose, startBoneIndex, endBoneIndex)) { return(0); } var skeleton = skeletonPose.Skeleton; int numberOfBones = 0; int boneIndex = endBoneIndex; while (boneIndex >= 0) { numberOfBones++; if (boneIndex == startBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } return(numberOfBones); }
//-------------------------------------------------------------- #region Creation & Cleanup //-------------------------------------------------------------- /// <summary> /// Initializes a new instance of the <see cref="SkeletonMapper"/> class. /// </summary> /// <param name="skeletonPoseA">The first skeleton pose. Can be <see langword="null"/>.</param> /// <param name="skeletonPoseB">The second skeleton pose. Can be <see langword="null"/>.</param> public SkeletonMapper(SkeletonPose skeletonPoseA, SkeletonPose skeletonPoseB) { _skeletonPoseA = skeletonPoseA; _skeletonPoseB = skeletonPoseB; BoneMappers = new BoneMapperCollection(); BoneMappers.CollectionChanged += OnBoneMappersChanged; }
/// <summary> /// Creates an instance of the <see cref="SkeletonBoneAccessor"/> class. (This method /// reuses a previously recycled instance or allocates a new instance if necessary.) /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <returns> /// A new or reusable instance of the <see cref="SkeletonBoneAccessor"/> class. /// </returns> /// <remarks> /// <para> /// This method tries to obtain a previously recycled instance from a resource pool if resource /// pooling is enabled (see <see cref="ResourcePool.Enabled">ResourcePool.Enabled</see>). If no /// object is available, a new instance is automatically allocated on the heap. /// </para> /// <para> /// The owner of the object should call <see cref="Recycle"/> when the instance is no longer /// needed. /// </para> /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose"/> is <see langword="null"/>. /// </exception> public static SkeletonBoneAccessor Create(SkeletonPose skeletonPose) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeletonBoneAccessor = skeletonPose.Skeleton.SkeletonBoneAccessorPool.Obtain(); skeletonBoneAccessor.Initialize(skeletonPose); return(skeletonBoneAccessor); }
/// <summary> /// Initializes a new instance of the <see cref="AvatarPose"/> class for the given skeleton. /// </summary> /// <param name="skeleton">The skeleton.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeleton"/> is <see langword="null"/>. /// </exception> /// <exception cref="ArgumentException"> /// <paramref name="skeleton"/> is not a valid Xbox LIVE Avatar skeleton. /// </exception> public AvatarPose(Skeleton skeleton) { if (skeleton.NumberOfBones != AvatarRenderer.BoneCount) { throw new ArgumentException("The specified skeleton is not a valid Avatar skeleton.", "skeleton"); } _expressionWrapper = new DelegateAnimatableProperty <AvatarExpression>( () => _expression, // Getter e => _expression = e); // Setter _skeletonPose = SkeletonPose.Create(skeleton); }
private static void DoWork(QuaternionF 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 (Vector3F.AreNumericallyEqual(boneA, neckA) || Vector3F.AreNumericallyEqual(boneA, rightShoulderA) || Vector3F.AreNumericallyEqual(leftShoulderA, rightShoulderA) || Vector3F.AreNumericallyEqual(boneB, neckB) || Vector3F.AreNumericallyEqual(boneB, rightShoulderB) || Vector3F.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 = QuaternionF.CreateRotation(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 = QuaternionF.CreateRotation(spineAxisB, spineAxisA); // Apply the shoulder twist rotation followed by the spine swing rotation. skeletonB.RotateBoneAbsolute(boneIndexB, spineRotation * shoulderRotation); }
/// <summary> /// Copies the bone transforms from skeleton pose to another skeleton pose. /// </summary> /// <param name="source">The <see cref="SkeletonPose"/> from which the bone transforms are copied.</param> /// <param name="target">The <see cref="SkeletonPose"/> to which the bone transforms are copied.</param> /// <remarks> /// Copying a <see cref="SkeletonPose"/> using this method is faster than manually copying all /// bone transforms. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="source"/> or <paramref name="target"/> is <see langword="null"/>. /// </exception> /// <exception cref="ArgumentException"> /// <paramref name="source"/> and <paramref name="target"/> belong to different skeletons and /// <paramref name="target"/> has more bones than <paramref name="source"/>. /// </exception> public static void Copy(SkeletonPose source, SkeletonPose target) { if (source == null) { throw new ArgumentNullException("source"); } if (target == null) { throw new ArgumentNullException("target"); } if (target != source) { var sourceTransforms = source.BoneTransforms; var targetTransforms = target.BoneTransforms; Array.Copy(sourceTransforms, 0, targetTransforms, 0, targetTransforms.Length); target.Invalidate(); } }
/// <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); } }
private void Initialize(SkeletonPose skeletonPose) { _skeletonPose = skeletonPose; if (_bonePoseRelative == null) { // The SkeletonPoseAccessor is initialized for the first time. int numberOfBones = skeletonPose.Skeleton.NumberOfBones; // Create arrays. (Note: SkinningMatrices are created on demand.) _bonePoseRelative = new SrtTransform[numberOfBones]; _bonePoseAbsolute = new SrtTransform[numberOfBones]; // Set all dirty flags. _isDirty = true; _isBonePoseRelativeDirty = new FastBitArray(numberOfBones); _isBonePoseRelativeDirty.SetAll(true); _isBonePoseAbsoluteDirty = new FastBitArray(numberOfBones); _isBonePoseAbsoluteDirty.SetAll(true); _isSkinningMatrixDirty = new FastBitArray(numberOfBones); _isSkinningMatrixDirty.SetAll(true); } else { Debug.Assert(_bonePoseRelative != null, "SkeletonBoneAccessor is not properly initialized. Array _bonePoseRelative is not set."); Debug.Assert(_bonePoseAbsolute != null, "SkeletonBoneAccessor is not properly initialized. Array _bonePoseAbsolute is not set."); Debug.Assert(_isBonePoseRelativeDirty != null, "SkeletonBoneAccessor is not properly initialized. BitArray _isBonePoseRelativeDirty is not set."); Debug.Assert(_isBonePoseAbsoluteDirty != null, "SkeletonBoneAccessor is not properly initialized. BitArray _isBonePoseAbsoluteDirty is not set."); Debug.Assert(_isSkinningMatrixDirty != null, "SkeletonBoneAccessor is not properly initialized. BitArray _isSkinningMatrixDirty is not set."); Debug.Assert(_bonePoseRelative.Length != skeletonPose.Skeleton.NumberOfBones, "SkeletonBoneAccessor is incompatible. Array _bonePoseRelative has wrong length."); Debug.Assert(_bonePoseAbsolute.Length != skeletonPose.Skeleton.NumberOfBones, "SkeletonBoneAccessor is incompatible. Array _bonePoseAbsolute has wrong length."); Debug.Assert(_isBonePoseRelativeDirty.Length != skeletonPose.Skeleton.NumberOfBones, "SkeletonBoneAccessor is incompatible. BitArray _isBonePoseRelativeDirty has wrong length."); Debug.Assert(_isBonePoseAbsoluteDirty.Length != skeletonPose.Skeleton.NumberOfBones, "SkeletonBoneAccessor is incompatible. BitArray _isBonePoseAbsoluteDirty has wrong length."); Debug.Assert(_isSkinningMatrixDirty.Length != skeletonPose.Skeleton.NumberOfBones, "SkeletonBoneAccessor is incompatible. BitArray _isSkinningMatrixDirty has wrong length."); // Set all dirty flags. Invalidate(); } }
/// <summary> /// Determines whether the given bone indices form a valid bone chain. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="ancestorBoneIndex">Index of the start bone (root of the chain). Can be -1.</param> /// <param name="childBoneIndex">Index of the end bone (tip of the chain). Must not be -1.</param> /// <returns> /// <see langword="true"/> if bone indices describe a valid chain; otherwise, <see langword="false"/>. /// </returns> /// <remarks> /// This method checks it the start bone is an ancestor of the end bone. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static bool IsAncestorOrSelf(this SkeletonPose skeletonPose, int ancestorBoneIndex, int childBoneIndex) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } if (ancestorBoneIndex > childBoneIndex) { return(false); } if (childBoneIndex < 0) { return(false); } if (ancestorBoneIndex < 0) { return(true); } var skeleton = skeletonPose.Skeleton; int boneIndex = childBoneIndex; while (boneIndex >= 0) { if (boneIndex == ancestorBoneIndex) { return(true); } boneIndex = skeleton.GetParent(boneIndex); } // Parent not found in the chain. return(false); }
/// <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 = Vector3F.One; } if (resetRotation) { boneTransform.Rotation = QuaternionF.Identity; } if (resetTranslation) { boneTransform.Translation = Vector3F.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 = Vector3F.One; } if (resetRotation) { boneTransform.Rotation = QuaternionF.Identity; } if (resetTranslation) { boneTransform.Translation = Vector3F.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> /// Perform mapping in absolute space. /// </summary> private static void MapAbsolute(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, QuaternionF rotationBToA, QuaternionF rotationAToB) { // The current absolute bone pose of bone A. var boneAActualRotationAbsolute = skeletonA.GetBonePoseAbsolute(boneIndexA).Rotation; var boneABindRotationAbsolute = skeletonA.Skeleton.GetBindPoseAbsoluteInverse(boneIndexA).Rotation; var boneBBindRotationAbsolute = skeletonB.Skeleton.GetBindPoseAbsoluteInverse(boneIndexB).Rotation.Inverse; var relativeRotation = boneAActualRotationAbsolute * boneABindRotationAbsolute; // Rotation: Using similarity transformation: (Read from right to left.) // Rotate from model B space to model A space. // Apply the bone transform rotation in model A space // Rotate back from model A space to model B space. relativeRotation = rotationAToB * relativeRotation * rotationBToA; skeletonB.SetBoneRotationAbsolute(boneIndexB, relativeRotation * boneBBindRotationAbsolute); // TODO: Map translations. // How? // Map translation relative to model space? // Map translation relative to the next common bone ancestor that is in both skeletons // (then we would need a mechanism or user input that gives us this ancestor, complicated)? // Map translation relative to local space (the bone transform translation as in MapLocal)? }
/// <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. Vector3F[] axes = new Vector3F[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. Vector3F currentJointAbsolute = SkeletonPose.GetBonePoseAbsolute(currentBoneIndex).Translation; Vector3F jointToTarget = Target - currentJointAbsolute; Vector3F jointToTip = tipAbsolute - currentJointAbsolute; axes[i] = Vector3F.Cross(jointToTarget, jointToTip); if (!axes[i].TryNormalize()) { axes[i] = Vector3F.UnitX; // TODO: What should we really do in this case? } Vector3F jacobianColumnUpperPart = Vector3F.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. Vector3F axis = axes[i]; // Angle is computed using Euler integration with an arbitrary step size. float angle = velocities[i] * StepSize; // Apply rotation. QuaternionF rotationChange = QuaternionF.CreateRotation(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(); }
//-------------------------------------------------------------- #region Methods //-------------------------------------------------------------- /// <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). Vector3F forward = Forward; Vector3F up = Up; Vector3F side = Vector3F.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 Matrix33F(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 = Vector3F.Dot(targetDirection, up); float targetSide = Vector3F.Dot(targetDirection, side); float targetForward = Vector3F.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 = Vector3F.Cross(up, -forward); if (!side.TryNormalize()) { return; } up = Vector3F.Cross(side, forward); Debug.Assert(up.IsNumericallyNormalized); // Create new view space matrix. var boneFromNewView = new Matrix33F( 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. QuaternionF boneTransform = QuaternionF.CreateRotation(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) { 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); Vector3F 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 = QuaternionF.CreateRotation(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(); }
private void UpdateDerivedValues() { // Validate chain. Compute bone lengths, bone indices and total chain length. if (_isDirty) { return; } _boneLengths.Clear(); _boneIndices.Clear(); _totalChainLength = -1; try { // Check if chain is valid. if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, TipBoneIndex)) { throw new ArgumentException("The RootBoneIndex and the TipBoneIndex do not form a valid bone chain."); } // Get bone indices. SkeletonPose.GetChain(RootBoneIndex, TipBoneIndex, _boneIndices); var numberOfBones = _boneIndices.Count; // Get bone lengths. We compute the bone lengths from an actual bone pose because // our archer test model had a scale in the bone transforms. Therefore we cannot // compute the length from only the bind poses. _boneLengths.Capacity = numberOfBones; _totalChainLength = 0; for (int i = 0; i < numberOfBones - 1; i++) { int boneIndex = _boneIndices[i]; int childIndex = _boneIndices[i + 1]; var boneVector = SkeletonPose.GetBonePoseAbsolute(childIndex).Translation - SkeletonPose.GetBonePoseAbsolute(boneIndex).Translation; float boneLength = boneVector.Length; _boneLengths.Add(boneLength); _totalChainLength += boneLength; } // Tip bone. _boneLengths.Add((SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).Scale *TipOffset).Length); _totalChainLength += _boneLengths[numberOfBones - 1]; // Initialize _bones list with dummy values. _bones.Clear(); for (int i = 0; i < numberOfBones; i++) { _bones.Add(SrtTransform.Identity); } _isDirty = true; } catch { _boneLengths.Clear(); _boneIndices.Clear(); _totalChainLength = 0; throw; } }
/// <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. Vector3F 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. QuaternionF chainRotation = QuaternionF.Identity; Vector3F 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; //Vector3F 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 = QuaternionF.CreateRotation(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, QuaternionF.CreateRotation(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. Vector3F 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. Vector3F rotationAxis = Vector3F.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(Vector3F.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, (QuaternionF.CreateRotation(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 (_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. Vector3F 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 (Vector3F.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 = QuaternionF.CreateRotation(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 * Vector3F.Dot(rootToHinge, hingeAxis); hingeToTip -= hingeAxis * Vector3F.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(Vector3F.Dot(rootToHinge, hingeToTip), -1, 1)); // Make sure the computed angle is about the hingeAxis and not about -hingeAxis. if (Vector3F.Dot(Vector3F.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 = QuaternionF.CreateRotation(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 = QuaternionF.CreateRotation(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> /// Draws the skeleton bones, bone space axes and bone names for debugging. /// (Only available in the XNA-compatible build.) /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="graphicsDevice">The graphics device.</param> /// <param name="effect"> /// A <see cref="BasicEffect"/> instance. The effect parameters <see cref="BasicEffect.World"/>, /// <see cref="BasicEffect.View"/>, and <see cref="BasicEffect.Projection"/> must be /// correctly initialized before this method is called. /// </param> /// <param name="axisLength">The visible length of the bone space axes.</param> /// <param name="spriteBatch"> /// A <see cref="SpriteBatch"/>. Can be <see langword="null"/> to skip text rendering. /// </param> /// <param name="spriteFont"> /// A <see cref="SpriteFont"/>. Can be <see langword="null"/> to skip text rendering. /// </param> /// <param name="color">The color for the bones and the bone names.</param> /// <remarks> /// <para> /// This method is available only in the XNA-compatible build of the DigitalRune.Animation.dll. /// </para> /// <para> /// This method draws the skeleton for debugging. It draws a line for each bone and the bone /// name. At the bone origin it draws 3 lines (red, green, blue) that visualize the bone /// space axes (x, y, z). /// </para> /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose"/>, <paramref name="graphicsDevice"/> or /// <paramref name="effect"/> is <see langword="null"/>. /// </exception> public static void DrawBones(this SkeletonPose skeletonPose, GraphicsDevice graphicsDevice, BasicEffect effect, float axisLength, SpriteBatch spriteBatch, SpriteFont spriteFont, Color color) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } if (graphicsDevice == null) { throw new ArgumentNullException("graphicsDevice"); } if (effect == null) { throw new ArgumentNullException("effect"); } var oldVertexColorEnabled = effect.VertexColorEnabled; effect.VertexColorEnabled = true; // No font, then we don't need the sprite batch. if (spriteFont == null) { spriteBatch = null; } if (spriteBatch != null) { spriteBatch.Begin(); } List <VertexPositionColor> vertices = new List <VertexPositionColor>(); var skeleton = skeletonPose.Skeleton; for (int i = 0; i < skeleton.NumberOfBones; i++) { string name = skeleton.GetName(i); SrtTransform bonePose = skeletonPose.GetBonePoseAbsolute(i); var translation = (Vector3)bonePose.Translation; var rotation = (Quaternion)bonePose.Rotation; int parentIndex = skeleton.GetParent(i); if (parentIndex >= 0) { // Draw line to parent joint representing the parent bone. SrtTransform parentPose = skeletonPose.GetBonePoseAbsolute(parentIndex); vertices.Add(new VertexPositionColor(translation, color)); vertices.Add(new VertexPositionColor((Vector3)parentPose.Translation, color)); } // Add three lines in Red, Green and Blue. vertices.Add(new VertexPositionColor(translation, Color.Red)); vertices.Add(new VertexPositionColor(translation + Vector3.Transform(Vector3.UnitX, rotation) * axisLength, Color.Red)); vertices.Add(new VertexPositionColor(translation, Color.Green)); vertices.Add(new VertexPositionColor(translation + Vector3.Transform(Vector3.UnitY, rotation) * axisLength, Color.Green)); vertices.Add(new VertexPositionColor(translation, Color.Blue)); vertices.Add(new VertexPositionColor(translation + Vector3.Transform(Vector3.UnitZ, rotation) * axisLength, Color.Blue)); // Draw name. if (spriteBatch != null && !string.IsNullOrEmpty(name)) { // Compute the 3D position in view space. Text is rendered near drawn x axis. Vector3 textPosition = translation + Vector3.TransformNormal(Vector3.UnitX, bonePose) * axisLength * 0.5f; var textPositionWorld = Vector3.Transform(textPosition, effect.World); var textPositionView = Vector3.Transform(textPositionWorld, effect.View); // Check if the text is in front of the camera. if (textPositionView.Z < 0) { // Project text position to screen. Vector3 textPositionProjected = graphicsDevice.Viewport.Project(textPosition, effect.Projection, effect.View, effect.World); spriteBatch.DrawString(spriteFont, name + " " + i, new Vector2(textPositionProjected.X, textPositionProjected.Y), color); } } } if (spriteBatch != null) { spriteBatch.End(); } // Draw axis lines in one batch. graphicsDevice.DepthStencilState = DepthStencilState.None; effect.CurrentTechnique.Passes[0].Apply(); graphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, vertices.ToArray(), 0, vertices.Count / 2); effect.VertexColorEnabled = oldVertexColorEnabled; }