// keyframe interpolation in 2D
// Written June 2006 by Jarek Rossignac
int nr=6;                // number of cars
car C[] = new car [nr];   // array of cars
vec I=new vec (1,0);      // basis vector for angle calculations
vec labelD=new vec(-3,2); // offset vector for painting labels on cars
float t=0;                // animation time
car X;                    // moving car
int mode=0;               // interpolaiton mode: 0=linear, 1=Bezier, 2=twist
class car {              // cars
//  float h=40, w=100;                       // car dimensions
  float h=30, w=70;                       // car dimensions
  color col;                               // color of car
  pt B=new pt(0,0), F=new pt(w,0);         // front and back points (centers of disks)
  float speed = 0;                        // speed of car (user adjusted)
  float a=0.0;                             // angle of car orientation wrt I vector
  boolean picked = false;                  // true if car being dragged
  boolean pickedSpeed = false;             // true if speed being adjusted
  car (float px, float py, float pa, float ps, color pc) {B.x=px; B.y=py; a=pa; speed=ps; col=pc; F.x=px+w; F.y=py; F.turnAround(B,-a);}     // creation
  void paint() {pushMatrix(); translate(B.x,B.y); rotate(a);              // paints car
      noStroke();
      fill(col); rect(w/2,0,w,h); ellipse(0,0,w/7,h*0.95); ellipse(w,0,w/5,h*0.95);      
      fill(55);  ellipse(w*0.2,0,w*0.3,h); ellipse(w*0.2+w/3,0,w*0.4,h);
      fill(col);  rect(w*0.2+w/6,0,2*w/6,h);    
      popMatrix(); }
  boolean selected() {boolean pic = F.vecTo(mouse()).norm()<w/5; picked=pic; return(pic); }  // returns true and sets picked if mouse-clicked on car
  boolean selectedSpeed() {  // returns true and sets pickedSpeed if mouse-clicked on tip of car speed vector
     vec V=this.velocity(); pt G = B.make(); G.addVec(V); boolean pic = G.vecTo(mouse()).norm()<w/5; pickedSpeed=pic; return(pic); }
  void write() {println("Car: a="+a+", speed="+speed); print(". B="); B.write(); print(". F="); F.write(); }
  void track() {    // drag the picked car to follow mouse
    pt M=mouse();   
    vec FB = F.vecTo(B); float fb = FB.norm(); vec MB = M.vecTo(B); float mb = MB.norm();
    if (mb<0.0001) {F.setTo(M); B.addVec(FB); } else {F.setTo(M); M.addScaledVec(fb/mb,MB); B.setTo(M); };
    a=angle(I,B.vecTo(F)); 
    }
  void trackSpeed() { // adjust speed using dot product to follow mouse
    pt M=mouse();  vec BF= B.vecTo(F);  BF.unit(); vec BM = B.vecTo(M); speed=dot(BM,BF);
    }
 void paintDots() {if (!picked) { F.show(20); }; }; // paints disk in the car front
 void paintSpeed() {vec V=this.velocity();  V.show(B); }; // draws speed as line in front of car
 
 void interpolateLinear (car P, float s, car Q) {// performs linear interpolation
    B.interpolate(P.B,s,Q.B);
    a=P.a+s*(Q.a-P.a); 
    F.turnAround(B,-a);
    P.B.showLineTo(Q.B);
    } 

 void interpolateTwist (car P, float s, car Q) {  // Interpolation through pure rotation
     vec U = P.velocity(); U.unit();
     vec V = Q.velocity(); V.unit();
     float b=angle(U,V);
     pt M = midPt(P.B,Q.B);   
     float d=P.B.disTo(Q.B);
     float h = d/2/tan(b/2);
     vec W = P.B.vecTo(Q.B); W.unit();
     vec L = W.left();   L.mul(h);
     M.addVec(L); M.show(6);
     L.back();  L.unit(); 
     L.mul(abs(h+sq(d)/4/h)); 
     pt N = M.make(); N.addVec(L); drawArc(P.B,N,Q.B,5); 
     pt O = new pt(width/2,height/2);
     B.setTo(P.B);
     B.turnAround(M,-s*b);
     a=P.a+s*b; 
     F.turnAround(B,a);
     } 
     
 void interpolateBezier (car P, float s, car Q) { // performs bezier interpolation
     pt P1=P.B.make(); pt P2=P1.make();  P2.addVec(P.velocity()); 
     pt P4=Q.B.make();  pt P3=P4.make();  P3.subVec(Q.velocity()); 
     stroke(50,60,40); beginShape(LINE_STRIP); P1.vert(); P2.vert(); P3.vert(); P4.vert(); endShape();
     stroke(20,60,40); bezier(P1.x, P1.y, P2.x, P2.y, P3.x, P3.y, P4.x, P4.y);
     B.setTo(bezier(P1,P2,P3,P4,s));  
     a=bezierAngle(P1,P2,P3,P4,s); 
     F.turnAround(B,-a);
     } 
     
 vec velocity() {vec BF=B.vecTo(F); BF.unit(); BF.mul(speed); return(BF); };
  }     
 
void setup(){ size(600, 600);  colorMode(HSB,60);
   PFont font = loadFont("Courier-14.vlw"); textFont(font, 12);  
   createCars ();
   X = new car(C[0].B.x, C[0].B.y,  C[0].a, C[0].speed, color(20,60,30,22));
   }

void createCars () {for (int i=0; i<nr; i++)  C[i] = new car(width/2+width*0.3*cos(2*PI*i/nr),height/2+width*0.3*sin(2*PI*i/nr),PI/2+2*PI*i/nr,120,color(i*60/nr,60,60)); }
   
void draw()   {background (60);  rectMode(CENTER);  
  for (int i=0; i<nr; i++) {
    C[i].paint(); 
    if (C[i].pickedSpeed) stroke(0,60,60); else stroke(30,60,0); C[i].paintSpeed(); 
    fill(60); C[i].paintDots(); 
    fill(0); C[i].F.label(str(i),labelD);}; 
    t+=0.002; if (t>1) t=0;
    int i=floor(t*nr); int j=i+1; if (j==nr) j=0; float s=(t*nr-i); 
    if (mode==0) X.interpolateLinear(C[i],s,C[j]); 
    if (mode==1) X.interpolateBezier(C[i],s,C[j]); 
    if (mode==2) X.interpolateTwist(C[i],s,C[j]); 
     X.paint();
  }
    
void mousePressed() {boolean found=false; 
  for (int i=nr-1; i>=0; i--) { if (!found) {if(C[i].selected()) { found=true;}; }; };
  if (!found) {
  for (int i=nr-1; i>=0; i--) { if (!found) {if(C[i].selectedSpeed()) { found=true;}; }; };
     };
  } 
void mouseDragged() { 
  for (int i=0; i<nr; i++) { if(C[i].picked) {C[i].track(); }; };
  for (int i=0; i<nr; i++) { if(C[i].pickedSpeed) {C[i].trackSpeed(); }; };
  }
void mouseReleased() {
  for (int i=0; i<nr; i++) {C[i].picked=false; C[i].pickedSpeed=false;}; 
  };
  
void keyPressed() { 
   if (key=='0') {mode=0; println("mode="+mode); };  
   if (key=='1') {mode=1; println("mode="+mode); };  
   if (key=='2') {mode=2; println("mode="+mode); };  
   if (key=='3') {mode=3; println("mode="+mode); };  
   if (key=='h') {createCars();};  
   if (key=='s') {saveCars();};  
   if (key=='l') {loadCars();};   
   if (key=='w') {for (int i=0; i<nr; i++)  C[i].write();};
   }