Quaternion: Fix and enhance class incl. Extensive Unit Tests (all passed)

- Add documentation incl references (Matrix-FAQ, Euclideanspace, ..)

- Compared w/ other impl., i.e. WildMagic, Ardor3D, ..
  and added missing functionality incl unit tests.

- PMVMatrix: Added convenient Quaternion 'hooks'
  - glRotate(Quaternion)
  - glLoadMatrix(Quaternion)

5 files changed, 1839 insertions, 234 deletions

diff --git a/make/scripts/tests.sh b/make/scripts/tests.sh
index a8807b09e..4cb95476b 100644
--- a/make/scripts/tests.sh
+++ b/make/scripts/tests.sh
@@ -398,6 +398,7 @@ function testawtswt() {
 #
 # CORE [NEWT + AWT] (testnoawt and testawt)
 #
+#testnoawt com.jogamp.opengl.test.junit.jogl.math.TestFloatUtil01NOUI $*
 #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestPMVMatrix01NEWT $*
 #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestPMVMatrix02NOUI $*
 #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestPMVMatrix03NOUI $*
@@ -405,6 +406,7 @@ function testawtswt() {
 #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestGluUnprojectDoubleNOUI $*
 #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestFloatUtil01MatrixMatrixMultNOUI $*
 #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestBinary16NOUI $*
+testnoawt com.jogamp.opengl.test.junit.jogl.math.TestQuaternion01NOUI $*
 
 #testnoawt com.jogamp.opengl.test.junit.jogl.acore.TestShutdownCompleteNEWT $*
 #testnoawt com.jogamp.opengl.test.junit.jogl.acore.TestInitConcurrent01NEWT $*
@@ -500,7 +502,7 @@ function testawtswt() {
 #testawt com.jogamp.opengl.test.junit.jogl.acore.anim.Bug898AnimatorFromEDTAWT $*
 
 #testawt com.jogamp.opengl.test.junit.jogl.acore.TestGLReadBuffer01GLJPanelAWT $*
-testawt com.jogamp.opengl.test.junit.jogl.acore.TestGLReadBuffer01GLCanvasAWT $*
+#testawt com.jogamp.opengl.test.junit.jogl.acore.TestGLReadBuffer01GLCanvasAWT $*
 #testnoawt com.jogamp.opengl.test.junit.jogl.acore.TestGLReadBuffer01GLWindowNEWT $*
 
 #
@@ -738,7 +740,7 @@ testawt com.jogamp.opengl.test.junit.jogl.acore.TestGLReadBuffer01GLCanvasAWT $*
 #testnoawt com.jogamp.opengl.test.junit.graph.TestTextRendererNEWT10 $*
 #testnoawt com.jogamp.opengl.test.junit.graph.demos.ui.UINewtDemo01 $*
 #testnoawt com.jogamp.opengl.test.junit.graph.demos.GPUTextNewtDemo01 $*
-testnoawt com.jogamp.opengl.test.junit.graph.demos.GPUTextNewtDemo02 $*
+#testnoawt com.jogamp.opengl.test.junit.graph.demos.GPUTextNewtDemo02 $*
 #testnoawt com.jogamp.opengl.test.junit.graph.demos.GPUTextNewtDemo03 $*
 #testnoawt com.jogamp.opengl.test.junit.graph.demos.GPURegionNewtDemo01 $*
 #testnoawt com.jogamp.opengl.test.junit.graph.demos.GPURegionNewtDemo02 $*
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());

+    }

+}