inverse kinematics

by frogggeee McFrogggeee

HTML

<p id = "text">

</p>
<canvas id = "canvas" width = "480" height = "360" style = "border: 1px solid;"></canvas>

JavaScript

let ctx = document.getElementById("canvas").getContext("2d");
let mx = 0;
let my = 0;
let arm1 = 155;
let arm2 = 155;
let x = 240;
let y = 360;
let endX;
let endY;
let servo1 = 0;
let servo2 = 0;
document.addEventListener("mousemove", function moved(e) {
  const rect = e.target.getBoundingClientRect();
  mx = e.clientX - rect.left;
  my = e.clientY - rect.top;
});


function line(x, y, x2, y2, style) {
  ctx.strokeStyle = style;
  ctx.lineWidth = 3;
  ctx.beginPath();
  ctx.moveTo(x, y);
  ctx.lineTo(x2, y2);
  ctx.stroke();
  endX = x2;
  endY = y2;
}

function dir(x, y, x2, y2) {
  return toDeg(Math.atan((y - y2) / (x - x2))) + (180 * (x - x2 > 0))

}

function arm(x, y, length, dir, style) {
  dir -= 0;
  line(x, y, x + Math.cos(toRad(dir)) * length, y + Math.sin(toRad(dir)) * length, style);

}

function toRad(deg) {
  return deg * 3.14159 / 180;
}

function toDeg(rad) {
  return rad * (180 / 3.14159);
}

function render() {
  ctx.clearRect(0, 0, 1000, 1000);
  const mouseDir = dir(x, y, mx, my);
  const distance = Math.sqrt((x - mx) * (x - mx) + (y - my) * (y - my));
  /*let C = toDeg(Math.acos(((distance * distance) - ((arm1 * arm1) + (arm2 * arm2))) / (-2 * arm1 * arm2)));
  if (isNaN(C)) {
    C = 180;
  }
  let B = toDeg(Math.asin((Math.sin(toRad(C)) / distance) * arm2));
  if (C == 180) {
    B = 0;
  }*/
  let q2 = -1*toDeg(Math.acos(((x - mx) * (x - mx) + (y - my) * (y - my)-arm1*arm1-arm2*arm2)/(2*arm1*arm2)))
  let q1 = toDeg(Math.atan((y - my)/(x - mx)))+toDeg(Math.atan((arm2*Math.sin(toRad(q2)))/(arm1+arm2*Math.cos(toRad(q2)))))
  if (mx<x) {
  q1+=180;
  }
  if (distance > arm1+arm2) {
  	q1 =  mouseDir;
  	q2 =  0;
  }
  const servo1Target =  q1;
  const servo2Target = ( q1) -q2;
  servo1 = servo1Target;
  servo2 = servo2Target;
  arm(x, y, arm1, servo1, "red");
  arm(endX, endY, arm2, servo2, "blue")
}
setInterval(render, 10);