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 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);
}
public RigidTransform GetOriginToBindPoseTransform(ChannelOutputs outputs)
{
Vector3 orientationAngles = Orientation.GetValue(outputs);
Vector3 centerPoint = CenterPoint.GetValue(outputs);
Quaternion orientation = RotationOrder.DazStandard.FromEulerAngles(MathExtensions.DegreesToRadians(orientationAngles));
return(RigidTransform.FromRotationTranslation(orientation, centerPoint));
}
public void TestFromAndToRotationTranslation()
{
Quaternion rotation = Quaternion.RotationYawPitchRoll(1, 2, 3);
Vector3 translation = new Vector3(2, 3, 4);
RigidTransform transform = RigidTransform.FromRotationTranslation(rotation, translation);
Assert.AreEqual(0, Vector3.Distance(translation, transform.Translation), 1e-6);
Assert.IsTrue(transform.Rotation == rotation || transform.Rotation == -rotation);
}
public void TestTransform()
{
Vector3 translation = new Vector3(2, 3, 4);
Quaternion rotation = Quaternion.RotationYawPitchRoll(1, 2, 3);
Matrix matrix = Matrix.RotationQuaternion(rotation) * Matrix.Translation(translation);
RigidTransform transform = RigidTransform.FromRotationTranslation(rotation, translation);
Vector3 v = new Vector3(3, 4, 5);
Assert.IsTrue(transform.Transform(v) == Vector3.TransformCoordinate(v, matrix));
}
public void TestInvert()
{
Vector3 translation = new Vector3(2, 3, 4);
Quaternion rotation = Quaternion.RotationYawPitchRoll(1, 2, 3);
RigidTransform transform = RigidTransform.FromRotationTranslation(rotation, translation);
Vector3 v = new Vector3(3, 4, 5);
Vector3 transformed = transform.Transform(v);
Vector3 inverseTransformed = transform.Invert().Transform(transformed);
Assert.AreEqual(
v,
inverseTransformed);
}