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