/// <summary> /// Get orientation of three points, where on point defines forward /// </summary> /// <param name="back">position vector of back marker</param> /// <param name="left">position vector of left marker</param> /// <param name="right">position vector of right marker</param> /// <returns>Quaternion rotation</returns> public static Quaternion GetOrientation(Vector3 forwardPoint, Vector3 leftPoint, Vector3 rightPoint) { Vector3 x = rightPoint - leftPoint; Vector3 z = forwardPoint - Vector3Helper.MidPoint(leftPoint, rightPoint); return(GetOrientationFromZY(z, Vector3.Cross(z, x))); }
/// <summary> /// Prepare the markerset for joint localization /// </summary> /// <param name="labelMarkers">The list of labeled markers</param> /// <param name="newMarkers">a reference to the dictionary to be </param> /// <param name="prefix">Optional prefix of all markers</param> public void ProcessMarkers(out Dictionary <string, Vector3> newMarkers) { // sacrum can be defined by two markers if (sacrumBetween) { markers[m.bodyBase] = Vector3Helper.MidPoint(markers[sacrumBetweenMarkers[0]], markers[sacrumBetweenMarkers[1]]); } // if (frontHeadBetween) { markers[m.head] = Vector3Helper.MidPoint(markers[frontHeadBetweenMarkers[0]], markers[frontHeadBetweenMarkers[1]]); } if (markers[m.leftHip].IsNaN() || markers[m.rightHip].IsNaN() || markers[m.bodyBase].IsNaN() ) { MissingEssentialMarkers(markers); } else { lastSACRUMknown = markers[m.bodyBase]; lastRIASknown = markers[m.rightHip]; lastLIASknown = markers[m.leftHip]; } newMarkers = markers; }
/// <summary> /// /// </summary> /// <param name="isRightAnkle"></param> /// <returns></returns> private Vector3 GetAnklePos(bool isRightAnkle) { //Stolen from Visual3d Vector3 x, z, M1, M2, M3, negateY = new Vector3(1f, -1f, 1f); Matrix4 R; if (isRightAnkle) { M1 = markers[m.rightOuterKnee]; //FLE M3 = markers[m.rightLowerKnee]; //TTC M2 = markers[m.rightOuterAnkle]; //FAL } else { M1 = markers[m.leftOuterKnee]; //FLE M3 = markers[m.leftLowerKnee]; //TTC M2 = markers[m.leftOuterAnkle]; //FAL } x = Vector3Helper.MidPoint(M1, M2) - M3; z = M2 - M1; float scalefactor = z.Length; R = Matrix4Helper.GetOrientationMatrix(x, z); Vector3 trans = new Vector3( -0.07675f * scalefactor, 0.05482f * scalefactor, -0.02741f * scalefactor); if (!isRightAnkle) { Vector3.Multiply(ref trans, ref negateY, out trans); } return(Vector3.TransformVector(trans, R) + M2); }
/// <summary> /// /// </summary> /// <param name="isRightKnee"></param> /// <returns></returns> private Vector3 GetKneePos(bool isRightKnee) { Vector3 x, z, M1, M2, M3, negateY = new Vector3(1f, -1f, 1f); if (isRightKnee) { M1 = markers[m.rightOuterKnee]; //FLE M2 = markers[m.rightOuterAnkle]; //FAL M3 = markers[m.rightLowerKnee]; //TTC } else { M1 = markers[m.leftOuterKnee]; //FLE M2 = markers[m.leftOuterAnkle]; //FAL M3 = markers[m.leftLowerKnee]; //TTC } x = Vector3Helper.MidPoint(M1, M2) - M3; z = M1 - M2; float scalingFactor = z.Length; Matrix4 R = Matrix4Helper.GetOrientationMatrix(x, z); Vector3 trans = new Vector3( -0.1033f * scalingFactor, -0.09814f * scalingFactor, 0.0597f * scalingFactor); if (isRightKnee) { Vector3.Multiply(ref trans, ref negateY, out trans); } return(Vector3.TransformVector(trans, R) + M1); }
/// <summary> /// Get quaternion with front and right vector /// </summary> /// <param name="source">Front vector</param> /// <param name="target">Right vector</param> /// <returns>Quaternion with rotation</returns> public static Quaternion GetOrientationFromZY(Vector3 z, Vector3 y) { Vector3Helper.OrthoNormalize(ref y, ref z); Quaternion zRot = Quaternion.FromAxisAngle(Vector3.Cross(Vector3.UnitZ, z), Vector3.CalculateAngle(Vector3.UnitZ, z)); Vector3 t = Vector3.Transform(Vector3.UnitY, zRot); return(Quaternion.FromAxisAngle(Vector3.Cross(t, y), Vector3.CalculateAngle(t, y)) * zRot); }
/// <summary> /// Prepare the markerset for Joint localization, predicts the /// </summary> /// <param name="labelMarkers">The list of labelmarkets</param> /// <param name="newMarkers">a reference to the dictionary to be </param> /// <param name="prefix">The possible prefix of all markers</param> public void ProcessMarkers(List <LabeledMarker> labelMarkers, out Dictionary <string, Vector3> newMarkers, string prefix) { var temp = markers; markers = markersLastFrame; markersLastFrame = temp; markers.Clear(); for (int i = 0; i < labelMarkers.Count; i++) { markers.Add(labelMarkers[i].Label, labelMarkers[i].Position.Convert()); } foreach (var markername in m) { if (!markers.ContainsKey(markername)) { markers.Add(markername, Vector3Helper.NaN); } } // sacrum can be defined by two markers if (sacrumBetween) { markers[m.bodyBase] = Vector3Helper.MidPoint(markers[sacrumBetweenMarkers[0]], markers[sacrumBetweenMarkers[1]]); } // if (frontHeadBetween) { markers[m.head] = Vector3Helper.MidPoint(markers[frontHeadBetweenMarkers[0]], markers[frontHeadBetweenMarkers[1]]); } if (markers[m.leftHip].IsNaN() || markers[m.rightHip].IsNaN() || markers[m.bodyBase].IsNaN()) { MissingEssientialMarkers(markers); } else { lastSACRUMknown = markers[m.bodyBase]; lastRIASknown = markers[m.rightHip]; lastLIASknown = markers[m.leftHip]; } newMarkers = markers; }
/// <summary> /// /// </summary> /// <param name="isRightHip"></param> /// <returns></returns> private Vector3 GetHipJoint(bool isRightHip) { // As described by Harrington et al. 2006 // Prediction of the hip joint centre in adults, children, and patients with // cerebral palsy based on magnetic resonance imaging Vector3 ASISMid = Vector3Helper.MidPoint(markers[m.rightHip], markers[m.leftHip]); float Z, X, Y, pelvisDepth = (ASISMid - markers[m.bodyBase]).Length * 1000, pelvisWidth = (markers[m.leftHip] - markers[m.rightHip]).Length * 1000; X = 0.33f * pelvisWidth - 7.3f; Y = -0.30f * pelvisWidth - 10.9f; Z = -0.24f * pelvisDepth - 9.9f; if (!isRightHip) { X = -X; } return(ASISMid + Vector3.Transform((new Vector3(X, Y, Z) / 1000), HipOrientation)); }
/// <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 Pelvis(Bone b) { b.Pos = Vector3Helper.MidPoint(HipJointRight, HipJointLeft); b.Orientation = HipOrientation; }
/// <summary> /// Check the positional constraints of the bone, if the bone is inside the legal cone, returns true, otherwise false /// Originally modeled from Andreas Aristidou and Joan Lasenby FABRIK: A fast, iterative solver for the Inverse Kinematics problem /// </summary> /// <param name="joint">The bone to be checked if its has a legal position</param> /// <param name="parentsRots">The rotation if the parent</param> /// <param name="target">The position of the joint</param> /// <param name="res">The resulting position, the legal position of the bone if its legal, the same as target otherwise</param> /// <param name="rot">The rotation that should be applied to the bone if it has a illegal rotation, Identity otherwise</param> /// <returns></returns> public bool CheckRotationalConstraints(Bone joint, Quaternion parentsRots, Vector3 target, out Vector3 res, out Quaternion rot) { Quaternion referenceRotation = parentsRots * joint.ParentPointer; Vector3 L1 = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, referenceRotation)); Vector3 jointPos = joint.Pos; Vector4 constraints = joint.Constraints; Vector3 targetPos = new Vector3(target.X, target.Y, target.Z); Vector3 joint2Target = (targetPos - jointPos); bool behind = false; bool reverseCone = false; bool sideCone = false; //3.1 Find the line equation L1 //3.2 Find the projection O of the target t on line L1 Vector3 O = Vector3Helper.Project(joint2Target, L1); if (Math.Abs(Vector3.Dot(L1, joint2Target)) < precision) // target is ortogonal with L1 { O = Vector3.NormalizeFast(L1) * precision; } else if (Math.Abs(Vector3.Dot(O, L1) - O.LengthFast * L1.LengthFast) >= precision) // O not same direction as L1 { behind = true; } //3.3 Find the distance between the point O and the joint position float S = O.Length; //3.4 Map the target (rotate and translate) in such a //way that O is now located at the axis origin and oriented //according to the x and y-axis ) Now it is a 2D simplified problem Quaternion rotation = Quaternion.Invert(referenceRotation); //Quaternion.Invert(parent.Orientation); Vector3 TRotated = Vector3.Transform(joint2Target, rotation); // align joint2target vector to y axis get x z offset Vector2 target2D = new Vector2(TRotated.X, TRotated.Z); //only intrested in the X Z cordinates //3.5 Find in which quadrant the target belongs // Locate target in a particular quadrant //3.6 Find what conic section describes the allowed //range of motion Vector2 radius; Quadrant q; #region find Quadrant if (target2D.X >= 0 && target2D.Y >= 0) { radius = new Vector2(constraints.X, constraints.Y); q = Quadrant.q1; } else if (target2D.X >= 0 && target2D.Y < 0) { q = Quadrant.q2; radius = new Vector2(constraints.X, constraints.W); } else if (target2D.X < 0 && target2D.Y < 0) { q = Quadrant.q3; radius = new Vector2(constraints.Z, constraints.W); } else //if (target.X > 0 && target.Y < 0) { q = Quadrant.q4; radius = new Vector2(constraints.Z, constraints.Y); } #endregion #region check cone if (radius.X > 90 && radius.Y > 90) // cone is reversed if both angles are larget then 90 degrees { reverseCone = true; radius.X = 90 - (radius.X - 90); radius.Y = 90 - (radius.Y - 90); } else if (behind && radius.X <= 90 && radius.Y <= 90) // target behind and cone i front { O = -Vector3.NormalizeFast(O) * precision; S = O.Length; } else if (radius.X > 90 || radius.Y > 90) // has one angle > 90, other not, very speciall case { Vector3 L2 = GetNewL(rotation, q, radius); if (!behind) { L2 = (L2 - L1) / 2 + L1; } float angle = Vector3.CalculateAngle(L2, L1); Vector3 axis = Vector3.Cross(L2, L1); rotation = rotation * Quaternion.FromAxisAngle(axis, angle); TRotated = Vector3.Transform(joint2Target, rotation); target2D = new Vector2(TRotated.X, TRotated.Z); O = Vector3Helper.Project(joint2Target, L2); if (Math.Abs(Vector3.Dot(L2, joint2Target)) < precision) // target is ortogonal with L2 { O = Vector3.Normalize(L2) * precision; } S = behind ? O.Length : O.Length * 1.4f; //magic number if (behind) { sideCone = true; if (radius.X > 90) { radius.X = (radius.X - 90); } else { radius.Y = (radius.Y - 90); } } } #endregion radius.X = Mathf.Clamp(radius.X, precision, 90 - precision); // clamp it so if <=0 -> 0.01, >=90 -> 89.99 radius.Y = Mathf.Clamp(radius.Y, precision, 90 - precision); //3.7 Find the conic section which is associated with //that quadrant using the distances qj = Stanhj, where //j = 1,..,4 float radiusX = S * Mathf.Tan(MathHelper.DegreesToRadians(radius.X)); float radiusY = S * Mathf.Tan(MathHelper.DegreesToRadians(radius.Y)); //3.8 Check whether the target is within the conic section or not bool inside = (target2D.X * target2D.X) / (radiusX * radiusX) + (target2D.Y * target2D.Y) / (radiusY * radiusY) <= 1 + precision; //3.9 if within the conic section then if ((inside && !behind) || (!inside && reverseCone) || (inside && sideCone) ) { //3.10 use the true target position t res = target; rot = Quaternion.Identity; return(false); } //3.11 else else { //3.12 Find the nearest point on that conic section from the target Vector2 newPoint = NearestPoint(radiusX, radiusY, target2D); Vector3 newPointV3 = new Vector3(newPoint.X, 0.0f, newPoint.Y); //3.13 Map (rotate and translate) that point on the //conic section via reverse of 3.4 and use that point as //the new target position rotation = Quaternion.Invert(rotation); Vector3 vectorToMoveTo = Vector3.Transform(newPointV3, rotation) + O; Vector3 axis = Vector3.Cross(joint2Target, vectorToMoveTo); float angle = Vector3.CalculateAngle(joint2Target, vectorToMoveTo); rot = Quaternion.FromAxisAngle(axis, angle); res = Vector3.Transform(joint2Target, rot) + jointPos; return(true); } //3.14 end }