/// <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;
}