diff options
Diffstat (limited to 'src/jogl/classes/com/jogamp/opengl/math/Quaternion.java')
| -rw-r--r-- | src/jogl/classes/com/jogamp/opengl/math/Quaternion.java | 1188 |
1 files changed, 0 insertions, 1188 deletions
diff --git a/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java b/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java deleted file mode 100644 index 0d04c69cc..000000000 --- a/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java +++ /dev/null @@ -1,1188 +0,0 @@ -/** - * Copyright 2010-2023 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.math; - -/** - * 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 { - 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 ~10 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() { - x = y = z = 0; w = 1; - } - - public Quaternion(final Quaternion q) { - set(q); - } - - public Quaternion(final float x, final float y, final float z, final float w) { - set(x, y, z, w); - } - - /** - * See {@link #magnitude()} for special handling of {@link FloatUtil#EPSILON epsilon}, - * which is not applied here. - * @return the squared magnitude of this quaternion. - */ - public final float magnitudeSquared() { - return w*w + x*x + y*y + z*z; - } - - /** - * Return the magnitude of this quaternion, i.e. sqrt({@link #magnitudeSquared()}) - * <p> - * A magnitude of zero shall equal {@link #isIdentity() identity}, - * as performed by {@link #normalize()}. - * </p> - * <p> - * Implementation Details: - * <ul> - * <li> returns 0f if {@link #magnitudeSquared()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * <li> returns 1f if {@link #magnitudeSquared()} is {@link FloatUtil#isEqual(float, float, float) equals 1f} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - */ - public final float magnitude() { - final float magnitudeSQ = magnitudeSquared(); - if ( FloatUtil.isZero(magnitudeSQ) ) { - return 0f; - } - if ( FloatUtil.isEqual(1f, magnitudeSQ) ) { - return 1f; - } - return FloatUtil.sqrt(magnitudeSQ); - } - - public final float w() { - return w; - } - - public final void setW(final float w) { - this.w = w; - } - - public final float x() { - return x; - } - - public final void setX(final float x) { - this.x = x; - } - - public final float y() { - return y; - } - - public final void setY(final float y) { - this.y = y; - } - - public final float z() { - return 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.x(), quat.y(), quat.z(), quat.w()); - } - - /** - * Returns <code>true</code> if this quaternion has identity. - * <p> - * Implementation uses {@link FloatUtil#EPSILON epsilon} to compare - * {@link #w() W} {@link FloatUtil#isEqual(float, float) against 1f} and - * {@link #x() X}, {@link #y() Y} and {@link #z() Z} - * {@link FloatUtil#isZero(float) against zero}. - * </p> - */ - public final boolean isIdentity() { - return FloatUtil.isEqual(1f, w) && VectorUtil.isZero(x, y, z); - // return w == 1f && x == 0f && y == 0f && z == 0f; - } - - /*** - * 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 = 0f; w = 1f; - return this; - } - - /** - * Normalize a quaternion required if to be used as a rotational quaternion. - * <p> - * Implementation Details: - * <ul> - * <li> {@link #setIdentity()} if {@link #magnitude()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - * @return this quaternion for chaining. - */ - public final Quaternion normalize() { - final float norm = magnitude(); - if ( FloatUtil.isZero(norm, FloatUtil.EPSILON) ) { - setIdentity(); - } else { - final float invNorm = 1f/norm; - w *= invNorm; - x *= invNorm; - y *= invNorm; - z *= invNorm; - } - 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 - * <p> - * Implementation Details: - * <ul> - * <li> {@link #conjugate() conjugates} if {@link #magnitudeSquared()} is is {@link FloatUtil#isEqual(float, float, float) equals 1f} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - * @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.isEqual(1.0f, magnitudeSQ) ) { - conjugate(); - } else { - final float invmsq = 1f/magnitudeSQ; - w *= invmsq; - x = -x * invmsq; - y = -y * invmsq; - z = -z * invmsq; - } - 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 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 final Quaternion subtract(final Quaternion q) { - x -= q.x; - y -= q.y; - z -= q.z; - w -= q.w; - return this; - } - - /** - * Multiply this quaternion by the param quaternion - * - * @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 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 ); - } - - /** - * Scale this quaternion by a constant - * - * @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 final Quaternion scale(final float n) { - x *= n; - y *= n; - z *= n; - w *= n; - return this; - } - - /** - * 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); - } - - /** - * 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 axis Vec3f coord of rotation axis - * @return this quaternion for chaining. - */ - public Quaternion rotateByAngleNormalAxis(final float angle, final Vec3f axis) { - return rotateByAngleNormalAxis(angle, axis.x(), axis.y(), axis.z()); - } - - /** - * 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); - } - - /** - * Rotate this quaternion around Y axis with the given angle in radians - * - * @param angle in radians - * @return this quaternion for chaining. - */ - 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); - } - - /** - * Rotates 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> - * For details see {@link #rotateByEuler(float, float, float)}. - * @param angradXYZ euler angle array in radians - * @return this quaternion for chaining. - * @see #rotateByEuler(float, float, float) - */ - public final Quaternion rotateByEuler(final Vec3f angradXYZ) { - return rotateByEuler(angradXYZ.x(), angradXYZ.y(), angradXYZ.z()); - } - - /** - * Rotates this quaternion from the given Euler rotation angles in radians. - * <p> - * The rotations are applied in the given order and using chained rotation per axis: - * <ul> - * <li>y - heading - {@link #rotateByAngleY(float)}</li> - * <li>z - attitude - {@link #rotateByAngleZ(float)}</li> - * <li>x - bank - {@link #rotateByAngleX(float)}</li> - * </ul> - * </p> - * <p> - * Implementation Details: - * <ul> - * <li> NOP if all angles are {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * <li> result is {@link #normalize()}ed</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 #rotateByAngleY(float) - * @see #rotateByAngleZ(float) - * @see #rotateByAngleX(float) - * @see #setFromEuler(float, float, float) - */ - public final Quaternion rotateByEuler(final float bankX, final float headingY, final float attitudeZ) { - if ( VectorUtil.isZero(bankX, headingY, attitudeZ, FloatUtil.EPSILON) ) { - return this; - } else { - // setFromEuler muls: ( 8 + 4 ) , + quat muls 24 = 36 - // this: 8 + 8 + 8 + 4 = 28 muls - return rotateByAngleY(headingY).rotateByAngleZ(attitudeZ).rotateByAngleX(bankX).normalize(); - } - } - - /*** - * Rotate the given vector by this quaternion - * @param vecIn vector to be rotated - * @param vecOut result storage for rotated vector, maybe equal to vecIn for in-place rotation - * - * @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 final Vec3f rotateVector(final Vec3f vecIn, final Vec3f vecOut) { - if( vecIn.isZero() ) { - vecOut.set(0, 0, 0); - } else { - final float vecX = vecIn.x(); - final float vecY = vecIn.y(); - final float vecZ = vecIn.z(); - final float x_x = x*x; - final float y_y = y*y; - final float z_z = z*z; - final float w_w = w*w; - - vecOut.setX( w_w * vecX - + x_x * vecX - - z_z * vecX - - y_y * vecX - + 2f * ( y*w*vecZ - z*w*vecY + y*x*vecY + z*x*vecZ ) ); - ; - - vecOut.setY( y_y * vecY - - z_z * vecY - + w_w * vecY - - x_x * vecY - + 2f * ( x*y*vecX + z*y*vecZ + w*z*vecX - x*w*vecZ ) );; - - vecOut.setZ( z_z * vecZ - - y_y * vecZ - - x_x * vecZ - + w_w * vecZ - + 2f * ( x*z*vecX + y*z*vecY - w*y*vecX + w*x*vecY ) ); - } - return vecOut; - } - - /** - * 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 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 { - 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; - - 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; - } - - /** - * 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> - * Implementation Details: - * <ul> - * <li> result is {@link #normalize()}ed</li> - * </ul> - * </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 Quaternion setLookAt(final Vec3f directionIn, final Vec3f upIn, - final Vec3f xAxisOut, final Vec3f yAxisOut, final Vec3f zAxisOut) { - // Z = norm(dir) - zAxisOut.set(directionIn).normalize(); - - // X = upIn x Z - // (borrow yAxisOut for upNorm) - yAxisOut.set(upIn).normalize(); - xAxisOut.cross(yAxisOut, zAxisOut).normalize(); - - // Y = Z x X - // - yAxisOut.cross(zAxisOut, xAxisOut).normalize(); - - /** - 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 - // - - /** - * 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> - * <p> - * Implementation Details: - * <ul> - * <li> {@link #setIdentity()} if square vector-length is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - * @param v1 not normalized - * @param v2 not normalized - * @param tmpPivotVec temp storage for cross product - * @param tmpNormalVec temp storage to normalize vector - * @return this quaternion for chaining. - */ - public final Quaternion setFromVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec, final Vec3f tmpNormalVec) { - final float factor = v1.length() * v2.length(); - if ( FloatUtil.isZero(factor, FloatUtil.EPSILON ) ) { - return setIdentity(); - } else { - final float dot = v1.dot(v2) / factor; // normalize - final float theta = FloatUtil.acos(Math.max(-1.0f, Math.min(dot, 1.0f))); // clipping [-1..1] - - tmpPivotVec.cross(v1, v2); - - if ( dot < 0.0f && FloatUtil.isZero( tmpPivotVec.length(), 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.x()) > Math.abs(v1.y())) { - if (Math.abs(v1.x()) > Math.abs(v1.z())) { - dominantIndex = 0; - } else { - dominantIndex = 2; - } - } else { - if (Math.abs(v1.y()) > Math.abs(v1.z())) { - dominantIndex = 1; - } else { - dominantIndex = 2; - } - } - tmpPivotVec.set( dominantIndex, -v1.get( (dominantIndex + 1) % 3 ) ); - tmpPivotVec.set( (dominantIndex + 1) % 3, v1.get( dominantIndex ) ); - tmpPivotVec.set( (dominantIndex + 2) % 3, 0f ); - } - return setFromAngleAxis(theta, tmpPivotVec, tmpNormalVec); - } - } - - /** - * 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> - * <p> - * Implementation Details: - * <ul> - * <li> {@link #setIdentity()} if square vector-length is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - * @param v1 normalized - * @param v2 normalized - * @param tmpPivotVec temp storage for cross product - * @return this quaternion for chaining. - */ - public final Quaternion setFromNormalVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec) { - final float factor = v1.length() * v2.length(); - if ( FloatUtil.isZero(factor, FloatUtil.EPSILON ) ) { - return setIdentity(); - } else { - final float dot = v1.dot(v2) / factor; // normalize - final float theta = FloatUtil.acos(Math.max(-1.0f, Math.min(dot, 1.0f))); // clipping [-1..1] - - tmpPivotVec.cross(v1, v2); - - if ( dot < 0.0f && FloatUtil.isZero( tmpPivotVec.length(), 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.x()) > Math.abs(v1.y())) { - if (Math.abs(v1.x()) > Math.abs(v1.z())) { - dominantIndex = 0; - } else { - dominantIndex = 2; - } - } else { - if (Math.abs(v1.y()) > Math.abs(v1.z())) { - dominantIndex = 1; - } else { - dominantIndex = 2; - } - } - tmpPivotVec.set( dominantIndex, -v1.get( (dominantIndex + 1) % 3 ) ); - tmpPivotVec.set( (dominantIndex + 1) % 3, v1.get( dominantIndex ) ); - tmpPivotVec.set( (dominantIndex + 2) % 3, 0f ); - } - return setFromAngleNormalAxis(theta, tmpPivotVec); - } - } - - /*** - * Initialize this quaternion with given non-normalized axis vector and rotation angle - * <p> - * Implementation Details: - * <ul> - * <li> {@link #setIdentity()} if axis is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - * @param angle rotation angle (rads) - * @param vector axis vector not normalized - * @param tmpV3f 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(Vec3f) - */ - public final Quaternion setFromAngleAxis(final float angle, final Vec3f vector, final Vec3f tmpV3f) { - tmpV3f.set(vector).normalize(); - return setFromAngleNormalAxis(angle, tmpV3f); - } - - /*** - * Initialize this quaternion with given normalized axis vector and rotation angle - * <p> - * Implementation Details: - * <ul> - * <li> {@link #setIdentity()} if axis is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - * @param angle rotation angle (rads) - * @param vector axis vector normalized - * @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(Vec3f) - */ - public final Quaternion setFromAngleNormalAxis(final float angle, final Vec3f vector) { - if( vector.isZero() ) { - setIdentity(); - } else { - final float halfangle = angle * 0.5f; - final float sin = FloatUtil.sin(halfangle); - x = vector.x() * sin; - y = vector.y() * sin; - z = vector.z() * sin; - w = FloatUtil.cos(halfangle); - } - return this; - } - - /** - * Transform the rotational quaternion to axis based rotation angles - * - * @param axis storage for computed axis - * @return the rotation angle in radians - * @see #setFromAngleAxis(float, Vec3f, Vec3f) - */ - public final float toAngleAxis(final Vec3f 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.set( 1.0f, 0.0f, 0.0f ); - } else { - angle = FloatUtil.acos(w) * 2.0f; - final float invLength = 1.0f / FloatUtil.sqrt(sqrLength); - axis.set( x * invLength, - y * invLength, - z * invLength ); - } - return angle; - } - - /** - * Initializes this quaternion from the given Euler rotation array <code>angradXYZ</code> in radians. - * <p> - * The <code>angradXYZ</code> vector is laid out in natural order: - * <ul> - * <li>x - bank</li> - * <li>y - heading</li> - * <li>z - attitude</li> - * </ul> - * </p> - * For details see {@link #setFromEuler(float, float, float)}. - * @param angradXYZ euler angle vector in radians holding x-bank, y-heading and z-attitude - * @return this quaternion for chaining. - * @see #setFromEuler(float, float, float) - */ - public final Quaternion setFromEuler(final Vec3f angradXYZ) { - return setFromEuler(angradXYZ.x(), angradXYZ.y(), angradXYZ.z()); - } - - /** - * 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> - * <p> - * Implementation Details: - * <ul> - * <li> {@link #setIdentity()} if all angles are {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * <li> result is {@link #normalize()}ed</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(Vec3f) - */ - public final Quaternion setFromEuler(final float bankX, final float headingY, final float attitudeZ) { - if ( VectorUtil.isZero(bankX, headingY, attitudeZ, FloatUtil.EPSILON) ) { - return setIdentity(); - } else { - 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); - - // 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; - - w = cosHeadingXcosAttitude * cosBankX - sinHeadingXsinAttitude * sinBankX; - x = cosHeadingXcosAttitude * sinBankX + sinHeadingXsinAttitude * cosBankX; - y = sinHeadingXcosAttitude * cosBankX + cosHeadingXsinAttitude * sinBankX; - z = cosHeadingXsinAttitude * cosBankX - sinHeadingXcosAttitude * sinBankX; - return normalize(); - } - } - - /** - * Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ). - * <p> - * The <code>result</code> array is laid out in natural order: - * <ul> - * <li>x - bank</li> - * <li>y - heading</li> - * <li>z - attitude</li> - * </ul> - * </p> - * - * @param result euler angle result vector for radians x-bank, y-heading and z-attitude - * @return the Vec3f `result` filled with x-bank, y-heading and z-attitude - * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm">euclideanspace.com-quaternionToEuler</a> - * @see #setFromEuler(float, float, float) - */ - public Vec3f toEuler(final Vec3f 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.set( 0f, // x-bank - 2f * FloatUtil.atan2(x, w), // y-heading - FloatUtil.HALF_PI ); // z-attitude - } else if (test < -0.499f * unit) { // singularity at south pole - result.set( 0f, // x-bank - -2 * FloatUtil.atan2(x, w), // y-heading - -FloatUtil.HALF_PI ); // z-attitude - } else { - result.set( FloatUtil.atan2(2f * x * w - 2 * y * z, -sqx + sqy - sqz + sqw), // x-bank - FloatUtil.atan2(2f * y * w - 2 * x * z, sqx - sqy - sqz + sqw), // y-heading - FloatUtil.asin( 2f * test / unit) ); // z-attitude - } - return result; - } - - /** - * 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 #setFromMatrix(Matrix4f) - */ - 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 { - // 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; - } - - /** - * 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 Matrix4f#getRotation(Quaternion) - * @see #setFromMatrix(float, float, float, float, float, float, float, float, float) - */ - public Quaternion setFromMatrix(final Matrix4f m) { - return m.getRotation(this); - } - - /** - * Transform this quaternion to a normalized 4x4 column matrix representing the rotation. - * <p> - * Implementation Details: - * <ul> - * <li> makes identity matrix if {@link #magnitudeSquared()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - * - * @param matrix float[16] store for the resulting normalized column matrix 4x4 - * @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(Matrix4f) - * @see #setFromMatrix(float, float, float, float, float, float, float, float, float) - */ - public final float[] toMatrix(final float[] matrix) { - // pre-multiply scaled-reciprocal-magnitude to reduce multiplications - final float norm = magnitudeSquared(); - if ( FloatUtil.isZero(norm) ) { - // identity matrix -> srecip = 0f - return FloatUtil.makeIdentity(matrix); - } - final float srecip; - if ( FloatUtil.isEqual(1f, norm) ) { - srecip = 2f; - } else { - srecip = 2.0f / norm; - } - - 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] = 1f - ( yy + zz ); - matrix[0+1*4] = ( xy - zw ); - matrix[0+2*4] = ( xz + yw ); - matrix[0+3*4] = 0f; - - matrix[1+0*4] = ( xy + zw ); - matrix[1+1*4] = 1f - ( xx + zz ); - matrix[1+2*4] = ( yz - xw ); - matrix[1+3*4] = 0f; - - matrix[2+0*4] = ( xz - yw ); - matrix[2+1*4] = ( yz + xw ); - matrix[2+2*4] = 1f - ( xx + yy ); - matrix[2+3*4] = 0f; - - matrix[3+0*4] = 0f; - matrix[3+1*4] = 0f; - matrix[3+2*4] = 0f; - matrix[3+3*4] = 1f; - return matrix; - } - - /** - * Transform this quaternion to a normalized 4x4 column matrix representing the rotation. - * <p> - * Implementation Details: - * <ul> - * <li> makes identity matrix if {@link #magnitudeSquared()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li> - * </ul> - * </p> - * - * @param matrix store for the resulting normalized column matrix 4x4 - * @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, float, float, float, float, float, float, float, float) - * @see Matrix4f#setToRotation(Quaternion) - */ - public final Matrix4f toMatrix(final Matrix4f matrix) { - return matrix.setToRotation(this); - } - - /** - * 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 Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis) { - return setFromMatrix(xAxis.x(), yAxis.x(), zAxis.x(), - xAxis.y(), yAxis.y(), zAxis.y(), - xAxis.z(), yAxis.z(), zAxis.z()); - } - - /** - * 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 Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis, final Matrix4f tmpMat4) { - tmpMat4.setToRotation(this); - tmpMat4.getColumn(2, zAxis); - tmpMat4.getColumn(1, yAxis); - tmpMat4.getColumn(0, xAxis); - } - - /** - * Check if the the 3x3 matrix (param) is in fact an affine rotational - * matrix - * - * @param m 3x3 column matrix - * @return true if representing a rotational matrix, false otherwise - */ - @Deprecated - public final boolean isRotationMatrix3f(final float[] m) { - final float epsilon = 0.01f; // margin to allow for rounding errors - if (Math.abs(m[0] * m[3] + m[3] * m[4] + m[6] * m[7]) > epsilon) - return false; - if (Math.abs(m[0] * m[2] + m[3] * m[5] + m[6] * m[8]) > epsilon) - return false; - if (Math.abs(m[1] * m[2] + m[4] * m[5] + m[7] * m[8]) > epsilon) - return false; - if (Math.abs(m[0] * m[0] + m[3] * m[3] + m[6] * m[6] - 1) > epsilon) - return false; - if (Math.abs(m[1] * m[1] + m[4] * m[4] + m[7] * m[7] - 1) > epsilon) - return false; - if (Math.abs(m[2] * m[2] + m[5] * m[5] + m[8] * m[8] - 1) > epsilon) - return false; - return (Math.abs(determinant3f(m) - 1) < epsilon); - } - - @Deprecated - private final float determinant3f(final 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.x()) <= ALLOWED_DEVIANCE && - Math.abs(y - comp.y()) <= ALLOWED_DEVIANCE && - Math.abs(z - comp.z()) <= ALLOWED_DEVIANCE && - Math.abs(w - comp.w()) <= ALLOWED_DEVIANCE; - } - @Override - public final int hashCode() { - throw new InternalError("hashCode not designed"); - } - - @Override - public String toString() { - return "Quat[x "+x+", y "+y+", z "+z+", w "+w+"]"; - } -} |