private void ApplyPartialSolution(RigidBone bone, BonePartialSolution partialSolution, RigidTransform[] boneTransforms, Vector3 figureCenterOverride, RigidBoneSystemInputs inputs, float time)
{
var twistAxis = bone.RotationOrder.TwistAxis;
var originalRotationQ = inputs.Rotations[bone.Index].AsQuaternion(twistAxis);
var rotationDelta = QuaternionExtensions.FromRotationVector(time * partialSolution.angularVelocity);
var newRotationQ = originalRotationQ.Chain(rotationDelta);
var newRotation = TwistSwing.Decompose(twistAxis, newRotationQ);
inputs.Rotations[bone.Index] = bone.Constraint.Clamp(newRotation);
if (bone == boneSystem.RootBone)
{
var preTotalTransform = boneTransforms[bone.Index];
var postTotalTransform = bone.GetChainedTransform(inputs);
var unposedFigureCenterOverride = preTotalTransform.InverseTransform(figureCenterOverride);
var postFigureCenterOverride = postTotalTransform.Transform(unposedFigureCenterOverride);
var centerDisplacement = figureCenterOverride - postFigureCenterOverride;
inputs.RootTranslation += centerDisplacement;
}
}
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);
}
