public static GetRotationBetween ( OpenTK.Vector3 a, OpenTK.Vector3 b, float stiffness = 1f ) : Quaternion | ||
a | OpenTK.Vector3 | The first vector |
b | OpenTK.Vector3 | The secound vector |
stiffness | float | Stiffness value |
return | Quaternion |
/// <summary> /// Draws the twist constraints accoriding in Unity using Giszmos /// </summary> /// <param name="b">The bone with its constraints</param> /// <param name="refBone">The to be twisted against</param> /// <param name="poss">The position of where it should be drawn</param> /// <param name="scale">The scale of the constraints</param> public static void DrawTwistConstraints(Bone b, Bone refBone, OpenTK.Vector3 poss, float scale) { if (b.Orientation.Xyz.IsNaN() || refBone.Orientation.Xyz.IsNaN()) { return; } OpenTK.Vector3 thisY = b.GetYAxis(); OpenTK.Quaternion referenceRotation = refBone.Orientation * b.ParentPointer; OpenTK.Vector3 parentY = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitY, referenceRotation); OpenTK.Vector3 parentZ = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitZ, referenceRotation); OpenTK.Quaternion rot = QuaternionHelper2.GetRotationBetween(parentY, thisY); OpenTK.Vector3 reference = OpenTK.Vector3.Transform(parentZ, rot); reference.Normalize(); Debug.DrawRay(poss.Convert(), (b.GetZAxis() * scale * 2).Convert(), Color.cyan); float startTwistLimit = OpenTK.MathHelper.DegreesToRadians(b.StartTwistLimit); OpenTK.Vector3 m = OpenTK.Vector3.Transform(reference, OpenTK.Quaternion.FromAxisAngle(thisY, startTwistLimit)); m.Normalize(); Debug.DrawRay(poss.Convert(), m.Convert() * scale, Color.yellow); float endTwistLimit = OpenTK.MathHelper.DegreesToRadians(b.EndTwistLimit); OpenTK.Vector3 m2 = OpenTK.Vector3.Transform(reference, OpenTK.Quaternion.FromAxisAngle(thisY, endTwistLimit)); m2.Normalize(); Debug.DrawRay(poss.Convert(), m2.Convert() * scale, Color.magenta); Debug.DrawLine((poss + (m * scale)).Convert(), (poss + (m2 * scale)).Convert(), Color.cyan); }
// An orientational constraint is the twist of the bone around its own direction vector // with respect to its parent // It is defined as an allowed range betwen angles [start,end] // where start != end && 0 < start, end <= 360 // If both start and end is 0 no twist constraint exist /// <summary> /// Checks if the bone has a legal rotation in regards to its parent, returns true if legal, false otherwise. /// The out rotation gives the rotation the bone should be applied to to be inside the twist constraints /// </summary> /// <param name="b">The bone under consideration</param> /// <param name="refBone">The parent of the bone, to check whether the child has legal rotation</param> /// <param name="rotation">The rotation bone b should applie to be inside the constraints</param> /// <returns></returns> public bool CheckOrientationalConstraint(Bone b, Bone refBone, out Quaternion rotation) { if (b.Orientation.Xyz.IsNaN() || refBone.Orientation.Xyz.IsNaN()) { rotation = Quaternion.Identity; return(false); } Vector3 thisY = b.GetYAxis(); Quaternion referenceRotation = refBone.Orientation * b.ParentPointer; Vector3 reference = Vector3.Transform( Vector3.Transform(Vector3.UnitZ, referenceRotation), QuaternionHelper2.GetRotationBetween( Vector3.Transform(Vector3.UnitY, referenceRotation), thisY)); float twistAngle = MathHelper.RadiansToDegrees(Vector3.CalculateAngle(reference, b.GetZAxis())); if (Vector3.CalculateAngle(reference, b.GetXAxis()) > Mathf.PI / 2) // b is twisted left with respect to parent { twistAngle = 360 - twistAngle; } float leftLimit = b.StartTwistLimit; float rightLimit = b.EndTwistLimit; float precision = 0.5f; bool inside = (leftLimit >= rightLimit) ? // The allowed range is on both sides of the reference vector inside = twistAngle - leftLimit >= precision || twistAngle - rightLimit <= precision : inside = twistAngle - leftLimit >= precision && twistAngle - rightLimit <= precision; if (!inside) // not inside constraints { // Create a rotation to the closest limit float toLeft = Math.Min(360 - Math.Abs(twistAngle - leftLimit), Math.Abs(twistAngle - leftLimit)); float toRight = Math.Min(360 - Math.Abs(twistAngle - rightLimit), Math.Abs(twistAngle - rightLimit)); if (toLeft < toRight) { // Anti-clockwise rotation to left limit rotation = Quaternion.FromAxisAngle(thisY, -MathHelper.DegreesToRadians(toLeft)); return(true); } else { // Clockwise to right limit rotation = Quaternion.FromAxisAngle(thisY, MathHelper.DegreesToRadians(toRight)); return(true); } } rotation = Quaternion.Identity; return(false); }
/// <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]; } }
/// <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 RotateTowards(Vector3 v, float stiffness = 1f) { Rotate(QuaternionHelper2.GetRotationBetween(GetYAxis(), v, stiffness = this.stiffness)); }