/**
* Constructs a quaternion representing a rotation matrix. Note: if the matrix is not orthogonal
* then the quaternion will not have unit norm. Note: unit quaternions are a double cover of
* SO(3).
*
* @param m a rotation matrix
* @author Steve Lamont
*/
public static Quaternion rotation(AbstractDoubleSquareMatrix m) {
if (m.rows() != 3 && m.columns() != 3)
throw new MatrixDimensionException("The matrix is not 3-dimensional.");
double re, imi, imj, imk;
double wSqr = (1.0 + m.trace()) / 4.0;
if (wSqr > GlobalSettings.ZERO_TOL) {
re = Math.sqrt(wSqr);
imi = (m.getElement(2, 1) - m.getElement(1, 2)) / (re * 4.0);
imj = (m.getElement(0, 2) - m.getElement(2, 0)) / (re * 4.0);
imk = (m.getElement(1, 0) - m.getElement(0, 1)) / (re * 4.0);
} else {
double xSqr = -(m.getElement(1, 1) + m.getElement(2, 2)) / 2.0;
re = 0.0;
if (xSqr > GlobalSettings.ZERO_TOL) {
imi = Math.sqrt(xSqr);
imj = m.getElement(1, 0) / (2.0 * imi);
imk = m.getElement(2, 0) / (2.0 * imi);
} else {
double ySqr = (1.0 - m.getElement(2, 2)) / 2.0;
imi = 0.0;
if (ySqr > GlobalSettings.ZERO_TOL) {
imj = Math.sqrt(ySqr);
imk = m.getElement(2, 1) / (2.0 * imj);
} else {
imj = 0.0;
imk = 1.0;
}
}
}
return new Quaternion(re, imi, imj, imk);
}
/** Returns the l<sup>2</sup>-norm (magnitude), which is also the C<sup>*</sup> norm. */
public double norm() {
return Math.sqrt(sumSquares());
}