in commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaternionRotation.java [751:820]
private static QuaternionRotation orthogonalRotationMatrixToQuaternion(
final double m00, final double m01, final double m02,
final double m10, final double m11, final double m12,
final double m20, final double m21, final double m22) {
// reference formula:
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
// The overall approach here is to take the equations for converting a quaternion to
// a matrix along with the fact that 1 = x^2 + y^2 + z^2 + w^2 for a normalized quaternion
// and solve for the various terms. This can theoretically be done using just the diagonal
// terms from the matrix. However, there are a few issues with this:
// 1) The term that we end up taking the square root of may be negative.
// 2) It's ambiguous as to whether we should use a plus or minus for the value of the
// square root.
// We'll address these concerns by only calculating a single term from one of the diagonal
// elements and then calculate the rest from the non-diagonals, which do not involve
// a square root. This solves the first issue since we can make sure to choose a diagonal
// element that will not cause us to take a square root of a negative number. The second
// issue is solved since only the relative signs between the quaternion terms are important
// (q and -q represent the same 3D rotation). It therefore doesn't matter whether we choose
// a plus or minus for our initial square root solution.
final double trace = m00 + m11 + m22;
final double w;
final double x;
final double y;
final double z;
if (trace > 0) {
// let s = 4*w
final double s = 2.0 * Math.sqrt(1.0 + trace);
final double sinv = 1.0 / s;
x = (m21 - m12) * sinv;
y = (m02 - m20) * sinv;
z = (m10 - m01) * sinv;
w = 0.25 * s;
} else if ((m00 > m11) && (m00 > m22)) {
// let s = 4*x
final double s = 2.0 * Math.sqrt(1.0 + m00 - m11 - m22);
final double sinv = 1.0 / s;
x = 0.25 * s;
y = (m01 + m10) * sinv;
z = (m02 + m20) * sinv;
w = (m21 - m12) * sinv;
} else if (m11 > m22) {
// let s = 4*y
final double s = 2.0 * Math.sqrt(1.0 + m11 - m00 - m22);
final double sinv = 1.0 / s;
x = (m01 + m10) * sinv;
y = 0.25 * s;
z = (m21 + m12) * sinv;
w = (m02 - m20) * sinv;
} else {
// let s = 4*z
final double s = 2.0 * Math.sqrt(1.0 + m22 - m00 - m11);
final double sinv = 1.0 / s;
x = (m02 + m20) * sinv;
y = (m21 + m12) * sinv;
z = 0.25 * s;
w = (m10 - m01) * sinv;
}
return of(w, x, y, z);
}