/**
 * 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.distVec3(vecOut1, vecOut2) ), FloatUtil.EPSILON );

        quat1.rotateVector(vecOut1, 0, UNIT_Z, 0);
        Assert.assertEquals(0f, Math.abs( VectorUtil.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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];
        final float[] mat2 = new float[4*4];
        final Quaternion quat = new Quaternion();

        //
        // IDENTITY CHECK
        //
        FloatUtil.makeIdentity(mat1);
        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.distVec3(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.distVec3(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.multMatrixVec(mat2, NEG_ONE_v4, vecOut2);
        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(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.distVec3(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.distVec3(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.multMatrixVec(mat2, ONE_v4, vecOut2);
        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(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.distVec3(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.distVec3(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.multMatrixVec(mat2, NEG_ONE_v4, vecOut2);
        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(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.distVec3(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.distVec3(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.multMatrixVec(mat2, ONE_v4, vecOut2);
        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );

        //
        // Test Matrix-Columns
        //

        a = FloatUtil.QUARTER_PI;
        final float[] vecExp = new float[3];
        final 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.distVec3(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.distVec3(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.distVec3(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(matExp, 0, eulerExp[0], eulerExp[1], eulerExp[2]); // 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(matExp, 0, eulerExp[0], eulerExp[1], eulerExp[2]); // 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(matExp, 0, eulerExp[0], eulerExp[1], eulerExp[2]); // 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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(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.distVec3(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);

        if( DEBUG ) System.err.println("LookAt #02");
        VectorUtil.normalizeVec3(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.normVec3(xAxis));
            System.err.println("yAxis "+Arrays.toString(yAxis)+", len "+VectorUtil.normVec3(yAxis));
            System.err.println("zAxis "+Arrays.toString(zAxis)+", len "+VectorUtil.normVec3(zAxis));
            System.err.println("exp0 "+Arrays.toString(direction)+", len "+VectorUtil.normVec3(direction));
            System.err.println("has0 "+Arrays.toString(vecHas)+", len "+VectorUtil.normVec3(vecHas));
        }
        // Assert.assertEquals(0f, VectorUtil.distance(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);
        Assert.assertEquals(0f, VectorUtil.distVec3(direction, vecHas), Quaternion.ALLOWED_DEVIANCE);

        if( DEBUG )System.err.println("LookAt #03");
        VectorUtil.normalizeVec3(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.normVec3(xAxis));
            System.err.println("yAxis "+Arrays.toString(yAxis)+", len "+VectorUtil.normVec3(yAxis));
            System.err.println("zAxis "+Arrays.toString(zAxis)+", len "+VectorUtil.normVec3(zAxis));
            System.err.println("exp0 "+Arrays.toString(direction)+", len "+VectorUtil.normVec3(direction));
            System.err.println("has0 "+Arrays.toString(vecHas)+", len "+VectorUtil.normVec3(vecHas));
        }
        // Assert.assertEquals(0f, VectorUtil.distance(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);
        Assert.assertEquals(0f, VectorUtil.distVec3(direction, vecHas), Quaternion.ALLOWED_DEVIANCE);
    }

    public static void main(final String args[]) {
        org.junit.runner.JUnitCore.main(TestQuaternion01NOUI.class.getName());
    }
}
