/// <summary>
/// Gets the "anotomic" angles from the orientation of a Quaternion
/// </summary>
/// <param name="q">The Quaternion to calculate the angles from</param>
/// <param name="flexion">Returns the "flexion/extension" of the orientation</param>
/// <param name="abduction">Returns the "abduction/adduction" of the orientation</param>
/// <param name="external">Returns the "internal/external rotation" of the orientation</param>
public static void getAnatomicAngles(Quaternion q, out double flexion, out double abduction, out double external)
{
// This method dynamically switches between 2 rotation sequences
// Both sequences assume '1' is the final axis of rotation
// This means if you're only flexing from the starting position, you will get only flexion
// And if you're abducting from the starting position, you will only get abduction
// This creates a switch in the middle that may make data look noisy in certain spots
// This was a design decision
Vector3D vx = new Vector3D(1, 0, 0);
double angle1 = 0, angle2 = 0, angle3 = 0;
vx = vx.Rotate(q);
if (Math.Abs(vx.y) > Math.Abs(vx.z)) // if you're pointing more in the direction of "flexion"
{
getEuler321(q, out angle1, out angle2, out angle3);
flexion = angle1;
abduction = angle2;
external = angle3;
}
else // if you're pointing more in the direction of "abduction"
{
getEuler231(q, out angle1, out angle2, out angle3);
abduction = angle1;
flexion = angle2;
external = angle3;
}
}
/// <summary>
/// Gets the "anotomic" angles from the orientation of a Quaternion
/// </summary>
/// <param name="q">The Quaternion to calculate the angles from</param>
/// <param name="flexion">Returns the "flexion/extension" of the orientation</param>
/// <param name="abduction">Returns the "abduction/adduction" of the orientation</param>
/// <param name="external">Returns the "internal/external rotation" of the orientation</param>
public static void getAnatomicAngles(Quaternion q, out double flexion, out double abduction, out double external)
{
// This method dynamically switches between 2 rotation sequences
// Both sequences assume '1' is the final axis of rotation
// This means if you're only flexing from the starting position, you will get only flexion
// And if you're abducting from the starting position, you will only get abduction
// This creates a switch in the middle that may make data look noisy in certain spots
// This was a design decision
Vector3D vx = new Vector3D(1, 0, 0);
double angle1 = 0, angle2 = 0, angle3 = 0;
vx = vx.Rotate(q);
if (Math.Abs(vx.y) > Math.Abs(vx.z)) // if you're pointing more in the direction of "flexion"
{
getEuler321(q, out angle1, out angle2, out angle3);
flexion = angle1;
abduction = angle2;
external = angle3;
}
else // if you're pointing more in the direction of "abduction"
{
getEuler231(q, out angle1, out angle2, out angle3);
abduction = angle1;
flexion = angle2;
external = angle3;
}
}
private static void getEulerCR(Quaternion QG, int i1, int i2, int i3, out double theta1, out double theta2, out double theta3)
{
Vector3D v3 = new Vector3D(0, 0, 0);
v3[i3] = 1;
Vector3D v3n = new Vector3D(0, 0, 0);
v3n[(i3 + 1) % 3] = 1;
v3 = v3.Rotate(QG);
theta1 = Math.Atan2(v3[(i1 + 1) % 3], -v3[(i1 + 2) % 3]);
theta2 = Math.Acos(v3[i1]);
Quaternion Q1 = new Quaternion((float)Math.Cos(theta1 / 2), 0, 0, 0);
Q1[i1 + 1] = (float)Math.Sin(theta1 / 2);
Quaternion Q2 = new Quaternion((float)Math.Cos(theta2 / 2), 0, 0, 0);
Q2[i2 + 1] = (float)Math.Sin(theta2 / 2);
theta3 = getThirdAngle(Q1 * Q2, QG, v3, v3n, theta1, theta2);
}
/// <summary>
/// Calculates theta3 from the given information.
/// Refer to the paper documented above for detailed info
/// </summary>
/// <param name="Q12"></param>
/// <param name="QG"></param>
/// <param name="v3"></param>
/// <param name="v3n"></param>
/// <param name="theta1"></param>
/// <param name="theta2"></param>
/// <returns></returns>
private static double getThirdAngle(Quaternion Q12, Quaternion QG, Vector3D v3, Vector3D v3n, double theta1, double theta2)
{
Vector3D v3n12, v3nG;
v3n12 = v3nG = v3n;
v3n12 = v3n12.Rotate(Q12);
v3nG = v3nG.Rotate(QG);
return(Math.Sign(QMath.Dot(QMath.Cross(v3n12, v3nG), v3)) * Math.Acos(QMath.Dot(v3n12, v3nG)));
}
/// <summary>
/// Calculates theta3 from the given information.
/// Refer to the paper documented above for detailed info
/// </summary>
/// <param name="Q12"></param>
/// <param name="QG"></param>
/// <param name="v3"></param>
/// <param name="v3n"></param>
/// <param name="theta1"></param>
/// <param name="theta2"></param>
/// <returns></returns>
private static double getThirdAngle(Quaternion Q12, Quaternion QG, Vector3D v3, Vector3D v3n, double theta1, double theta2)
{
Vector3D v3n12, v3nG;
v3n12 = v3nG = v3n;
v3n12 = v3n12.Rotate(Q12);
v3nG = v3nG.Rotate(QG);
return Math.Sign(QMath.Dot(QMath.Cross(v3n12, v3nG), v3)) * Math.Acos(QMath.Dot(v3n12, v3nG));
}
private static void getEulerCR(Quaternion QG, int i1, int i2, int i3, out double theta1, out double theta2, out double theta3)
{
Vector3D v3 = new Vector3D(0, 0, 0);
v3[i3] = 1;
Vector3D v3n = new Vector3D(0, 0, 0);
v3n[(i3 + 1) % 3] = 1;
v3 = v3.Rotate(QG);
theta1 = Math.Atan2(v3[(i1 + 1) % 3], -v3[(i1 + 2) % 3]);
theta2 = Math.Acos(v3[i1]);
Quaternion Q1 = new Quaternion((float)Math.Cos(theta1 / 2), 0, 0, 0);
Q1[i1 + 1] = (float)Math.Sin(theta1 / 2);
Quaternion Q2 = new Quaternion((float)Math.Cos(theta2 / 2), 0, 0, 0);
Q2[i2 + 1] = (float)Math.Sin(theta2 / 2);
theta3 = getThirdAngle(Q1 * Q2, QG, v3, v3n, theta1, theta2);
}
