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 w0,w1,w2; //angular velocities about the principal axes float v0,v1,v2; //initial angular velocities float dt=0.05; //time step 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(); I1=(L2*L2+L3*L3)/10000; I2=(L1*L1+L3*L3)/10000; I3=(L1*L1+L2*L2)/10000; v0=0; v1=0.01; v2=0; w0=0; w1=0.01; w2=0; currentReferenceVelocity.a[1][2]=w0; currentReferenceVelocity.a[2][1]=w0*-1; currentReferenceVelocity.a[0][2]=w1; currentReferenceVelocity.a[2][0]=w1*-1; currentReferenceVelocity.a[0][1]=w2; currentReferenceVelocity.a[1][0]=w2*-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) { updateReferenceVelocity(); 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 updateReferenceVelocity() { float dw0,dw1,dw2; w0=(currentReferenceVelocity.a[1][2]-currentReferenceVelocity.a[2][1])/2.0; w1=(currentReferenceVelocity.a[0][2]-currentReferenceVelocity.a[2][0])/2.0; w2=(currentReferenceVelocity.a[0][1]-currentReferenceVelocity.a[1][0])/2.0; dw0=w1*w2*(I2-I3)/I1; //Euler equations! dw1=w0*w2*(I3-I1)/I2; dw2=w0*w1*(I1-I2)/I3; w0+=dw0*dt; w1-=dw1*dt; w2+=dw2*dt; currentReferenceVelocity.a[1][2]=w0; //updates reference velocity vector with changes due to imbalanced moments of inertia currentReferenceVelocity.a[2][1]=w0*-1; currentReferenceVelocity.a[0][2]=w1; currentReferenceVelocity.a[2][0]=w1*-1; currentReferenceVelocity.a[0][1]=w2; currentReferenceVelocity.a[1][0]=w2*-1; } void setVelocity() { //resets viewer axes and resets velocity Matrix change=rotation(-1*xr,1).times(rotation(yr,0)); currentConfiguration=change.times(currentConfiguration); Matrix ref=new Matrix(0); ref.a[1][2]=v0; ref.a[2][1]=v0*-1; ref.a[0][2]=v1; ref.a[2][0]=v1*-1; ref.a[0][1]=v2; ref.a[1][0]=v2*-1; currentReferenceVelocity=(currentConfiguration.inverse()).times(ref.times(currentConfiguration)); xr=0; yr=0; } void mouseDragged() { xr+=0.015*(mouseX-pmouseX); yr+=0.015*(mouseY-pmouseY); } void keyTyped() { if(key=='p') { setVelocity(); 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