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