/**
* 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);
}
public static Quaternion cosh(Quaternion q) {
Double3Vector imag = q.imag();
final double imagNorm = imag.norm();
return new Quaternion(
ExtraMath.cosh(q.re) * Math.cos(imagNorm),
(Double3Vector) imag.normalize().scalarMultiply(ExtraMath.sinh(q.re) * Math.sin(imagNorm)));
}
public static Quaternion exp(Quaternion q) {
final double k = Math.exp(q.re);
Double3Vector imag = q.imag();
final double imagNorm = imag.norm();
return new Quaternion(
k * Math.cos(imagNorm),
(Double3Vector) imag.normalize().scalarMultiply(k * Math.sin(imagNorm)));
}
public static Quaternion log(Quaternion q) {
final double norm = q.norm();
return new Quaternion(
Math.log(norm),
(Double3Vector) q.imag().normalize().scalarMultiply(Math.acos(q.re / norm)));
}
/** Returns the l<sup>2</sup>-norm (magnitude), which is also the C<sup>*</sup> norm. */
public double norm() {
return Math.sqrt(sumSquares());
}
/** Returns a hashcode for this quaternion. */
public int hashCode() {
return (int) (Math.exp(norm()));
}
