diff options
Diffstat (limited to 'src')
4 files changed, 1835 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): diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtil01NOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtil01NOUI.java new file mode 100644 index 000000000..370cb4a2f --- /dev/null +++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtil01NOUI.java @@ -0,0 +1,50 @@ +/**
+ * Copyright 2014 JogAmp Community. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, are
+ * permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, this list of
+ * conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, this list
+ * of conditions and the following disclaimer in the documentation and/or other materials
+ * provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * The views and conclusions contained in the software and documentation are those of the
+ * authors and should not be interpreted as representing official policies, either expressed
+ * or implied, of JogAmp Community.
+ */
+
+package com.jogamp.opengl.test.junit.jogl.math;
+
+import org.junit.Test; +import org.junit.FixMethodOrder; +import org.junit.runners.MethodSorters;
+
+import com.jogamp.opengl.math.FloatUtil;
+
+@FixMethodOrder(MethodSorters.NAME_ASCENDING) +public class TestFloatUtil01NOUI {
+ static final float MACH_EPSILON = FloatUtil.getMachineEpsilon();
+
+ @Test
+ public void test01Epsilon() {
+ System.err.println("Machine Epsilon: "+MACH_EPSILON);
+ System.err.println("Fixed Epsilon: "+FloatUtil.EPSILON+", diff "+Math.abs(MACH_EPSILON-FloatUtil.EPSILON));
+ }
+
+ public static void main(String args[]) {
+ org.junit.runner.JUnitCore.main(TestFloatUtil01NOUI.class.getName());
+ }
+}
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestQuaternion01NOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestQuaternion01NOUI.java new file mode 100644 index 000000000..0f47f5889 --- /dev/null +++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestQuaternion01NOUI.java @@ -0,0 +1,817 @@ +/**
+ * Copyright 2014 JogAmp Community. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, are
+ * permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, this list of
+ * conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, this list
+ * of conditions and the following disclaimer in the documentation and/or other materials
+ * provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * The views and conclusions contained in the software and documentation are those of the
+ * authors and should not be interpreted as representing official policies, either expressed
+ * or implied, of JogAmp Community.
+ */
+
+package com.jogamp.opengl.test.junit.jogl.math;
+
+import java.util.Arrays;
+
+import org.junit.Assert;
+import org.junit.Test; +import org.junit.FixMethodOrder; +import org.junit.runners.MethodSorters;
+
+import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Quaternion;
+import com.jogamp.opengl.math.VectorUtil;
+
+@FixMethodOrder(MethodSorters.NAME_ASCENDING) +public class TestQuaternion01NOUI {
+ static final boolean DEBUG = false;
+
+ static final Quaternion QUAT_IDENT = new Quaternion(0f, 0f, 0f, 1f);
+
+ static final float[] ZERO = new float[] { 0f, 0f, 0f };
+ static final float[] ONE = new float[] { 1f, 1f, 1f };
+ static final float[] NEG_ONE = new float[] { -1f, -1f, -1f };
+ static final float[] UNIT_X = new float[] { 1f, 0f, 0f };
+ static final float[] UNIT_Y = new float[] { 0f, 1f, 0f };
+ static final float[] UNIT_Z = new float[] { 0f, 0f, 1f };
+ static final float[] NEG_UNIT_X = new float[] { -1f, 0f, 0f };
+ static final float[] NEG_UNIT_Y = new float[] { 0f, -1f, 0f };
+ static final float[] NEG_UNIT_Z = new float[] { 0f, 0f, -1f };
+
+ static final float[] NEG_ONE_v4 = new float[] { -1f, -1f, -1f, 0f };
+ static final float[] ONE_v4 = new float[] { 1f, 1f, 1f, 0f };
+
+ static final float MACH_EPSILON = FloatUtil.getMachineEpsilon();
+
+ //
+ // Basic
+ //
+
+ @Test
+ public void test01Normalize() {
+ final Quaternion quat = new Quaternion(0, 1, 2, 3);
+ final Quaternion quat2 = new Quaternion(quat).normalize();
+ // Assert.assertTrue(Math.abs(1 - quat2.magnitude()) <= MACH_EPSILON);
+ Assert.assertEquals(0f, Math.abs(1 - quat2.magnitude()), MACH_EPSILON);
+ }
+
+ @Test
+ public void test02RotateZeroVector() {
+ final Quaternion quat = new Quaternion();
+ final float[] rotVec0 = quat.rotateVector(new float[3], 0, ZERO, 0);
+ Assert.assertArrayEquals(ZERO, rotVec0, FloatUtil.EPSILON);
+ }
+
+ @Test
+ public void test03InvertAndConj() {
+ // inversion check
+ {
+ final Quaternion quat0 = new Quaternion(0, 1, 2, 3);
+ final Quaternion quat0Inv = new Quaternion(quat0).invert();
+ Assert.assertEquals(quat0, quat0Inv.invert());
+ }
+ // conjugate check
+ {
+ final Quaternion quat0 = new Quaternion(-1f, -2f, -3f, 4f);
+ final Quaternion quat0Conj = new Quaternion( 1f, 2f, 3f, 4f).conjugate();
+ Assert.assertEquals(quat0, quat0Conj);
+ }
+ }
+
+ @Test
+ public void test04Dot() {
+ final Quaternion quat = new Quaternion(7f, 2f, 5f, -1f);
+ Assert.assertTrue(35.0f == quat.dot(3f, 1f, 2f, -2f));
+ Assert.assertTrue(-11.0f == quat.dot(new Quaternion(-1f, 1f, -1f, 1f)));
+ }
+
+
+ //
+ // Conversion
+ //
+
+ @Test
+ public void test10AngleAxis() {
+ final float[] tmpV3f = new float[3];
+ final Quaternion quat1 = new Quaternion().setFromAngleAxis(FloatUtil.HALF_PI, new float[] { 2, 0, 0 }, tmpV3f );
+ final Quaternion quat2 = new Quaternion().setFromAngleNormalAxis(FloatUtil.HALF_PI, new float[] { 1, 0, 0 } );
+
+ Assert.assertEquals(quat2, quat1);
+ // System.err.println("M "+quat2.magnitude()+", 1-M "+(1f-quat2.magnitude())+", Eps "+FloatUtil.EPSILON);
+ Assert.assertEquals(0f, 1 - quat2.magnitude(), FloatUtil.EPSILON);
+ Assert.assertTrue(1 - quat1.magnitude() <= FloatUtil.EPSILON);
+
+ final float[] vecOut1 = new float[3];
+ final float[] vecOut2 = new float[3];
+ quat1.rotateVector(vecOut1, 0, ONE, 0);
+ quat2.rotateVector(vecOut2, 0, ONE, 0);
+ Assert.assertArrayEquals(vecOut1, vecOut2, FloatUtil.EPSILON);
+ Assert.assertEquals(0f, Math.abs( VectorUtil.distance(vecOut1, vecOut2) ), FloatUtil.EPSILON );
+
+ quat1.rotateVector(vecOut1, 0, UNIT_Z, 0);
+ Assert.assertEquals(0f, Math.abs( VectorUtil.distance(NEG_UNIT_Y, vecOut1) ), FloatUtil.EPSILON );
+
+ quat2.setFromAngleAxis(FloatUtil.HALF_PI, ZERO, tmpV3f);
+ Assert.assertEquals(QUAT_IDENT, quat2);
+
+ float angle = quat1.toAngleAxis(vecOut1);
+ quat2.setFromAngleAxis(angle, vecOut1, tmpV3f);
+ Assert.assertEquals(quat1, quat2);
+
+ quat1.set(0, 0, 0, 0);
+ angle = quat1.toAngleAxis(vecOut1);
+ Assert.assertTrue(0.0f == angle);
+ Assert.assertArrayEquals(UNIT_X, vecOut1, FloatUtil.EPSILON);
+ }
+
+ @Test
+ public void test11FromVectorToVector() {
+ final float[] tmp0V3f = new float[3];
+ final float[] tmp1V3f = new float[3];
+ final float[] vecOut = new float[3];
+ final Quaternion quat = new Quaternion();
+ quat.setFromVectors(UNIT_Z, UNIT_X, tmp0V3f, tmp1V3f);
+
+ final Quaternion quat2 = new Quaternion();
+ quat2.setFromNormalVectors(UNIT_Z, UNIT_X, tmp0V3f);
+ Assert.assertEquals(quat, quat2);
+
+ quat2.setFromAngleAxis(FloatUtil.HALF_PI, UNIT_Y, tmp0V3f);
+ Assert.assertEquals(quat2, quat);
+
+ quat.setFromVectors(UNIT_Z, NEG_UNIT_Z, tmp0V3f, tmp1V3f);
+ quat.rotateVector(vecOut, 0, UNIT_Z, 0);
+ // System.err.println("vecOut: "+Arrays.toString(vecOut));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_Z, vecOut) ), Quaternion.ALLOWED_DEVIANCE );
+
+ quat.setFromVectors(UNIT_X, NEG_UNIT_X, tmp0V3f, tmp1V3f);
+ quat.rotateVector(vecOut, 0, UNIT_X, 0);
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_X, vecOut) ), Quaternion.ALLOWED_DEVIANCE );
+
+ quat.setFromVectors(UNIT_Y, NEG_UNIT_Y, tmp0V3f, tmp1V3f);
+ quat.rotateVector(vecOut, 0, UNIT_Y, 0);
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_Y, vecOut) ), Quaternion.ALLOWED_DEVIANCE );
+
+ quat.setFromVectors(ONE, NEG_ONE, tmp0V3f, tmp1V3f);
+ quat.rotateVector(vecOut, 0, ONE, 0);
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_ONE, vecOut) ), Quaternion.ALLOWED_DEVIANCE );
+
+ quat.setFromVectors(ZERO, ZERO, tmp0V3f, tmp1V3f);
+ Assert.assertEquals(QUAT_IDENT, quat);
+ }
+
+ @Test
+ public void test12FromAndToEulerAngles() {
+ // Y.Z.X -> X.Y.Z
+ final Quaternion quat = new Quaternion();
+ final float[] angles0Exp = new float[] { 0f, FloatUtil.HALF_PI, 0f};
+ quat.setFromEuler(angles0Exp);
+ Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
+
+ final float[] angles0Has = quat.toEuler(new float[3]);
+ // System.err.println("exp0 "+Arrays.toString(angles0Exp));
+ // System.err.println("has0 "+Arrays.toString(angles0Has));
+ Assert.assertArrayEquals(angles0Exp, angles0Has, FloatUtil.EPSILON);
+
+ final Quaternion quat2 = new Quaternion();
+ quat2.setFromEuler(angles0Has);
+ Assert.assertEquals(quat, quat2);
+
+ ///
+
+ final float[] angles1Exp = new float[] { 0f, 0f, -FloatUtil.HALF_PI };
+ quat.setFromEuler(angles1Exp);
+ Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
+
+ final float[] angles1Has = quat.toEuler(new float[3]);
+ // System.err.println("exp1 "+Arrays.toString(angles1Exp));
+ // System.err.println("has1 "+Arrays.toString(angles1Has));
+ Assert.assertArrayEquals(angles1Exp, angles1Has, FloatUtil.EPSILON);
+
+ quat2.setFromEuler(angles1Has);
+ Assert.assertEquals(quat, quat2);
+
+ ///
+
+ final float[] angles2Exp = new float[] { FloatUtil.HALF_PI, 0f, 0f };
+ quat.setFromEuler(angles2Exp);
+ Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
+
+ final float[] angles2Has = quat.toEuler(new float[3]);
+ // System.err.println("exp2 "+Arrays.toString(angles2Exp));
+ // System.err.println("has2 "+Arrays.toString(angles2Has));
+ Assert.assertArrayEquals(angles2Exp, angles2Has, FloatUtil.EPSILON);
+
+ quat2.setFromEuler(angles2Has);
+ Assert.assertEquals(quat, quat2);
+ }
+
+ @Test
+ public void test13FromEulerAnglesAndRotateVector() {
+ final Quaternion quat = new Quaternion();
+ quat.setFromEuler(0, FloatUtil.HALF_PI, 0); // 90 degrees y-axis
+ Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
+
+ final float[] v2 = quat.rotateVector(new float[3], 0, UNIT_X, 0);
+ Assert.assertEquals(0f, Math.abs(VectorUtil.distance(NEG_UNIT_Z, v2)), FloatUtil.EPSILON);
+
+ quat.setFromEuler(0, 0, -FloatUtil.HALF_PI);
+ Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
+ quat.rotateVector(v2, 0, UNIT_X, 0);
+ Assert.assertEquals(0f, Math.abs(VectorUtil.distance(NEG_UNIT_Y, v2)), FloatUtil.EPSILON);
+
+ quat.setFromEuler(FloatUtil.HALF_PI, 0, 0);
+ Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
+ quat.rotateVector(v2, 0, UNIT_Y, 0);
+ Assert.assertEquals(0f, Math.abs(VectorUtil.distance(UNIT_Z, v2)), FloatUtil.EPSILON);
+ }
+
+ @Test
+ public void test14Matrix() {
+ final float[] vecHas = new float[3];
+ final float[] vecOut2 = new float[4];
+ float[] mat1 = new float[4*4];
+ float[] mat2 = new float[4*4];
+ final Quaternion quat = new Quaternion();
+
+ //
+ // IDENTITY CHECK
+ //
+ FloatUtil.makeIdentityf(mat1, 0);
+ quat.set(0, 0, 0, 0);
+ quat.toMatrix(mat2, 0);
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+
+ //
+ // 90 degrees rotation on X
+ //
+
+ float a = FloatUtil.HALF_PI;
+ mat1 = new float[] { // Column Order
+ 1, 0, 0, 0, //
+ 0, FloatUtil.cos(a), FloatUtil.sin(a), 0, //
+ 0, -FloatUtil.sin(a), FloatUtil.cos(a), 0,
+ 0, 0, 0, 1 };
+ {
+ // Validate Matrix via Euler rotation on Quaternion!
+ quat.setFromEuler(a, 0f, 0f);
+ quat.toMatrix(mat2, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+ quat.rotateVector(vecHas, 0, UNIT_Y, 0);
+ // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(UNIT_Z, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+ }
+ quat.setFromMatrix(mat1, 0);
+ quat.rotateVector(vecHas, 0, UNIT_Y, 0);
+ // System.err.println("exp0 "+Arrays.toString(UNIT_Z));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(UNIT_Z, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+
+ quat.toMatrix(mat2, 0);
+ // System.err.println(FloatUtil.matrixToString(null, null, "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+
+ quat.rotateVector(vecHas, 0, NEG_ONE, 0);
+ FloatUtil.multMatrixVecf(mat2, NEG_ONE_v4, vecOut2);
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );
+
+ //
+ // 180 degrees rotation on X
+ //
+ a = FloatUtil.PI;
+ mat1 = new float[] { // Column Order
+ 1, 0, 0, 0, //
+ 0, FloatUtil.cos(a), FloatUtil.sin(a), 0, //
+ 0, -FloatUtil.sin(a), FloatUtil.cos(a), 0,
+ 0, 0, 0, 1 };
+ {
+ // Validate Matrix via Euler rotation on Quaternion!
+ quat.setFromEuler(a, 0f, 0f);
+ quat.toMatrix(mat2, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+ quat.rotateVector(vecHas, 0, UNIT_Y, 0);
+ // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_Y, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+ }
+ quat.setFromMatrix(mat1, 0);
+ quat.rotateVector(vecHas, 0, UNIT_Y, 0);
+ // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_Y));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_Y, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+
+ quat.toMatrix(mat2, 0);
+ // System.err.println(FloatUtil.matrixToString(null, null, "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+
+ quat.rotateVector(vecHas, 0, ONE, 0);
+ FloatUtil.multMatrixVecf(mat2, ONE_v4, vecOut2);
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );
+
+ //
+ // 180 degrees rotation on Y
+ //
+ a = FloatUtil.PI;
+ mat1 = new float[] { // Column Order
+ FloatUtil.cos(a), 0, -FloatUtil.sin(a), 0, //
+ 0, 1, 0, 0, //
+ FloatUtil.sin(a), 0, FloatUtil.cos(a), 0,
+ 0, 0, 0, 1 };
+ {
+ // Validate Matrix via Euler rotation on Quaternion!
+ quat.setFromEuler(0f, a, 0f);
+ quat.toMatrix(mat2, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+ quat.rotateVector(vecHas, 0, UNIT_X, 0);
+ // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_X, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+ }
+ quat.setFromMatrix(mat1, 0);
+ quat.rotateVector(vecHas, 0, UNIT_X, 0);
+ // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_X, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+
+ quat.toMatrix(mat2, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "matr-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+
+ quat.rotateVector(vecHas, 0, NEG_ONE, 0);
+ FloatUtil.multMatrixVecf(mat2, NEG_ONE_v4, vecOut2);
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );
+
+ //
+ // 180 degrees rotation on Z
+ //
+ a = FloatUtil.PI;
+ mat1 = new float[] { // Column Order
+ FloatUtil.cos(a), FloatUtil.sin(a), 0, 0, //
+ -FloatUtil.sin(a), FloatUtil.cos(a), 0, 0,
+ 0, 0, 1, 0,
+ 0, 0, 0, 1 };
+ {
+ // Validate Matrix via Euler rotation on Quaternion!
+ quat.setFromEuler(0f, 0f, a);
+ quat.toMatrix(mat2, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+ quat.rotateVector(vecHas, 0, UNIT_X, 0);
+ // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_X, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+ }
+ quat.setFromMatrix(mat1, 0);
+ quat.rotateVector(vecHas, 0, UNIT_X, 0);
+ // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_X, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+
+ quat.toMatrix(mat2, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "matr-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+
+ quat.rotateVector(vecHas, 0, ONE, 0);
+ FloatUtil.multMatrixVecf(mat2, ONE_v4, vecOut2);
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );
+
+ //
+ // Test Matrix-Columns
+ //
+
+ a = FloatUtil.QUARTER_PI;
+ float[] vecExp = new float[3];
+ float[] vecCol = new float[3];
+ mat1 = new float[] { // Column Order
+ FloatUtil.cos(a), FloatUtil.sin(a), 0, 0, //
+ -FloatUtil.sin(a), FloatUtil.cos(a), 0, 0,
+ 0, 0, 1, 0,
+ 0, 0, 0, 1 };
+ quat.setFromMatrix(mat1, 0);
+ FloatUtil.copyMatrixColumn(mat1, 0, 0, vecExp, 0);
+ quat.copyMatrixColumn(0, vecCol, 0);
+ // System.err.println("exp0 "+Arrays.toString(vecExp));
+ // System.err.println("has0 "+Arrays.toString(vecCol));
+ Assert.assertEquals(0f, Math.abs( VectorUtil.distance(vecExp, vecCol)), FloatUtil.EPSILON);
+
+ FloatUtil.copyMatrixColumn(mat1, 0, 1, vecExp, 0);
+ quat.copyMatrixColumn(1, vecCol, 0);
+ // System.err.println("exp1 "+Arrays.toString(vecExp));
+ // System.err.println("has1 "+Arrays.toString(vecCol));
+ Assert.assertEquals(0f, Math.abs( VectorUtil.distance(vecExp, vecCol)), FloatUtil.EPSILON);
+
+ FloatUtil.copyMatrixColumn(mat1, 0, 2, vecExp, 0);
+ quat.copyMatrixColumn(2, vecCol, 0);
+ // System.err.println("exp2 "+Arrays.toString(vecExp));
+ // System.err.println("has2 "+Arrays.toString(vecCol));
+ Assert.assertEquals(0f, Math.abs( VectorUtil.distance(vecExp, vecCol)), FloatUtil.EPSILON);
+
+ quat.set(0f, 0f, 0f, 0f);
+ Assert.assertArrayEquals(UNIT_X, quat.copyMatrixColumn(0, vecCol, 0), FloatUtil.EPSILON);
+
+ }
+
+ @Test
+ public void test15aAxesAndMatrix() {
+ final float[] eulerExp = new float[] { 0f, FloatUtil.HALF_PI, 0f };
+ final float[] matExp = new float[4*4];
+ FloatUtil.makeRotationEuler(eulerExp[0], eulerExp[1], eulerExp[2], matExp, 0); // 45 degr on X, 90 degr on Y
+
+ final float[] matHas = new float[4*4];
+ final Quaternion quat1 = new Quaternion();
+ quat1.setFromEuler(eulerExp);
+ quat1.toMatrix(matHas, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "exp-has", "%10.5f", matExp, 0, matHas, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(matExp, matHas, FloatUtil.EPSILON);
+
+ final float[] eulerHas = new float[3];
+ final Quaternion quat2 = new Quaternion();
+ quat2.setFromMatrix(matExp, 0);
+ quat2.toEuler(eulerHas);
+ // System.err.println("exp-euler "+Arrays.toString(eulerExp));
+ // System.err.println("has-euler "+Arrays.toString(eulerHas));
+ Assert.assertArrayEquals(eulerExp, eulerHas, FloatUtil.EPSILON);
+
+ Assert.assertEquals(quat2, quat1);
+
+ final float[] angles = new float[3];
+ quat2.toEuler(angles);
+ quat1.setFromEuler(angles);
+ Assert.assertEquals(quat2, quat1);
+ }
+
+ @Test
+ public void test15bAxesAndMatrix() {
+ final float[] eulerExp = new float[] { FloatUtil.HALF_PI, 0f, 0f };
+ final float[] matExp = new float[4*4];
+ FloatUtil.makeRotationEuler(eulerExp[0], eulerExp[1], eulerExp[2], matExp, 0); // 45 degr on X, 90 degr on Y
+
+ final float[] matHas = new float[4*4];
+ final Quaternion quat1 = new Quaternion();
+ quat1.setFromEuler(eulerExp);
+ quat1.toMatrix(matHas, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "exp-has", "%10.5f", matExp, 0, matHas, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(matExp, matHas, FloatUtil.EPSILON);
+
+ final float[] eulerHas = new float[3];
+ final Quaternion quat2 = new Quaternion();
+ quat2.setFromMatrix(matExp, 0);
+ quat2.toEuler(eulerHas);
+ // System.err.println("exp-euler "+Arrays.toString(eulerExp));
+ // System.err.println("has-euler "+Arrays.toString(eulerHas));
+ Assert.assertArrayEquals(eulerExp, eulerHas, FloatUtil.EPSILON);
+
+ Assert.assertEquals(quat2, quat1);
+
+ final float[] angles = new float[3];
+ quat2.toEuler(angles);
+ quat1.setFromEuler(angles);
+ Assert.assertEquals(quat2, quat1);
+ }
+
+ @Test
+ public void test15cAxesAndMatrix() {
+ final float[] eulerExp = new float[] { FloatUtil.QUARTER_PI, FloatUtil.HALF_PI, 0f };
+ final float[] matExp = new float[4*4];
+ FloatUtil.makeRotationEuler(eulerExp[0], eulerExp[1], eulerExp[2], matExp, 0); // 45 degr on X, 90 degr on Y
+
+ final float[] matHas = new float[4*4];
+ final Quaternion quat1 = new Quaternion();
+ quat1.setFromEuler(eulerExp);
+ quat1.toMatrix(matHas, 0);
+ // System.err.println(FloatUtil.matrixToString(null, "exp-has", "%10.5f", matExp, 0, matHas, 0, 4, 4, false).toString());
+ Assert.assertArrayEquals(matExp, matHas, FloatUtil.EPSILON);
+
+ final float[] eulerHas = new float[3];
+ final Quaternion quat2 = new Quaternion();
+ quat2.setFromMatrix(matExp, 0);
+ quat2.toEuler(eulerHas);
+ // System.err.println("exp-euler "+Arrays.toString(eulerExp));
+ // System.err.println("has-euler "+Arrays.toString(eulerHas));
+ Assert.assertArrayEquals(eulerExp, eulerHas, FloatUtil.EPSILON);
+
+ Assert.assertEquals(quat2, quat1);
+
+ final float[] angles = new float[3];
+ quat2.toEuler(angles);
+ quat1.setFromEuler(angles);
+ Assert.assertEquals(quat2, quat1);
+ }
+
+ //
+ // Functions
+ //
+
+ @Test
+ public void test20AddSubtract() {
+ final Quaternion quatExp1 = new Quaternion(1, 2, 3, 4);
+ final Quaternion quat1 = new Quaternion(0, 1, 2, 3);
+ final Quaternion quat2 = new Quaternion(1, 1, 1, 1);
+
+ final Quaternion quatHas = new Quaternion();
+ quatHas.set(quat1);
+ quatHas.add(quat2); // q3 = q1 + q2
+ Assert.assertEquals(quatExp1, quatHas);
+
+ quat1.set(0, 1, 2, 3);
+ quat2.set(1, 1, 1, 1);
+ quatHas.set(quat1);
+ quatHas.subtract(quat2); // q3 = q1 - q2
+ Assert.assertEquals(new Quaternion(-1, 0, 1, 2), quatHas);
+ }
+
+ @Test
+ public void test21Multiply() {
+ final Quaternion quat1 = new Quaternion(0.5f, 1f, 2f, 3f);
+ final Quaternion quat2 = new Quaternion();
+
+ quat2.set(quat1);
+ quat2.scale(2f); // q2 = q1 * 2f
+ Assert.assertEquals(new Quaternion(1, 2, 4, 6), quat2);
+
+ quat2.set(quat1);
+ quat2.scale(4f); // q2 = q1 * 4f
+ Assert.assertEquals(new Quaternion(2, 4, 8, 12), quat2);
+
+ //
+ // mul and cmp rotated vector
+ //
+ quat1.setFromAngleNormalAxis(FloatUtil.QUARTER_PI, UNIT_Y); // 45 degr on Y
+ quat2.set(quat1);
+ quat2.mult(quat1); // q2 = q1 * q1 -> 2 * 45 degr -> 90 degr on Y
+
+ final float[] vecOut = new float[3];
+ quat2.rotateVector(vecOut, 0, UNIT_Z, 0);
+ Assert.assertTrue( Math.abs( VectorUtil.distance(UNIT_X, vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
+
+ quat2.setFromAngleNormalAxis(FloatUtil.HALF_PI, UNIT_Y); // 90 degr on Y
+ quat1.mult(quat1); // q1 = q1 * q1 -> 2 * 45 degr -> 90 degr on Y
+ quat1.mult(quat2); // q1 = q1 * q2 -> 2 * 90 degr -> 180 degr on Y
+ quat1.rotateVector(vecOut, 0, UNIT_Z, 0);
+ Assert.assertTrue( Math.abs( VectorUtil.distance(NEG_UNIT_Z, vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
+
+ quat2.setFromEuler(0f, FloatUtil.HALF_PI, 0f);
+ quat1.mult(quat2); // q1 = q1 * q2 = q1 * rotMat(0, 90degr, 0)
+ quat1.rotateVector(vecOut, 0, UNIT_Z, 0);
+ Assert.assertTrue( Math.abs( VectorUtil.distance(NEG_UNIT_X, vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
+ }
+
+ @Test
+ public void test22InvertMultNormalAndConj() {
+ final Quaternion quat0 = new Quaternion(0, 1, 2, 3);
+ final Quaternion quat1 = new Quaternion(quat0);
+ final Quaternion quat2 = new Quaternion(quat0);
+ quat1.invert(); // q1 = invert(q0)
+ quat2.mult(quat1); // q2 = q0 * q1 = q0 * invert(q0)
+ Assert.assertEquals(QUAT_IDENT, quat2);
+ quat1.invert();
+ Assert.assertEquals(quat0, quat1);
+
+ // normalized version
+ quat0.setFromAngleNormalAxis(FloatUtil.QUARTER_PI, UNIT_Y);
+ quat1.set(quat0);
+ quat1.invert(); // q1 = invert(q0)
+ quat2.set(quat0);
+ quat2.mult(quat1); // q2 = q0 * q1 = q0 * invert(q0)
+ Assert.assertEquals(QUAT_IDENT, quat2);
+ quat1.invert();
+ Assert.assertEquals(quat0, quat1);
+
+ // conjugate check
+ quat0.set(-1f, -2f, -3f, 4f);
+ quat1.set( 1f, 2f, 3f, 4f);
+ quat2.set(quat1);
+ quat2.conjugate();
+ Assert.assertEquals(quat0, quat2);
+ }
+
+ @Test
+ public void test23RotationOrder() {
+ {
+ final Quaternion quat1 = new Quaternion().setFromEuler( -2f*FloatUtil.HALF_PI, 0f, 0f); // -180 degr X
+ final Quaternion quat2 = new Quaternion().rotateByAngleX( -2f * FloatUtil.HALF_PI); // angle: -180 degrees, axis X
+ Assert.assertEquals(quat1, quat2);
+ }
+ {
+ final Quaternion quat1 = new Quaternion().setFromEuler( FloatUtil.HALF_PI, 0f, 0f); // 90 degr X
+ final Quaternion quat2 = new Quaternion().rotateByAngleX( FloatUtil.HALF_PI); // angle: 90 degrees, axis X
+ Assert.assertEquals(quat1, quat2);
+ }
+ {
+ final Quaternion quat1 = new Quaternion().setFromEuler( FloatUtil.HALF_PI, FloatUtil.QUARTER_PI, 0f);
+ final Quaternion quat2 = new Quaternion().rotateByAngleY(FloatUtil.QUARTER_PI).rotateByAngleX(FloatUtil.HALF_PI);
+ Assert.assertEquals(quat1, quat2);
+ }
+ {
+ final Quaternion quat1 = new Quaternion().setFromEuler( FloatUtil.PI, FloatUtil.QUARTER_PI, FloatUtil.HALF_PI);
+ final Quaternion quat2 = new Quaternion().rotateByAngleY(FloatUtil.QUARTER_PI).rotateByAngleZ(FloatUtil.HALF_PI).rotateByAngleX(FloatUtil.PI);
+ Assert.assertEquals(quat1, quat2);
+ }
+
+
+ float[] vecExp = new float[3];
+ float[] vecRot = new float[3];
+ final Quaternion quat = new Quaternion();
+
+ // Try a new way with new angles...
+ quat.setFromEuler(FloatUtil.HALF_PI, FloatUtil.QUARTER_PI, FloatUtil.PI);
+ vecRot = new float[] { 1f, 1f, 1f };
+ quat.rotateVector(vecRot, 0, vecRot, 0);
+
+ // expected
+ vecExp = new float[] { 1f, 1f, 1f };
+ final Quaternion worker = new Quaternion();
+ // put together matrix, then apply to vector, so YZX
+ worker.rotateByAngleY(FloatUtil.QUARTER_PI).rotateByAngleZ(FloatUtil.PI).rotateByAngleX(FloatUtil.HALF_PI);
+ quat.rotateVector(vecExp, 0, vecExp, 0);
+ Assert.assertEquals(0f, VectorUtil.distance(vecExp, vecRot), FloatUtil.EPSILON);
+
+ // test axis rotation methods against general purpose
+ // X AXIS
+ vecExp = new float[] { 1f, 1f, 1f };
+ vecRot = new float[] { 1f, 1f, 1f };
+ worker.setIdentity().rotateByAngleX(FloatUtil.QUARTER_PI).rotateVector(vecExp, 0, vecExp, 0);
+ worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 1f, 0f, 0f).rotateVector(vecRot, 0, vecRot, 0);
+ // System.err.println("exp0 "+Arrays.toString(vecExp)+", len "+VectorUtil.length(vecExp));
+ // System.err.println("has0 "+Arrays.toString(vecRot)+", len "+VectorUtil.length(vecRot));
+ Assert.assertEquals(0f, VectorUtil.distance(vecExp, vecRot), FloatUtil.EPSILON);
+
+ // Y AXIS
+ vecExp = new float[] { 1f, 1f, 1f };
+ vecRot = new float[] { 1f, 1f, 1f };
+ worker.setIdentity().rotateByAngleY(FloatUtil.QUARTER_PI).rotateVector(vecExp, 0, vecExp, 0);
+ worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 0f, 1f, 0f).rotateVector(vecRot, 0, vecRot, 0);
+ // System.err.println("exp0 "+Arrays.toString(vecExp));
+ // System.err.println("has0 "+Arrays.toString(vecRot));
+ Assert.assertEquals(0f, VectorUtil.distance(vecExp, vecRot), FloatUtil.EPSILON);
+
+ // Z AXIS
+ vecExp = new float[] { 1f, 1f, 1f };
+ vecRot = new float[] { 1f, 1f, 1f };
+ worker.setIdentity().rotateByAngleZ(FloatUtil.QUARTER_PI).rotateVector(vecExp, 0, vecExp, 0);
+ worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 0f, 0f, 1f).rotateVector(vecRot, 0, vecRot, 0);
+ // System.err.println("exp0 "+Arrays.toString(vecExp));
+ // System.err.println("has0 "+Arrays.toString(vecRot));
+ Assert.assertEquals(0f, VectorUtil.distance(vecExp, vecRot), FloatUtil.EPSILON);
+
+ quat.set(worker);
+ worker.rotateByAngleNormalAxis(0f, 0f, 0f, 0f);
+ Assert.assertEquals(quat, worker);
+ }
+
+ @Test
+ public void test24Axes() {
+ final Quaternion quat0 = new Quaternion().rotateByAngleX(FloatUtil.QUARTER_PI).rotateByAngleY(FloatUtil.HALF_PI);
+ final float[] rotMat = new float[4*4];
+ quat0.toMatrix(rotMat, 0);
+ final float[] xAxis = new float[3];
+ final float[] yAxis = new float[3];
+ final float[] zAxis = new float[3];
+ FloatUtil.copyMatrixColumn(rotMat, 0, 0, xAxis, 0);
+ FloatUtil.copyMatrixColumn(rotMat, 0, 1, yAxis, 0);
+ FloatUtil.copyMatrixColumn(rotMat, 0, 2, zAxis, 0);
+
+ final Quaternion quat1 = new Quaternion().setFromAxes(xAxis, yAxis, zAxis);
+ Assert.assertEquals(quat0, quat1);
+ final Quaternion quat2 = new Quaternion().setFromMatrix(rotMat, 0);
+ Assert.assertEquals(quat2, quat1);
+
+ quat1.toAxes(xAxis, yAxis, zAxis, rotMat);
+ quat2.setFromAxes(xAxis, yAxis, zAxis);
+ Assert.assertEquals(quat0, quat2);
+ Assert.assertEquals(quat1, quat2);
+ }
+
+ @Test
+ public void test25Slerp() {
+ final Quaternion quat1 = new Quaternion(); // angle: 0 degrees
+ final Quaternion quat2 = new Quaternion().rotateByAngleY(FloatUtil.HALF_PI); // angle: 90 degrees, axis Y
+
+ float[] vecExp = new float[] { FloatUtil.sin(FloatUtil.QUARTER_PI), 0f, FloatUtil.sin(FloatUtil.QUARTER_PI) };
+ final float[] vecHas = new float[3];
+ final Quaternion quatS = new Quaternion();
+ // System.err.println("Slerp #01: 1/2 * 90 degrees Y");
+ quatS.setSlerp(quat1, quat2, 0.5f);
+ quatS.rotateVector(vecHas, 0, UNIT_Z, 0);
+ // System.err.println("exp0 "+Arrays.toString(vecExp));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(vecExp, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+
+ // delta == 100%
+ quat2.setIdentity().rotateByAngleZ(FloatUtil.PI); // angle: 180 degrees, axis Z
+ // System.err.println("Slerp #02: 1 * 180 degrees Z");
+ quatS.setSlerp(quat1, quat2, 1.0f);
+ quatS.rotateVector(vecHas, 0, UNIT_X, 0);
+ // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(NEG_UNIT_X, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+
+ quat2.setIdentity().rotateByAngleZ(FloatUtil.PI); // angle: 180 degrees, axis Z
+ // System.err.println("Slerp #03: 1/2 * 180 degrees Z");
+ quatS.setSlerp(quat1, quat2, 0.5f);
+ quatS.rotateVector(vecHas, 0, UNIT_X, 0);
+ // System.err.println("exp0 "+Arrays.toString(UNIT_Y));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(UNIT_Y, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+
+ // delta == 0%
+ quat2.setIdentity().rotateByAngleZ(FloatUtil.PI); // angle: 180 degrees, axis Z
+ // System.err.println("Slerp #04: 0 * 180 degrees Z");
+ quatS.setSlerp(quat1, quat2, 0.0f);
+ quatS.rotateVector(vecHas, 0, UNIT_X, 0);
+ // System.err.println("exp0 "+Arrays.toString(UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(UNIT_X, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+
+ // a==b
+ quat2.setIdentity();
+ // System.err.println("Slerp #05: 1/4 * 0 degrees");
+ quatS.setSlerp(quat1, quat2, 0.25f); // 1/4 of identity .. NOP
+ quatS.rotateVector(vecHas, 0, UNIT_X, 0);
+ // System.err.println("exp0 "+Arrays.toString(UNIT_X));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(UNIT_X, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+
+ // negative dot product
+ vecExp = new float[] { 0f, -FloatUtil.sin(FloatUtil.QUARTER_PI), FloatUtil.sin(FloatUtil.QUARTER_PI) };
+ quat1.setIdentity().rotateByAngleX( -2f * FloatUtil.HALF_PI); // angle: -180 degrees, axis X
+ quat2.setIdentity().rotateByAngleX( FloatUtil.HALF_PI); // angle: 90 degrees, axis X
+ // System.err.println("Slerp #06: 1/2 * 270 degrees");
+ quatS.setSlerp(quat1, quat2, 0.5f);
+ quatS.rotateVector(vecHas, 0, UNIT_Y, 0);
+ // System.err.println("exp0 "+Arrays.toString(vecExp));
+ // System.err.println("has0 "+Arrays.toString(vecHas));
+ Assert.assertEquals( 0f, Math.abs( VectorUtil.distance(vecExp, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+
+
+ }
+
+ @Test
+ public void test26LookAt() {
+ final float[] direction = new float[3];
+ final float[] xAxis = new float[3];
+ final float[] yAxis = new float[3];
+ final float[] zAxis = new float[3];
+ final float[] vecHas = new float[3];
+
+ if( DEBUG ) System.err.println("LookAt #01");
+ VectorUtil.copyVec3(direction, 0, NEG_UNIT_X, 0);
+ final Quaternion quat = new Quaternion().setLookAt(direction, UNIT_Y, xAxis, yAxis, zAxis);
+ Assert.assertEquals(0f, VectorUtil.distance(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);
+
+ if( DEBUG ) System.err.println("LookAt #02");
+ VectorUtil.normalize(VectorUtil.copyVec3(direction, 0, ONE, 0));
+ quat.setLookAt(direction, UNIT_Y, xAxis, yAxis, zAxis);
+ if( DEBUG )System.err.println("quat0 "+quat);
+ quat.rotateVector(vecHas, 0, UNIT_Z, 0);
+ if( DEBUG ) {
+ System.err.println("xAxis "+Arrays.toString(xAxis)+", len "+VectorUtil.length(xAxis));
+ System.err.println("yAxis "+Arrays.toString(yAxis)+", len "+VectorUtil.length(yAxis));
+ System.err.println("zAxis "+Arrays.toString(zAxis)+", len "+VectorUtil.length(zAxis));
+ System.err.println("exp0 "+Arrays.toString(direction)+", len "+VectorUtil.length(direction));
+ System.err.println("has0 "+Arrays.toString(vecHas)+", len "+VectorUtil.length(vecHas));
+ }
+ // Assert.assertEquals(0f, VectorUtil.distance(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);
+ Assert.assertEquals(0f, VectorUtil.distance(direction, vecHas), Quaternion.ALLOWED_DEVIANCE);
+
+ if( DEBUG )System.err.println("LookAt #03");
+ VectorUtil.normalize(VectorUtil.copyVec3(direction, 0, new float[] { -1f, 2f, -1f }, 0));
+ quat.setLookAt(direction, UNIT_Y, xAxis, yAxis, zAxis);
+ if( DEBUG )System.err.println("quat0 "+quat);
+ quat.rotateVector(vecHas, 0, UNIT_Z, 0);
+ if( DEBUG ) {
+ System.err.println("xAxis "+Arrays.toString(xAxis)+", len "+VectorUtil.length(xAxis));
+ System.err.println("yAxis "+Arrays.toString(yAxis)+", len "+VectorUtil.length(yAxis));
+ System.err.println("zAxis "+Arrays.toString(zAxis)+", len "+VectorUtil.length(zAxis));
+ System.err.println("exp0 "+Arrays.toString(direction)+", len "+VectorUtil.length(direction));
+ System.err.println("has0 "+Arrays.toString(vecHas)+", len "+VectorUtil.length(vecHas));
+ }
+ // Assert.assertEquals(0f, VectorUtil.distance(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);
+ Assert.assertEquals(0f, VectorUtil.distance(direction, vecHas), Quaternion.ALLOWED_DEVIANCE);
+ }
+
+ public static void main(String args[]) {
+ org.junit.runner.JUnitCore.main(TestQuaternion01NOUI.class.getName());
+ }
+}
|