path: root/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java

/**
 * Copyright 2010 JogAmp Community. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are
 * permitted provided that the following conditions are met:
 *
 *    1. Redistributions of source code must retain the above copyright notice, this list of
 *       conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above copyright notice, this list
 *       of conditions and the following disclaimer in the documentation and/or other materials
 *       provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * The views and conclusions contained in the software and documentation are those of the
 * authors and should not be interpreted as representing official policies, either expressed
 * or implied, of JogAmp Community.
 */
package com.jogamp.opengl.math;

public class Quaternion {
    protected float x, y, z, w;

    public Quaternion() {
        setIdentity();
    }
    
    public Quaternion(Quaternion q) {
        x = q.x;
        y = q.y;
        z = q.z;
        w = q.w;
    }

    public Quaternion(float x, float y, float z, float w) {
        this.x = x;
        this.y = y;
        this.z = z;
        this.w = w;
    }

    /**
     * Constructor to create a rotation based quaternion from two vectors
     * 
     * @param vector1
     * @param vector2
     */
    public Quaternion(float[] vector1, float[] vector2) {
        final float theta = FloatUtil.acos(VectorUtil.dot(vector1, vector2));
        final float[] cross = VectorUtil.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);
        x = (nv[0] * sin);
        y = (nv[1] * sin);
        z = (nv[2] * sin);
        w = FloatUtil.cos(halfangle);
    }

    /**
     * Transform the rotational quaternion to axis based rotation angles
     * 
     * @return new float[4] with ,theta,Rx,Ry,Rz
     */
    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 float getW() {
        return w;
    }

    public void setW(float w) {
        this.w = w;
    }

    public float getX() {
        return x;
    }

    public void setX(float x) {
        this.x = x;
    }

    public float getY() {
        return y;
    }

    public void setY(float y) {
        this.y = y;
    }

    public float getZ() {
        return z;
    }

    public void setZ(float z) {
        this.z = z;
    }

    /**
     * Add a quaternion
     * 
     * @param q quaternion
     */
    public void add(Quaternion q) {
        x += q.x;
        y += q.y;
        z += q.z;
    }

    /**
     * Subtract a quaternion
     * 
     * @param q quaternion
     */
    public void subtract(Quaternion q) {
        x -= q.x;
        y -= q.y;
        z -= q.z;
    }

    /**
     * Divide a quaternion by a constant
     * 
     * @param n a float to divide by
     */
    public void divide(float n) {
        x /= n;
        y /= n;
        z /= n;
    }

    /**
     * Multiply this quaternion by the param quaternion
     * 
     * @param q a quaternion to multiply with
     */
    public void mult(Quaternion q) {
        final float w1 = w * q.w - x * q.x - y * q.y - z * q.z;

        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;

        w = w1;
        x = x1;
        y = y1;
        z = z1;
    }

    /**
     * Multiply a quaternion by a constant
     * 
     * @param n a float constant
     */
    public void mult(float n) {
        x *= n;
        y *= n;
        z *= n;
    }
    
    /***
     * Rotate given vector by this quaternion
     * 
     * @param vector input vector
     * @return rotated vector
     */
    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;
    }

    /**
     * Normalize a quaternion required if to be used as a rotational quaternion
     */
    public void normalize() {
        final float norme = (float) FloatUtil.sqrt(w * w + x * x + y * y + z * z);
        if (norme == 0.0f) {
            setIdentity();
        } else {
            final float recip = 1.0f / norme;

            w *= recip;
            x *= recip;
            y *= recip;
            z *= recip;
        }
    }

    /**
     * Invert the quaternion If rotational, will produce a the inverse rotation
     */
    public void inverse() {
        final float norm = w * w + x * x + y * y + z * z;

        final float recip = 1.0f / norm;

        w *= recip;
        x = -1 * x * recip;
        y = -1 * y * recip;
        z = -1 * z * recip;
    }

    /**
     * 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;
    }

    /**
     * Set this quaternion from a Sphereical interpolation of two param
     * quaternion, used mostly for rotational animation.
     * <p>
     * Note: Method does not normalize this quaternion!
     * </p>
     * <p>
     * See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/
     * quaternions/slerp/
     * </p>
     * 
     * @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;
        }

        // 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;
        }

        // 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);

        final float scla = FloatUtil.sin(t1 * omega) / sinom;
        final float sclb = FloatUtil.sin(t * omega) / sinom;

        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;
    }

    /**
     * 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 boolean isIdentity() {
        return w == 1 && x == 0 && y == 0 && z == 0;
    }
    
    /***
     * Set this quaternion to identity (x=0,y=0,z=0,w=1)
     */
    public void setIdentity() {
        x = y = z = 0;
        w = 1;
    }

    /**
     * compute the quaternion from a 3x3 column matrix
     * 
     * @param m 3x3 column matrix
     */
    public void setFromMatrix(float[] m) {
        final float T = m[0] + m[4] + m[8] + 1;
        if (T > 0) {
            final float S = 0.5f / (float) 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;
        } 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;
            }
        }
    }

    /**
     * Check if the the 3x3 matrix (param) is in fact an affine rotational
     * matrix
     * 
     * @param m 3x3 column matrix
     * @return true if representing a rotational matrix, false otherwise
     */
    public boolean isRotationMatrix(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;
        if (FloatUtil.abs(m[0] * m[2] + m[3] * m[5] + m[6] * m[8]) > epsilon)
            return false;
        if (FloatUtil.abs(m[1] * m[2] + m[4] * m[5] + m[7] * m[8]) > epsilon)
            return false;
        if (FloatUtil.abs(m[0] * m[0] + m[3] * m[3] + m[6] * m[6] - 1) > epsilon)
            return false;
        if (FloatUtil.abs(m[1] * m[1] + m[4] * m[4] + m[7] * m[7] - 1) > epsilon)
            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);
    }

    private float determinant(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];
    }
}