public TwistSwing Clamp(TwistSwing twistSwing)
{
var clampedTwist = Twist.Clamp(twistSwing.Twist);
var clampedSwing = Swing.Clamp(twistSwing.Swing);
return(new TwistSwing(clampedTwist, clampedSwing));
}
public RigidBoneSystemInputs ApplyDeltas(RigidBoneSystemInputs baseInputs, RigidBoneSystemInputs deltaInputs)
{
var sumInputs = new RigidBoneSystemInputs(bones.Length)
{
};
RigidTransform baseRootTransform = RigidTransform.FromRotationTranslation(
RootBone.GetRotation(baseInputs),
baseInputs.RootTranslation);
RigidTransform deltaRootTransform = RigidTransform.FromRotationTranslation(
RootBone.GetRotation(deltaInputs),
deltaInputs.RootTranslation);
RigidTransform sumRootTransform = deltaRootTransform.Chain(baseRootTransform);
sumInputs.RootTranslation = sumRootTransform.Translation;
RootBone.SetRotation(sumInputs, sumRootTransform.Rotation);
for (int boneIdx = 1; boneIdx < bones.Length; ++boneIdx)
{
var bone = bones[boneIdx];
sumInputs.Rotations[boneIdx] = bone.Constraint.Clamp(
TwistSwing.ApplyDelta(baseInputs.Rotations[boneIdx], deltaInputs.Rotations[boneIdx]));
}
return(sumInputs);
}
public void SetRotation(RigidBoneSystemInputs inputs, Quaternion objectSpaceRotation, bool applyClamp = false)
{
Quaternion orientatedSpaceRotation = orientationSpace.TransformToOrientedSpace(objectSpaceRotation);
TwistSwing orientedSpaceTwistSwing = TwistSwing.Decompose(RotationOrder.TwistAxis, orientatedSpaceRotation);
SetOrientedSpaceRotation(inputs, orientedSpaceTwistSwing, applyClamp);
}
public TwistSwing GetOrientedSpaceRotation(RigidBoneSystemInputs inputs)
{
TwistSwing rotationTwistSwing = inputs.Rotations[Index];
rotationTwistSwing = Constraint.Clamp(rotationTwistSwing);
return(rotationTwistSwing);
}
public void TestAsQuaternion()
{
var twist = new Twist(0.4f);
var swing = new Swing(0.2f, 0.3f);
var twistSwing = new TwistSwing(twist, swing);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
}
public RigidBoneSystemInputs CalculateDeltas(RigidBoneSystemInputs baseInputs, RigidBoneSystemInputs sumInputs)
{
var deltaInputs = new RigidBoneSystemInputs(bones.Length)
{
};
RigidTransform baseRootTransform = RigidTransform.FromRotationTranslation(
RootBone.GetRotation(baseInputs),
baseInputs.RootTranslation);
RigidTransform sumRootTransform = RigidTransform.FromRotationTranslation(
RootBone.GetRotation(sumInputs),
sumInputs.RootTranslation);
RigidTransform deltaRootTransform = sumRootTransform.Chain(baseRootTransform.Invert());
deltaInputs.RootTranslation = deltaRootTransform.Translation;
RootBone.SetRotation(deltaInputs, deltaRootTransform.Rotation);
for (int boneIdx = 1; boneIdx < bones.Length; ++boneIdx)
{
var bone = bones[boneIdx];
deltaInputs.Rotations[boneIdx] = TwistSwing.CalculateDelta(baseInputs.Rotations[boneIdx], sumInputs.Rotations[boneIdx]);
}
return(deltaInputs);
}
private void TestClampRotation(Vector3 input, Vector3 expected)
{
var inputTS = new TwistSwing(Twist.MakeFromAngle(input.X), Swing.MakeFromAxisAngleProduct(input.Y, input.Z));
var clampedTS = constraint.Clamp(inputTS);
var expectedTS = new TwistSwing(Twist.MakeFromAngle(expected.X), Swing.MakeFromAxisAngleProduct(expected.Y, expected.Z));
MathAssert.AreEqual(expectedTS, clampedTS, 1e-4f);
}
public void TestDecomposeHalfRevolution()
{
var q = new Quaternion(1, 0, 0, 0);
TwistSwing twistSwing = TwistSwing.Decompose(CartesianAxis.Z, q);
Assert.AreEqual(0, twistSwing.Twist.X, Acc);
Assert.AreEqual(1, twistSwing.Swing.Y, Acc);
Assert.AreEqual(0, twistSwing.Swing.Z, Acc);
}
private void TestToTwistSwingAngles(RotationOrder order)
{
MathAssert.AreEqual(new Vector3(0, 0, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.Identity)), Acc);
MathAssert.AreEqual(new Vector3(+1, 0, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitX, +1))), Acc);
MathAssert.AreEqual(new Vector3(-1, 0, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitX, -1))), Acc);
MathAssert.AreEqual(new Vector3(0, +1, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitY, +1))), Acc);
MathAssert.AreEqual(new Vector3(0, -1, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitY, -1))), Acc);
MathAssert.AreEqual(new Vector3(0, 0, +1), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitZ, +1))), Acc);
MathAssert.AreEqual(new Vector3(0, 0, -1), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitZ, -1))), Acc);
}
public void TestCenter()
{
var constraint = new TwistSwingConstraint(
new TwistConstraint(-0.1f, 0.3f),
new SwingConstraint(
-0.2f, 0.6f,
-0.4f, 1.0f));
MathAssert.AreEqual(TwistSwing.MakeFromCoordinates(0.1f, 0.2f, 0.3f), constraint.Center, 1e-4f);
}
/**
* Set only the twist component of a bone's rotation. The original swing is preserved.
*/
public static void SetTwistOnly(this RigidBone bone, RigidBoneSystemInputs inputs, Quaternion localRotation)
{
var orientedRotation = bone.OrientationSpace.TransformToOrientedSpace(localRotation);
TwistSwing twistSwing = TwistSwing.Decompose(bone.RotationOrder.TwistAxis, orientedRotation);
var originalTwistSwing = inputs.Rotations[bone.Index];
var twistWithOriginalSwing = new TwistSwing(
twistSwing.Twist,
originalTwistSwing.Swing);
bone.SetOrientedSpaceRotation(inputs, twistWithOriginalSwing, true);
}
public void SetOrientedSpaceRotation(RigidBoneSystemInputs inputs, TwistSwing orientatedSpaceRotation, bool applyClamp = false)
{
DebugUtilities.AssertFinite(orientatedSpaceRotation);
if (applyClamp)
{
orientatedSpaceRotation = Constraint.Clamp(orientatedSpaceRotation);
}
inputs.Rotations[Index] = orientatedSpaceRotation;
}
public void TestDecompose()
{
var q = Quaternion.Normalize(new Quaternion(0.1f, 0.2f, 0.3f, 0.4f));
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.X, q).AsQuaternion(CartesianAxis.X), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.Y, q).AsQuaternion(CartesianAxis.Y), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.Z, q).AsQuaternion(CartesianAxis.Z), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.X, -q).AsQuaternion(CartesianAxis.X), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.Y, -q).AsQuaternion(CartesianAxis.Y), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.Z, -q).AsQuaternion(CartesianAxis.Z), Acc);
}
public Vector3 ToTwistSwingAngles(TwistSwing twistSwing)
{
Vector2 swingAxisAngleProduct = twistSwing.Swing.AxisAngleProduct;
Vector3 angles = default(Vector3);
angles[primaryAxis] = twistSwing.Twist.Angle;
angles[(primaryAxis + 1) % 3] = swingAxisAngleProduct.X;
angles[(primaryAxis + 2) % 3] = swingAxisAngleProduct.Y;
return(angles);
}
public Quaternion GetRotation(ChannelOutputs outputs)
{
OrientationSpace orientationSpace = GetOrientationSpace(outputs);
Vector3 rotationAngles = Rotation.GetValue(outputs);
TwistSwing rotationTwistSwing = RotationOrder.FromTwistSwingAngles(MathExtensions.DegreesToRadians(rotationAngles));
rotationTwistSwing = RotationConstraint.Clamp(rotationTwistSwing);
Quaternion orientedSpaceRotation = rotationTwistSwing.AsQuaternion(RotationOrder.TwistAxis);
Quaternion worldSpaceRotation = orientationSpace.TransformFromOrientedSpace(orientedSpaceRotation);
return(worldSpaceRotation);
}
public Vector3 ConvertRotationToAngles(ChannelOutputs orientationOutputs, Quaternion objectSpaceRotation, bool applyClamp)
{
OrientationSpace orientationSpace = GetOrientationSpace(orientationOutputs);
Quaternion orientatedSpaceRotationQ = orientationSpace.TransformToOrientedSpace(objectSpaceRotation);
TwistSwing orientatedSpaceRotation = TwistSwing.Decompose(RotationOrder.TwistAxis, orientatedSpaceRotationQ);
if (applyClamp)
{
orientatedSpaceRotation = RotationConstraint.Clamp(orientatedSpaceRotation);
}
Vector3 rotationAnglesRadians = RotationOrder.ToTwistSwingAngles(orientatedSpaceRotation);
Vector3 rotationAnglesDegrees = MathExtensions.RadiansToDegrees(rotationAnglesRadians);
return(rotationAnglesDegrees);
}
private void ApplyPartialSolution(RigidBone bone, BonePartialSolution partialSolution, RigidTransform[] boneTransforms, Vector3 figureCenterOverride, RigidBoneSystemInputs inputs, float time)
{
var twistAxis = bone.RotationOrder.TwistAxis;
var originalRotationQ = inputs.Rotations[bone.Index].AsQuaternion(twistAxis);
var rotationDelta = QuaternionExtensions.FromRotationVector(time * partialSolution.angularVelocity);
var newRotationQ = originalRotationQ.Chain(rotationDelta);
var newRotation = TwistSwing.Decompose(twistAxis, newRotationQ);
inputs.Rotations[bone.Index] = bone.Constraint.Clamp(newRotation);
if (bone == boneSystem.RootBone)
{
var preTotalTransform = boneTransforms[bone.Index];
var postTotalTransform = bone.GetChainedTransform(inputs);
var unposedFigureCenterOverride = preTotalTransform.InverseTransform(figureCenterOverride);
var postFigureCenterOverride = postTotalTransform.Transform(unposedFigureCenterOverride);
var centerDisplacement = figureCenterOverride - postFigureCenterOverride;
inputs.RootTranslation += centerDisplacement;
}
}
public static TwistSwing ApplyDelta(TwistSwing initial, TwistSwing delta)
{
return(new TwistSwing(
Twist.ApplyDelta(initial.Twist, delta.Twist),
Swing.ApplyDelta(initial.Swing, delta.Swing)));
}
public static void AssertFinite(TwistSwing ts)
{
AssertFinite(ts.Twist);
AssertFinite(ts.Swing);
}
public static void AssertSame(TwistSwing ts1, TwistSwing ts2)
{
AssertSame(ts1.Twist, ts2.Twist);
AssertSame(ts1.Swing, ts2.Swing);
}
private void TestTwistSwingRoundTrip(RotationOrder order, Quaternion q)
{
var ts = TwistSwing.Decompose(order.TwistAxis, q);
MathAssert.AreEqual(q, order.FromTwistSwingAngles(order.ToTwistSwingAngles(ts)).AsQuaternion(order.TwistAxis), Acc);
}
public bool Test(TwistSwing twistSwing)
{
return(Twist.Test(twistSwing.Twist) && Swing.Test(twistSwing.Swing));
}
public static void AreEqual(TwistSwing expected, TwistSwing actual, float delta)
{
MathAssert.AreEqual(expected.Twist, actual.Twist, delta);
MathAssert.AreEqual(expected.Swing, actual.Swing, delta);
}
public static TwistSwing CalculateDelta(TwistSwing initial, TwistSwing final)
{
return(new TwistSwing(
Twist.CalculateDelta(initial.Twist, final.Twist),
Swing.CalculateDelta(initial.Swing, final.Swing)));
}
