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 GetChainedTransform(RigidBoneSystemInputs inputs, RigidTransform parentTransform)
{
RigidTransform rotationTransform = GetObjectCenteredRotationTransform(inputs);
RigidTransform chainedRotationTransform = rotationTransform.Chain(parentTransform);
return(chainedRotationTransform);
}
public void TestChain()
{
var rnd = new Random(0);
RigidTransform transform1 = RandomUtil.RigidTransform(rnd);
RigidTransform transform2 = RandomUtil.RigidTransform(rnd);
var testPoint = RandomUtil.Vector3(rnd);
MathAssert.AreEqual(
transform2.Transform(transform1.Transform(testPoint)),
transform1.Chain(transform2).Transform(testPoint),
Acc);
}