float xr=0.0, yr=0.0;  //x rotation and y rotation of field of view
float L1=100,L2=100,L3=100;  //lengths of the box
float I1,I2,I3;  //moments of inertia of the box
float v0,v1,v2;  //angular velocities
boolean paused=true;
Matrix currentReferenceVelocity;
Matrix currentConfiguration;

void setup() {
  size(700,700,P3D);  //initializes display window
  stroke(0,0,0,200);
  fill(255,255,255);

  currentConfiguration=new Matrix();
  currentReferenceVelocity=new Matrix(0);

  v0=0;
  v1=0.01;
  v2=0;
  currentReferenceVelocity.a[1][2]=v0;
  currentReferenceVelocity.a[2][1]=v0*-1;
  currentReferenceVelocity.a[0][2]=v1;
  currentReferenceVelocity.a[2][0]=v1*-1;
  currentReferenceVelocity.a[0][1]=v2;
  currentReferenceVelocity.a[1][0]=v2*-1;
}

void draw() {
  background(100,100,100);
  translate(350,350,0);
  rotateY(xr);
  rotateX(-1*yr);   //changes field of view according to data obtained from mouse input
  stroke(0,0,0,100);
  line(-200,0,0,200,0,0); // draws x-axis
  line(0,-200,0,0,200,0); // draws y-axis
  line(0,0,-200,0,0,200); // draw z-axis

  updateConfiguration();
  float[][] m=currentConfiguration.a;
  applyMatrix(m[0][0],m[0][1],m[0][2],0,
  m[1][0],m[1][1],m[1][2],0,
  m[2][0],m[2][1],m[2][2],0,
  0,0,0,1);  //moves box to current configuration
  stroke(0,0,0,150);
  strokeWeight(2);
  box(L1,L2,L3);
}

void updateConfiguration() {
  if(!paused){
    Matrix currentVelocity=currentConfiguration.times(currentReferenceVelocity.times(currentConfiguration.inverse()));  // v= Q v_ref Q^(-1)
    Matrix smallRotation=currentVelocity.exponential();  //converts an infinitesimal rotation to a genuine (small) rotation
    currentConfiguration=smallRotation.times(currentConfiguration);
  }
}

void mouseDragged() {
  xr+=0.015*(mouseX-pmouseX);
  yr+=0.015*(mouseY-pmouseY);
}

void keyTyped() {
  if(key=='p') {
    paused=!paused;
  }
}

class Matrix {
  float[][] a= new float[3][3];
  public Matrix(float scalar) {
    for(int i=0;i<3;i++) {
      a[i][i]=scalar;
      for(int j=0;j<i;j++) {
        a[i][j]=0;
        a[j][i]=0;
      }
    }
  }
  public Matrix() {
    this(1.0);
  }
  public Matrix(float[][] b) {
    a=b;
  }
  Matrix times(Matrix m) {
    float[][] p= new float[3][3];
    for(int i=0;i<3;i++)
      for(int j=0;j<3;j++) {
        float sum=0;
        for(int k=0;k<3;k++) 
          sum+=a[i][k]*m.a[k][j];
        p[i][j]=sum;
      }
    return new Matrix(p);
  }
  Matrix plus(Matrix m) {
    float[][] s= new float[3][3];
    for(int i=0;i<3;i++)
      for(int j=0;j<3;j++) {
        s[i][j]=a[i][j]+m.a[i][j];
      }
    return new Matrix(s);
  }
  float det() {
    return (a[0][0]*(a[1][1]*a[2][2]-a[1][2]*a[2][1])-a[0][1]*(a[1][0]*a[2][2]-a[1][2]*a[2][0])+a[0][2]*(a[1][0]*a[2][1]-a[1][1]*a[2][0]));
  }
  Matrix inverse() {
    float d= det();
    float[][] inv= new float[3][3];
    for(int i=0;i<3;i++)
      for(int j=0;j<3;j++) {
        inv[j][i]=(a[(i+1)%3][(j+1)%3]*a[(i+2)%3][(j+2)%3]-a[(i+1)%3][(j+2)%3]*a[(i+2)%3][(j+1)%3])/d;
      }
    return new Matrix(inv);
  }
  Matrix times(float sc) {
    float[][] b=a;
    for(int i=0;i<3;i++)
      for(int j=0;j<3;j++)
        b[i][j]=a[i][j]*sc;
    return new Matrix(b);
  }
  Matrix exponential() {
    Matrix t0= new Matrix(1);
    Matrix t1=this;
    Matrix t2=t1.times(this).times(0.5);
    Matrix t3=t2.times(this).times(1/3.0);
    Matrix t4=t3.times(this).times(1/4.0);
    Matrix t5=t4.times(this).times(1/5.0);
    return t0.plus(t1.plus(t2.plus(t3.plus(t4.plus(t5)))));
  }
  String toString() {
    return "\n"+a[0][0]+"  "+a[0][1]+"  "+a[0][2] +"\n"+a[1][0]+"  "+a[1][1]+"  "+a[1][2]+"\n"+a[2][0]+"  "+a[2][1]+"  "+a[2][2];
  }
}

Matrix rotation(float angle, int axis) {
  float[][] temp=new float[3][3];
  temp[(axis+1)%3][(axis+1)%3]=cos(angle);
  temp[(axis+2)%3][(axis+2)%3]=cos(angle);
  temp[(axis+1)%3][(axis+2)%3]=sin(angle);
  temp[(axis+2)%3][(axis+1)%3]=-1*sin(angle);
  temp[axis][axis]=1;
  temp[axis][(axis+1)%3]=0;
  temp[axis][(axis+2)%3]=0;
  temp[(axis+1)%3][axis]=0;
  temp[(axis+2)%3][axis]=0;
  return new Matrix(temp);
}