/// <summary>
/// Called when <see cref="IKSolver.Solve"/> is called.
/// </summary>
/// <param name="deltaTime">The current time step (in seconds).</param>
protected override void OnSolve(float deltaTime)
{
if (_isDirty)
{
// Validate bone chain.
if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, HingeBoneIndex))
{
throw new ArgumentException("The RootBoneIndex and the HingeBoneIndex do not form a valid bone chain.");
}
if (!SkeletonPose.IsAncestorOrSelf(HingeBoneIndex, TipBoneIndex))
{
throw new ArgumentException("The HingeBoneIndex and the TipBoneIndex do not form a valid bone chain.");
}
}
_isDirty = false;
if (MinHingeAngle > MaxHingeAngle)
{
throw new AnimationException("The MinHingeAngle must be less than or equal to the MaxHingeAngle");
}
// Remember original bone transforms for interpolation at the end.
var originalRootBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex);
var originalHingeBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex);
var originalTipBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex);
var rootBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(RootBoneIndex);
var hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex);
var tipBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex);
// Get tip position in model space.
Vector3 tipAbsolute;
if (TipBoneOrientation != null)
{
// If the user has specified an absolute tip rotation, then we consider this rotation and
// use the tip bone origin as tip.
tipAbsolute = tipBonePoseAbsolute.Translation;
Target -= tipBonePoseAbsolute.ToParentDirection(tipBonePoseAbsolute.Scale * TipOffset);
}
else
{
// The user hasn't specified a desired tip rotation. Therefore we do not modify the
// tip rotation and use the offset position in the tip bone as tip.
tipAbsolute = tipBonePoseAbsolute.ToParentPosition(TipOffset);
}
// Abort if we already touch the target.
if (Vector3.AreNumericallyEqual(tipAbsolute, Target))
{
return;
}
// Root to target vector.
var rootToTarget = Target - rootBonePoseAbsolute.Translation;
var rootToTargetLength = rootToTarget.Length;
if (Numeric.IsZero(rootToTargetLength))
{
return;
}
rootToTarget /= rootToTargetLength;
// ----- Align chain with target.
// Align the root to target vector with the root to tip vector.
var rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation;
var rootToTipLength = rootToTip.Length;
if (!Numeric.IsZero(rootToTipLength))
{
rootToTip /= rootToTipLength;
var rotation = Quaternion.CreateFromRotationMatrix(rootToTip, rootToTarget);
if (rotation.Angle > Numeric.EpsilonF)
{
// Apply rotation to root bone.
rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation;
SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation);
hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex);
// Compute new tip absolute tip position from the known quantities.
tipAbsolute = rootBonePoseAbsolute.Translation + rootToTarget * rootToTipLength;
}
}
// ----- Compute ideal angle.
var rootToHinge = hingeBonePoseAbsolute.Translation - rootBonePoseAbsolute.Translation;
var hingeToTip = tipAbsolute - hingeBonePoseAbsolute.Translation;
var hingeAxis = hingeBonePoseAbsolute.ToParentDirection(HingeAxis);
// Project vectors to hinge plane. Everything should be in a plane for the following
// computations.
rootToHinge -= hingeAxis * Vector3.Dot(rootToHinge, hingeAxis);
hingeToTip -= hingeAxis * Vector3.Dot(hingeToTip, hingeAxis);
// Get lengths.
float rootToHingeLength = rootToHinge.Length;
if (Numeric.IsZero(rootToHingeLength))
{
return;
}
rootToHinge /= rootToHingeLength;
float hingeToTipLength = hingeToTip.Length;
if (Numeric.IsZero(hingeToTipLength))
{
return;
}
hingeToTip /= hingeToTipLength;
// Compute current hinge angle (angle between root bone and hinge bone).
float currentHingeAngle = (float)Math.Acos(MathHelper.Clamp(Vector3.Dot(rootToHinge, hingeToTip), -1, 1));
// Make sure the computed angle is about the hingeAxis and not about -hingeAxis.
if (Vector3.Dot(Vector3.Cross(rootToHinge, hingeToTip), hingeAxis) < 0)
{
currentHingeAngle = -currentHingeAngle;
}
// Using law of cosines to compute the desired hinge angle using the triangle lengths.
float cosDesiredHingeAngle = (rootToHingeLength * rootToHingeLength + hingeToTipLength * hingeToTipLength - rootToTargetLength * rootToTargetLength)
/ (2 * rootToHingeLength * hingeToTipLength);
float desiredHingeAngle = ConstantsF.Pi - (float)Math.Acos(MathHelper.Clamp(cosDesiredHingeAngle, -1, 1));
// Apply hinge limits.
if (desiredHingeAngle < MinHingeAngle)
{
desiredHingeAngle = MinHingeAngle;
}
else if (desiredHingeAngle > MaxHingeAngle)
{
desiredHingeAngle = MaxHingeAngle;
}
// Compute delta rotation between current and desired angle.
float deltaAngle = desiredHingeAngle - currentHingeAngle;
var hingeRotation = Quaternion.CreateFromRotationMatrix(hingeAxis, deltaAngle);
hingeBonePoseAbsolute.Rotation = hingeRotation * hingeBonePoseAbsolute.Rotation;
// Update tip position.
tipAbsolute = hingeBonePoseAbsolute.Translation + hingeRotation.Rotate(tipAbsolute - hingeBonePoseAbsolute.Translation);
// ----- Align chain with target.
// If we hit a hinge limit, then we can move the tip closer to the target by aligning
// the whole chain again.
rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation;
rootToTipLength = rootToTip.Length;
if (!Numeric.IsZero(rootToTipLength))
{
rootToTip /= rootToTipLength;
var rotation = Quaternion.CreateFromRotationMatrix(rootToTip, rootToTarget);
rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation;
hingeBonePoseAbsolute.Rotation = rotation * hingeBonePoseAbsolute.Rotation;
}
// ----- Set results.
SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation);
SkeletonPose.SetBoneRotationAbsolute(HingeBoneIndex, hingeBonePoseAbsolute.Rotation);
if (TipBoneOrientation != null)
{
SkeletonPose.SetBoneRotationAbsolute(TipBoneIndex, TipBoneOrientation.Value);
}
// ----- Apply weight, velocity limit and set results.
bool requiresBlending = RequiresBlending();
float maxRotationAngle;
bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle);
if (requiresBlending || requiresLimiting)
{
var targetBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex);
if (requiresBlending)
{
BlendBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform);
}
if (requiresLimiting)
{
LimitBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(RootBoneIndex, targetBoneTransform);
targetBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex);
if (requiresBlending)
{
BlendBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform);
}
if (requiresLimiting)
{
LimitBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(HingeBoneIndex, targetBoneTransform);
targetBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex);
if (requiresBlending)
{
BlendBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform);
}
if (requiresLimiting)
{
LimitBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(TipBoneIndex, targetBoneTransform);
}
}
/// <summary>
/// Called when <see cref="IKSolver.Solve"/> is called.
/// </summary>
/// <param name="deltaTime">The current time step (in seconds).</param>
protected override void OnSolve(float deltaTime)
{
if (NumberOfIterations <= 0)
{
return;
}
if (_isDirty)
{
if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, TipBoneIndex))
{
throw new ArgumentException("The RootBoneIndex and the TipBoneIndex do not form a valid bone chain.");
}
}
_isDirty = false;
var skeleton = SkeletonPose.Skeleton;
bool requiresBlending = RequiresBlending();
float maxRotationAngle;
bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle);
if (requiresBlending || requiresLimiting)
{
// Remember original bone transforms for interpolation with the result at the end.
// Transforms are stored from tip to root (reverse order!).
_originalTransforms.Clear();
int boneIndex = TipBoneIndex;
while (true)
{
_originalTransforms.Add(SkeletonPose.GetBoneTransform(boneIndex));
if (boneIndex == RootBoneIndex)
{
break;
}
boneIndex = skeleton.GetParent(boneIndex);
}
}
int numberOfBones = SkeletonPose.GetNumberOfBones(RootBoneIndex, TipBoneIndex);
// The transposed jacobian matrix.
var jacobianTransposed = new MatrixF(numberOfBones, 6);
// The force vector (3 linear and 3 angular (torque) entries).
VectorF force = new VectorF(6);
// The rotation axes of the bones.
Vector3[] axes = new Vector3[numberOfBones];
float toleranceSquared = AllowedDeviation * AllowedDeviation;
// In each iteration we compute the jacobian matrix, compute the bone velocities
// an make an euler integration step.
for (int iteration = 0; iteration < NumberOfIterations; iteration++)
{
var tipBoneAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex);
var tipAbsolute = tipBoneAbsolute.ToParentPosition(TipOffset);
var targetToTip = tipAbsolute - Target;
if (targetToTip.LengthSquared() < toleranceSquared)
{
if (iteration == 0)
{
return;
}
break;
}
// Loop from tip to root and fill Jacobian.
// (See description of Jacobian Transpose method to see how the rotation axes and
// Jacobian entries must look like).
var currentBoneIndex = TipBoneIndex;
int exitBoneIndex = RootBoneIndex >= 0 ? skeleton.GetParent(RootBoneIndex) : -1;
int i = numberOfBones;
do
{
i--;
// Compute rotation axis.
Vector3 currentJointAbsolute = SkeletonPose.GetBonePoseAbsolute(currentBoneIndex).Translation;
Vector3 jointToTarget = Target - currentJointAbsolute;
Vector3 jointToTip = tipAbsolute - currentJointAbsolute;
axes[i] = Vector3.Cross(jointToTarget, jointToTip);
if (!axes[i].TryNormalize())
{
axes[i] = Vector3.UnitX; // TODO: What should we really do in this case?
}
Vector3 jacobianColumnUpperPart = Vector3.Cross(jointToTip, axes[i]);
// Fill J.
jacobianTransposed[i, 0] = jacobianColumnUpperPart.X;
jacobianTransposed[i, 1] = jacobianColumnUpperPart.Y;
jacobianTransposed[i, 2] = jacobianColumnUpperPart.Z;
jacobianTransposed[i, 3] = axes[i].X;
jacobianTransposed[i, 4] = axes[i].Y;
jacobianTransposed[i, 5] = axes[i].Z;
currentBoneIndex = skeleton.GetParent(currentBoneIndex);
} while (currentBoneIndex != exitBoneIndex && currentBoneIndex >= 0);
Debug.Assert(i == 0);
// Set the force.
force[0] = targetToTip.X;
force[1] = targetToTip.Y;
force[2] = targetToTip.Z;
force[3] = 0;
force[4] = 0;
force[5] = 0;
// Compute pseudo velocities.
VectorF velocities = jacobianTransposed * force; // TODO: Garbage!
// Euler integration step.
currentBoneIndex = TipBoneIndex;
i = numberOfBones;
do
{
i--;
// Rotation axis for this bone.
Vector3 axis = axes[i];
// Angle is computed using Euler integration with an arbitrary step size.
float angle = velocities[i] * StepSize;
// Apply rotation.
Quaternion rotationChange = Quaternion.CreateFromRotationMatrix(axis, angle);
SkeletonPose.RotateBoneAbsolute(currentBoneIndex, rotationChange);
currentBoneIndex = skeleton.GetParent(currentBoneIndex);
} while (currentBoneIndex != exitBoneIndex && currentBoneIndex >= 0);
// Call delegate that checks bone limits.
if (LimitBoneTransforms != null)
{
LimitBoneTransforms();
}
}
if (requiresBlending || requiresLimiting)
{
// Apply weight and the angular velocity limit.
int boneIndex = TipBoneIndex;
int i = 0;
while (true)
{
var originalTransform = _originalTransforms[i];
var targetTransform = SkeletonPose.GetBoneTransform(boneIndex);
// Apply weight.
if (requiresBlending)
{
BlendBoneTransform(ref originalTransform, ref targetTransform);
}
// Apply angular velocity limit.
if (requiresLimiting)
{
LimitBoneTransform(ref originalTransform, ref targetTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(boneIndex, targetTransform);
if (boneIndex == RootBoneIndex)
{
break;
}
boneIndex = skeleton.GetParent(boneIndex);
i++;
}
}
_originalTransforms.Clear();
}
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)
{
if (NumberOfIterations <= 0)
{
return;
}
// Validate new chains.
if (_isDirty && !SkeletonPose.IsAncestorOrSelf(RootBoneIndex, TipBoneIndex))
{
throw new ArgumentException("The RootBoneIndex and the TipBoneIndex do not form a valid bone chain.");
}
_isDirty = false;
var skeleton = SkeletonPose.Skeleton;
bool requiresBlending = RequiresBlending();
float maxRotationAngle;
bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle);
if (requiresBlending || requiresLimiting)
{
// Remember original bone transforms for interpolation with the result at the end.
// Transforms are stored from tip to root (reverse order!).
_originalTransforms.Clear();
int boneIndex = TipBoneIndex;
while (true)
{
_originalTransforms.Add(SkeletonPose.GetBoneTransform(boneIndex));
if (boneIndex == RootBoneIndex)
{
break;
}
boneIndex = skeleton.GetParent(boneIndex);
}
}
// We iterate NumberOfIteration times or until we are within the allowed deviation.
// In each iteration we move each bone once.
float toleranceSquared = AllowedDeviation * AllowedDeviation;
bool targetReached = false;
for (int i = 0; i < NumberOfIterations && !targetReached; i++)
{
// Iterate bones from tip to root.
int boneIndex = TipBoneIndex;
while (true)
{
// Get current tip position in local bone space.
var bonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(boneIndex);
var targetPositionLocal = bonePoseAbsolute.ToLocalPosition(Target);
Vector3 tipLocal;
if (boneIndex == TipBoneIndex)
{
tipLocal = TipOffset;
}
else
{
var tipBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex);
var tipAbsolute = tipBonePoseAbsolute.ToParentPosition(TipOffset);
tipLocal = bonePoseAbsolute.ToLocalPosition(tipAbsolute);
}
if ((tipLocal - targetPositionLocal).LengthSquared() < toleranceSquared)
{
// Target reached! If this is the first iteration and the first bone, then we
// didn't have to do anything and can abort. Otherwise we just leave the loops.
if (i == 0 && boneIndex == TipBoneIndex)
{
return;
}
targetReached = true;
break;
}
// Rotate bone so that it points to the target.
if (tipLocal.TryNormalize() && targetPositionLocal.TryNormalize())
{
var rotation = Quaternion.CreateFromRotationMatrix(tipLocal, targetPositionLocal);
var angle = rotation.Angle;
// If the bone gain is less than 1, then we make a smaller correction. We will need
// more iterations but the change is more evenly distributed over the chain.
if (BoneGain < 1)
{
angle = angle * BoneGain;
rotation.Angle = angle;
}
// Apply rotation to bone transform.
if (Numeric.IsGreater(angle, 0))
{
var boneTransform = SkeletonPose.GetBoneTransform(boneIndex);
boneTransform.Rotation = boneTransform.Rotation * rotation;
SkeletonPose.SetBoneTransform(boneIndex, boneTransform);
// Call delegate that enforces bone limits.
if (LimitBoneTransforms != null)
{
LimitBoneTransforms(boneIndex);
}
}
}
if (boneIndex == RootBoneIndex)
{
break;
}
boneIndex = skeleton.GetParent(boneIndex);
}
}
if (requiresBlending || requiresLimiting)
{
// Apply weight and the angular velocity limit.
int boneIndex = TipBoneIndex;
int i = 0;
while (true)
{
var originalTransform = _originalTransforms[i];
var targetTransform = SkeletonPose.GetBoneTransform(boneIndex);
// Apply weight.
if (requiresBlending)
{
BlendBoneTransform(ref originalTransform, ref targetTransform);
}
// Apply angular velocity limit.
if (requiresLimiting)
{
LimitBoneTransform(ref originalTransform, ref targetTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(boneIndex, targetTransform);
if (boneIndex == RootBoneIndex)
{
break;
}
boneIndex = skeleton.GetParent(boneIndex);
i++;
}
}
_originalTransforms.Clear();
}
