/// <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 (_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>
/// 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);
}
}
}