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