Exemplo n.º 1
0
 /**
  * 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);
 }
Exemplo n.º 2
0
 /** Returns the l<sup>2</sup>-norm (magnitude), which is also the C<sup>*</sup> norm. */
 public double norm() {
   return Math.sqrt(sumSquares());
 }