public void TestTransformConsistency()
{
var builder = new BoneSystemBuilder();
var bone0 = builder.AddBone("bone0", null, new Vector3(1, 0, 0), new Vector3(2, 0, 0), Vector3.Zero);
var bone1 = builder.AddBone("bone1", bone0, new Vector3(2, 0, 0), new Vector3(3, 0, 0), Vector3.Zero);
var bone2 = builder.AddBone("bone2", bone1, new Vector3(3, 0, 0), new Vector3(4, 0, 0), Vector3.Zero);
var channelSystem = builder.BuildChannelSystem();
var boneSystem = builder.BuildBoneSystem();
var rigidBoneSystem = new RigidBoneSystem(boneSystem);
var baseInputs = channelSystem.MakeDefaultChannelInputs();
bone1.Scale.SetValue(baseInputs, new Vector3(2, 3, 4));
bone1.Translation.SetValue(baseInputs, new Vector3(4, 5, 6));
bone2.Translation.SetValue(baseInputs, new Vector3(5, 6, 7));
var baseOutputs = channelSystem.Evaluate(null, baseInputs);
rigidBoneSystem.Synchronize(baseOutputs);
var rigidBaseInputs = rigidBoneSystem.ReadInputs(baseOutputs);
var rigidInputs = new RigidBoneSystemInputs(rigidBaseInputs);
rigidBoneSystem.Bones[0].SetRotation(rigidInputs, Quaternion.RotationYawPitchRoll(0.1f, 0.2f, 0.3f));
rigidBoneSystem.Bones[1].SetRotation(rigidInputs, Quaternion.RotationYawPitchRoll(0.2f, 0.3f, 0.4f));
rigidBoneSystem.Bones[2].SetRotation(rigidInputs, Quaternion.RotationYawPitchRoll(0.3f, 0.4f, 0.5f));
var inputs = new ChannelInputs(baseInputs);
rigidBoneSystem.WriteInputs(inputs, baseOutputs, rigidInputs);
var outputs = channelSystem.Evaluate(null, inputs);
var baseTransforms = boneSystem.GetBoneTransforms(baseOutputs);
var transforms = boneSystem.GetBoneTransforms(outputs);
var rigidBaseTransforms = rigidBoneSystem.GetBoneTransforms(rigidBaseInputs);
var rigidTransforms = rigidBoneSystem.GetBoneTransforms(rigidInputs);
for (int transformIdx = 0; transformIdx < transforms.Length; ++transformIdx)
{
var baseTransform = baseTransforms[transformIdx];
var transform = transforms[transformIdx];
var rigidBaseTransform = rigidBaseTransforms[transformIdx];
var rigidTransform = rigidTransforms[transformIdx];
foreach (var testPoint in new [] { Vector3.Zero, Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ })
{
var unposedPoint = baseTransform.InverseTransform(testPoint);
var posedPoint = transform.Transform(unposedPoint);
var unposedRigidPoint = rigidBaseTransform.InverseTransform(testPoint);
var posedRigidPoint = rigidTransform.Transform(unposedRigidPoint);
float distance = Vector3.Distance(posedPoint, posedRigidPoint);
Assert.AreEqual(0, distance, 1e-3);
}
}
}
private void CheckConsistency(ChannelOutputs channelOutputs)
{
var inputs = rigidBoneSystem.ReadInputs(channelOutputs);
var boneTransformsA = boneSystem.GetBoneTransforms(channelOutputs);
var boneTransformsB = rigidBoneSystem.GetBoneTransforms(inputs);
for (int i = 0; i < boneSystem.Bones.Count; ++i)
{
var boneTransformA = boneTransformsA[i];
var boneTransformB = boneTransformsB[i];
var unposedCenterA = boneSystem.Bones[i].CenterPoint.GetValue(channelOutputs);
var unposedCenterB = rigidBoneSystem.Bones[i].CenterPoint;
foreach (var testVector in new Vector3[] { Vector3.Zero, Vector3.Right, Vector3.Up, Vector3.BackwardRH })
{
var transformedVectorA = boneTransformA.Transform(unposedCenterA + testVector);
var transformedVectorB = boneTransformB.Transform(
unposedCenterB + boneTransformA.ScalingStage.Transform(testVector));
float distance = Vector3.Distance(transformedVectorA, transformedVectorB);
if (distance > 1e-3)
{
throw new Exception("rigid and non-rigid bone transforms are inconsistent");
}
}
}
}
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);
}
private void DoIteration(RigidBoneSystem boneSystem, InverseKinematicsGoal goal, RigidBoneSystemInputs inputs)
{
var boneTransforms = boneSystem.GetBoneTransforms(inputs);
var sourcePosition = boneTransforms[goal.SourceBone.Index].Transform(goal.SourceBone.CenterPoint + goal.UnposedSourcePosition);
float weight = 0.5f;
for (var bone = goal.SourceBone; bone != boneSystem.RootBone && bone.Parent != boneSystem.RootBone; bone = bone.Parent)
{
ApplyCorrection(inputs, boneTransforms, bone, ref sourcePosition, goal.TargetPosition, weight);
}
}
private void ApplyPositionGoal(InverseKinematicsGoal goal, RigidBoneSystemInputs inputs)
{
var boneTransforms = boneSystem.GetBoneTransforms(inputs);
var centersOfMass = GetCentersOfMass(boneTransforms);
var sourcePosition = boneTransforms[goal.SourceBone.Index].Transform(goal.SourceBone.CenterPoint + goal.UnposedSourcePosition);
var bones = GetBoneChain(goal.SourceBone, goal.HasOrientation).ToArray();
//var bones = new RigidBone[] { boneSystem.BonesByName["lForearmBend"], boneSystem.BonesByName["lShldrBend"] };
var massMoments = GetMassMoments(boneTransforms, bones);
var figureCenterOverride = massMoments[0].GetCenterOfMass();
float totalRate = 0;
var bonePartialSolutions = new BonePartialSolution[bones.Length];
for (int i = 0; i < bones.Length; ++i)
{
var partialSolution = SolveSingleBone(bones[i], sourcePosition, goal.TargetPosition, massMoments, figureCenterOverride, inputs, boneTransforms);
bonePartialSolutions[i] = partialSolution;
totalRate += 1 / partialSolution.time;
}
var rootTranslationPartialSolution = SolveRootTranslation(sourcePosition, goal.TargetPosition);
totalRate += 1 / rootTranslationPartialSolution.time;
float time = 1 / totalRate;
for (int i = 0; i < bones.Length; ++i)
{
ApplyPartialSolution(bones[i], bonePartialSolutions[i], boneTransforms, figureCenterOverride, inputs, time);
}
ApplyPartialSolution(rootTranslationPartialSolution, inputs, time);
CountertransformOffChainBones(boneTransforms, centersOfMass, inputs, bones);
}