private void Solve(RigidBoneSystem boneSystem, InverseKinematicsGoal goal, RigidBoneSystemInputs inputs)
{
var bone = boneSystem.BonesByName[boneName];
//get bone transforms with the current bone rotation zeroed out
inputs.Rotations[bone.Index] = TwistSwing.Zero;
var boneTransforms = boneSystem.GetBoneTransforms(inputs);
var boneTransform = boneTransforms[bone.Index];
var worldSourcePosition = boneTransforms[goal.SourceBone.Index].Transform(goal.SourceBone.CenterPoint + goal.UnposedSourcePosition);
var worldTargetPosition = goal.TargetPosition;
var worldCenterPosition = boneTransform.Transform(bone.CenterPoint);
var worldSourceDirection = Vector3.Normalize(worldSourcePosition - worldCenterPosition);
var worldTargetDirection = Vector3.Normalize(worldTargetPosition - worldCenterPosition);
//transform source and target to bone's oriented space
var parentTotalRotation = bone.Parent != null ? boneTransforms[bone.Parent.Index].Rotation : Quaternion.Identity;
var orientedSpaceToWorldTransform = bone.OrientationSpace.Orientation.Chain(parentTotalRotation);
var worldToOrientatedSpaceTransform = Quaternion.Invert(orientedSpaceToWorldTransform);
var orientedSourceDirection = Vector3.Transform(worldSourceDirection, worldToOrientatedSpaceTransform);
var orientedTargetDirection = Vector3.Transform(worldTargetDirection, worldToOrientatedSpaceTransform);
CartesianAxis twistAxis = (CartesianAxis)bone.RotationOrder.primaryAxis;
var newOrientedRotation = Swing.FromTo(twistAxis, orientedSourceDirection, orientedTargetDirection);
bone.SetOrientedSpaceRotation(inputs, new TwistSwing(Twist.Zero, newOrientedRotation), true);
}
public void TestFromTo()
{
var rnd = new Random(0);
Vector3 a = MakeRandomUnitVector(rnd);
Vector3 b = MakeRandomUnitVector(rnd);
foreach (CartesianAxis twistAxis in CartesianAxes.Values)
{
Swing fromAToB = Swing.FromTo(twistAxis, a, b);
MathAssert.AreEqual(b, Vector3.Transform(a, fromAToB.AsQuaternion(twistAxis)), Acc);
Swing fromBToA = Swing.FromTo(twistAxis, b, a);
MathAssert.AreEqual(a, Vector3.Transform(b, fromBToA.AsQuaternion(twistAxis)), Acc);
}
}
