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