public IKApplier(BipedSkeleton firstSkel)
{
lastSkel = firstSkel;
IKSolver = new CCD();
FABRIK = new FABRIK();
FABRIK.MaxIterations = 100;
}
/// <summary>
/// If a joints is missing from the skeletontree, fill the joints with the previus frames joints and solve with ik if a joint is found, or return the previus frames joint pos offseted the new position
/// </summary>
/// <param name="skelEnum">The enumurator to the missing bone position</param>
/// <param name="lastSkelEnum">The enumurator to the missing bone position from the last skeleton</param>
private void MissingJoint(TreeNode <Bone> missingJoint)
{
// missings joints parent from last frame is root in solution
//root of chain
TreeNode <Bone> lastSkelBone = lastSkel.Root.FindTreeNode(node => node.Data.Name.Equals(missingJoint.Data.Name));
List <Bone> missingChain = new List <Bone>(); // chain to be solved
// The root if the chain
Vector3 offset = missingJoint.Parent.Data.Pos - lastSkelBone.Parent.Data.Pos; // offset to move last frames chain to this frames' position
CopyFromLast(missingJoint.Parent.Data, lastSkelBone.Parent.Data);
missingJoint.Parent.Data.Pos += offset;
missingChain.Add(missingJoint.Parent.Data);
bool iksolved = false;
IEnumerator lastSkelEnum = lastSkelBone.GetEnumerator();
Bone last;
foreach (var curr in missingJoint)
{
lastSkelEnum.MoveNext();
last = ((TreeNode <Bone>)lastSkelEnum.Current).Data;
if (curr.Data.Exists) // target found! it the last in list
{
Bone target = new Bone(
curr.Data.Name,
new Vector3(curr.Data.Pos)
);
if (!curr.Data.Orientation.IsNaN())
{
target.Orientation =
new Quaternion(curr.Data.Orientation.Xyz, curr.Data.Orientation.W);
}
CopyFromLast(curr.Data, last);
curr.Data.Pos += offset;
missingChain.Add(curr.Data);
if (!IKSolver.SolveBoneChain(missingChain.ToArray(), target, missingJoint.Parent.Parent.Data))// solve with IK
{
FABRIK.SolveBoneChain(missingChain.ToArray(), target, missingJoint.Parent.Parent.Data);
}
iksolved = true;
break;
}
CopyFromLast(curr.Data, last);
curr.Data.Pos += offset;
missingChain.Add(curr.Data);
if (curr.IsLeaf)
{
break;
}
}
// If target not found on chain, rotate the body according to its grandparents knowed diffrence
if (!iksolved)
{
var q2 = missingJoint.Parent.Parent.Data.Orientation;
var q1 = lastSkelBone.Parent.Parent.Data.Orientation;
FK(missingJoint.Parent, (q2 * Quaternion.Invert(q1)));
}
ConstraintsBeforeReturn(missingJoint.Parent, false);
if (iksolved && Interpolation && (missingJoint.Data.IsArm() || missingJoint.Data.IsLeg()))
{
JerkingTest(missingJoint.Parent, false, true);
}
}
