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