private void MaybeStartTracking(FrameUpdateParameters updateParameters, RigidBoneSystemInputs inputs, ControlVertexInfo[] previousFrameControlVertexInfos)
{
if (tracking == true)
{
//already tracking
return;
}
if (!stateTracker.NonMenuActive)
{
return;
}
bool triggerPressed = stateTracker.IsPressed(EVRButtonId.k_EButton_SteamVR_Trigger);
if (!triggerPressed)
{
return;
}
tracking = true;
TrackedDevicePose_t gamePose = updateParameters.GamePoses[stateTracker.DeviceIdx];
Matrix controllerTransform = gamePose.mDeviceToAbsoluteTracking.Convert();
DualQuaternion controllerTransformDq = DualQuaternion.FromMatrix(controllerTransform);
var worldSourcePosition = controllerTransformDq.Translation * 100;
var worldSourceOrientation = controllerTransformDq.Rotation;
sourceBone = parentInstance.MapPositionToBone(worldSourcePosition, previousFrameControlVertexInfos);
var inverseSourceBoneTotalTransform = sourceBone.GetChainedTransform(inputs).Invert();
boneRelativeSourcePosition = inverseSourceBoneTotalTransform.Transform(worldSourcePosition) - sourceBone.CenterPoint;
boneRelativeSourceOrientation = worldSourceOrientation.Chain(inverseSourceBoneTotalTransform.Rotation);
}
public RigidTransform GetChainedTransform(RigidBoneSystemInputs inputs)
{
RigidTransform rootTransform = RigidTransform.FromTranslation(inputs.RootTranslation);
RigidTransform parentTransform = Parent != null?Parent.GetChainedTransform(inputs) : rootTransform;
return(GetChainedTransform(inputs, parentTransform));
}
public InverseKinematicsGoal GetGoal(FrameUpdateParameters updateParameters, RigidBoneSystemInputs inputs, ControlVertexInfo[] previousFrameControlVertexInfos)
{
MaybeStartTracking(updateParameters, inputs, previousFrameControlVertexInfos);
var goal = MaybeContinueTracking(updateParameters);
return(goal);
}
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);
}
private InverseKinematicsGoal MakeKeepFootInPlaceGoal(RigidBoneSystemInputs inputs)
{
return(new InverseKinematicsGoal(
boneSystem.BonesByName["lShin"],
boneSystem.BonesByName["lFoot"].CenterPoint - boneSystem.BonesByName["lShin"].CenterPoint,
boneSystem.BonesByName["lFoot"].GetChainedTransform(inputs).Transform(boneSystem.BonesByName["lFoot"].CenterPoint)));
}
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 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 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);
}
private void ApplyOrientationGoal(InverseKinematicsGoal goal, RigidBoneSystemInputs inputs)
{
if (!areOrientable[goal.SourceBone.Index] || !goal.HasOrientation)
{
return;
}
var bone = goal.SourceBone;
var parentBone = bone.Parent;
var grandparentBone = parentBone.Parent;
var grandparentTotalTransform = grandparentBone.GetChainedTransform(inputs);
var parentTotalTransform = parentBone.GetChainedTransform(inputs, grandparentTotalTransform);
var goalTotalRotation = Quaternion.Invert(goal.UnposedSourceOrientation).Chain(goal.TargetOrientation);
var newLocalRotation = goalTotalRotation.Chain(Quaternion.Invert(parentTotalTransform.Rotation));
bone.SetRotation(inputs, newLocalRotation, true);
var clampedNewLocalRotation = bone.GetRotation(inputs);
var residualGoalTotalRotation = Quaternion.Invert(clampedNewLocalRotation).Chain(goalTotalRotation);
var parentNewLocalRotation = residualGoalTotalRotation.Chain(Quaternion.Invert(grandparentTotalTransform.Rotation));
parentBone.SetTwistOnly(inputs, parentNewLocalRotation);
}
public Quaternion GetRotation(RigidBoneSystemInputs inputs)
{
Quaternion orientedSpaceRotation = GetOrientedSpaceRotation(inputs).AsQuaternion(RotationOrder.TwistAxis);
Quaternion objectSpaceRotation = orientationSpace.TransformFromOrientedSpace(orientedSpaceRotation);
return(objectSpaceRotation);
}
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);
}
public TwistSwing GetOrientedSpaceRotation(RigidBoneSystemInputs inputs)
{
TwistSwing rotationTwistSwing = inputs.Rotations[Index];
rotationTwistSwing = Constraint.Clamp(rotationTwistSwing);
return(rotationTwistSwing);
}
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);
}
}
}
public RigidTransform GetChainedTransform(RigidBoneSystemInputs inputs, RigidTransform parentTransform)
{
RigidTransform rotationTransform = GetObjectCenteredRotationTransform(inputs);
RigidTransform chainedRotationTransform = rotationTransform.Chain(parentTransform);
return(chainedRotationTransform);
}
private void Trial()
{
var inputs = new RigidBoneSystemInputs(initialInputs);
var frameUpdateParameters = new FrameUpdateParameters(0, 1 / 90f, null, Vector3.Zero);
var goals = goalProvider.GetGoals(frameUpdateParameters, initialInputs, null);
solver.Solve(rigidBoneSystem, goals, inputs);
}
private BonePartialSolution SolveSingleBone(
RigidBone bone,
Vector3 worldSource, Vector3 worldTarget, MassMoment[] massMoments, Vector3 figureCenterOverride,
RigidBoneSystemInputs inputs, RigidTransform[] boneTransforms)
{
if (bone.Parent == boneSystem.RootBone)
{
//skip the hip bone because it's the same as the root bone but with a different center
return(BonePartialSolution.Zero);
}
var center = bone != boneSystem.RootBone ? boneTransforms[bone.Index].Transform(bone.CenterPoint) : figureCenterOverride;
var parentTotalRotation = bone.Parent != null ? boneTransforms[bone.Parent.Index].Rotation : Quaternion.Identity;
var boneToWorldSpaceRotation = bone.OrientationSpace.Orientation.Chain(parentTotalRotation);
var worldToBoneSpaceRotation = Quaternion.Invert(boneToWorldSpaceRotation);
var boneSpaceSource = Vector3.Transform(worldSource - center, worldToBoneSpaceRotation);
var boneSpaceTarget = Vector3.Transform(worldTarget - center, worldToBoneSpaceRotation);
var force = boneSpaceTarget - boneSpaceSource;
var torque = Vector3.Cross(boneSpaceSource, force);
float mass = boneAttributes[bone.Index].MassIncludingDescendants;
Vector3 unnormalizedAxisOfRotation = Vector3.Cross(worldSource - center, worldTarget - center);
float unnormalizedAxisOfRotationLength = unnormalizedAxisOfRotation.Length();
if (MathUtil.IsZero(unnormalizedAxisOfRotationLength))
{
return(BonePartialSolution.Zero);
}
Vector3 axisOfRotation = unnormalizedAxisOfRotation / unnormalizedAxisOfRotationLength;
float momentOfInertia = massMoments[bone.Index].GetMomentOfInertia(axisOfRotation, center);
var angularVelocity = torque / momentOfInertia;
var twistAxis = bone.RotationOrder.TwistAxis;
var existingRotation = bone.GetOrientedSpaceRotation(inputs).AsQuaternion(twistAxis);
var relaxedRotation = bone.Constraint.Center.AsQuaternion(twistAxis);
float relaxationBias = InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, existingRotation, angularVelocity);
angularVelocity *= relaxationBias;
var linearVelocity = Vector3.Cross(angularVelocity, boneSpaceSource);
var rotation = QuaternionExtensions.RotateBetween(
Vector3.Normalize(boneSpaceSource),
Vector3.Normalize(boneSpaceTarget));
var radius = boneSpaceSource.Length();
var distance = rotation.AccurateAngle() * radius;
float time = distance == 0 ? 0 : distance / linearVelocity.Length();
DebugUtilities.AssertFinite(angularVelocity);
return(new BonePartialSolution {
angularVelocity = angularVelocity,
time = time
});
}
private InverseKinematicsGoal MakeMoveHandDownGoal(RigidBoneSystemInputs inputs)
{
var forearmBone = boneSystem.BonesByName["lForearmBend"];
var handBone = boneSystem.BonesByName["lHand"];
return(new InverseKinematicsGoal(
forearmBone,
handBone.CenterPoint - forearmBone.CenterPoint,
forearmBone.CenterPoint + Vector3.Down * Vector3.Distance(handBone.CenterPoint, forearmBone.CenterPoint)));
}
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();
}
public void SetOrientedSpaceRotation(RigidBoneSystemInputs inputs, TwistSwing orientatedSpaceRotation, bool applyClamp = false)
{
DebugUtilities.AssertFinite(orientatedSpaceRotation);
if (applyClamp)
{
orientatedSpaceRotation = Constraint.Clamp(orientatedSpaceRotation);
}
inputs.Rotations[Index] = orientatedSpaceRotation;
}
/**
* 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);
}
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 void Update(FrameUpdateParameters updateParameters, ChannelInputs channelInputs, ControlVertexInfo[] previousFrameControlVertexInfos)
{
var channelOutputs = channelSystem.Evaluate(null, channelInputs);
boneSystem.Synchronize(channelOutputs);
var baseInputs = boneSystem.ReadInputs(channelOutputs);
var resultInputs = boneSystem.ApplyDeltas(baseInputs, poseDeltas);
List <InverseKinematicsGoal> goals = goalProvider.GetGoals(updateParameters, resultInputs, previousFrameControlVertexInfos);
solver.Solve(boneSystem, goals, resultInputs);
poseDeltas = boneSystem.CalculateDeltas(baseInputs, resultInputs);
boneSystem.WriteInputs(channelInputs, channelOutputs, resultInputs);
}
public RigidTransform[] GetBoneTransforms(RigidBoneSystemInputs inputs)
{
RigidTransform[] boneTransforms = new RigidTransform[bones.Length];
RigidTransform rootTransform = RigidTransform.FromTranslation(inputs.RootTranslation);
for (int boneIdx = 0; boneIdx < bones.Length; ++boneIdx)
{
RigidBone bone = bones[boneIdx];
RigidBone parent = bone.Parent;
RigidTransform parentTransform = parent != null ? boneTransforms[parent.Source.Index] : rootTransform;
boneTransforms[boneIdx] = bone.GetChainedTransform(inputs, parentTransform);
}
return(boneTransforms);
}
public RigidBoneSystemInputs ReadInputs(ChannelOutputs channelOutputs)
{
var inputs = new RigidBoneSystemInputs(bones.Length)
{
};
inputs.RootTranslation = source.RootBone.Translation.GetValue(channelOutputs);
for (int boneIdx = 0; boneIdx < bones.Length; ++boneIdx)
{
var bone = bones[boneIdx];
var rotationAngles = bone.Source.Rotation.GetValue(channelOutputs);
var rotation = bone.RotationOrder.FromTwistSwingAngles(MathExtensions.DegreesToRadians(rotationAngles));
inputs.Rotations[boneIdx] = rotation;
}
return(inputs);
}
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);
}
private void ApplyCorrection(RigidBoneSystemInputs inputs, RigidTransform[] boneTransforms, RigidBone bone, ref Vector3 sourcePosition, Vector3 targetPosition, float weight)
{
var centerPosition = GetCenterPosition(boneTransforms, bone);
var rotationCorrection = QuaternionExtensions.RotateBetween(
sourcePosition - centerPosition,
targetPosition - centerPosition);
var boneTransform = boneTransforms[bone.Index];
var baseLocalRotation = bone.GetRotation(inputs);
var localRotationCorrection = Quaternion.Invert(boneTransform.Rotation) * rotationCorrection * boneTransform.Rotation;
var lerpedRotation = Quaternion.Lerp(
baseLocalRotation,
baseLocalRotation * localRotationCorrection,
weight);
bone.SetRotation(inputs, lerpedRotation, true);
var newBoneTransform = bone.GetChainedTransform(inputs, bone.Parent != null ? boneTransforms[bone.Parent.Index] : RigidTransform.Identity);
var newSourcePosition = newBoneTransform.Transform(boneTransform.InverseTransform(sourcePosition));
sourcePosition = newSourcePosition;
}
public void Solve(RigidBoneSystem boneSystem, List <InverseKinematicsGoal> goals, RigidBoneSystemInputs inputs)
{
foreach (var goal in goals)
{
Solve(boneSystem, goal, inputs);
}
}
public void WriteInputs(ChannelInputs channelInputs, ChannelOutputs channelOutputs, RigidBoneSystemInputs inputs)
{
source.RootBone.Translation.SetEffectiveValue(channelInputs, channelOutputs, inputs.RootTranslation, SetMask.ApplyClampAndVisibleOnly);
for (int boneIdx = 0; boneIdx < bones.Length; ++boneIdx)
{
var bone = bones[boneIdx];
var rotation = inputs.Rotations[boneIdx];
var rotationAngles = MathExtensions.RadiansToDegrees(bone.RotationOrder.ToTwistSwingAngles(rotation));
bone.Source.Rotation.SetEffectiveValue(channelInputs, channelOutputs, rotationAngles, SetMask.ApplyClampAndVisibleOnly);
}
}
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 List <InverseKinematicsGoal> GetGoals(FrameUpdateParameters updateParameters, RigidBoneSystemInputs inputs, ControlVertexInfo[] previousFrameControlVertexInfos)
{
return(new List <InverseKinematicsGoal> {
MakeMoveHandDownGoal(inputs),
//MakeKeepFootInPlaceGoal(inputs)
});
}