/// <summary> /// Given a array of bones, and a target bone, solves the chain so that the last bone in the chain is at at the same position as the target /// </summary> /// <param name="bones">An array of bones, the chain to be solved by IK</param> /// <param name="target">The target for the chain</param> /// <param name="grandparent">the parent of the first bone in the bones chain, is used to ensure constraints</param> /// <returns>True if target was reached, false if maximum iteration was reached first </returns> override public bool SolveBoneChain(Bone[] bones, Bone target, Bone grandparent) { if (!IsReachable(bones, target)) { TargetUnreachable(bones, target.Pos, grandparent); bones[bones.Length - 1].Orientation = new Quaternion(target.Orientation.Xyz, target.Orientation.W); return(true); } int numberOfBones = bones.Length; int iter = 0; int degrees = degreeStep; bool toggle = false; bool doneOneLapAroundYAxis = false; int maxdegrees = 120; float lastDistanceToTarget = float.MaxValue; float distanceToTarget = (bones[bones.Length - 1].Pos - target.Pos).Length; // main loop while (distanceToTarget > threshold && MaxIterations > ++iter) { // if CCD is stuck becouse of constraints, we twist the chain if (distanceToTarget >= lastDistanceToTarget) { if (!doneOneLapAroundYAxis && degrees > maxdegrees) { doneOneLapAroundYAxis = true; degrees = degreeStep; } else if (doneOneLapAroundYAxis && degrees > maxdegrees) { break; } Quaternion q = doneOneLapAroundYAxis ? QuaternionHelper2.RotationX(MathHelper.DegreesToRadians(toggle ? degrees : -degrees)) : QuaternionHelper2.RotationY(MathHelper.DegreesToRadians(toggle ? degrees : -degrees)); ForwardKinematics(ref bones, q); if (toggle) { degrees += degreeStep; } toggle = !toggle; } // for each bone, starting with the one closest to the end effector // (but not the end effector itself) Vector3 a, b; Quaternion rotation; for (int i = numberOfBones - 2; i >= 0; i--) { // Get the vectors between the points a = bones[numberOfBones - 1].Pos - bones[i].Pos; b = target.Pos - bones[i].Pos; // Make a rotation quaternion and rotate // - first the endEffector // - then the rest of the affected joints rotation = (a.LengthFast == 0 || b.LengthFast == 0) ? Quaternion.Identity : QuaternionHelper2.GetRotationBetween(a, b, bones[i].Stiffness); if (bones[i].HasConstraints) { Vector3 res; Quaternion rot; if (constraints.CheckRotationalConstraints( bones[i], ((i > 0) ? bones[i - 1] : grandparent).Orientation, //Reference bones[i].Pos + Vector3.Transform(bones[i + 1].Pos - bones[i].Pos, rotation), // Target out res, out rot)) { rotation = rot * rotation; } } // Move the chain ForwardKinematics(ref bones, rotation, i); // Check for twist constraints if (bones[i].HasTwistConstraints) { Quaternion rotation2; if (constraints.CheckOrientationalConstraint(bones[i], (i > 0) ? bones[i - 1] : grandparent, out rotation2)) { ForwardKinematics(ref bones, rotation2, i); } } } lastDistanceToTarget = distanceToTarget; distanceToTarget = (bones[bones.Length - 1].Pos - target.Pos).LengthFast; } // Copy the targets rotation so that rotation is consistant bones[bones.Length - 1].Orientation = new Quaternion(target.Orientation.Xyz, target.Orientation.W); return(distanceToTarget <= threshold); }
public void SetConstraints(BipedSkeleton skeleton) { #region Cone constraints #region Spine too head skeleton[Joint.SPINE0].Constraints = (Spine); skeleton[Joint.SPINE1].Constraints = (Spine); skeleton[Joint.NECK].Constraints = (Neck); #endregion #region Legs skeleton[Joint.HIP_L].Constraints = (SwapXZ(Femur)); skeleton[Joint.HIP_R].Constraints = (Femur); skeleton[Joint.KNEE_L].Constraints = (SwapXZ(Knee)); skeleton[Joint.KNEE_R].Constraints = (Knee); skeleton[Joint.ANKLE_L].Constraints = (SwapXZ(Ankle)); skeleton[Joint.ANKLE_R].Constraints = (Ankle); skeleton[Joint.FOOTBASE_L].Constraints = (SwapXZ(FootBase)); skeleton[Joint.FOOTBASE_R].Constraints = (FootBase); #endregion #region Arms skeleton[Joint.CLAVICLE_L].Constraints = (SwapXZ(Clavicula)); skeleton[Joint.CLAVICLE_R].Constraints = (Clavicula); skeleton[Joint.SHOULDER_L].Constraints = (SwapXZ(Shoulder)); skeleton[Joint.SHOULDER_R].Constraints = (Shoulder); skeleton[Joint.ELBOW_L].Constraints = (SwapXZ(Elbow)); skeleton[Joint.ELBOW_R].Constraints = (Elbow); skeleton[Joint.WRIST_L].Constraints = (SwapXZ(Wrist)); skeleton[Joint.WRIST_R].Constraints = (Wrist); #endregion #endregion #region ParentPointers skeleton[Joint.CLAVICLE_R].ParentPointer = QuaternionHelper2.RotationZ(-MathHelper.PiOver2); skeleton[Joint.CLAVICLE_L].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.PiOver2); skeleton[Joint.HIP_R].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.Pi); skeleton[Joint.HIP_L].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.Pi); skeleton[Joint.ANKLE_R].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(-MathHelper.PiOver4); skeleton[Joint.ANKLE_L].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(MathHelper.PiOver4); skeleton[Joint.FOOTBASE_L].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(-MathHelper.PiOver4); skeleton[Joint.FOOTBASE_R].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(MathHelper.PiOver4); #endregion #region TwistConstraints #region Spine skeleton[Joint.SPINE0].TwistLimit = (SpineTwist); skeleton[Joint.SPINE1].TwistLimit = (SpineTwist); skeleton[Joint.SPINE3].TwistLimit = (NoTwist); skeleton[Joint.NECK].TwistLimit = (NeckTwist); #endregion #region Legs skeleton[Joint.HIP_L].TwistLimit = (FemurTwist); skeleton[Joint.HIP_R].TwistLimit = (FemurTwist); skeleton[Joint.KNEE_L].TwistLimit = (KneeTwist); skeleton[Joint.KNEE_R].TwistLimit = (KneeTwist); skeleton[Joint.ANKLE_L].TwistLimit = (AnkleTwist); skeleton[Joint.ANKLE_R].TwistLimit = (AnkleTwist); skeleton[Joint.FOOTBASE_L].TwistLimit = (FootBaseTwist); skeleton[Joint.FOOTBASE_R].TwistLimit = (FootBaseTwist); #endregion #region Arms skeleton[Joint.CLAVICLE_L].TwistLimit = (ClaviculaTwist); skeleton[Joint.SHOULDER_L].TwistLimit = (ShoulderTwist); skeleton[Joint.ELBOW_L].TwistLimit = (ElbowTwist); skeleton[Joint.WRIST_L].TwistLimit = (WristTwist); skeleton[Joint.CLAVICLE_R].TwistLimit = (ClaviculaTwist); skeleton[Joint.SHOULDER_R].TwistLimit = (ShoulderTwist); skeleton[Joint.ELBOW_R].TwistLimit = (ElbowTwist); skeleton[Joint.WRIST_R].TwistLimit = (WristTwist); #endregion #endregion #region stiffness #region Arms skeleton[Joint.CLAVICLE_L].Stiffness = (verystiff); skeleton[Joint.CLAVICLE_R].Stiffness = (verystiff); skeleton[Joint.WRIST_L].Stiffness = (barelymoving); skeleton[Joint.WRIST_R].Stiffness = (barelymoving); #endregion #region Legs skeleton[Joint.ANKLE_L].Stiffness = (barelymoving); skeleton[Joint.ANKLE_R].Stiffness = (barelymoving); skeleton[Joint.FOOTBASE_L].Stiffness = (barelymoving); skeleton[Joint.FOOTBASE_R].Stiffness = (barelymoving); #endregion #endregion }