/// <summary>
/// Constructs a new IKSolver.
/// </summary>
public IKSolver()
{
ActiveSet = new ActiveSet();
ControlIterationCount = 50;
FixerIterationCount = 70;
VelocitySubiterationCount = 2;
}
/// <summary>
/// Updates the positions of bones acted upon by the joints given to this solver.
/// This variant of the solver can be used when there are no goal-driven controls in the simulation.
/// This amounts to running just the 'fixer iterations' of a normal control-driven solve.
/// </summary>
public void Solve(List <IKJoint> joints)
{
ActiveSet.UpdateActiveSet(joints);
//Reset the permutation index; every solve should proceed in exactly the same order.
permutationMapper.PermutationIndex = 0;
float updateRate = 1 / TimeStepDuration;
foreach (var joint in ActiveSet.joints)
{
joint.Preupdate(TimeStepDuration, updateRate);
}
for (int i = 0; i < FixerIterationCount; i++)
{
//Update the world inertia tensors of objects for the latest position.
foreach (Bone bone in ActiveSet.bones)
{
bone.UpdateInertiaTensor();
}
//Update the per-constraint jacobians and effective mass for the current bone orientations and positions.
foreach (IKJoint joint in ActiveSet.joints)
{
joint.UpdateJacobiansAndVelocityBias();
joint.ComputeEffectiveMass();
joint.WarmStart();
}
for (int j = 0; j < VelocitySubiterationCount; j++)
{
//A permuted version of the indices is used. The randomization tends to avoid issues with solving order in corner cases.
for (int jointIndex = 0; jointIndex < ActiveSet.joints.Count; ++jointIndex)
{
int remappedIndex = permutationMapper.GetMappedIndex(jointIndex, ActiveSet.joints.Count);
ActiveSet.joints[remappedIndex].SolveVelocityIteration();
}
//Increment to use the next permutation.
++permutationMapper.PermutationIndex;
}
//Integrate the positions of the bones forward.
foreach (Bone bone in ActiveSet.bones)
{
bone.UpdatePosition();
}
}
//Clear out accumulated impulses; they should not persist through to another solving round because the state could be arbitrarily different.
for (int j = 0; j < ActiveSet.joints.Count; j++)
{
ActiveSet.joints[j].ClearAccumulatedImpulses();
}
}
public void Dispose()
{
ActiveSet.Dispose();
}
/// <summary>
/// Updates the positions of bones acted upon by the controls given to this solver.
/// </summary>
/// <param name="controls">List of currently active controls.</param>
public void Solve(List <Control> controls)
{
//Update the list of active joints.
ActiveSet.UpdateActiveSet(controls);
if (AutoscaleControlImpulses)
{
//Update the control strengths to match the mass of the target bones and the desired maximum force.
foreach (var control in controls)
{
control.MaximumForce = control.TargetBone.Mass * AutoscaleControlMaximumForce;
}
}
//Reset the permutation index; every solve should proceed in exactly the same order.
permutationMapper.PermutationIndex = 0;
float updateRate = 1 / TimeStepDuration;
foreach (var joint in ActiveSet.joints)
{
joint.Preupdate(TimeStepDuration, updateRate);
}
foreach (var control in controls)
{
control.Preupdate(TimeStepDuration, updateRate);
}
//Go through the set of controls and active joints, updating the state of bones.
for (int i = 0; i < ControlIterationCount; i++)
{
//Update the world inertia tensors of objects for the latest position.
foreach (Bone bone in ActiveSet.bones)
{
bone.UpdateInertiaTensor();
}
//Update the per-constraint jacobians and effective mass for the current bone orientations and positions.
foreach (IKJoint joint in ActiveSet.joints)
{
joint.UpdateJacobiansAndVelocityBias();
joint.ComputeEffectiveMass();
joint.WarmStart();
}
foreach (var control in controls)
{
if (control.TargetBone.Pinned)
{
throw new InvalidOperationException("Pinned objects cannot be moved by controls.");
}
control.UpdateJacobiansAndVelocityBias();
control.ComputeEffectiveMass();
control.WarmStart();
}
for (int j = 0; j < VelocitySubiterationCount; j++)
{
//Controls are updated first.
foreach (Control control in controls)
{
control.SolveVelocityIteration();
}
//A permuted version of the indices is used. The randomization tends to avoid issues with solving order in corner cases.
for (int jointIndex = 0; jointIndex < ActiveSet.joints.Count; ++jointIndex)
{
int remappedIndex = permutationMapper.GetMappedIndex(jointIndex, ActiveSet.joints.Count);
ActiveSet.joints[remappedIndex].SolveVelocityIteration();
}
//Increment to use the next permutation.
++permutationMapper.PermutationIndex;
}
//Integrate the positions of the bones forward.
foreach (Bone bone in ActiveSet.bones)
{
bone.UpdatePosition();
}
}
//Clear out the control iteration accumulated impulses; they should not persist through to the fixer iterations since the stresses are (potentially) totally different.
//This just helps stability in some corner cases. Without clearing this, previous high stress would prime the fixer iterations with bad guesses,
//making the system harder to solve (i.e. introducing instability and requiring more iterations).
for (int j = 0; j < ActiveSet.joints.Count; j++)
{
ActiveSet.joints[j].ClearAccumulatedImpulses();
}
//The previous loop may still have significant errors in the active joints due to
//unreachable targets. Run a secondary pass without the influence of the controls to
//fix the errors without interference from impossible goals
//This can potentially cause the bones to move away from the control targets, but with a sufficient
//number of control iterations, the result is generally a good approximation.
for (int i = 0; i < FixerIterationCount; i++)
{
//Update the world inertia tensors of objects for the latest position.
foreach (Bone bone in ActiveSet.bones)
{
bone.UpdateInertiaTensor();
}
//Update the per-constraint jacobians and effective mass for the current bone orientations and positions.
foreach (IKJoint joint in ActiveSet.joints)
{
joint.UpdateJacobiansAndVelocityBias();
joint.ComputeEffectiveMass();
joint.WarmStart();
}
for (int j = 0; j < VelocitySubiterationCount; j++)
{
//A permuted version of the indices is used. The randomization tends to avoid issues with solving order in corner cases.
for (int jointIndex = 0; jointIndex < ActiveSet.joints.Count; ++jointIndex)
{
int remappedIndex = permutationMapper.GetMappedIndex(jointIndex, ActiveSet.joints.Count);
ActiveSet.joints[remappedIndex].SolveVelocityIteration();
}
//Increment to use the next permutation.
++permutationMapper.PermutationIndex;
}
//Integrate the positions of the bones forward.
foreach (Bone bone in ActiveSet.bones)
{
bone.UpdatePosition();
}
}
//Clear out accumulated impulses; they should not persist through to another solving round because the state could be arbitrarily different.
for (int j = 0; j < ActiveSet.joints.Count; j++)
{
ActiveSet.joints[j].ClearAccumulatedImpulses();
}
foreach (Control control in controls)
{
control.ClearAccumulatedImpulses();
}
}