arm

var l1 = 100;

var l2 = 80;

var l3 = 10;

var h = 50;

var a1 = 0;

var a2 = 0;

var posError = 0;

var a1Target = 0;

var a2Target = 0;

​

function setup() {

  createCanvas(windowWidth, windowHeight);

  background(255);

  stroke(0);

  angleMode(DEGREES);

}

​

function draw() {

​

  background(255);

  var maxRadius = l1+l2;

  strokeWeight(1);

​

  fill(30);

  ellipse(width/2, height/2, 2*maxRadius, 2*maxRadius);

  stroke(255);

  rect(0, height/2, width, height);

​

  var angle = 0;

  if (mouseY>=height/2) {

    if (mouseX > width/2) angle = 90;

    else angle = -90;

  } else {

    angle = atan((width/2-mouseX) / (min(height/2, mouseY)-height/2));

    // angle = atan((width/2-mouseX) / min(0, mouseY-height/2));

  }

​

  var x = 0;

  if(mouseX>width/2)

    x = min(maxRadius*sin(angle), mouseX-width/2);

  else

    x = max(maxRadius*sin(angle), mouseX-width/2);

  var y = min(max(0, height/2-mouseY), maxRadius*cos(angle));

​

  fill(0);

  text("angle (deg): " + angle, 10, 30);

  text("x: " + x, 10, 100);

  text("y: " + y, 10, 130);

​

  push();

    translate(width/2, height/2);

    stroke(100);

    line(0,0,maxRadius*sin(angle), -maxRadius*cos(angle));

    stroke(100,0,0);

    line(0, 0, x, -y);

  pop();

​

  ///// 

  estimateAngles(x,y);

​

  a1 += (a1Target - a1)/20;

  a2 += (a2Target - a2)/20;

​

  text("a1: " + a1, 10, 170);

  text("a2: " + a2, 10, 200);

  text("err: " + posError, 10, 260);

​

  strokeWeight(5);

  push();

    translate(width/2, height/2);

    stroke(200,0,0);

    line(0,0,l1*sin(a1), -l1*cos(a1));

    push();

      translate(l1*sin(a1), -l1*cos(a1));

      stroke(0,200,0);

      line(0,0,l2*sin(a1+a2), -l2*cos(a1+a2));

    pop();

  pop();

}

​

function estimateAngles(x,y){

  var r = createVector(x,y);

  var best1 = 0;

  var best2 = 0;

  var minErr = Number.MAX_VALUE;

  for(var t1=-90; t1<=90; t1++){

    var v1 = createVector(l1*sin(t1), l1*cos(t1));  

    for(var t2=-180; t2<=180; t2++){

      var v2 = createVector(v1.x + l2*sin(t1+t2), v1.y + l2*cos(t1+t2));      

      var d = p5.Vector.sub(r,v2).mag();

      if(d < minErr){

        best1 = t1;

        best2 = t2;

        minErr = d;

      }

    }

  }

​

  a1Target = best1;

  a2Target = best2;

  posError = minErr;

}

​

​

​