markerprinter_update

markerPrinter mprint1, mprint2;
float move_x, move_y, counter;
int curmillis, prevmillis;
void setup (){
  size(400,600);
  mprint1 = new markerPrinter(70, 250, 250, 'l');
  mprint2 = new markerPrinter(-70, 250, 250, 'r');
  curmillis = millis ();
  prevmillis = curmillis;
  move_x = 0;
}

void draw (){
  curmillis = millis ();
  if (curmillis - prevmillis > 200){
    fill (200);
    rect (0, 0, width, height);
    fill (0);
    text (width+"---"+height, 20, height-40);
    for (int i = 0; i < width; i++){
      stroke (250, 0, 250);
      point(i, 400+30*sin (radians (2*i)));
    }
    move_y = 400+30*sin (radians (2*move_x));
    mprint1.update(move_x, move_y);
    mprint2.update(move_x, move_y);
    mprint1.display();
    mprint2.display();
    fill (0);
    text ("left : "+degrees (mprint1.angle_sh), width-150, height-50);
    text ("right: "+degrees (mprint2.angle_sh), width-150, height-25);
    move_x++;
    if (move_x >= width){
      move_x = 0;
      clear ();
    }
    prevmillis = curmillis;
  }
 
}

void mousePressed(){
 
}

class markerPrinter{
  PVector center = new PVector ();   //position center acxes
  PVector marker = new PVector ();   //position marker
  PVector arms = new PVector ();     //(|arm1|, |arm2|) length arms
  PVector arm1 = new PVector ();     //position hinge
  PVector arm2 = new PVector ();     //position marker, caculated, used to draw arm 2
  char side;         //position servo vs center
  float offsetX;     //offsetX servo vs centerpoint
  float line_sm;     //virtual line marker to servo
  float angle_m;     //angle between X and line_sm
  float angle_mh;    //angle between line_sm and arm1
  float angle_sm;    //angle between arm1 and arm2
  float angle_sh;    //angle between arm1 and X
 
  markerPrinter(float tempoffx, float temparm1, float temparm2, char temps){
    offsetX = tempoffx;
    center.set(width/2-offsetX, 50);
    arms.set(temparm1, temparm2);
    side = temps;
  }
 
  void update(float temp_mx, float temp_my){
    marker.set (temp_mx-center.x, temp_my-center.y);
    if (marker.y < 0){
      marker.y = 0;
    }
    line_sm = sqrt (sq (marker.x)+sq (marker.y));
    angle_m = acos (marker.x/line_sm);
    switch (side){
      case 'l':
        angle_mh = acos ((sq (arms.y)-sq (arms.x)-sq (line_sm))/(-2*arms.x*line_sm));
        angle_sm = acos ((sq (line_sm)-sq (arms.x)-sq (arms.y))/(-2*arms.x*arms.y));
        break;
      case 'r':
        angle_mh = -acos ((sq (arms.y)-sq (arms.x)-sq (line_sm))/(-2*arms.x*line_sm));
        angle_sm = -acos ((sq (line_sm)-sq (arms.x)-sq (arms.y))/(-2*arms.x*arms.y));
        break;
    }
    angle_sh = angle_m + angle_mh;
    arm1.x = arms.x*cos (angle_sh);
    arm1.y = arms.x*sin (angle_sh);
    arm2.x = arms.y*cos (angle_sm-(PI-angle_sh));
    arm2.y = arms.y*sin (angle_sm-(PI-angle_sh));
  }
 
  void display(){
    pushMatrix ();
    translate (center.x, center.y);
    stroke (180);
    line (-center.x, 0, width, 0);   //X-acxe servo
    //line (0, -center.y, 0, height);  //Y-acxe servo
    //stroke (255, 0, 0, 25);
    //line (0, 0, marker.x, marker.y ); //virtual line servomarker
    stroke (0, 0, 255);
    line (0, 0, arm1.x, arm1.y);     //arm1
    translate (arm1.x, arm1.y);      //hingepoint
    line (0, 0, arm2.x, arm2.y);     //arm2
    popMatrix ();
  }
}