private void GetAnkleLeft(Bone b) { b.Pos = AnkleLeft; if (markers[m.leftInnerAnkle].IsNaN()) { Vector3 up = KneeLeft - AnkleLeft; b.Orientation = QuaternionHelper2.LookAtUp(AnkleLeft, FootBaseLeft, up); } else { Vector3 right = markers[m.leftInnerAnkle] - markers[m.leftOuterAnkle]; b.Orientation = QuaternionHelper2.LookAtRight(AnkleLeft, FootBaseLeft, right); } }
/// <summary> /// The function applied to each bone in the skeleton /// </summary> /// <param name="bone">The skeleton, a tree of bones</param> private void TraversFunc(TreeNode <Bone> bone) { if (!bone.Data.Exists) { if (bone.IsRoot || bone.Parent.IsRoot) { return; } if ( bone.Data.Name.Equals(Joint.CLAVICLE_L) || bone.Data.Name.Equals(Joint.CLAVICLE_R) || bone.Data.Name.Equals(Joint.TRAP_L) || bone.Data.Name.Equals(Joint.TRAP_R)) { bone.Data.Pos = new Vector3(bone.Parent.Data.Pos); bone.Data.Orientation = QuaternionHelper2.LookAtUp( bone.Data.Pos, bone.Children.First().Data.Pos, bone.Parent.Data.GetZAxis()); return; } MissingJoint(bone); } }
private void UpperLegRight(Bone b) { b.Pos = HipJointRight; b.Orientation = QuaternionHelper2.LookAtRight(HipJointRight, KneeRight, HipJointRight - HipJointLeft); }
private void MidSpine(Bone b) { b.Pos = Spine1; b.Orientation = QuaternionHelper2.LookAtRight(Spine1, SternumClavicle, HipJointLeft - HipJointRight); }
private void GetHandRight(Bone b) { b.Pos = markers[m.rightHand]; b.Orientation = QuaternionHelper2.LookAtUp(markers[m.rightHand], markers[m.rightIndex], LowerArmForwardRight); }
private void GetTrapRight(Bone b) { b.Pos = WristRight; b.Orientation = QuaternionHelper2.LookAtUp(WristRight, markers[m.rightThumb], LowerArmForwardRight); }
private void GetTrapLeft(Bone b) { b.Pos = WristLeft; b.Orientation = QuaternionHelper2.LookAtUp(WristLeft, markers[m.leftThumb], LowerArmForwardLeft); }
/// <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); }
private void GetUpperArmRight(Bone b) { b.Pos = ShoulderRight; b.Orientation = QuaternionHelper2.LookAtRight(ShoulderRight, ElbowRight, markers[m.rightOuterElbow] - markers[m.rightInnerElbow]); }
private void GetUpperArmLeft(Bone b) { b.Pos = ShoulderLeft; b.Orientation = QuaternionHelper2.LookAtRight(ShoulderLeft, ElbowLeft, markers[m.leftInnerElbow] - markers[m.leftOuterElbow]); }
private void GetShoulderRight(Bone b) { b.Pos = SternumClavicle; b.Orientation = QuaternionHelper2.LookAtUp(SternumClavicle, ShoulderRight, ChestForward); }
private void GetFootBaseRight(Bone b) { b.Pos = FootBaseRight; b.Orientation = QuaternionHelper2.LookAtUp(b.Pos, markers[m.rightToe2], LowerLegUpRight); }
private void GetFootBaseLeft(Bone b) { b.Pos = FootBaseLeft; b.Orientation = QuaternionHelper2.LookAtUp(b.Pos, markers[m.leftToe2], LowerLegUpLeft); }
public void RotateTowards(Vector3 v, float stiffness = 1f) { Rotate(QuaternionHelper2.GetRotationBetween(GetYAxis(), v, stiffness = this.stiffness)); }
private void LowerLegLeft(Bone b) { b.Pos = KneeLeft; b.Orientation = QuaternionHelper2.LookAtRight(KneeLeft, AnkleLeft, KneeForwardLeft); }
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 }
private void GetLowerArmLeft(Bone b) { b.Pos = ElbowLeft; b.Orientation = QuaternionHelper2.LookAtUp(ElbowLeft, WristLeft, LowerArmForwardLeft); }
/// <summary> /// If any of the hip markers are missing, we predict them using the last position /// </summary> /// <param name="markers">The dictionary of markers</param> private void MissingEssentialMarkers(Dictionary <string, Vector3> markers) { Vector3 dirVec1, dirVec2, possiblePos1, possiblePos2, sacrumLastFrame = lastSACRUMknown, liasLastFrame = lastLIASknown, riasLastFrame = lastRIASknown; Vector3 Sacrum = markers[m.bodyBase], RIAS = markers[m.rightHip], LIAS = markers[m.leftHip]; bool s = !Sacrum.IsNaN(), r = !RIAS.IsNaN(), l = !LIAS.IsNaN(); if (s) // sacrum exists { if (r) // sacrum and rias exist, lias missing { dirVec1 = liasLastFrame - sacrumLastFrame; // vector from sacrum too lias in last frame dirVec2 = liasLastFrame - riasLastFrame; Quaternion between = Quaternion.Invert( QuaternionHelper2.GetRotationBetween( (RIAS - Sacrum), (riasLastFrame - sacrumLastFrame)) ); Vector3 transVec1 = Vector3.Transform(dirVec1, (between)); Vector3 transVec2 = Vector3.Transform(dirVec2, (between)); possiblePos1 = Sacrum + transVec1; // add vector from sacrum to lias last frame to this frames sacrum possiblePos2 = RIAS + transVec2; markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], Vector3Helper.MidPoint(possiblePos1, possiblePos2)); // get mid point of possible positions } else if (l) // sacrum and lias exists, rias missing { dirVec1 = riasLastFrame - sacrumLastFrame; dirVec2 = riasLastFrame - liasLastFrame; Quaternion between = Quaternion.Invert( QuaternionHelper2.GetRotationBetween( (LIAS - Sacrum), (liasLastFrame - sacrumLastFrame)) ); Vector3 transVec1 = Vector3.Transform(dirVec1, (between)); Vector3 transVec2 = Vector3.Transform(dirVec2, (between)); possiblePos1 = Sacrum + transVec1; possiblePos2 = LIAS + transVec2; markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], Vector3Helper.MidPoint(possiblePos1, possiblePos2)); } else // only sacrum exists, lias and rias missing { markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], Sacrum + riasLastFrame - sacrumLastFrame); markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], Sacrum + liasLastFrame - sacrumLastFrame); } } else if (r) // rias exists, sacrum missing { if (l) // rias and ias exists, sacrum missing { dirVec1 = sacrumLastFrame - riasLastFrame; dirVec2 = sacrumLastFrame - liasLastFrame; Quaternion between = Quaternion.Invert( QuaternionHelper2.GetRotationBetween( (LIAS - RIAS), (liasLastFrame - riasLastFrame)) ); Vector3 transVec1 = Vector3.Transform(dirVec1, (between)); Vector3 transVec2 = Vector3.Transform(dirVec2, (between)); possiblePos1 = RIAS + transVec1; possiblePos2 = LIAS + transVec2; markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], Vector3Helper.MidPoint(possiblePos1, possiblePos2)); } else // only rias exists, lias and sacrum missing { markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], RIAS + sacrumLastFrame - riasLastFrame); markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], RIAS + liasLastFrame - riasLastFrame); } } else if (l) // only lias exists, rias and sacrum missing { markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], LIAS + sacrumLastFrame - liasLastFrame); markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], LIAS + riasLastFrame - liasLastFrame); } else // all markers missing { markers[m.bodyBase] = markersLastFrame[m.bodyBase]; markers[m.rightHip] = markersLastFrame[m.rightHip]; markers[m.leftHip] = markersLastFrame[m.leftHip]; } }
private void GetLowerArmRight(Bone b) { b.Pos = ElbowRight; b.Orientation = QuaternionHelper2.LookAtUp(ElbowRight, WristRight, LowerArmForwardRight); }