/// <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>
/// Returns the cross product of two Vector3Ds
/// </summary>
/// <param name="v"></param>
/// <param name="u"></param>
/// <returns></returns>
public static Vector3D Cross(Vector3D v, Vector3D u)
{
Vector3D r = new Vector3D(0, 0, 0);
r.x = v.y * u.z - v.z * u.y;
r.y = v.z * u.x - v.x * u.z;
r.z = v.x * u.y - v.y * u.x;
return r;
}
/// <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>
/// Returns the dot product of two Vector3Ds
/// </summary>
/// <param name="v"></param>
/// <param name="u"></param>
/// <returns></returns>
public static double Dot(Vector3D v, Vector3D u)
{
return v.x * u.x + v.y * u.y + v.z * u.z;
}
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>
/// Constructs a Quaternion by copying a float value and a Vector3D
/// </summary>
/// <param name="w"></param>
/// <param name="n"></param>
public Quaternion(float w, Vector3D n)
{
q0 = w;
v = n;
}
/// <summary>
/// Constructs a Quaternion from w, x, y, z
/// </summary>
/// <param name="w"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
public Quaternion(float w, float x, float y, float z)
{
q0 = w;
v = new Vector3D(x, y, z);
}
/// <summary>
/// Default Quaternion constructor.
/// Constructs the identity Quaternion
/// </summary>
public Quaternion()
{
q0 = 1;
v = new Vector3D();
}
/// <summary>
/// Constructs a Quaternion from an axis of rotation and an amount of rotation (in radians)
/// </summary>
/// <param name="axis"></param>
/// <param name="rotation"></param>
public Quaternion(Vector3D axis, double rotation)
{
q0 = (float)Math.Cos(rotation / 2);
v = axis * (float)Math.Sin(rotation / 2);
}
/// <summary>
/// Costructs a Quaternion from an array of floats
/// </summary>
/// <param name="n"></param>
public Quaternion(float[] n)
{
q0 = n[0];
v = new Vector3D(n[1], n[2], n[3]);
}
