/**
* Simply returns {@code log(a. inverse() * b)}.
*
* <p>Useful for {@link #squadTangent(Quaternion, Quaternion, Quaternion)}.
*
* @param a the first Quaternion
* @param b the second Quaternion
*/
public static final Quaternion lnDif(Quaternion a, Quaternion b) {
Quaternion dif = a.inverse();
dif.multiply(b);
dif.normalize();
return dif.log();
}
/**
* Set the Quaternion from a (supposedly correct) 3x3 rotation matrix.
*
* <p>The matrix is expressed in European format: its three columns are the images by the rotation
* of the three vectors of an orthogonal basis.
*
* <p>{@link #fromRotatedBasis(PVector, PVector, PVector)} sets a Quaternion from the three axis
* of a rotated frame. It actually fills the three columns of a matrix with these rotated basis
* vectors and calls this method.
*
* @param m the 3*3 matrix of float values
*/
public final void fromRotationMatrix(float m[][]) {
// Compute one plus the trace of the matrix
float onePlusTrace = 1.0f + m[0][0] + m[1][1] + m[2][2];
if (onePlusTrace > 1E-5f) {
// Direct computation
float s = PApplet.sqrt(onePlusTrace) * 2.0f;
this.x = (m[2][1] - m[1][2]) / s;
this.y = (m[0][2] - m[2][0]) / s;
this.z = (m[1][0] - m[0][1]) / s;
this.w = 0.25f * s;
} else {
// Computation depends on major diagonal term
if ((m[0][0] > m[1][1]) & (m[0][0] > m[2][2])) {
float s = PApplet.sqrt(1.0f + m[0][0] - m[1][1] - m[2][2]) * 2.0f;
this.x = 0.25f * s;
this.y = (m[0][1] + m[1][0]) / s;
this.z = (m[0][2] + m[2][0]) / s;
this.w = (m[1][2] - m[2][1]) / s;
} else if (m[1][1] > m[2][2]) {
float s = PApplet.sqrt(1.0f + m[1][1] - m[0][0] - m[2][2]) * 2.0f;
this.x = (m[0][1] + m[1][0]) / s;
this.y = 0.25f * s;
this.z = (m[1][2] + m[2][1]) / s;
this.w = (m[0][2] - m[2][0]) / s;
} else {
float s = PApplet.sqrt(1.0f + m[2][2] - m[0][0] - m[1][1]) * 2.0f;
this.x = (m[0][2] + m[2][0]) / s;
this.y = (m[1][2] + m[2][1]) / s;
this.z = 0.25f * s;
this.w = (m[0][1] - m[1][0]) / s;
}
}
normalize();
}
