/**
* 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();
}
/**
* Returns a random unit Quaternion.
*
* <p>You can create a randomly directed unit vector using:
*
* <p>{@code PVector randomDir = new PVector(1.0f, 0.0f, 0.0f);} <br>
* {@code randomDir = Quaternion.multiply(Quaternion.randomQuaternion(), randomDir);}
*/
public static final Quaternion randomQuaternion() {
float seed = (float) Math.random();
float r1 = PApplet.sqrt(1.0f - seed);
float r2 = PApplet.sqrt(seed);
float t1 = 2.0f * PI * (float) Math.random();
float t2 = 2.0f * PI * (float) Math.random();
return new Quaternion(
PApplet.sin(t1) * r1, PApplet.cos(t1) * r1, PApplet.sin(t2) * r2, PApplet.cos(t2) * r2);
}
/**
* Returns the exponential of the Quaternion.
*
* @see #log()
*/
public final Quaternion exp() {
float theta = PApplet.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
if (theta < 1E-6f) return new Quaternion(this.x, this.y, this.z, PApplet.cos(theta));
else {
float coef = PApplet.sin(theta) / theta;
return new Quaternion(this.x * coef, this.y * coef, this.z * coef, PApplet.cos(theta));
}
}
/**
* Returns the logarithm of the Quaternion.
*
* @see #exp()
*/
public final Quaternion log() {
// Warning: this method should not normalize the Quaternion
float len = PApplet.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
if (len < 1E-6f) return new Quaternion(this.x, this.y, this.z, 0.0f, false);
else {
float coef = PApplet.acos(this.w) / len;
return new Quaternion(this.x * coef, this.y * coef, this.z * coef, 0.0f, false);
}
}
/** Normalizes the value of this Quaternion in place and return its {@code norm}. */
public final float normalize() {
float norm =
PApplet.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
if (norm > 0.0f) {
this.x /= norm;
this.y /= norm;
this.z /= norm;
this.w /= norm;
} else {
this.x = (float) 0.0;
this.y = (float) 0.0;
this.z = (float) 0.0;
this.w = (float) 1.0;
}
return norm;
}
/**
* Constructs and initializes a Quaternion from the array of length 4.
*
* @param q the array of length 4 containing xyzw in order
*/
public Quaternion(float[] q, boolean normalize) {
if (normalize) {
float mag = PApplet.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
if (mag > 0.0f) {
this.x = q[0] / mag;
this.y = q[1] / mag;
this.z = q[2] / mag;
this.w = q[3] / mag;
} else {
this.x = 0;
this.y = 0;
this.z = 0;
this.w = 1;
}
} else {
this.x = q[0];
this.y = q[1];
this.z = q[2];
this.w = q[3];
}
}
/**
* Constructs and initializes a Quaternion from the specified xyzw coordinates.
*
* @param x the x coordinate
* @param y the y coordinate
* @param z the z coordinate
* @param w the w scalar component
* @param normalize tells whether or not the constructed Quaternion should be normalized.
*/
public Quaternion(float x, float y, float z, float w, boolean normalize) {
if (normalize) {
float mag = PApplet.sqrt(x * x + y * y + z * z + w * w);
if (mag > 0.0f) {
this.x = x / mag;
this.y = y / mag;
this.z = z / mag;
this.w = w / mag;
} else {
this.x = 0;
this.y = 0;
this.z = 0;
this.w = 1;
}
} else {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
}
/**
* Sets the Quaternion as a rotation from the {@code from} direction to the {@code to} direction.
*
* <p><b>Attention:</b> this rotation is not uniquely defined. The selected axis is usually
* orthogonal to {@code from} and {@code to}, minimizing the rotation angle. This method is robust
* and can handle small or almost identical vectors.
*
* @see #fromAxisAngle(PVector, float)
*/
public void fromTo(PVector from, PVector to) {
float fromSqNorm = MathUtils.squaredNorm(from);
float toSqNorm = MathUtils.squaredNorm(to);
// Identity Quaternion when one vector is null
if ((fromSqNorm < 1E-10f) || (toSqNorm < 1E-10f)) {
this.x = this.y = this.z = 0.0f;
this.w = 1.0f;
} else {
PVector axis = from.cross(to);
float axisSqNorm = MathUtils.squaredNorm(axis);
// Aligned vectors, pick any axis, not aligned with from or to
if (axisSqNorm < 1E-10f) axis = MathUtils.orthogonalVector(from);
float angle = PApplet.asin(PApplet.sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));
if (from.dot(to) < 0.0) angle = PI - angle;
fromAxisAngle(axis, angle);
}
}
private final float sqrt(float a) {
return parent.sqrt(a);
}