Vector L = new Vector(random(-0.5,0.5),random(-0.5,0.5),random(-0.5,0.5));
Quaternion rotation1 = new Quaternion();
Quaternion rotation2 = new Quaternion();

Vector cube_dims = new Vector(1,2,3);
Vector cube_inertia = new Vector(cube_dims.y*cube_dims.y+cube_dims.z*cube_dims.z,cube_dims.x*cube_dims.x+cube_dims.z*cube_dims.z,cube_dims.x*cube_dims.x+cube_dims.y*cube_dims.y);
Vector cube_inertia_inverse = new Vector(1/cube_inertia.x,1/cube_inertia.y,1/cube_inertia.z);

Quaternion rotation = new Quaternion();

java.util.Vector trace1 = new java.util.Vector();
boolean tracing1 = true;

java.util.Vector trace2 = new java.util.Vector();
boolean tracing2 = true;

boolean sim = true;

float zoom = 1;

void setup() {
  size(300,300,P3D);
  frameRate(60);
}

void applyMat(Matrix m) {
  applyMatrix((float)m.m[0], (float)m.m[4], (float)m.m[8], (float)m.m[12],
              (float)m.m[1], (float)m.m[5], (float)m.m[9], (float)m.m[13],
              (float)m.m[2], (float)m.m[6], (float)m.m[10],(float)m.m[14],
              (float)m.m[3], (float)m.m[7], (float)m.m[11],(float)m.m[15]);
}

Vector stepEuler(Quaternion r,Vector L,Vector inertia,float timestep) {
  Vector om = omega(r,L,inertia);
  om.mul(timestep);
  r.premul(new Quaternion(om));
  om.mul(1/timestep);
  return om;
}

Vector omega(Quaternion r,Vector L,Vector inertia) {
  return r.mul(Vector.componentDiv(r.unmul(L),inertia));
}

Vector stepRK(Quaternion r,Vector L,Vector inertia,float timestep) {
  Vector omega = new Vector();
  
  // k1 is omega the standard way

  Vector k1 = omega(r,L,cube_inertia);
  omega.addScaled(k1,1.0/6.0);
  
  // k2 is slope at midpoint of interval, using slope k1 to determine the rotation at the half-step
  
  k1.mul(timestep/2.0);
  
  Quaternion r2 = new Quaternion(k1); r2.mul(r);
  Vector k2 = omega(r2,L,cube_inertia);
  omega.addScaled(k2,1.0/3.0);
  
  // k3 is the rotation at midpoint, but using slope k2 to determine rotation
  
  k2.mul(timestep/2.0);
  
  Quaternion r3 = new Quaternion(k2); r3.mul(r);
  Vector k3 = omega(r3,L,cube_inertia);
  omega.addScaled(k3,1.0/3.0);
  
  // k4 is the rotation at the end of the timestep, using k3
  
  k3.mul(timestep);
  
  Quaternion r4 = new Quaternion(k3); r4.mul(r);
  Vector k4 = omega(r4,L,cube_inertia);
  omega.addScaled(k4,1.0/6.0);
  
  omega.mul(timestep);
  r.premul(new Quaternion(omega));
  omega.mul(1/timestep);
  
  return omega;
}

int oldX,oldY;

void mousePressed() {
  oldX = mouseX;
  oldY = mouseY;
}

void mouseDragged() {
  float dx = (float)(mouseX - oldX);
  float dy = -(float)(mouseY - oldY);
  oldX = mouseX;
  oldY = mouseY;
  
  rotation.premul(new Quaternion(new Vector(0,dx*0.01,0)));
  rotation.premul(new Quaternion(new Vector(dy*0.01,0,0)));
}



void keyPressed() {
  if(key == 't') {
      tracing1 = !tracing1;
      tracing2 = !tracing2;
    }
  if(key == 'g') {
    sim = !sim;
  }
  if(key == 'i') {
    zoom *= 1.1;
  }
  if(key == 'o') {
    zoom *= 0.9;
  }
}
void draw() {
  Vector omega1=new Vector(),omega2 = new Vector();
  
  background(0,0,0,0);
  lights();
  if(keyPressed) {
    if(key == 'w') {
      L.add(rotation.unmul(new Vector(0.01,0.0,0.0)));
    } 
    else if(key == 's') {
      L.add(rotation.unmul(new Vector(-0.01,0,0)));
    }
    else if(key == 'a') {
      L.add(rotation.unmul(new Vector(0,0.01,0)));
    }
    else if(key == 'd') {
      L.add(rotation.unmul(new Vector(0,-0.01,0)));
    }
    else if(key == 'q') {
      L.add(rotation.unmul(new Vector(0,0,0.01)));
    }
    else if(key == 'e') {
      L.add(rotation.unmul(new Vector(0,0,-0.01)));
    }
    
    if(key == 'r') {
      L.x = L.y = L.z = 0;
      rotation1 = new Quaternion();
      rotation2 = new Quaternion();
      sim = false;
      tracing1 = false;
      tracing2 = false;
    }
  }
  
  pushMatrix();
 
  translate(width/2,height/2,0);
  applyMat(new Matrix(rotation));
  scale(zoom,zoom,zoom);
  
  pushMatrix();
  
  Vector fs_ap = rotation1.unmul(L);
  Vector fs_om = Vector.componentDiv(fs_ap,cube_inertia);
  
  if(sim) {
    omega1 = stepEuler(rotation1,L,cube_inertia,1);
  }

  stroke(0,0,255);
  line(0.,0.,0.,(float)L.x*100,(float)L.y*100,(float)L.z*100);

  applyMat(new Matrix(rotation1));
  
  stroke(100,200,100);
  
  stroke(255,200,200,255);
  fill(255,100,100,100);
  scale((float)cube_dims.x,(float)cube_dims.y,(float)cube_dims.z);
  
  box(50);
  
  popMatrix();
  
  pushMatrix();
  
  
  if(sim) {
    omega2 = stepRK(rotation2,L,cube_inertia,1);
  }
  
  omega1.mul(1000);
  omega2.mul(1000);

  stroke(255,0,0);
  line(0.0,0.0,0.0,(float)omega1.x,(float)omega1.y,(float)omega1.z);
  if(tracing1) {
    trace1.add(new Vector(omega1));
    
    stroke(255,0,0,100);
    for(int i = 1 ; i < trace1.size() ; i++) {
      Vector v1 = (Vector)trace1.elementAt(i-1);
      Vector v2 = (Vector)trace1.elementAt(i);
      
      line((float)v1.x,(float)v1.y,(float)v1.z,(float)v2.x,(float)v2.y,(float)v2.z);
    }
  } else {
    trace1.clear();
  }
  
  stroke(0,255,0);
  line(0.0,0.0,0.0,(float)omega2.x,(float)omega2.y,(float)omega2.z);
  if(tracing2) {
    trace2.add(new Vector(omega2));
    
    stroke(0,255,0,100);
    for(int i = 1 ; i < trace2.size() ; i++) {
      Vector v1 = (Vector)trace2.elementAt(i-1);
      Vector v2 = (Vector)trace2.elementAt(i);
      
      line((float)v1.x,(float)v1.y,(float)v1.z,(float)v2.x,(float)v2.y,(float)v2.z);
    }
  } else {
    trace2.clear();
  }
 
  applyMat(new Matrix(rotation2));
  
  stroke(200,255,200,255);
  fill(100,255,100,100);
  scale((float)cube_dims.x,(float)cube_dims.y,(float)cube_dims.z);
  
  box(50);
  
  popMatrix();
  popMatrix();
}