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; }