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); } }