/// <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
}
