public InverseKinematicsPerformanceDemo()
{
var figureDir = UnpackedArchiveDirectory.Make(new System.IO.DirectoryInfo("work/figures/genesis-3-female"));
var channelSystemRecipe = Persistance.Load <ChannelSystemRecipe>(figureDir.File("channel-system-recipe.dat"));
channelSystem = channelSystemRecipe.Bake(null);
var boneSystemRecipe = Persistance.Load <BoneSystemRecipe>(figureDir.File("bone-system-recipe.dat"));
boneSystem = boneSystemRecipe.Bake(channelSystem.ChannelsByName);
var inverterParameters = Persistance.Load <InverterParameters>(figureDir.File("inverter-parameters.dat"));
rigidBoneSystem = new RigidBoneSystem(boneSystem);
goalProvider = new DemoInverseKinematicsGoalProvider(rigidBoneSystem);
solver = new HarmonicInverseKinematicsSolver(rigidBoneSystem, inverterParameters.BoneAttributes);
var pose = Persistance.Load <List <Pose> >(figureDir.File("animations/idle.dat"))[0];
var channelInputs = channelSystem.MakeDefaultChannelInputs();
new Poser(channelSystem, boneSystem).Apply(channelInputs, pose, DualQuaternion.Identity);
var channelOutputs = channelSystem.Evaluate(null, channelInputs);
rigidBoneSystem.Synchronize(channelOutputs);
initialInputs = rigidBoneSystem.ReadInputs(channelOutputs);
}
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);
}
public void Solve(RigidBoneSystem boneSystem, List <InverseKinematicsGoal> goals, RigidBoneSystemInputs inputs)
{
foreach (var goal in goals)
{
Solve(boneSystem, goal, inputs);
}
}
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 static bool[] MakeAreOrientable(RigidBoneSystem boneSystem)
{
bool[] areOrientable = new bool[boneSystem.Bones.Length];
areOrientable[boneSystem.BonesByName["lHand"].Index] = true;
areOrientable[boneSystem.BonesByName["rHand"].Index] = true;
areOrientable[boneSystem.BonesByName["lFoot"].Index] = true;
areOrientable[boneSystem.BonesByName["rFoot"].Index] = true;
return(areOrientable);
}
public void Solve(RigidBoneSystem boneSystem, List <InverseKinematicsGoal> goals, RigidBoneSystemInputs inputs)
{
for (int i = 0; i < 1; ++i)
{
foreach (var goal in goals)
{
DoIteration(boneSystem, goal, inputs);
}
}
}
public InverseKinematicsUserInterface(ControllerManager controllerManager, ChannelSystem channelSystem, RigidBoneSystem boneSystem, InverterParameters inverterParameters)
{
this.channelSystem = channelSystem;
this.boneSystem = boneSystem;
this.inverterParameters = inverterParameters;
deviceTrackers = controllerManager.StateTrackers
.Select(stateTracker => new DeviceTracker(this, stateTracker))
.ToArray();
}
public InverseKinematicsAnimator(ControllerManager controllerManager, FigureDefinition definition, InverterParameters inverterParameters)
{
channelSystem = definition.ChannelSystem;
boneSystem = new RigidBoneSystem(definition.BoneSystem);
goalProvider = new InverseKinematicsUserInterface(controllerManager, channelSystem, boneSystem, inverterParameters);
//goalProvider = new DemoInverseKinematicsGoalProvider(boneSystem);
solver = new HarmonicInverseKinematicsSolver(boneSystem, inverterParameters.BoneAttributes);
poseDeltas = boneSystem.MakeZeroInputs();
Reset();
}
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);
}
}
public RigidBoneSystemPerformanceDemo()
{
var figureDir = UnpackedArchiveDirectory.Make(new System.IO.DirectoryInfo("work/figures/genesis-3-female"));
var channelSystemRecipe = Persistance.Load <ChannelSystemRecipe>(figureDir.File("channel-system-recipe.dat"));
channelSystem = channelSystemRecipe.Bake(null);
var boneSystemRecipe = Persistance.Load <BoneSystemRecipe>(figureDir.File("bone-system-recipe.dat"));
boneSystem = boneSystemRecipe.Bake(channelSystem.ChannelsByName);
var pose = Persistance.Load <List <Pose> >(figureDir.File("animations/idle.dat"))[0];
channelInputs = channelSystem.MakeDefaultChannelInputs();
new Poser(channelSystem, boneSystem).Apply(channelInputs, pose, DualQuaternion.Identity);
rigidBoneSystem = new RigidBoneSystem(boneSystem);
}
public DemoInverseKinematicsGoalProvider(RigidBoneSystem boneSystem)
{
this.boneSystem = boneSystem;
}
public HarmonicInverseKinematicsSolver(RigidBoneSystem boneSystem, BoneAttributes[] boneAttributes)
{
this.boneSystem = boneSystem;
this.boneAttributes = boneAttributes;
areOrientable = MakeAreOrientable(boneSystem);
}