Vector ap =  new Vector();
Quaternion rotation = new Quaternion(new Vector(0,0,0),PI/4);

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);

float rotX,rotY;
int oMouseX,oMouseY;
void setup() {
  size(200,200,P3D);
  noStroke();
}

void mouseDragged() {
  rotY += ((float)(mouseX - oMouseX)) * 0.01;
  rotX -= ((float)(mouseY - oMouseY)) * 0.01;
  
  if(rotX >= PI/2 - 0.05) {
    rotX = PI/2;
  }
  if(rotX <= -PI/2 + 0.05) {
    rotX = -PI/2;
  }
}

void draw() {
  lights();
  if(keyPressed) {
    if(key == 'w') {
      ap.x += 0.01;
    } 
    else if(key == 's') {
      ap.x -= 0.01;
    }
    else if(key == 'a') {
      ap.y += 0.01;
    }
    else if(key == 'd') {
      ap.y -= 0.01;
    }
    else if(key == 'q') {
      ap.z += 0.01;
    }
    else if(key == 'e') {
      ap.z -= 0.01;
    }
    
    if(key == 'r') {
      ap.x = ap.y = ap.z = 0;
      rotation = new Quaternion();
    }
  }

  background(0);
  pushMatrix();
  
  Vector fs_ap = rotation.mul(ap);  // ap (angular momentum vector) in rotating frame
  Vector fs_om = cube_inertia_inverse.component_mul(fs_ap);  // L=Iw or w=LI^-1
  
  rotation.mul(new Quaternion(fs_om));

  Matrix m = new Matrix(rotation);
  
  translate(width/2,height/2,0);
 
  stroke(200,100,100);
  line(0.,0.,0.,(float)ap.x*100,(float)ap.y*100,(float)ap.z*100);
 
  applyMatrix((float)m.m[0],(float)m.m[1],(float)m.m[2],(float)m.m[3],
              (float)m.m[4],(float)m.m[5],(float)m.m[6],(float)m.m[7],
              (float)m.m[8],(float)m.m[9],(float)m.m[10],(float)m.m[11],
              (float)m.m[12],(float)m.m[13],(float)m.m[14],(float)m.m[15]);
  
  stroke(100,200,100);
  line(0.0,0.0,0.0,(float)fs_om.x*1000,(float)fs_om.y*1000,(float)fs_om.z*1000);
  
  stroke(255);
  
  fill(255,255,255,100);
  scale((float)cube_dims.x,(float)cube_dims.y,(float)cube_dims.z);
  box(50);
  
  popMatrix();
}