/*
 * pQuaternion.java
 *
 * (C) 2005, Alex S.
 */
 


public class pQuaternion {

    public double w,x,y,z;

    private pQuaternion(){
    }

    public pQuaternion(double w,double x,double y,double z){
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
        normalize();
    }

    public pQuaternion(pQuaternion q){
        this.w = q.w;
        this.x = q.x;
        this.y = q.y;
        this.z = q.z;
    }


    /**
     * sets this quaternion to length 1
     */
    public void normalize(){
        double len = length();
        if(len > 0){
            w /= len;
            x /= len;
            y /= len;
            z /= len;
        }
    }

    /**
     * returns the norm
     */
    public double length(){
        return Math.sqrt(w*w+x*x+y*y+z*z);
    }

    /**
     * convert to rotation matrix
     */
    public double[] to3x3matrix(){
        double[] m = {
            1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y,
            2*x*y + 2*w*z, 1 - 2*x*x-2*z*z, 2*y*z - 2*w*x,
            2*x*z - 2*w*y,  2*y*z + 2*w*x,  1-2*x*x-2*y*y
        };
        return m;
    }

    /**
     * convert to rotation matrix
     */
    public double[] to4x4matrix(){
        double[] m = {
            1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y, 0,
            2*x*y + 2*w*z, 1 - 2*x*x-2*z*z, 2*y*z - 2*w*x, 0,
            2*x*z - 2*w*y,  2*y*z + 2*w*x,  1-2*x*x-2*y*y, 0,
            0, 0, 0, 1
        };
        return m;
    }

    /**
     * convert to axis/angle form
     * 
     * Ax, Ay, Az, Theta
     */
    public double[] toAxisAngle(){
        double s = x*x + y*y + z*z;
        double ax = x / s;
        double ay = y / s;
        double az = z / s;
        double theta = 2 * Math.acos(w);
        double[] aa = {ax,ay,az,theta};
        return aa;
    }

    /**
     * get a quaternion from axis and angle.
     */
    public static pQuaternion fromAxisAngle(double ax,double ay,double az,double theta){
        double s = Math.sin(theta/2);
        return new pQuaternion(Math.cos(theta/2),s*ax,s*ay,s*az);
    }

    /**
     * return the inverse
     */
    public pQuaternion conjugate(){
        return new pQuaternion(w,-x,-y,-z);
    }
    
    /**
     * Qr = Q1.Q2 = ( w1.w2 - v1.v2, w1.v2 + w2.v1 + v1 x v2 )
     */    
    public pQuaternion mult(pQuaternion q){
        pQuaternion a = new pQuaternion(this);
        return mult(a,q);
    }

    public static pQuaternion mult(pQuaternion A,pQuaternion B){
        pQuaternion C = new pQuaternion();
        C.x = A.w*B.x + A.x*B.w + A.y*B.z - A.z*B.y;
        C.y = A.w*B.y - A.x*B.z + A.y*B.w + A.z*B.x;
        C.z = A.w*B.z + A.x*B.y - A.y*B.x + A.z*B.w;
        C.w = A.w*B.w - A.x*B.x - A.y*B.y - A.z*B.z;
        return C;
    }

    /**
     * from angles
     */
    public pQuaternion fromEulerAngles(double ax,double ay,double az){
        pQuaternion qx,qy,qz,qt;
        qx = fromAxisAngle(1,0,0,ax);
        qy = fromAxisAngle(0,1,0,ay);
        qz = fromAxisAngle(0,0,1,az);
        return qx.mult(qy).mult(qz);
    }

    /**
     * render the thing
     */
    public String toString(){
        return "["+w+",<"+x+","+y+","+z+">]";
    }


}