/*
 * Moonlight|3D Copyright (C) 2005 The Moonlight|3D team
 * 
 * This library is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 * 
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
 * details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 * 
 * Created on Jan 5, 2005
 */
package ml.math;

/**
 * This class represents a quaternion and some of the operations possible
 * on it.
 * 
 * @author gregor
 */
public class Quaternion implements Cloneable {
    public double x, y, z, w;
    
    /**
     * Simple constructor.
     */
    public Quaternion()
    {
       // left empty for performance
    }

    /**
     * Reset this quaternion to non-rotating form.
     */
    public void clear()
    {
        Vector3D xAxis=new Vector3D();
        
        xAxis.clear();
        xAxis.X1=1;
        
        fromAxisAngle(xAxis,0);
    }
    
    /**
     * Build the quaternion so that it rotates the direction given by
     * the from vector into the direction given by the to vector.
     * 
     * @param from the starting orientation
     * @param to the finishing orientation the starting orientation will be transformed to
     */
    public void fromPoints(Vector3D from, Vector3D to)
    {
        from=from.norm();
        to=to.norm();
        x = from.X2 * to.X3 - from.X3 * to.X2;
        y = from.X3 * to.X1 - from.X1 * to.X3;
        z = from.X1 * to.X2 - from.X2 * to.X1;
        w = from.X1 * to.X1 + from.X2 * to.X2 + from.X3 * to.X3;
    }

    /**
     * Clone this quaternion
     * 
     * @return the cloned quaternion
     */
    @Override
    public Quaternion clone()
    {
        Quaternion q=new Quaternion();
        
        q.w=w; q.x=x; q.y=y; q.z=z;
        
        return q;
    }
    
    /**
     * Test if the given quaternion is equal to the current one
     * 
     * @param q the quaternion to test for equality
     * @return true if the quaternions are equal, false otherwise
     */
    public boolean equals(Quaternion q)
    {
        if(w!=q.w || x!=q.x || y!=q.y || z!=q.z) {
            return false;
        }
        
        return true;
    }

    /**
     * Calculate the absolute value of this quaternion
     * 
     * @return the absolute value of the quaternion
     */
    public double abs()
    {
        return Math.sqrt(w*w+x*x+y*y+z*z);
    }
    
    /**
     * Calculate a normalised form of this quaternion. This operation
     * does not affect the current quaternion.
     * 
     * @return the normalised quaternion
     */
    public Quaternion normalize()
    {
      Quaternion q=new Quaternion();
      double inverseLength=1.0/abs();
      
      q.w=inverseLength*w;
      q.x=inverseLength*x;
      q.y=inverseLength*y;
      q.z=inverseLength*z;
      
      return q;
    }
    
    /**
     * Return the rotation axis this quaternion does rotate around.
     * 
     * @return the rotation axis
     */
    public Vector3D getRotationAxis()
    {
        Vector3D axis=new Vector3D();
          
        axis.X1=x;
        axis.X2=y;
        axis.X3=z;
          
        if(axis.abs()==0) { // if angle is 0 then the axis cannot be recovered - any axis will do anyway
            axis.X1=0.0;
            axis.X2=0.0;
            axis.X3=1.0;
            
            return axis;
        }
          
        return axis.norm();
    }

    /**
     * Return the rotation angle of this quaternion
     * 
     * @return the rotation angle
     */
    public double getRotationAngle()
    {
        return Math.acos(w)*2.0;
    }

    /**
     * Return the Euler roll angle.
     * 
     * @return the roll angle in radians
     */
    public double getRoll() {
    	return Math.atan2(2*(x*y + w*z), w*w + x*x - y*y - z*z);
    }
    
    /**
     * Return the Euler pitch angle.
     * 
     * @return the pitch angle in radians
     */
    public double getPitch() {
    	return Math.asin(-2*(x*z - w*y));
    }
    
    /**
     * Return the euler yaw angle.
     * 
     * @return the yaw angle in radians
     */
    public double getYaw() {
    	return Math.asin(-2*(x*z - w*y));
    }
    
    /**
     * Return a 3x3 rotation matrix corresponding to the
     * current quaternion. 
     * 
     * @return the 3x3 rotation matrix
     */
    public Matrix3 toRotationMatrix3()
    {
        Matrix3 matrix=new Matrix3();

        matrix.X[0][0]=1-2*z*z-2*y*y;
        matrix.X[0][1]=2*x*y+2*w*z;
        matrix.X[0][2]=2*z*x-2*w*y;
        matrix.X[1][0]=2*x*y-2*w*z;
        matrix.X[1][1]=1-2*x*x-2*z*z;
        matrix.X[1][2]=2*y*z+2*w*x;
        matrix.X[2][0]=2*x*z+2*w*y;
        matrix.X[2][1]=2*y*z-2*w*x;
        matrix.X[2][2]=1-2*x*x-2*y*y;
          
        return matrix;
    }
    
    /**
     * Return a 4x4 rotation matrix corresponding to the
     * current quaternion. 
     * 
     * @return the 4x4 rotation matrix
     */
    public Matrix4 toRotationMatrix4()
    {
        Matrix4 matrix=new Matrix4();

        matrix.clear();
        matrix.X[3][3]=1.0;
        
        matrix.X[0][0]=1-2*z*z-2*y*y;
        matrix.X[0][1]=2*x*y+2*w*z;
        matrix.X[0][2]=2*z*x-2*w*y;
        matrix.X[1][0]=2*x*y-2*w*z;
        matrix.X[1][1]=1-2*x*x-2*z*z;
        matrix.X[1][2]=2*y*z+2*w*x;
        matrix.X[2][0]=2*x*z+2*w*y;
        matrix.X[2][1]=2*y*z-2*w*x;
        matrix.X[2][2]=1-2*x*x-2*y*y;
          
        return matrix;
    }
    
    /**
     * Add the given quaternion to the current one and return
     * the result. the current quaternion is not affected by
     * this operation.
     * 
     * @param q the quaternion to add
     * @return the resulting sum of the quaternions
     */
    public Quaternion add(Quaternion q)
    {
        Quaternion res=new Quaternion();
        
        res.w=w+q.w;
        res.x=x+q.x;
        res.y=y+q.y;
        res.z=z+q.z;
        
        return res;
    }
        
    /**
     * Subtract the given quaternion from the current one and 
     * return the result. the current quaternion is not 
     * affected by this operation.
     * 
     * @param q the quaternion to subtract
     * @return the resulting difference between the quaternions
     */
    public Quaternion sub(Quaternion q)
    {
        Quaternion res=new Quaternion();
        
        res.w=w-q.w;
        res.x=x-q.x;
        res.y=y-q.y;
        res.z=z-q.z;
        
        return res;
    }
    
    /**
     * Multiply the current quaternion with the given scalar and
     * return the result. The current quaternion is not affected
     * by this operation.
     * 
     * @param scalar the scalar to multiply with
     * @return the result of the multiplication
     */
    public Quaternion multiply(double scalar)
    {
        Quaternion res=new Quaternion();
        
        res.w=w*scalar;
        res.x=x*scalar;
        res.y=y*scalar;
        res.z=z*scalar;
        
        return res;
    }

    /**
     * Multiply the current quaternion with the given quaternion
     * and return the result.  The current quaternion is not
     * affected by this operation.
     * 
     * @param q the quaternion to multiply with
     * @return the result of the multiplication
     */
    public Quaternion multiply(Quaternion q)
    {
      Quaternion res=new Quaternion();
      
      res.w=w*q.w - x*q.x - y*q.x - z*q.z;
      res.x=w*q.x + x*q.w + y*q.z - z*q.y;
      res.y=w*q.y + y*q.w + z*q.x - x*q.z;
      res.z=w*q.z + z*q.w + x*q.y - y*q.x;
      
      return res;
    }
    
    /**
     * Calculate the dot product between the current and the given quaternion.
     * This does not affect the current quaternion.
     * 
     * @param q the quaterion to calculate the dot product with
     * @return the dot product \f$ x_1  x_2 + y_1 y_2 + z_1 z_2 + w_1 w_2 \f$
     */
    public double dotProduct(Quaternion q)
    {
        return w * q.w + x * q.x + y * q.y + z * q.z;
    }

    /**
     * Set the quaternion from the rotation by the given angly around the given
     * axis.
     * 
     * @param axis the axis to rotate around
     * @param angle the amout to rotate by
     */
    public void fromAxisAngle(Vector3D axis, double angle)
    {
        Vector3D normedAxis;

        normedAxis = axis.norm();

        w = Math.cos(angle / 2.0);
        x = Math.sin(angle / 2.0) * normedAxis.X1;
        y = Math.sin(angle / 2.0) * normedAxis.X2;
        z = Math.sin(angle / 2.0) * normedAxis.X3;
    }
    
    public void fromMatrix3(Matrix3 matrix) {
        double trace = matrix.X[0][0]+matrix.X[1][1]+matrix.X[2][2];
        double root;

        if ( trace > 0.0 )
        {
            // |w| > 1/2, may as well choose w > 1/2
            root = Math.sqrt(trace + 1.0);  // 2w
            w = 0.5*root;
            root = 0.5/root;  // 1/(4w)
            x = (matrix.X[2][1]-matrix.X[1][2])*root;
            y = (matrix.X[0][2]-matrix.X[2][0])*root;
            z = (matrix.X[1][0]-matrix.X[0][1])*root;
        }
        else
        {
            // |w| <= 1/2
            int s_iNext[] = { 1, 2, 0 };
            int  i = 0;
            if ( matrix.X[1][1] > matrix.X[0][0] )
                i = 1;
            if ( matrix.X[2][2] > matrix.X[i][i] )
                i = 2;
            int j = s_iNext[i];
            int k = s_iNext[j];

            root = Math.sqrt(matrix.X[i][i]-matrix.X[j][j]-matrix.X[k][k] + 1.0);
//            double apkQuat[] = { x, y, z };
            if(i==0) {
            	x=0.5*root;
            } else if(i==1) { 
            	y=0.5*root;
            } else if(i==2) {
            	z=0.5*root;
            }
//            apkQuat[i] = 0.5*root;
            root = 0.5/root;
            w = (matrix.X[k][j]-matrix.X[j][k])*root;
            if(j==0) {
            	x=(matrix.X[j][i]+matrix.X[i][j])*root;
            } else if(j==1) { 
            	y=(matrix.X[j][i]+matrix.X[i][j])*root;
            } else if(j==2) {
            	z=(matrix.X[j][i]+matrix.X[i][j])*root;
            }
//            apkQuat[j] = (matrix.X[j][i]+matrix.X[i][j])*root;
            if(k==0) {
            	x=(matrix.X[k][i]+matrix.X[i][k])*root;
            } else if(k==1) { 
            	y=(matrix.X[k][i]+matrix.X[i][k])*root;
            } else if(k==2) {
            	z=(matrix.X[k][i]+matrix.X[i][k])*root;
            }
//            apkQuat[k] = (matrix.X[k][i]+matrix.X[i][k])*root;
        }
    }
    
    /**
     * Do a spherical linear interpolation between the current quaternion and the
     * given one. ratio goes from 0 to 1 and specifies the ratio of the orientation
     * given by the current quaternion to the orientation of the given quaternion
     * in the result. The operation does not affect the current quaternion.
     * 
     * @param q the quaternion to serve as an interpolation target
     * @param ratio the ratio between the two quaternions
     * @return the interpolated quaternion.
     */
    public Quaternion slerpTo(Quaternion q, double ratio)
    {
        Quaternion result=new Quaternion();
        double theta = Math.acos(dotProduct(q));
        double inverseSin = 1.0 / Math.sin(theta);
        double coeff1, coeff2;

        coeff1 = Math.sin((1.0 - ratio) * theta);
        coeff2 = Math.sin(ratio * theta);

        if (coeff1 < 0) {
            coeff1 = -coeff1;

            result.w = (coeff1 * w * coeff2 * q.w) * inverseSin;
            result.x = (coeff1 * x * coeff2 * q.x) * inverseSin;
            result.y = (coeff1 * y * coeff2 * q.y) * inverseSin;
            result.z = (coeff1 * z * coeff2 * q.z) * inverseSin;

            result = result.normalize();
        } else {
            result.w = (coeff1 * w * coeff2 * q.w) * inverseSin;
            result.x = (coeff1 * x * coeff2 * q.x) * inverseSin;
            result.y = (coeff1 * y * coeff2 * q.y) * inverseSin;
            result.z = (coeff1 * z * coeff2 * q.z) * inverseSin;
        }

        return result;
    }

}