public class Quaternion {

  public double x,y,z,w;

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

  public Quaternion(Quaternion q1,Quaternion q2) {
    x = q1.x; 
    y = q1.y; 
    z = q1.z; 
    w = q1.w;

    mul(q2);
  }

  public Quaternion(Vector start,Vector end) {
    set(start,end);
  }

  public Quaternion(Vector axis,double radians) {
    set(axis,radians);
  }
  
  public Quaternion(Vector omega) {
     set(omega); 
  }

  public void set(Vector axis,double radians) {
    double costheta = Math.cos(radians / 2);
    double sintheta = Math.sin(radians / 2);

    double len = axis.length();
    sintheta /= len;

    x = axis.x * sintheta;
    y = axis.y * sintheta;
    z = axis.z * sintheta;
    w = costheta; 

    normalize();
  }
  
  public void set(Vector omega) {
    double len = omega.length();
    double costheta = Math.cos(len / 2);
    double sintheta = Math.sin(len / 2);

    x = (omega.x/len) * sintheta;
    y = (omega.y/len) * sintheta;
    z = (omega.z/len) * sintheta;
    w = costheta;

    normalize();
  }

  public void set(Vector start,Vector end) {
    start.normalize();
    end.normalize();

    double tx, ty, tz, temp, dist;

    double	cost, len, ss;

    // get dot product of two vectors
    cost = start.x * end.x + start.y * end.y + start.z * end.z;

    // check if parallel
    if (cost > 0.99999999999f) {
      //printf("parallel\n");
      x = y = z = 0.0f;
      z = 1.0f;
      return;
    }
    else if (cost < -0.999999999f) {		// check if opposite
      //printf("opposite\n");
      // check if we can use cross product of from vector with [1, 0, 0]
      tx = 0.0;
      ty = start.x;
      tz = -start.y;

      len = Math.sqrt(ty*ty + tz*tz);

      if (len < 0.000000001)
      {
        // nope! we need cross product of from vector with [0, 1, 0]
        tx = -start.z;
        ty = 0.0;
        tz = start.x;
      }

      // normalize
      temp = tx*tx + ty*ty + tz*tz;

      dist = (double)(1.0 / Math.sqrt(temp));

      tx *= dist;
      ty *= dist;
      tz *= dist;

      x= tx;
      y = ty;
      z = tz;
      w = 0.0;

      return;
    }

    //printf("normal\n");
    // ... else we can just cross two vectors

    tx = start.y * end.z - start.z * end.y;
    ty = start.z * end.x - start.x * end.z;
    tz = start.x * end.y - start.y * end.x;

    temp = tx*tx + ty*ty + tz*tz;

    if(temp == 0) {
    // System.out.printf("FUCKFUCK\n");
    }

    dist = (double)(1.0 / Math.sqrt(temp));

    tx *= dist;
    ty *= dist;
    tz *= dist;


    // we have to use half-angle formulae (sin^2 t = ( 1 - cos (2t) ) /2)

    ss = Math.sqrt(0.5f * (1.0f - cost));

    tx *= ss;
    ty *= ss;
    tz *= ss;

    // scale the axis to get the normalized quaternion
    x = tx;
    y = ty;
    z = tz;

    // cos^2 t = ( 1 + cos (2t) ) / 2
    // mV[3] part is cosine of half the rotation angle
    w = (double)Math.sqrt(0.5f * (1.0f + cost));
  }

  public void invert() {
    double norm, invNorm;

    norm = x * x + y * y + z * z
      + w * w;

    invNorm = (double) (1.0 / norm);

    x = -x * invNorm;
    y = -y * invNorm;
    z = -z * invNorm;
    w =  w * invNorm;
  }

  public void negate() {
    x = -x;
    y = -y;
    z = -z;
  }

  /**
   * 	 * Multiply something by this quat and store the result in this quat.
   * 	 * @param quat
   	 */

  public void premul(Quaternion quat) {
    double tw = ( quat.w * w ) - ( quat.x * x ) - ( quat.y * y ) - ( quat.z * z );
    double tx = ( quat.w * x ) + ( quat.x * w ) + ( quat.y * z ) - ( quat.z * y );
    double ty = ( quat.w * y ) - ( quat.x * z ) + ( quat.y * w ) + ( quat.z * x );
    double tz = ( quat.w * z ) + ( quat.x * y ) - ( quat.y * x ) + ( quat.z * w );

    w = tw;
    x = tx;
    y = ty;
    z = tz;

    normalize();
  }

  public void mul(Quaternion quat) {
    double tw = ( w * quat.w ) - ( x * quat.x ) - ( y * quat.y ) - ( z * quat.z );
    double tx = ( w * quat.x ) + ( x * quat.w ) + ( y * quat.z ) - ( z * quat.y );
    double ty = ( w * quat.y ) - ( x * quat.z ) + ( y * quat.w ) + ( z * quat.x );
    double tz = ( w * quat.z ) + ( x * quat.y ) - ( y * quat.x ) + ( z * quat.w );

    w = tw;
    x = tx;
    y = ty;
    z = tz;

    normalize();
  }

  public void normalize() {
    double	dist, square;

    square = x*x + y*y + z*z + w*w;

    if (square > 0.0) {
      dist = (double)(1.0 / Math.sqrt(square));
      x = x*dist;
      y = y*dist;
      z = z*dist;
      w = w*dist;
    }
    else {
      x = 0;
      y = 0;
      z = 0;
      w = 1;
    }
  }

  public Vector mul(Vector v) {
    return (new Matrix(this)).mul(v);
  }

  public Vector axis() {
    double fRadians = Math.acos(w) * 2;
    double sintheta = Math.sin(fRadians/2);

    if(sintheta == 0) { 
      sintheta = 1; 
    }

    Vector v = new Vector(x/sintheta,y/sintheta,z/sintheta);
    v.normalize();

    return v;
  }

  public double angle() {
    normalize();

    double len = x*x+y*y+z*z;

    if (len > 0.00001)
    {
      return 2.0 * Math.acos(w);
    }

    else {
      return 0;
    }
  }
}