diff options
author | Sven Gothel <[email protected]> | 2023-09-20 19:51:55 +0200 |
---|---|---|
committer | Sven Gothel <[email protected]> | 2023-09-20 19:51:55 +0200 |
commit | 5d6e8a367c03644740187e500c6de5d3ac039d5e (patch) | |
tree | a649f559413c51272ee3f4afff1f68ebfea45477 /src/jogl/classes/com/jogamp/opengl/math/Quaternion.java | |
parent | bbe845846ffc00807395a5070a7352c6bbe7e4ef (diff) |
Bug 1452 - Decouple math functionality to 'com.jogamp.math' to be toolkit agnostic (PMVMatrix, Matrix4f, Vec4f, ..)
Math functionality (PMVMatrix, Matrix4f, Vec4f, ..)
- shall be used toolkit agnostic, e.g. independent from OpenGL
- shall be reused within our upcoming Vulkan implementation
- may also move outside of JOGL, i.e. GlueGen or within its own package to be reused for other purposed.
The 'com.jogamp.opengl.util.PMVMatrix' currently also used to feed in GLUniformData
via the toolkit agnostic SyncAction and SyncBuffer
shall also be split to a toolkit agnostic variant.
An OpenGL PMVMatrix specialization implementing GLMatrixFunc can still exist,
being derived from the toolkit agnostic base implementation.
+++
Initial commit .. compile clean, passing most unit tests.
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+"]"; - } -} |