// 2D drag and turn
// Written June 2006 by Jarek Rossignac
float t; float t0;
Mouse M = new Mouse();                  // to track mouse
int nr=4;                              // number of rectangles
Rectangle R [] = new Rectangle [nr];   // table of rectangles
pt C = new pt();

class Mouse {
   Mouse() {};
   pt P = new pt();     // mouse position 
   pt L = new pt();     // last mouse position 
   vec V = new vec();   // mouse speed 
   vec A = new vec();   // mouse acceleration
   float l = millis();  // time of last update
   void update() {
      L.setTo(P);
      P.setToMouse();
      float t=millis()-l;             // time since last update
      t=1;
      vec U =  L.vecTo(P); U.div(t);  // new speed U = LP/t;
      A.setTo(U); A.sub(V); A.div(t); // new acceleration A=(U-V)/t
      V.setTo(U);
      l=millis();
      };
   }

class Rectangle {
  pt  O = new pt(400,400);             // origin 
  vec V = new vec(0,0);                // velocity
  vec R0= new vec(0,0);                // picked position in body coord.
  float a=0.0, dadt=0.0;               // angle and angular speed 
  float h=30, w=70;                    // dimensions
  vec I=new vec(1,0), J=new vec(0,1);  // local CS
  boolean picked = false;              // true if being dragged
  color col;                           // color
  float Mass = .001;
  float Inertia = (h*h*h*w + h*w*w*w)/3 * (Mass/w/h);

  Rectangle (float px, float py, color pc) {O.x=px; O.y=py; col=pc;};  // create from position and color
  Rectangle (float px, float py, float pa, color pc) {a=pa; O.x=px; O.y=py; col=pc;};  // create from position and color
  void move () { println("moved");
     O.addVec(V);
     this.updateBasis();    // rotates to have angle a
     };                     // old mouse is now new mouse

  void updateBasis() {I.x=cos(a); I.y=sin(a); J.x=-sin(a); J.y=cos(a);};// reco,putes basis by rotating by a
  void paint() {pushMatrix(); translate(O.x,O.y); rotate(a);  fill(col); if(picked) fill(100); rect(0,0,2*w,2*h);  popMatrix(); }; 
  boolean mouseIsIn() {pt N = new pt(); N.setToMouse(); vec D=O.vecTo(N); float x=dot(I,D); float y=dot(J,D);  return((abs(x)<w)&&(abs(y)<h)); }; // true if mouse in rectangle
  void register() {
    if(this.mouseIsIn()) {
      picked=true;
      dadt = 0;
      M.P.setToMouse();
      vec RR = O.vecTo(M.P);
      R0.x = RR.x*I.x + RR.y*I.y; 
      R0.y = RR.x*J.x + RR.y*J.y; 
      };
    }        // sets picked status and remember mouse if picked
    
  void track() {
    float dt = .3;
    vec R = R0.turn(a);
//  float alp = - (Mass)/(Mass*R.norm()+Inertia) * (M.A.x*R.y - M.A.y*R.x);    // this shoule be physically correct, but M.A is noisy
    float alp = - (Mass)/(Mass*R.norm()+Inertia) * (M.V.x*R.y - M.V.y*R.x);
    dadt += dt * alp - 0.1*dadt;                  // increment angular velocity 
    a    += dt * dadt;
    R = R0.turn(a);
    O.setTo(M.P);    O.subVec(R);
    this.updateBasis(); 
    }
  }     

void setup(){ size(600, 600);  create (400,100); t0=millis(); C.setTo(width/2,height/2);}

void create (float x, float y) {
  R[0] = new Rectangle(x,y,color(200,0,0));
  R[1] = new Rectangle(x,y+75,color(0,200,0));
  R[2] = new Rectangle(x,y+2*75,color(0,0,200));
  R[3] = new Rectangle(x,y+3*75,color(150,150,20));
  }

void draw()   {background (255); rectMode(CENTER);  
  t=millis()-t0; t0=millis(); // t=time from last frame, usually between 5 and 15
  M.update();  for (int i=0; i<nr; i++) { if (R[i].picked)  R[i].track(); }; 
  for (int i=0; i<nr; i++) {
    R[i].paint();  
    };
  vec W = new vec(); 
  }

void mouseReleased() { for (int i=0; i<nr; i++) R[i].picked=false; };
void mousePressed() { boolean found=false; for (int i=nr-1; i>=0; i--) { if (!found) {R[i].register(); found=R[i].picked;}; }; }