From 70979247aad156418c32959bbf4962f175191ec2 Mon Sep 17 00:00:00 2001 From: Sven Gothel Date: Fri, 14 Mar 2014 08:05:07 +0100 Subject: Quaternion: Fix and enhance class incl. Extensive Unit Tests (all passed) - Add documentation incl references (Matrix-FAQ, Euclideanspace, ..) - Compared w/ other impl., i.e. WildMagic, Ardor3D, .. and added missing functionality incl unit tests. - PMVMatrix: Added convenient Quaternion 'hooks' - glRotate(Quaternion) - glLoadMatrix(Quaternion) --- .../classes/com/jogamp/opengl/math/Quaternion.java | 1183 ++++++++++++++++---- .../classes/com/jogamp/opengl/util/PMVMatrix.java | 17 + 2 files changed, 968 insertions(+), 232 deletions(-) (limited to 'src/jogl/classes/com/jogamp') 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 + * Gimbal-Lock free rotations. + *

+ * All matrix operation provided are in column-major order, + * as specified in the OpenGL fixed function pipeline, i.e. compatibility profile. + * See {@link FloatUtil}. + *

+ *

+ * See Matrix-FAQ + *

+ *

+ * See euclideanspace.com-Quaternion + *

+ */ 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. + *

+ * Using {@value}, which is ~20 times {@link FloatUtil#EPSILON}. + *

+ */ + 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 [-x, -y, -z, w]. + * @return this quaternion for chaining. + * @see Matrix-FAQ Q49 + */ + 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 Matrix-FAQ Q50 + */ + 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 euclideanspace.com-QuaternionAdd */ - 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 euclideanspace.com-QuaternionAdd */ - 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 Matrix-FAQ Q53 + * @see euclideanspace.com-QuaternionMul */ - 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 euclideanspace.com-QuaternionScale */ - 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. + *

+ * The axis must be a normalized vector. + *

+ *

+ * A rotational quaternion is made from the given angle and axis. + *

+ * + * @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 Matrix-FAQ Q63 */ - 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. + *

+ * Note: Method does not normalize this quaternion! + *

+ * + * @param a start quaternion + * @param b end quaternion + * @param changeAmnt float between 0 and 1 representing interpolation. + * @return this quaternion for chaining. + * @see euclideanspace.com-QuaternionSlerp */ - 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 direction and the y-axis to up. + *

+ * Implementation generates a 3x3 matrix + * and is equal with ProjectFloat's lookAt(..).
+ *

+ * + * @param directionIn where to look at + * @param upIn a vector indicating the local up direction. + * @param xAxisOut vector storing the orthogonal x-axis of the coordinate system. + * @param yAxisOut vector storing the orthogonal y-axis of the coordinate system. + * @param zAxisOut vector storing the orthogonal z-axis of the coordinate system. + * @return this quaternion for chaining. + * @see euclideanspace.com-LookUp */ - 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 + * 
+     *   q = (s,v) = (v1•v2 , v1 × v2),
+     *     angle = angle(v1, v2) = v1•v2
+     *      axis = normal(v1 x v2)
+     *
+ * @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 + * 
+     *   q = (s,v) = (v1•v2 , v1 × v2),
+     *     angle = angle(v1, v2) = v1•v2
+     *      axis = v1 x v2
+     *
+ * @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 *

- * Note: Method does not normalize this quaternion! + * If axis == 0,0,0 the quaternion is set to identity. *

+ * @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 Matrix-FAQ Q56 + * @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 *

- * See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/ - * quaternions/slerp/ + * If axis == 0,0,0 the quaternion is set to identity. *

+ * @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 Matrix-FAQ Q56 + * @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 angradXYZ in radians. + *

+ * The angradXYZ array is laid out in natural order: + *

+ *

+ *

+ * The rotations are applied in the given order: + *

+ *

+ * + * @param angradXYZ euler angel array in radians + * @return this quaternion for chaining. + * @see Matrix-FAQ Q60 + * @see Gems + * @see euclideanspace.com-eulerToQuaternion + * @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. + *

+ * The rotations are applied in the given order: + *

+ *

+ * @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 Matrix-FAQ Q60 + * @see Gems + * @see euclideanspace.com-eulerToQuaternion + * @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 euclideanspace.com-quaternionToEuler + * @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 + *

+ * See Graphics Gems Code,
+ * MatrixTrace. + *

+ *

+ * Buggy Matrix-FAQ Q55 + *

+ * + * @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 + *

+ * See Graphics Gems Code,
+ * MatrixTrace. + *

+ *

+ * Buggy Matrix-FAQ Q55 + *

* - * @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 + *

+ * See Graphics Gems Code,
+ * MatrixTrace. + *

+ *

+ * Buggy Matrix-FAQ Q55 + *

+ * + * @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 Matrix-FAQ Q54 + * @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 Matrix-FAQ Q54 + * @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 orthogonal axes. + *

+ * No validation whether the axes are orthogonal is performed. + *

+ * + * @param xAxis vector representing the orthogonal x-axis of the coordinate system. + * @param yAxis vector representing the orthogonal y-axis of the coordinate system. + * @param zAxis vector representing the orthogonal 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 orthogonal rotation axes. + * + * @param xAxis vector representing the orthogonal x-axis of the coordinate system. + * @param yAxis vector representing the orthogonal y-axis of the coordinate system. + * @param zAxis vector representing the orthogonal 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): -- cgit v1.2.3