private void AdjustBone(SkeletonInputs inputs, Bone bone, Vector2 source, Vector2 target, float weight)
{
var transform = bone.GetChainedTransform(inputs);
var center = Matrix3x2.TransformPoint(transform, bone.Center);
float rotationDelta = Vector2Utils.AngleBetween(source - center, target - center);
bone.IncrementRotation(inputs, rotationDelta * weight);
}
public void TestAngleBetween()
{
Assert.AreEqual(0, Vector2Utils.AngleBetween(+Vector2.UnitX, +Vector2.UnitX), 1e-6f);
Assert.AreEqual(MathUtil.PiOverFour, Vector2Utils.AngleBetween(+Vector2.UnitX, new Vector2(1, 1)), 1e-6f);
Assert.AreEqual(MathUtil.PiOverTwo, Vector2Utils.AngleBetween(+Vector2.UnitX, +Vector2.UnitY), 1e-6f);
Assert.AreEqual(MathUtil.PiOverTwo, Vector2Utils.AngleBetween(+Vector2.UnitY, -Vector2.UnitX), 1e-6f);
Assert.AreEqual(MathUtil.PiOverTwo, Vector2Utils.AngleBetween(-Vector2.UnitX, -Vector2.UnitY), 1e-6f);
Assert.AreEqual(MathUtil.PiOverTwo, Vector2Utils.AngleBetween(-Vector2.UnitY, +Vector2.UnitX), 1e-6f);
Assert.AreEqual(-MathUtil.PiOverTwo, Vector2Utils.AngleBetween(+Vector2.UnitY, +Vector2.UnitX), 1e-6f);
Assert.AreEqual(-MathUtil.PiOverTwo, Vector2Utils.AngleBetween(-Vector2.UnitX, +Vector2.UnitY), 1e-6f);
Assert.AreEqual(-MathUtil.PiOverTwo, Vector2Utils.AngleBetween(-Vector2.UnitY, -Vector2.UnitX), 1e-6f);
Assert.AreEqual(-MathUtil.PiOverTwo, Vector2Utils.AngleBetween(+Vector2.UnitX, -Vector2.UnitY), 1e-6f);
}
public static FabrIkChain Make(SkeletonInputs inputs, Bone sourceBone, Vector2 unposedSource, Vector2 target)
{
Vector2 posedSource = Matrix3x2.TransformPoint(sourceBone.GetChainedTransform(inputs), sourceBone.End);
List <Bone> bones = new List <Bone> {
};
List <Vector2> unposedBoneVectors = new List <Vector2> {
};
List <float> rotations = new List <float>();
List <Vector2> positions = new List <Vector2> {
};
positions.Add(posedSource);
Vector2 previousUnposedPosition = unposedSource;
Vector2 previousPosedPosition = posedSource;
for (var bone = sourceBone; bone != null; bone = bone.Parent)
{
var unposedCenter = bone.Center;
var posedCenter = Matrix3x2.TransformPoint(bone.GetChainedTransform(inputs), unposedCenter);
Vector2 unposedBoneVector = previousUnposedPosition - unposedCenter;
Vector2 posedBoneVector = previousPosedPosition - posedCenter;
float rotation = Vector2Utils.AngleBetween(
unposedBoneVector,
posedBoneVector);
bones.Add(bone);
unposedBoneVectors.Add(unposedBoneVector);
rotations.Add(rotation);
positions.Add(posedCenter);
previousUnposedPosition = unposedCenter;
previousPosedPosition = posedCenter;
}
var startTarget = target;
var endTarget = positions[bones.Count];
return(new FabrIkChain(bones, unposedBoneVectors, rotations, positions, target, endTarget));
}
// From root to end-effector
public void DoBackwardPass()
{
Vector2 targetCenter = lastTargetCenter;
for (int boneIdx = bones.Count - 1; boneIdx >= 0; --boneIdx)
{
Vector2 currentEnd = positions[boneIdx];
Vector2 currentCenter = positions[boneIdx + 1];
float targetRotationDelta = Vector2Utils.AngleBetween(
currentEnd - currentCenter,
currentEnd - targetCenter);
float unconstrainedTargetRotation = rotations[boneIdx] + targetRotationDelta;
float targetRotation = ConstraintRotationAgainstParent(boneIdx, unconstrainedTargetRotation);
Vector2 targetEnd = targetCenter + Vector2Utils.RotateBy(targetRotation, unposedBoneVectors[boneIdx]);
positions[boneIdx + 1] = targetCenter;
rotations[boneIdx] = targetRotation;
targetCenter = targetEnd;
}
positions[0] = targetCenter;
}
//From end-effector to root
public void DoForwardPass()
{
Vector2 targetEnd = firstTargetEnd;
for (int boneIdx = 0; boneIdx < bones.Count; ++boneIdx)
{
Vector2 currentEnd = positions[boneIdx];
Vector2 currentCenter = positions[boneIdx + 1];
float targetRotationDelta = Vector2Utils.AngleBetween(
currentEnd - currentCenter,
targetEnd - currentCenter);
float unconstrainedTargetRotation = rotations[boneIdx] + targetRotationDelta;
float targetRotation = ConstrainRotationAgainstChild(boneIdx, unconstrainedTargetRotation);
Vector2 targetCenter = targetEnd - Vector2Utils.RotateBy(targetRotation, unposedBoneVectors[boneIdx]);
positions[boneIdx] = targetEnd;
rotations[boneIdx] = targetRotation;
targetEnd = targetCenter;
}
positions[bones.Count] = targetEnd;
}
