package org.waltermilner;

import javax.media.opengl.GL;

/** This class represents a quaternion
 *
 * @author Walter Milner
 */

public class Quaternion {
    /** The quaternion = ( s, [vx, yv, vz] ) */
    private double s,vx,vy,vz;
    
    public static void test() // some testing
    {
        // check the rotate thing
        WMVector forward = new WMVector(0,0,1);
        WMVector result = forward.rotate(Math.PI/2,new WMVector(0,0,1));
        result.show("Rotated");
    }

    public void debug(String message) // for debugging
    {
        System.out.println(message);
        System.out.print("s = "+s);
        System.out.print(" vx= "+vx);
        System.out.print(" vy= "+vy);
        System.out.println(" vz= "+vz);
    }

    public WMVector getVector()
    {
        return new WMVector(vx,vy,vz);
    }

    /**
     * Construct a Quaternion from an angle and a WMVector
     *
     *
     * @param s The scalar part (angle) 
     * @param v The WMVector
     */
    public Quaternion(double angle, WMVector v)
    {
        this.s=Math.cos(angle/2);
        this.vx=Math.sin(angle/2)*v.getX();;
        this.vy=Math.sin(angle/2)*v.getY();
        this.vz=Math.sin(angle/2)*v.getZ();
    }


    /**
     * Construct a Quaternion from its elements
     *
     *
     * @param s The scalar part
     * @param vx x component of the vector
     * @param vy y component of the vector
     * @param vz z component of the vector
     */
    public Quaternion(double s, double vx, double vy, double vz)
    {
        this.s=s;
        this.vx=vx;
        this.vy=vy;
        this.vz=vz;
    }
    
    /** Construct a Quaternion from roll, pitch and yaw angles in radians
     * 
     * 
     * @param roll - angle in radians
     * @param pitch
     * @param yaw
     */
    public Quaternion( double roll, double pitch, double yaw)
    {
        // roll axis is z
        Quaternion qroll= new Quaternion( Math.cos(roll/2), 0,0,Math.sin(roll/2));
        // pitch is x
        Quaternion qpitch= new Quaternion( Math.cos(pitch/2), Math.sin(pitch/2),0, 0);
        // yaw is y
        Quaternion qyaw= new Quaternion( Math.cos(yaw/2), 0,Math.sin(yaw/2), 0);

        Quaternion result = qyaw.times(qpitch.times(qroll));
        s=result.s;
        vx=result.vx;
        vy=result.vy;
        vz=result.vz;
    }
/**
 * Multiply this Quaternion by another
 *
 * @param other The other Quaternion
 * @return the product of this Quaternion and the other one
 */
    public Quaternion times(Quaternion other)
    {
        double scalarbit = this.s*other.s-this.vx*other.vx-this.vy*other.vy-this.vz*other.vz;
        double newvx = this.vy*other.vz - this.vz*other.vy +this.s*other.vx+other.s*this.vx;
        double newvy = -this.vx*other.vz + other.vx*this.vz +this.s*other.vy+other.s*this.vy;
        double newvz = this.vx*other.vy-other.vx*this.vy +this.s*other.vz+other.s*this.vz;
        return new Quaternion(scalarbit, newvx, newvy, newvz);
    }

    /**
     * Do an OpenGL Rotate transform corresponding to this Quaternion
     *
     * @param gl The GL to do it in
     */
    public void rotate(GL gl)
    {      
        double ang = Math.acos(s);
        double sd = ang*180/Math.PI;
        gl.glRotated(sd, vx, vy, vz);
    }

    /**
     * The inverse
     * @return The Quaternion which is the inverse of this one.
     */
    public Quaternion inverse()
    {
        double scalar=s*s+vx*vx+vy*vy+vz*vz;
        scalar=1.0/scalar; // avoid 4 divisions
        return new Quaternion(s*scalar, -vx*scalar, -vy*scalar, -vz*scalar);
    }

}