/** * 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()); }