aboutsummaryrefslogtreecommitdiffstats
path: root/src/jogl/classes/com/jogamp/opengl
diff options
context:
space:
mode:
Diffstat (limited to 'src/jogl/classes/com/jogamp/opengl')
-rw-r--r--src/jogl/classes/com/jogamp/opengl/math/Quaternion.java1183
-rw-r--r--src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java17
2 files changed, 968 insertions, 232 deletions
diff --git a/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java b/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java
index 3c3510b7f..ab419e3cd 100644
--- a/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java
+++ b/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java
@@ -27,361 +27,1053 @@
*/
package com.jogamp.opengl.math;
+import java.nio.FloatBuffer;
+
+/**
+ * Quaternion implementation supporting
+ * <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q34">Gimbal-Lock</a> free rotations.
+ * <p>
+ * All matrix operation provided are in column-major order,
+ * as specified in the OpenGL fixed function pipeline, i.e. compatibility profile.
+ * See {@link FloatUtil}.
+ * </p>
+ * <p>
+ * See <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html">Matrix-FAQ</a>
+ * </p>
+ * <p>
+ * See <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm">euclideanspace.com-Quaternion</a>
+ * </p>
+ */
public class Quaternion {
- protected float x, y, z, w;
+ private float x, y, z, w;
+
+ /**
+ * Quaternion Epsilon, used with equals method to determine if two Quaternions are close enough to be considered equal.
+ * <p>
+ * Using {@value}, which is ~20 times {@link FloatUtil#EPSILON}.
+ * </p>
+ */
+ public static final float ALLOWED_DEVIANCE = 1.0E-6f; // FloatUtil.EPSILON == 1.1920929E-7f; double ALLOWED_DEVIANCE: 1.0E-8f
public Quaternion() {
- setIdentity();
+ x = y = z = 0; w = 1;
}
- public Quaternion(Quaternion q) {
- x = q.x;
- y = q.y;
- z = q.z;
- w = q.w;
+ public Quaternion(final Quaternion q) {
+ set(q);
}
- public Quaternion(float x, float y, float z, float w) {
- this.x = x;
- this.y = y;
- this.z = z;
- this.w = w;
+ public Quaternion(final float x, final float y, final float z, final float w) {
+ set(x, y, z, w);
}
/**
- * Constructor to create a rotation based quaternion from two vectors
- *
- * @param vector1
- * @param vector2
+ * @return the squared magnitude of this quaternion.
*/
- public Quaternion(float[] vector1, float[] vector2) {
- final float theta = FloatUtil.acos(VectorUtil.dot(vector1, vector2));
- final float[] cross = new float[3];
- VectorUtil.cross(cross, vector1, vector2);
- fromAxis(cross, theta);
- }
-
- /***
- * Constructor to create a rotation based quaternion from axis vector and angle
- * @param vector axis vector
- * @param angle rotation angle (rads)
- * @see #fromAxis(float[], float)
- */
- public Quaternion(float[] vector, float angle) {
- fromAxis(vector, angle);
- }
-
- /***
- * Initialize this quaternion with given axis vector and rotation angle
- *
- * @param vector axis vector
- * @param angle rotation angle (rads)
- */
- public void fromAxis(float[] vector, float angle) {
- final float halfangle = angle * 0.5f;
- final float sin = FloatUtil.sin(halfangle);
- final float[] nv = VectorUtil.normalize(vector, vector);
- x = (nv[0] * sin);
- y = (nv[1] * sin);
- z = (nv[2] * sin);
- w = FloatUtil.cos(halfangle);
+ public final float magnitudeSquared() {
+ return w*w + x*x + y*y + z*z;
}
/**
- * Transform the rotational quaternion to axis based rotation angles
- *
- * @return new float[4] with ,theta,Rx,Ry,Rz
+ * @return the magnitude of this quaternion, i.e. sqrt({@link #magnitude()})
*/
- public float[] toAxis() {
- final float[] vec = new float[4];
- final float scale = FloatUtil.sqrt(x * x + y * y + z * z);
- vec[0] = FloatUtil.acos(w) * 2.0f;
- vec[1] = x / scale;
- vec[2] = y / scale;
- vec[3] = z / scale;
- return vec;
+ public final float magnitude() {
+ final float magnitudeSQ = magnitudeSquared();
+ if (magnitudeSQ == 1f) {
+ return 1f;
+ }
+ return FloatUtil.sqrt(magnitudeSQ);
}
- public float getW() {
+ public final float getW() {
return w;
}
- public void setW(float w) {
+ public final void setW(final float w) {
this.w = w;
}
- public float getX() {
+ public final float getX() {
return x;
}
- public void setX(float x) {
+ public final void setX(final float x) {
this.x = x;
}
- public float getY() {
+ public final float getY() {
return y;
}
- public void setY(float y) {
+ public final void setY(final float y) {
this.y = y;
}
- public float getZ() {
+ public final float getZ() {
return z;
}
- public void setZ(float z) {
+ public final void setZ(final float z) {
this.z = z;
}
/**
+ * Returns the dot product of this quaternion with the given x,y,z and w components.
+ */
+ public final float dot(final float x, final float y, final float z, final float w) {
+ return this.x * x + this.y * y + this.z * z + this.w * w;
+ }
+
+ /**
+ * Returns the dot product of this quaternion with the given quaternion
+ */
+ public final float dot(final Quaternion quat) {
+ return dot(quat.getX(), quat.getY(), quat.getZ(), quat.getW());
+ }
+
+ /**
+ * Check if this quaternion represents an identity matrix for rotation,
+ * , ie (0,0,0,1).
+ *
+ * @return true if it is an identity rep., false otherwise
+ */
+ public final boolean isIdentity() {
+ return w == 1 && x == 0 && y == 0 && z == 0;
+ }
+
+ /***
+ * Set this quaternion to identity (x=0,y=0,z=0,w=1)
+ * @return this quaternion for chaining.
+ */
+ public final Quaternion setIdentity() {
+ x = y = z = 0; w = 1;
+ return this;
+ }
+
+ /**
+ * Normalize a quaternion required if to be used as a rotational quaternion
+ * @return this quaternion for chaining.
+ */
+ public final Quaternion normalize() {
+ final float norm = magnitude();
+ if (norm == 0.0f) {
+ setIdentity();
+ } else {
+ w /= norm;
+ x /= norm;
+ y /= norm;
+ z /= norm;
+ }
+ return this;
+ }
+
+ /**
+ * Conjugates this quaternion <code>[-x, -y, -z, w]</code>.
+ * @return this quaternion for chaining.
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q49">Matrix-FAQ Q49</a>
+ */
+ public Quaternion conjugate() {
+ x = -x;
+ y = -y;
+ z = -z;
+ return this;
+ }
+
+ /**
+ * Invert the quaternion If rotational, will produce a the inverse rotation
+ * @return this quaternion for chaining.
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q50">Matrix-FAQ Q50</a>
+ */
+ public final Quaternion invert() {
+ final float magnitudeSQ = magnitudeSquared();
+ if ( FloatUtil.equals(1.0f, magnitudeSQ, FloatUtil.EPSILON) ) {
+ conjugate();
+ } else {
+ w /= magnitudeSQ;
+ x = -x / magnitudeSQ;
+ y = -y / magnitudeSQ;
+ z = -z / magnitudeSQ;
+ }
+ return this;
+ }
+
+ /**
+ * Set all values of this quaternion using the given src.
+ * @return this quaternion for chaining.
+ */
+ public final Quaternion set(final Quaternion src) {
+ this.x = src.x;
+ this.y = src.y;
+ this.z = src.z;
+ this.w = src.w;
+ return this;
+ }
+
+ /**
+ * Set all values of this quaternion using the given components.
+ * @return this quaternion for chaining.
+ */
+ public final Quaternion set(final float x, final float y, final float z, final float w) {
+ this.x = x;
+ this.y = y;
+ this.z = z;
+ this.w = w;
+ return this;
+ }
+
+ /**
* Add a quaternion
*
* @param q quaternion
+ * @return this quaternion for chaining.
+ * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#add">euclideanspace.com-QuaternionAdd</a>
*/
- public void add(Quaternion q) {
+ public final Quaternion add(final Quaternion q) {
x += q.x;
y += q.y;
z += q.z;
+ w += q.w;
+ return this;
}
/**
* Subtract a quaternion
*
* @param q quaternion
+ * @return this quaternion for chaining.
+ * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#add">euclideanspace.com-QuaternionAdd</a>
*/
- public void subtract(Quaternion q) {
+ public final Quaternion subtract(final Quaternion q) {
x -= q.x;
y -= q.y;
z -= q.z;
+ w -= q.w;
+ return this;
}
/**
- * Divide a quaternion by a constant
+ * Multiply this quaternion by the param quaternion
*
- * @param n a float to divide by
+ * @param q a quaternion to multiply with
+ * @return this quaternion for chaining.
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q53">Matrix-FAQ Q53</a>
+ * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#mul">euclideanspace.com-QuaternionMul</a>
*/
- public void divide(float n) {
- x /= n;
- y /= n;
- z /= n;
+ public final Quaternion mult(final Quaternion q) {
+ return set( w * q.x + x * q.w + y * q.z - z * q.y,
+ w * q.y - x * q.z + y * q.w + z * q.x,
+ w * q.z + x * q.y - y * q.x + z * q.w,
+ w * q.w - x * q.x - y * q.y - z * q.z );
}
/**
- * Multiply this quaternion by the param quaternion
+ * Scale this quaternion by a constant
*
- * @param q a quaternion to multiply with
+ * @param n a float constant
+ * @return this quaternion for chaining.
+ * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#scale">euclideanspace.com-QuaternionScale</a>
*/
- public void mult(Quaternion q) {
- final float w1 = w * q.w - x * q.x - y * q.y - z * q.z;
+ public final Quaternion scale(final float n) {
+ x *= n;
+ y *= n;
+ z *= n;
+ w *= n;
+ return this;
+ }
- final float x1 = w * q.x + x * q.w + y * q.z - z * q.y;
- final float y1 = w * q.y - x * q.z + y * q.w + z * q.x;
- final float z1 = w * q.z + x * q.y - y * q.x + z * q.w;
+ /**
+ * Rotate this quaternion by the given angle and axis.
+ * <p>
+ * The axis must be a normalized vector.
+ * </p>
+ * <p>
+ * A rotational quaternion is made from the given angle and axis.
+ * </p>
+ *
+ * @param angle in radians
+ * @param axisX x-coord of rotation axis
+ * @param axisY y-coord of rotation axis
+ * @param axisZ z-coord of rotation axis
+ * @return this quaternion for chaining.
+ */
+ public Quaternion rotateByAngleNormalAxis(final float angle, final float axisX, final float axisY, final float axisZ) {
+ if( VectorUtil.isZero(axisX, axisY, axisZ, FloatUtil.EPSILON) ) {
+ // no change
+ return this;
+ }
+ final float halfAngle = 0.5f * angle;
+ final float sin = FloatUtil.sin(halfAngle);
+ final float qw = FloatUtil.cos(halfAngle);
+ final float qx = sin * axisX;
+ final float qy = sin * axisY;
+ final float qz = sin * axisZ;
+ return set( x * qw + y * qz - z * qy + w * qx,
+ -x * qz + y * qw + z * qx + w * qy,
+ x * qy - y * qx + z * qw + w * qz,
+ -x * qx - y * qy - z * qz + w * qw);
+ }
- w = w1;
- x = x1;
- y = y1;
- z = z1;
+ /**
+ * Rotate this quaternion around X axis with the given angle in radians
+ *
+ * @param angle in radians
+ * @return this quaternion for chaining.
+ */
+ public Quaternion rotateByAngleX(final float angle) {
+ final float halfAngle = 0.5f * angle;
+ final float sin = FloatUtil.sin(halfAngle);
+ final float cos = FloatUtil.cos(halfAngle);
+ return set( x * cos + w * sin,
+ y * cos + z * sin,
+ -y * sin + z * cos,
+ -x * sin + w * cos);
}
/**
- * Multiply a quaternion by a constant
+ * Rotate this quaternion around Y axis with the given angle in radians
*
- * @param n a float constant
+ * @param angle in radians
+ * @return this quaternion for chaining.
*/
- public void mult(float n) {
- x *= n;
- y *= n;
- z *= n;
+ public Quaternion rotateByAngleY(final float angle) {
+ final float halfAngle = 0.5f * angle;
+ final float sin = FloatUtil.sin(halfAngle);
+ final float cos = FloatUtil.cos(halfAngle);
+ return set( x * cos - z * sin,
+ y * cos + w * sin,
+ x * sin + z * cos,
+ -y * sin + w * cos);
+ }
+
+ /**
+ * Rotate this quaternion around Z axis with the given angle in radians
+ *
+ * @param angle in radians
+ * @return this quaternion for chaining.
+ */
+ public Quaternion rotateByAngleZ(final float angle) {
+ final float halfAngle = 0.5f * angle;
+ final float sin = FloatUtil.sin(halfAngle);
+ final float cos = FloatUtil.cos(halfAngle);
+ return set( x * cos + y * sin,
+ -x * sin + y * cos,
+ z * cos + w * sin,
+ -z * sin + w * cos);
}
/***
- * Rotate given vector by this quaternion
+ * Rotate the given vector by this quaternion
*
- * @param vector input vector
- * @return rotated vector
+ * @param vecOut result float[3] storage for rotated vector, maybe equal to vecIn for in-place rotation
+ * @param vecOutOffset offset in result storage
+ * @param vecIn float[3] vector to be rotated
+ * @param vecInOffset offset in vecIn
+ * @return the given vecOut store for chaining
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q63">Matrix-FAQ Q63</a>
*/
- public float[] mult(float[] vector) {
- // TODO : optimize
- final float[] res = new float[3];
- final Quaternion a = new Quaternion(vector[0], vector[1], vector[2], 0.0f);
- final Quaternion b = new Quaternion(this);
- final Quaternion c = new Quaternion(this);
- b.inverse();
- a.mult(b);
- c.mult(a);
- res[0] = c.x;
- res[1] = c.y;
- res[2] = c.z;
- return res;
+ public final float[] rotateVector(final float[] vecOut, final int vecOutOffset, final float[] vecIn, final int vecInOffset) {
+ if ( VectorUtil.isZero(vecIn, vecInOffset, FloatUtil.EPSILON) ) {
+ vecOut[0+vecOutOffset] = 0f;
+ vecOut[1+vecOutOffset] = 0f;
+ vecOut[2+vecOutOffset] = 0f;
+ } else {
+ final float vecX = vecIn[0+vecInOffset];
+ final float vecY = vecIn[1+vecInOffset];
+ final float vecZ = vecIn[2+vecInOffset];
+ vecOut[0+vecOutOffset] = w * w * vecX + 2f * y * w * vecZ
+ - 2f * z * w * vecY + x * x * vecX
+ + 2f * y * x * vecY + 2f * z * x * vecZ
+ - z * z * vecX - y * y * vecX;
+ vecOut[1+vecOutOffset] = 2f * x * y * vecX + y * y * vecY
+ + 2f * z * y * vecZ + 2f * w * z * vecX
+ - z * z * vecY + w * w * vecY
+ - 2f * x * w * vecZ - x * x * vecY;
+ vecOut[2+vecOutOffset] = 2f * x * z * vecX + 2f * y * z * vecY
+ + z * z * vecZ - 2f * w * y * vecX
+ - y * y * vecZ + 2f * w * x * vecY
+ - x * x * vecZ + w * w * vecZ;
+ }
+ return vecOut;
}
/**
- * Normalize a quaternion required if to be used as a rotational quaternion
+ * Set this quaternion to a spherical linear interpolation
+ * between the given start and end quaternions by the given change amount.
+ * <p>
+ * Note: Method <i>does not</i> normalize this quaternion!
+ * </p>
+ *
+ * @param a start quaternion
+ * @param b end quaternion
+ * @param changeAmnt float between 0 and 1 representing interpolation.
+ * @return this quaternion for chaining.
+ * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/">euclideanspace.com-QuaternionSlerp</a>
*/
- public void normalize() {
- final float norme = FloatUtil.sqrt(w * w + x * x + y * y + z * z);
- if (norme == 0.0f) {
- setIdentity();
+ public final Quaternion setSlerp(final Quaternion a, final Quaternion b, final float changeAmnt) {
+ // System.err.println("Slerp.0: A "+a+", B "+b+", t "+changeAmnt);
+ if (changeAmnt == 0.0f) {
+ set(a);
+ } else if (changeAmnt == 1.0f) {
+ set(b);
} else {
- final float recip = 1.0f / norme;
+ float bx = b.x;
+ float by = b.y;
+ float bz = b.z;
+ float bw = b.w;
+
+ // Calculate angle between them (quat dot product)
+ float cosHalfTheta = a.x * bx + a.y * by + a.z * bz + a.w * bw;
+
+ final float scale0, scale1;
- w *= recip;
- x *= recip;
- y *= recip;
- z *= recip;
+ if( cosHalfTheta >= 0.95f ) {
+ // quaternions are close, just use linear interpolation
+ scale0 = 1.0f - changeAmnt;
+ scale1 = changeAmnt;
+ // System.err.println("Slerp.1: Linear Interpol; cosHalfTheta "+cosHalfTheta);
+ } else if ( cosHalfTheta <= -0.99f ) {
+ // the quaternions are nearly opposite,
+ // we can pick any axis normal to a,b to do the rotation
+ scale0 = 0.5f;
+ scale1 = 0.5f;
+ // System.err.println("Slerp.2: Any; cosHalfTheta "+cosHalfTheta);
+ } else {
+ // System.err.println("Slerp.3: cosHalfTheta "+cosHalfTheta);
+ if( cosHalfTheta <= -FloatUtil.EPSILON ) { // FIXME: .. or shall we use the upper bound 'cosHalfTheta < FloatUtil.EPSILON' ?
+ // Negate the second quaternion and the result of the dot product (Inversion)
+ bx *= -1f;
+ by *= -1f;
+ bz *= -1f;
+ bw *= -1f;
+ cosHalfTheta *= -1f;
+ // System.err.println("Slerp.4: Inverted cosHalfTheta "+cosHalfTheta);
+ }
+ final float halfTheta = FloatUtil.acos(cosHalfTheta);
+ final float sinHalfTheta = FloatUtil.sqrt(1.0f - cosHalfTheta*cosHalfTheta);
+ // if theta = 180 degrees then result is not fully defined
+ // we could rotate around any axis normal to qa or qb
+ if ( Math.abs(sinHalfTheta) < 0.001f ){ // fabs is floating point absolute
+ scale0 = 0.5f;
+ scale1 = 0.5f;
+ // throw new InternalError("XXX"); // FIXME should not be reached due to above inversion ?
+ } else {
+ // Calculate the scale for q1 and q2, according to the angle and
+ // it's sine value
+ scale0 = FloatUtil.sin((1f - changeAmnt) * halfTheta) / sinHalfTheta;
+ scale1 = FloatUtil.sin(changeAmnt * halfTheta) / sinHalfTheta;
+ }
+ }
+
+ x = a.x * scale0 + bx * scale1;
+ y = a.y * scale0 + by * scale1;
+ z = a.z * scale0 + bz * scale1;
+ w = a.w * scale0 + bw * scale1;
}
+ // System.err.println("Slerp.X: Result "+this);
+ return this;
}
/**
- * Invert the quaternion If rotational, will produce a the inverse rotation
+ * Set this quaternion to equal the rotation required
+ * to point the z-axis at <i>direction</i> and the y-axis to <i>up</i>.
+ * <p>
+ * Implementation generates a 3x3 matrix
+ * and is equal with ProjectFloat's lookAt(..).<br/>
+ * </p>
+ *
+ * @param directionIn where to <i>look</i> at
+ * @param upIn a vector indicating the local <i>up</i> direction.
+ * @param xAxisOut vector storing the <i>orthogonal</i> x-axis of the coordinate system.
+ * @param yAxisOut vector storing the <i>orthogonal</i> y-axis of the coordinate system.
+ * @param zAxisOut vector storing the <i>orthogonal</i> z-axis of the coordinate system.
+ * @return this quaternion for chaining.
+ * @see <a href="http://www.euclideanspace.com/maths/algebra/vectors/lookat/index.htm">euclideanspace.com-LookUp</a>
*/
- public void inverse() {
- final float norm = w * w + x * x + y * y + z * z;
+ public Quaternion setLookAt(final float[] directionIn, final float[] upIn,
+ final float[] xAxisOut, final float[] yAxisOut, final float[] zAxisOut) {
+ // Z = norm(dir)
+ VectorUtil.normalize(zAxisOut, directionIn);
+
+ // X = upIn x Z
+ // (borrow yAxisOut for upNorm)
+ VectorUtil.normalize(yAxisOut, upIn);
+ VectorUtil.cross(xAxisOut, yAxisOut, zAxisOut);
+ VectorUtil.normalize(xAxisOut);
- final float recip = 1.0f / norm;
+ // Y = Z x X
+ //
+ VectorUtil.cross(yAxisOut, zAxisOut, xAxisOut);
+ VectorUtil.normalize(yAxisOut);
- w *= recip;
- x = -1 * x * recip;
- y = -1 * y * recip;
- z = -1 * z * recip;
+ /**
+ final float m00 = xAxisOut[0];
+ final float m01 = yAxisOut[0];
+ final float m02 = zAxisOut[0];
+ final float m10 = xAxisOut[1];
+ final float m11 = yAxisOut[1];
+ final float m12 = zAxisOut[1];
+ final float m20 = xAxisOut[2];
+ final float m21 = yAxisOut[2];
+ final float m22 = zAxisOut[2];
+ */
+ return setFromAxes(xAxisOut, yAxisOut, zAxisOut).normalize();
}
+ //
+ // Conversions
+ //
+
/**
- * Transform this quaternion to a 4x4 column matrix representing the
- * rotation
- *
- * @return new float[16] column matrix 4x4
- */
- public float[] toMatrix() {
- final float[] matrix = new float[16];
- matrix[0] = 1.0f - 2 * y * y - 2 * z * z;
- matrix[1] = 2 * x * y + 2 * w * z;
- matrix[2] = 2 * x * z - 2 * w * y;
- matrix[3] = 0;
-
- matrix[4] = 2 * x * y - 2 * w * z;
- matrix[5] = 1.0f - 2 * x * x - 2 * z * z;
- matrix[6] = 2 * y * z + 2 * w * x;
- matrix[7] = 0;
-
- matrix[8] = 2 * x * z + 2 * w * y;
- matrix[9] = 2 * y * z - 2 * w * x;
- matrix[10] = 1.0f - 2 * x * x - 2 * y * y;
- matrix[11] = 0;
-
- matrix[12] = 0;
- matrix[13] = 0;
- matrix[14] = 0;
- matrix[15] = 1;
- return matrix;
+ * Initialize this quaternion from two vectors
+ * <pre>
+ * q = (s,v) = (v1•v2 , v1 × v2),
+ * angle = angle(v1, v2) = v1•v2
+ * axis = normal(v1 x v2)
+ * </pre>
+ * @param v1 not normalized
+ * @param v2 not normalized
+ * @param tmpPivotVec float[3] temp storage for cross product
+ * @param tmpNormalVec float[3] temp storage to normalize vector
+ * @return this quaternion for chaining.
+ */
+ public final Quaternion setFromVectors(final float[] v1, final float[] v2, final float[] tmpPivotVec, final float[] tmpNormalVec) {
+ final float factor = VectorUtil.length(v1) * VectorUtil.length(v2);
+ if ( FloatUtil.isZero(factor, FloatUtil.EPSILON ) ) {
+ return setIdentity();
+ } else {
+ final float dot = VectorUtil.dot(v1, v2) / factor; // normalize
+ final float theta = FloatUtil.acos(Math.max(-1.0f, Math.min(dot, 1.0f))); // clipping [-1..1]
+
+ VectorUtil.cross(tmpPivotVec, v1, v2);
+
+ if ( dot < 0.0f && FloatUtil.isZero( VectorUtil.length(tmpPivotVec), FloatUtil.EPSILON ) ) {
+ // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector
+ // perpendicular to this vector will rotate vector a onto vector b.
+ //
+ // The following guarantees the dot-product will be 0.0.
+ int dominantIndex;
+ if (Math.abs(v1[0]) > Math.abs(v1[1])) {
+ if (Math.abs(v1[0]) > Math.abs(v1[2])) {
+ dominantIndex = 0;
+ } else {
+ dominantIndex = 2;
+ }
+ } else {
+ if (Math.abs(v1[1]) > Math.abs(v1[2])) {
+ dominantIndex = 1;
+ } else {
+ dominantIndex = 2;
+ }
+ }
+ tmpPivotVec[dominantIndex] = -v1[(dominantIndex + 1) % 3];
+ tmpPivotVec[(dominantIndex + 1) % 3] = v1[dominantIndex];
+ tmpPivotVec[(dominantIndex + 2) % 3] = 0f;
+ }
+ return setFromAngleAxis(theta, tmpPivotVec, tmpNormalVec);
+ }
}
/**
- * Set this quaternion from a Sphereical interpolation of two param
- * quaternion, used mostly for rotational animation.
+ * Initialize this quaternion from two normalized vectors
+ * <pre>
+ * q = (s,v) = (v1•v2 , v1 × v2),
+ * angle = angle(v1, v2) = v1•v2
+ * axis = v1 x v2
+ * </pre>
+ * @param v1 normalized
+ * @param v2 normalized
+ * @param tmpPivotVec float[3] temp storage for cross product
+ * @return this quaternion for chaining.
+ */
+ public final Quaternion setFromNormalVectors(final float[] v1, final float[] v2, final float[] tmpPivotVec) {
+ final float factor = VectorUtil.length(v1) * VectorUtil.length(v2);
+ if ( FloatUtil.isZero(factor, FloatUtil.EPSILON ) ) {
+ return setIdentity();
+ } else {
+ final float dot = VectorUtil.dot(v1, v2) / factor; // normalize
+ final float theta = FloatUtil.acos(Math.max(-1.0f, Math.min(dot, 1.0f))); // clipping [-1..1]
+
+ VectorUtil.cross(tmpPivotVec, v1, v2);
+
+ if ( dot < 0.0f && FloatUtil.isZero( VectorUtil.length(tmpPivotVec), FloatUtil.EPSILON ) ) {
+ // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector
+ // perpendicular to this vector will rotate vector a onto vector b.
+ //
+ // The following guarantees the dot-product will be 0.0.
+ int dominantIndex;
+ if (Math.abs(v1[0]) > Math.abs(v1[1])) {
+ if (Math.abs(v1[0]) > Math.abs(v1[2])) {
+ dominantIndex = 0;
+ } else {
+ dominantIndex = 2;
+ }
+ } else {
+ if (Math.abs(v1[1]) > Math.abs(v1[2])) {
+ dominantIndex = 1;
+ } else {
+ dominantIndex = 2;
+ }
+ }
+ tmpPivotVec[dominantIndex] = -v1[(dominantIndex + 1) % 3];
+ tmpPivotVec[(dominantIndex + 1) % 3] = v1[dominantIndex];
+ tmpPivotVec[(dominantIndex + 2) % 3] = 0f;
+ }
+ return setFromAngleNormalAxis(theta, tmpPivotVec);
+ }
+ }
+
+ /***
+ * Initialize this quaternion with given non-normalized axis vector and rotation angle
* <p>
- * Note: Method does not normalize this quaternion!
+ * If axis == 0,0,0 the quaternion is set to identity.
* </p>
+ * @param angle rotation angle (rads)
+ * @param vector axis vector not normalized
+ * @param tmpV3f float[3] temp storage to normalize vector
+ * @return this quaternion for chaining.
+ *
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56">Matrix-FAQ Q56</a>
+ * @see #toAngleAxis(float[])
+ */
+ public final Quaternion setFromAngleAxis(final float angle, final float[] vector, final float[] tmpV3f) {
+ VectorUtil.normalize(tmpV3f, vector);
+ return setFromAngleNormalAxis(angle, tmpV3f);
+ }
+
+ /***
+ * Initialize this quaternion with given normalized axis vector and rotation angle
* <p>
- * See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/
- * quaternions/slerp/
+ * If axis == 0,0,0 the quaternion is set to identity.
* </p>
+ * @param angle rotation angle (rads)
+ * @param vector axis vector normalized
+ * @return this quaternion for chaining.
*
- * @param a initial quaternion
- * @param b target quaternion
- * @param t float between 0 and 1 representing interp.
- */
- public void slerp(Quaternion a, Quaternion b, float t) {
- final float cosom = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
- final float t1 = 1.0f - t;
-
- // if the two quaternions are close, just use linear interpolation
- if (cosom >= 0.95f) {
- x = a.x * t1 + b.x * t;
- y = a.y * t1 + b.y * t;
- z = a.z * t1 + b.z * t;
- w = a.w * t1 + b.w * t;
- return;
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56">Matrix-FAQ Q56</a>
+ * @see #toAngleAxis(float[])
+ */
+ public final Quaternion setFromAngleNormalAxis(final float angle, final float[] vector) {
+ if ( VectorUtil.isZero(vector, 0, FloatUtil.EPSILON) ) {
+ setIdentity();
+ } else {
+ final float halfangle = angle * 0.5f;
+ final float sin = FloatUtil.sin(halfangle);
+ x = vector[0] * sin;
+ y = vector[1] * sin;
+ z = vector[2] * sin;
+ w = FloatUtil.cos(halfangle);
}
+ return this;
+ }
- // the quaternions are nearly opposite, we can pick any axis normal to
- // a,b
- // to do the rotation
- if (cosom <= -0.99f) {
- x = 0.5f * (a.x + b.x);
- y = 0.5f * (a.y + b.y);
- z = 0.5f * (a.z + b.z);
- w = 0.5f * (a.w + b.w);
- return;
+ /**
+ * Transform the rotational quaternion to axis based rotation angles
+ *
+ * @param axis float[3] storage for computed axis
+ * @return the rotation angle in radians
+ * @see #setFromAngleAxis(float, float[], float[])
+ */
+ public final float toAngleAxis(final float[] axis) {
+ final float sqrLength = x*x + y*y + z*z;
+ float angle;
+ if ( FloatUtil.isZero(sqrLength, FloatUtil.EPSILON) ) { // length is ~0
+ angle = 0.0f;
+ axis[0] = 1.0f;
+ axis[1] = 0.0f;
+ axis[2] = 0.0f;
+ } else {
+ angle = FloatUtil.acos(w) * 2.0f;
+ final float invLength = 1.0f / FloatUtil.sqrt(sqrLength);
+ axis[0] = x * invLength;
+ axis[1] = y * invLength;
+ axis[2] = z * invLength;
}
+ return angle;
+ }
+
+ /**
+ * Initializes this quaternion from the given Euler rotation array <code>angradXYZ</code> in radians.
+ * <p>
+ * The <code>angradXYZ</code> array is laid out in natural order:
+ * <ul>
+ * <li>x - bank</li>
+ * <li>y - heading</li>
+ * <li>z - attitude</li>
+ * </ul>
+ * </p>
+ * <p>
+ * The rotations are applied in the given order:
+ * <ul>
+ * <li>y - heading</li>
+ * <li>z - attitude</li>
+ * <li>x - bank</li>
+ * </ul>
+ * </p>
+ *
+ * @param angradXYZ euler angel array in radians
+ * @return this quaternion for chaining.
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60">Matrix-FAQ Q60</a>
+ * @see <a href="http://vered.rose.utoronto.ca/people/david_dir/GEMS/GEMS.html">Gems</a>
+ * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm">euclideanspace.com-eulerToQuaternion</a>
+ * @see #setFromEuler(float, float, float)
+ * @see #toEuler(float[])
+ */
+ public final Quaternion setFromEuler(final float[] angradXYZ) {
+ return setFromEuler(angradXYZ[0], angradXYZ[1], angradXYZ[2]);
+ }
- // cosom is now withion range of acos, do a SLERP
- final float sinom = FloatUtil.sqrt(1.0f - cosom * cosom);
- final float omega = FloatUtil.acos(cosom);
+ /**
+ * Initializes this quaternion from the given Euler rotation angles in radians.
+ * <p>
+ * The rotations are applied in the given order:
+ * <ul>
+ * <li>y - heading</li>
+ * <li>z - attitude</li>
+ * <li>x - bank</li>
+ * </ul>
+ * </p>
+ * @param bankX the Euler pitch angle in radians. (rotation about the X axis)
+ * @param headingY the Euler yaw angle in radians. (rotation about the Y axis)
+ * @param attitudeZ the Euler roll angle in radians. (rotation about the Z axis)
+ * @return this quaternion for chaining.
+ *
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60">Matrix-FAQ Q60</a>
+ * @see <a href="http://vered.rose.utoronto.ca/people/david_dir/GEMS/GEMS.html">Gems</a>
+ * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm">euclideanspace.com-eulerToQuaternion</a>
+ * @see #toEuler(float[])
+ */
+ public final Quaternion setFromEuler(final float bankX, final float headingY, float attitudeZ) {
+ float angle = headingY * 0.5f;
+ final float sinHeadingY = FloatUtil.sin(angle);
+ final float cosHeadingY = FloatUtil.cos(angle);
+ angle = attitudeZ * 0.5f;
+ final float sinAttitudeZ = FloatUtil.sin(angle);
+ final float cosAttitudeZ = FloatUtil.cos(angle);
+ angle = bankX * 0.5f;
+ final float sinBankX = FloatUtil.sin(angle);
+ final float cosBankX = FloatUtil.cos(angle);
- final float scla = FloatUtil.sin(t1 * omega) / sinom;
- final float sclb = FloatUtil.sin(t * omega) / sinom;
+ // variables used to reduce multiplication calls.
+ final float cosHeadingXcosAttitude = cosHeadingY * cosAttitudeZ;
+ final float sinHeadingXsinAttitude = sinHeadingY * sinAttitudeZ;
+ final float cosHeadingXsinAttitude = cosHeadingY * sinAttitudeZ;
+ final float sinHeadingXcosAttitude = sinHeadingY * cosAttitudeZ;
- x = a.x * scla + b.x * sclb;
- y = a.y * scla + b.y * sclb;
- z = a.z * scla + b.z * sclb;
- w = a.w * scla + b.w * sclb;
+ w = cosHeadingXcosAttitude * cosBankX - sinHeadingXsinAttitude * sinBankX;
+ x = cosHeadingXcosAttitude * sinBankX + sinHeadingXsinAttitude * cosBankX;
+ y = sinHeadingXcosAttitude * cosBankX + cosHeadingXsinAttitude * sinBankX;
+ z = cosHeadingXsinAttitude * cosBankX - sinHeadingXcosAttitude * sinBankX;
+ return normalize();
}
/**
- * Check if this quaternion represents an identity matrix for rotation,
- * , ie (0,0,0,1).
+ * Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ).
*
- * @return true if it is an identity rep., false otherwise
+ * @param result the float[] array storing the computed angles.
+ * @return the double[] array, filled with heading, attitude and bank in that order..
+ * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm">euclideanspace.com-quaternionToEuler</a>
+ * @see #setFromEuler(float, float, float)
*/
- public boolean isIdentity() {
- return w == 1 && x == 0 && y == 0 && z == 0;
+ public float[] toEuler(final float[] result) {
+ final float sqw = w*w;
+ final float sqx = x*x;
+ final float sqy = y*y;
+ final float sqz = z*z;
+ final float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise
+ // is correction factor
+ final float test = x*y + z*w;
+
+ if (test > 0.499f * unit) { // singularity at north pole
+ result[0] = 0f;
+ result[1] = 2f * FloatUtil.atan2(x, w);
+ result[2] = FloatUtil.HALF_PI;
+ } else if (test < -0.499f * unit) { // singularity at south pole
+ result[0] = 0f;
+ result[1] = -2 * FloatUtil.atan2(x, w);
+ result[2] = -FloatUtil.HALF_PI;
+ } else {
+ result[0] = FloatUtil.atan2(2f * x * w - 2 * y * z, -sqx + sqy - sqz + sqw);
+ result[1] = FloatUtil.atan2(2f * y * w - 2 * x * z, sqx - sqy - sqz + sqw);
+ result[2] = FloatUtil.asin( 2f * test / unit);
+ }
+ return result;
}
- /***
- * Set this quaternion to identity (x=0,y=0,z=0,w=1)
+ /**
+ * Initializes this quaternion from a 4x4 column rotation matrix
+ * <p>
+ * See <a href="ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z">Graphics Gems Code</a>,<br/>
+ * <a href="http://mathworld.wolfram.com/MatrixTrace.html">MatrixTrace</a>.
+ * </p>
+ * <p>
+ * Buggy <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55">Matrix-FAQ Q55</a>
+ * </p>
+ *
+ * @param m 4x4 column matrix
+ * @return this quaternion for chaining.
+ * @see #toMatrix(float[], int)
*/
- public void setIdentity() {
- x = y = z = 0;
- w = 1;
+ public final Quaternion setFromMatrix(final float[] m, final int m_off) {
+ return setFromMatrix(m[0+0*4+m_off], m[0+1*4+m_off], m[0+2*4+m_off],
+ m[1+0*4+m_off], m[1+1*4+m_off], m[1+2*4+m_off],
+ m[2+0*4+m_off], m[2+1*4+m_off], m[2+2*4+m_off]);
}
/**
- * compute the quaternion from a 3x3 column matrix
+ * Initializes this quaternion from a 4x4 column rotation matrix
+ * <p>
+ * See <a href="ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z">Graphics Gems Code</a>,<br/>
+ * <a href="http://mathworld.wolfram.com/MatrixTrace.html">MatrixTrace</a>.
+ * </p>
+ * <p>
+ * Buggy <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55">Matrix-FAQ Q55</a>
+ * </p>
*
- * @param m 3x3 column matrix
+ * @param m 4x4 column matrix
+ * @return this quaternion for chaining.
+ * @see #toMatrix(FloatBuffer)
*/
- public void setFromMatrix(float[] m) {
- final float T = m[0] + m[4] + m[8] + 1;
- if (T > 0) {
- final float S = 0.5f / FloatUtil.sqrt(T);
- w = 0.25f / S;
- x = (m[5] - m[7]) * S;
- y = (m[6] - m[2]) * S;
- z = (m[1] - m[3]) * S;
+ public final Quaternion setFromMatrix(final FloatBuffer m) {
+ final int m_off = m.position();
+ return setFromMatrix(m.get(0+0*4+m_off), m.get(0+1*4+m_off), m.get(0+2*4+m_off),
+ m.get(1+0*4+m_off), m.get(1+1*4+m_off), m.get(1+2*4+m_off),
+ m.get(2+0*4+m_off), m.get(2+1*4+m_off), m.get(2+2*4+m_off));
+ }
+
+ /**
+ * Compute the quaternion from a 3x3 column rotation matrix
+ * <p>
+ * See <a href="ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z">Graphics Gems Code</a>,<br/>
+ * <a href="http://mathworld.wolfram.com/MatrixTrace.html">MatrixTrace</a>.
+ * </p>
+ * <p>
+ * Buggy <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55">Matrix-FAQ Q55</a>
+ * </p>
+ *
+ * @return this quaternion for chaining.
+ * @see #toMatrix(float[], int)
+ */
+ public Quaternion setFromMatrix(final float m00, final float m01, final float m02,
+ final float m10, final float m11, final float m12,
+ final float m20, final float m21, final float m22) {
+ // Note: Other implementations uses 'T' w/o '+1f' and compares 'T >= 0' while adding missing 1f in sqrt expr.
+ // However .. this causes setLookAt(..) to fail and actually violates the 'trace definition'.
+
+ // The trace T is the sum of the diagonal elements; see
+ // http://mathworld.wolfram.com/MatrixTrace.html
+ final float T = m00 + m11 + m22 + 1f;
+ // System.err.println("setFromMatrix.0 T "+T+", m00 "+m00+", m11 "+m11+", m22 "+m22);
+ if ( T > 0f ) {
+ // System.err.println("setFromMatrix.1");
+ final float S = 0.5f / FloatUtil.sqrt(T); // S = 1 / ( 2 t )
+ w = 0.25f / S; // w = 1 / ( 4 S ) = t / 2
+ x = ( m21 - m12 ) * S;
+ y = ( m02 - m20 ) * S;
+ z = ( m10 - m01 ) * S;
+ } else if ( m00 > m11 && m00 > m22) {
+ // System.err.println("setFromMatrix.2");
+ final float S = 0.5f / FloatUtil.sqrt(1.0f + m00 - m11 - m22); // S=4*qx
+ w = ( m21 - m12 ) * S;
+ x = 0.25f / S;
+ y = ( m10 + m01 ) * S;
+ z = ( m02 + m20 ) * S;
+ } else if ( m11 > m22 ) {
+ // System.err.println("setFromMatrix.3");
+ final float S = 0.5f / FloatUtil.sqrt(1.0f + m11 - m00 - m22); // S=4*qy
+ w = ( m02 - m20 ) * S;
+ x = ( m20 + m01 ) * S;
+ y = 0.25f / S;
+ z = ( m21 + m12 ) * S;
} else {
- if ((m[0] > m[4]) && (m[0] > m[8])) {
- final float S = FloatUtil.sqrt(1.0f + m[0] - m[4] - m[8]) * 2f; // S=4*qx
- w = (m[7] - m[5]) / S;
- x = 0.25f * S;
- y = (m[3] + m[1]) / S;
- z = (m[6] + m[2]) / S;
- } else if (m[4] > m[8]) {
- final float S = FloatUtil.sqrt(1.0f + m[4] - m[0] - m[8]) * 2f; // S=4*qy
- w = (m[6] - m[2]) / S;
- x = (m[3] + m[1]) / S;
- y = 0.25f * S;
- z = (m[7] + m[5]) / S;
- } else {
- final float S = FloatUtil.sqrt(1.0f + m[8] - m[0] - m[4]) * 2f; // S=4*qz
- w = (m[3] - m[1]) / S;
- x = (m[6] + m[2]) / S;
- y = (m[7] + m[5]) / S;
- z = 0.25f * S;
- }
+ // System.err.println("setFromMatrix.3");
+ final float S = 0.5f / FloatUtil.sqrt(1.0f + m22 - m00 - m11); // S=4*qz
+ w = ( m10 - m01 ) * S;
+ x = ( m02 + m20 ) * S;
+ y = ( m21 + m12 ) * S;
+ z = 0.25f / S;
}
+ return this;
+ }
+
+ /**
+ * Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
+ *
+ * @param matrix float[16] store for the resulting normalized column matrix 4x4
+ * @param mat_offset
+ * @return the given matrix store
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54">Matrix-FAQ Q54</a>
+ * @see #setFromMatrix(float[], int)
+ */
+ public final float[] toMatrix(final float[] matrix, final int mat_offset) {
+ // pre-multiply scaled-reciprocal-magnitude to reduce multiplications
+ final float norm = magnitudeSquared();
+ final float srecip = norm == 1.0f ? 2.0f : norm > 0.0f ? 2.0f / norm : 0f;
+
+ final float xs = srecip * x;
+ final float ys = srecip * y;
+ final float zs = srecip * z;
+
+ final float xx = x * xs;
+ final float xy = x * ys;
+ final float xz = x * zs;
+ final float xw = xs * w;
+ final float yy = y * ys;
+ final float yz = y * zs;
+ final float yw = ys * w;
+ final float zz = z * zs;
+ final float zw = zs * w;
+
+ matrix[0+0*4+mat_offset] = 1f - ( yy + zz );
+ matrix[0+1*4+mat_offset] = ( xy - zw );
+ matrix[0+2*4+mat_offset] = ( xz + yw );
+ matrix[0+3*4+mat_offset] = 0f;
+
+ matrix[1+0*4+mat_offset] = ( xy + zw );
+ matrix[1+1*4+mat_offset] = 1f - ( xx + zz );
+ matrix[1+2*4+mat_offset] = ( yz - xw );
+ matrix[1+3*4+mat_offset] = 0f;
+
+ matrix[2+0*4+mat_offset] = ( xz - yw );
+ matrix[2+1*4+mat_offset] = ( yz + xw );
+ matrix[2+2*4+mat_offset] = 1f - ( xx + yy );
+ matrix[2+3*4+mat_offset] = 0f;
+
+ matrix[3+0*4+mat_offset] = 0f;
+ matrix[3+1*4+mat_offset] = 0f;
+ matrix[3+2*4+mat_offset] = 0f;
+ matrix[3+3*4+mat_offset] = 1f;
+ return matrix;
+ }
+
+ /**
+ * Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
+ *
+ * @param matrix FloatBuffer store for the resulting normalized column matrix 4x4
+ * @param mat_offset
+ * @return the given matrix store
+ * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54">Matrix-FAQ Q54</a>
+ * @see #setFromMatrix(FloatBuffer)
+ */
+ public final FloatBuffer toMatrix(final FloatBuffer matrix) {
+ final int mat_offset = matrix.position();
+
+ // pre-multipliy scaled-reciprocal-magnitude to reduce multiplications
+ final float norm = magnitudeSquared();
+ final float srecip = norm == 1.0f ? 2.0f : norm > 0.0f ? 2.0f / norm : 0f;
+
+ final float xs = srecip * x;
+ final float ys = srecip * y;
+ final float zs = srecip * z;
+
+ final float xx = x * xs;
+ final float xy = x * ys;
+ final float xz = x * zs;
+ final float xw = xs * w;
+ final float yy = y * ys;
+ final float yz = y * zs;
+ final float yw = ys * w;
+ final float zz = z * zs;
+ final float zw = zs * w;
+
+ matrix.put(0+0*4+mat_offset, 1f - ( yy + zz ));
+ matrix.put(0+1*4+mat_offset, ( xy - zw ));
+ matrix.put(0+2*4+mat_offset, ( xz + yw ));
+ matrix.put(0+3*4+mat_offset, 0f);
+
+ matrix.put(1+0*4+mat_offset, ( xy + zw ));
+ matrix.put(1+1*4+mat_offset, 1f - ( xx + zz ));
+ matrix.put(1+2*4+mat_offset, ( yz - xw ));
+ matrix.put(1+3*4+mat_offset, 0f);
+
+ matrix.put(2+0*4+mat_offset, ( xz - yw ));
+ matrix.put(2+1*4+mat_offset, ( yz + xw ));
+ matrix.put(2+2*4+mat_offset, 1f - ( xx + yy ));
+ matrix.put(2+3*4+mat_offset, 0f);
+
+ matrix.put(3+0*4+mat_offset, 0f);
+ matrix.put(3+1*4+mat_offset, 0f);
+ matrix.put(3+2*4+mat_offset, 0f);
+ matrix.put(3+3*4+mat_offset, 1f);
+ return matrix;
+ }
+
+ /**
+ * @param index the 3x3 rotation matrix column to retrieve from this quaternion (normalized). Must be between 0 and 2.
+ * @param result the vector object to store the result in.
+ * @return the result column-vector for chaining.
+ */
+ public float[] copyMatrixColumn(final int index, final float[] result, final int resultOffset) {
+ final float norm = magnitudeSquared();
+ final float s = norm == 1.0f ? 2.0f : norm > 0.0f ? 2.0f / norm : 0f;
+
+ // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
+ // will be used 2-4 times each.
+ final float xs = x * s;
+ final float ys = y * s;
+ final float zs = z * s;
+ final float xx = x * xs;
+ final float xy = x * ys;
+ final float xz = x * zs;
+ final float xw = w * xs;
+ final float yy = y * ys;
+ final float yz = y * zs;
+ final float yw = w * ys;
+ final float zz = z * zs;
+ final float zw = w * zs;
+
+ // using s=2/norm (instead of 1/norm) saves 3 multiplications by 2 here
+ switch (index) {
+ case 0:
+ result[0+resultOffset] = 1.0f - (yy + zz);
+ result[1+resultOffset] = xy + zw;
+ result[2+resultOffset] = xz - yw;
+ break;
+ case 1:
+ result[0+resultOffset] = xy - zw;
+ result[1+resultOffset] = 1.0f - (xx + zz);
+ result[2+resultOffset] = yz + xw;
+ break;
+ case 2:
+ result[0+resultOffset] = xz + yw;
+ result[1+resultOffset] = yz - xw;
+ result[2+resultOffset] = 1.0f - (xx + yy);
+ break;
+ default:
+ throw new IllegalArgumentException("Invalid column index. " + index);
+ }
+ return result;
+ }
+
+ /**
+ * Initializes this quaternion to represent a rotation formed by the given three <i>orthogonal</i> axes.
+ * <p>
+ * No validation whether the axes are <i>orthogonal</i> is performed.
+ * </p>
+ *
+ * @param xAxis vector representing the <i>orthogonal</i> x-axis of the coordinate system.
+ * @param yAxis vector representing the <i>orthogonal</i> y-axis of the coordinate system.
+ * @param zAxis vector representing the <i>orthogonal</i> z-axis of the coordinate system.
+ * @return this quaternion for chaining.
+ */
+ public final Quaternion setFromAxes(final float[] xAxis, final float[] yAxis, final float[] zAxis) {
+ return setFromMatrix(xAxis[0], yAxis[0], zAxis[0],
+ xAxis[1], yAxis[1], zAxis[1],
+ xAxis[2], yAxis[2], zAxis[2]);
+ }
+
+ /**
+ * Extracts this quaternion's <i>orthogonal</i> rotation axes.
+ *
+ * @param xAxis vector representing the <i>orthogonal</i> x-axis of the coordinate system.
+ * @param yAxis vector representing the <i>orthogonal</i> y-axis of the coordinate system.
+ * @param zAxis vector representing the <i>orthogonal</i> z-axis of the coordinate system.
+ * @param tmpMat4 temporary float[4] matrix, used to transform this quaternion to a matrix.
+ */
+ public void toAxes(final float[] xAxis, final float[] yAxis, final float[] zAxis, final float[] tmpMat4) {
+ toMatrix(tmpMat4, 0);
+ FloatUtil.copyMatrixColumn(tmpMat4, 0, 2, zAxis, 0);
+ FloatUtil.copyMatrixColumn(tmpMat4, 0, 1, yAxis, 0);
+ FloatUtil.copyMatrixColumn(tmpMat4, 0, 0, xAxis, 0);
}
/**
@@ -391,7 +1083,7 @@ public class Quaternion {
* @param m 3x3 column matrix
* @return true if representing a rotational matrix, false otherwise
*/
- public boolean isRotationMatrix(float[] m) {
+ public final boolean isRotationMatrix3f(float[] m) {
final float epsilon = 0.01f; // margin to allow for rounding errors
if (FloatUtil.abs(m[0] * m[3] + m[3] * m[4] + m[6] * m[7]) > epsilon)
return false;
@@ -405,11 +1097,38 @@ public class Quaternion {
return false;
if (FloatUtil.abs(m[2] * m[2] + m[5] * m[5] + m[8] * m[8] - 1) > epsilon)
return false;
- return (FloatUtil.abs(determinant(m) - 1) < epsilon);
+ return (FloatUtil.abs(determinant4f(m) - 1) < epsilon);
}
- private float determinant(float[] m) {
+ private final float determinant4f(float[] m) {
return m[0] * m[4] * m[8] + m[3] * m[7] * m[2] + m[6] * m[1] * m[5]
- m[0] * m[7] * m[5] - m[3] * m[1] * m[8] - m[6] * m[4] * m[2];
}
+
+ //
+ // std java overrides
+ //
+
+ /**
+ * @param o the object to compare for equality
+ * @return true if this quaternion and the provided quaternion have roughly the same x, y, z and w values.
+ */
+ @Override
+ public boolean equals(final Object o) {
+ if (this == o) {
+ return true;
+ }
+ if (!(o instanceof Quaternion)) {
+ return false;
+ }
+ final Quaternion comp = (Quaternion) o;
+ return Math.abs(x - comp.getX()) <= ALLOWED_DEVIANCE &&
+ Math.abs(y - comp.getY()) <= ALLOWED_DEVIANCE &&
+ Math.abs(z - comp.getZ()) <= ALLOWED_DEVIANCE &&
+ Math.abs(w - comp.getW()) <= ALLOWED_DEVIANCE;
+ }
+
+ public String toString() {
+ return "Quaternion[x "+x+", y "+y+", z "+z+", w "+w+"]";
+ }
}
diff --git a/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java b/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java
index 2001f8cdf..2d88f7937 100644
--- a/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java
+++ b/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java
@@ -48,6 +48,7 @@ import com.jogamp.common.nio.Buffers;
import com.jogamp.common.os.Platform;
import com.jogamp.common.util.FloatStack;
import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Quaternion;
import com.jogamp.opengl.math.geom.Frustum;
/**
@@ -535,6 +536,14 @@ public class PMVMatrix implements GLMatrixFunc {
m.position(spos);
}
+ /**
+ * Load the current matrix with the values of the given {@link Quaternion}'s rotation {@link Quaternion#toMatrix(float[], int) matrix representation}.
+ */
+ public final void glLoadMatrix(final Quaternion quat) {
+ quat.toMatrix(tmpMatrix, 0);
+ glLoadMatrixf(tmpMatrix, 0);
+ }
+
@Override
public final void glPopMatrix() {
final FloatStack stack;
@@ -637,6 +646,14 @@ public class PMVMatrix implements GLMatrixFunc {
glMultMatrixf(matrixRot, 0);
}
+ /**
+ * Rotate the current matrix with the given {@link Quaternion}'s rotation {@link Quaternion#toMatrix(float[], int) matrix representation}.
+ */
+ public final void glRotate(final Quaternion quat) {
+ quat.toMatrix(tmpMatrix, 0);
+ glMultMatrixf(tmpMatrix, 0);
+ }
+
@Override
public final void glScalef(final float x, final float y, final float z) {
// Scale matrix (Any Order):
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-11 12:45:34 +0000