johnny-five/wip/ik-calculation.js

The JavaScript Arduino Programming Framework.

var abs = Math.abs;
var atan = Math.atan;
var atan2 = Math.atan2;
var acos = Math.acos;
var sqrt = Math.sqrt;
var pow = Math.pow;
var cos = Math.cos;
var sin = Math.sin;
var pi = Math.PI;

var canvas = document.getElementById("render");
var context = canvas.getContext("2d");
var presets = {
  lengths: {
    femur: 55.86,
    tibia: 123.22,
    coxa: 32
  },
  positions: {
    x: 0,
    y: 0,
    z: 0
  },
  angles: {
    lift: 90,
    knee: 90,
    coxa: 90
  }
};

var elements = {};

var lengths = {
  coxa: 32 * 2,
  femur: 55.86 * 2,
  tibia: 123.22 * 2
};
// var angles = {
//   lift: 90,
//   knee: 90,
//   coxa: 90
// };
var angles = {
  coxa: 0,
  femur: 90,
  tibia: 0
};
var positions = {
  x: 0,
  y: 0,
  z: 0
};
var colors = {
  coxa: "#ff0000",
  femur: "#00ff00",
  tibia: "#0000ff"
};

Object.keys(presets).forEach(function(group) {
  elements[group] = [].slice.call(
    document.querySelectorAll("[data-group='" + group + "']")
  );

  elements[group].forEach(function(element) {
    element.value = presets[group][element.id];
    elements[group][element.id] = element;
  });
});


function draw() {
  canvas.width = canvas.width;

  var sx = 20;
  var sy = 20;

  // context.lineTo(cos(angles[part]), sin(angles[part]));

  ["coxa", "femur", "tibia"].forEach(function(part) {
    var angle = angles[part];
    var length = lengths[part];

    var end = {
      x: sx + cos(angle) * length,
      y: sy + sin(angle) * length
    };

    context.lineWidth = 5;
    context.beginPath();
    context.moveTo(sx, sy);

    // console.log( part, [sx, sy], [end.x, end.y] );
    // console.log( "draw to: ", end.x, end.y );

    context.strokeStyle = colors[part];
    context.lineTo(end.x, end.y);


    context.closePath();

    context.stroke();

    // sx += , sy
    sx = end.x;
    sy = end.y;
  });
}

draw();

function _acos(number) {
  var result;

  if (abs(number) !== 1) {
    result = (pi / 2) - atan(number / sqrt(1 - number * number));
  } else {
    result = number === -1 ? pi : 0;
  }

  return Number.isNaN(result) ? 0 : result;
}


function handler(event) {

  console.log("message");
  // console.log( this.valueAsNumber );
  if (typeof event.target.id === 'undefined') {
    return;
  }


  positions[event.target.id] = this.valueAsNumber;

  console.log(positions);

  var x = positions.x;
  var y = positions.y;
  var z = positions.z;

  console.log(typeof x);
  var lcoxa = lengths.coxa;
  var lfemur = lengths.femur;
  var ltibia = lengths.tibia;

  var sx = pow(x, 2);
  var sy = pow(y, 2);
  var sz = pow(z, 2);

  var sltibia = pow(ltibia, 2);
  var slfemur = pow(lfemur, 2);

  var sqrtxz = sqrt(sx + sz);
  var ssanscoxa = pow(sqrtxz - lcoxa, 2);
  var ssanscoxaSy = sqrt(ssanscoxa + sy);


  angles.coxa = atan(z / x) * 180 / pi;

  angles.femur = (
    (
      atan((sqrtxz - lcoxa) / y) +
      _acos(
        (sltibia - slfemur - pow(ssanscoxaSy, 2)) /
        (-2 * lfemur * ssanscoxaSy)
      )
    ) * 180 / pi
  ) - 90;

  angles.tibia = (
    _acos(
      pow(ssanscoxaSy, 2) - sltibia - slfemur
    ) / (-2 * lfemur * ltibia)
  ) * 180 / pi;

  // console.log( "angles", angles );

  draw();
}

Object.keys(elements.positions).forEach(function(element) {
  elements.positions[element].addEventListener("input", handler, false);
  elements.positions[element].addEventListener("keydown", handler, false);
});