/// <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;
}
}
public void Update(Quaternion q)
{
float x = q.x;
float y = q.y;
float z = q.z;
float w = q.w;
rotationMatrix[0] = 1f - ( 2f * y * y ) - ( 2f * z * z ); //11
rotationMatrix[1] = ( 2f * x * y ) - ( 2f * z * w ); //12
rotationMatrix[2] = ( 2f * x * z ) + ( 2f * y * w ); //13
rotationMatrix[2] = (2f * x * y ) + ( 2f * z * w ); //21
rotationMatrix[0] = 1f - ( 2f * x * x ) - ( 2f * z * z ); //22
rotationMatrix[1] = ( 2f * y * z ) - ( 2f * x * w ); //23
rotationMatrix[1] = ( 2f * x * z ) - ( 2f * y * w ); //31
rotationMatrix[2] = ( 2f * y * z ) + ( 2f * x * w ); //32
rotationMatrix[0] = 1f - ( 2f * x * x ) - ( 2f * y * y ); //33
}
/// <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));
}
public static void getEuler232(Quaternion QG, out double theta1, out double theta2, out double theta3)
{
getEulerCR(QG, 1, 2, 1, out theta1, out theta2, out theta3);
}
public static void getEuler313(Quaternion QG, out double theta1, out double theta2, out double theta3)
{
getEulerCR(QG, 2, 0, 2, out theta1, out theta2, out theta3);
}
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);
}
public static void getEuler121(Quaternion QG, out double theta1, out double theta2, out double theta3)
{
getEulerCR(QG, 0, 1, 0, out theta1, out theta2, out theta3);
}
/// <summary>
/// Returns the conjugate of this Quaternion
/// </summary>
/// <returns></returns>
public Quaternion Conjugate()
{
Quaternion r = new Quaternion(w, 0, 0, 0);
r.v = -1 * v;
return r;
}
public static void getEuler321(Quaternion QG, out double theta1, out double theta2, out double theta3)
{
getEulerNCNR(QG, 2, 1, 0, out theta1, out theta2, out theta3);
}
public static Quaternion operator *(Quaternion p, Vector3D v)
{
Quaternion r = new Quaternion(0, 0, 0, 0);
r.w = 0;
r.v = v;
return p * r;
}
public static Quaternion operator *(Vector3D v, Quaternion p)
{
Quaternion r = new Quaternion(0, 0, 0, 0);
r.w = 0;
r.v = v;
return r * p;
}
public static Quaternion operator *(Quaternion q, Quaternion p)
{
Quaternion r = new Quaternion(0, 0, 0, 0);
r.w = q.w * p.w - (float)QMath.Dot(q.v, p.v);
r.v = q.w * p.v + p.w * q.v + QMath.Cross(q.v, p.v);
return r;
}
/// <summary>
/// Constructs a Vector from the x, y, and z parts of a Quaternion
/// </summary>
/// <param name="q"></param>
public Vector3D(Quaternion q)
{
v = new float[] { q.x, q.y, q.z };
}
/// <summary>
/// Returns a Vector that would be the result of this Vector rotated by a Quaternion.
/// This method creates a new Vector and does not alter this Vector
/// </summary>
/// <param name="q">The quaternion by which to rotate the vector</param>
/// <returns></returns>
public Vector3D Rotate(Quaternion q)
{
return new Vector3D(q * this * q.Conjugate());
}