Вращательное ограничение FABRIK при обратном расчете

Question

Я работаю над созданием библиотеки для обратной кинематики в Javascript для моего будущего проекта. В настоящее время я реализовал обратную кинематику и имею вращательные ограничения, которые применяются при выполнении прямого вычисления, основанного на локальном вращении предыдущего соединения. Я также добавил ограничения длины, которые позволяют длине шва изменяться в некоторых пределах. Это прекрасно работает для большинства случаев использования, если только я не установил ограничение на что-то очень маленькое или не позволил суставу вращаться вообще.

Вот JS Fiddle: https://jsfiddle.net/jrj2211/413yuabt/4/

class Joint {
  constructor(length) {
    this.position = new THREE.Vector2(0, 0);
    this.effectorPosition = new THREE.Vector2(0, 0);
    this.next = null;
    this.prev = null;
    this.length = length;
    this.angleConstraints = null;
    this.lengthLimits = null;
    this.direction = new THREE.Vector2(1, 0);
    this.update();
  }

  appendJoint(joint) {
    var last = this.getEndJoint();
    last.next = joint;
    joint.prev = last;
    joint.update();
    return joint;
  }

  update() {
    if (this.prev) {
      this.position = this.prev.effectorPosition;
    }
    this.localPosition = this.direction.clone().multiplyScalar(this.length);

    this.effectorPosition = this.position.clone();
    console.log(this.localPosition, this.effectorPosition);
    this.effectorPosition.add(this.localPosition);
  }

  setRootPosition(position) {
    this.position = position;
    this.update();
  }

  backward() {
    // Make copies of the current start and end locations
    var p1 = this.effectorPosition.clone();
    var p2 = this.position.clone();

    // Allow the length to shrink or expand if set
    if (this.lengthLimits) {
      var magnitude = p1.distanceTo(p2);
      this.length = THREE.Math.clamp(magnitude, this.lengthLimits[0], this.lengthLimits[1]);
    }

    // Get the new end position
    p2.sub(p1);
    p2.normalize();
    p2.multiplyScalar(this.length);

    // Set the new start position
    this.position = this.effectorPosition.clone();
    this.position.add(p2);

    // Check if there is a joint before this one
    if (this.prev) {
      // Update the direction for the previous join for use in angle constraints
      if (this.prev.fixed !== true) {
        this.prev.direction = p2.normalize();
      }

      this.prev.effectorPosition = this.position.clone();
      this.prev.backward();
    }
  }

  forward() {
    // Make copies of the current start and end locations
    var p1 = this.position.clone();
    var p2 = this.effectorPosition.clone();

    // Calculate the new start point based on angle and length
    p2.sub(p1);
    p2.normalize();
    p2.multiplyScalar(this.length);

    // Check within angular constraints
    var curAngle = p2.angle();
    var dirAngle = this.direction.angle();
    var localRotation = THREE.Math.radToDeg(curAngle - dirAngle);

    // Check that the angle is within its constraints
    if (this.angleConstraints != null && !this.isBetween(this.angleConstraints[0], this.angleConstraints[1], localRotation)) {
      // Not within angle constraints so find which angle its closest to
      if (localRotation < 0) {
        localRotation += 360;
      }
      var closest = this.closerConstraintAngle(this.angleConstraints[0], this.angleConstraints[1], localRotation) + THREE.Math.radToDeg(this.direction.angle());

      // Set the angle to the closest constraint angle
      this.effectorPosition = this.position.clone();
      this.effectorPosition.x += this.length * Math.cos(THREE.Math.degToRad(closest));
      this.effectorPosition.y += this.length * Math.sin(THREE.Math.degToRad(closest));

      // Update the position
      var p1 = new THREE.Vector2(this.position.x, this.position.y);
      var p2 = new THREE.Vector2(this.effectorPosition.x, this.effectorPosition.y);
      p2.sub(p1);
    } else {
      // No angle constraints or within bounds so update position
      this.effectorPosition.x = p2.x + p1.x;
      this.effectorPosition.y = p2.y + p1.y;
    }

    if (this.next) {
      // Set next position to this effector position
      this.next.position = this.effectorPosition.clone();
      // Update the next joints direction
      this.next.direction = p2.normalize();
      this.next.forward();
    }
  }

  solve() {
    var start = this.position.clone();
    var endJoint = this.getEndJoint();

    // Solve inverse kinematics
    endJoint.effectorPosition.x = this.target.x;
    endJoint.effectorPosition.y = this.target.y;
    endJoint.backward();

    // Reset the start position if its fixed and do forward kinematics
    if (this.fixed === true && this.prev == null) {
      this.position = start;
      this.forward();
    }
  }

  normalizeAngle(angle) {
    return (angle % 360 + 360) % 360;
  }

  isBetween(start, end, mid) {
    mid = Math.abs(mid);
    end = (end - start) < 0 ? end - start + 360 : end - start;
    mid = (mid - start) < 0 ? mid - start + 360 : mid - start;
    return (mid < end);
  }

  closerConstraintAngle(start, end, mid) {
    var end2 = (end - start) < 0 ? end - start + 360 : end - start;
    var mid2 = (mid - start) < 0 ? mid - start + 360 : mid - start;

    var startDistance = 360 - mid2;
    var endDistance = mid2 - end2;

    if (startDistance < endDistance) {
      return start;
    } else {
      return end;
    }
  }

  getEndJoint() {
    let end = this;
    while (end.next != null) {
      end = end.next;
    }
    return end;
  }

  setTarget(target) {
    this.target = target;

    this.solve();
  }

}

// Setup kinematics with just angular rotation constraints
var kinematics1 = new Joint(75);
kinematics1.fixed = true;
kinematics1.setRootPosition(new THREE.Vector2(200, 400));
kinematics1.angleConstraints = [315, 45];
var joint2 = kinematics1.appendJoint(new Joint(75));
var joint3 = kinematics1.appendJoint(new Joint(75));
joint3.angleConstraints = [0, 0];
kinematics1.setTarget(new THREE.Vector2(0, 0));

// Setup kinematics with just angular rotation constraints
var kinematics2 = new Joint(75);
kinematics2.fixed = true;
kinematics2.setRootPosition(new THREE.Vector2(800, 400));
kinematics2.angleConstraints = [315, 45];
kinematics2.direction = new THREE.Vector2(-1, 0);
var joint2 = kinematics2.appendJoint(new Joint(75));
joint2.angleConstraints = [315, 45];
var joint3 = kinematics2.appendJoint(new Joint(75));
joint3.angleConstraints = [315, 45];
kinematics2.setTarget(new THREE.Vector2(0, 0));

// Create Canvas
var canvas = document.getElementById("canvas");
var ctx = canvas.getContext("2d");

// Set target on mouse click
var mouse = {};
canvas.addEventListener('mousedown', function(event) {
  mouse.x = event.clientX;
  mouse.y = event.clientY;

  var x = mouse.x - canvas.offsetLeft + window.scrollX;
  var y = mouse.y - canvas.offsetTop + window.scrollY;

  kinematics1.setTarget(new THREE.Vector2(x, y));
  kinematics2.setTarget(new THREE.Vector2(x, y));
}, false);

function animate() {
  // clear canvas
  ctx.clearRect(0, 0, canvas.width, canvas.height);

  // Draw objects
  draw(kinematics1, true);
  draw(kinematics2, true);
  drawTarget();

  window.requestAnimationFrame(animate);
}

function resizeCanvas() {
  canvas.width = window.innerWidth || document.documentElement.clientWidth || document.body.clientWidth;
  canvas.height = window.innerHeight || document.documentElement.clientHeight || document.body.clientHeight;
}

function draw(kinematics, debug) {
  let cur = kinematics;
  while (cur != null) {

    // Draw Bone
    ctx.beginPath();
    ctx.moveTo(cur.position.x, cur.position.y);
    ctx.lineTo(cur.effectorPosition.x, cur.effectorPosition.y);
    ctx.closePath();
    ctx.lineWidth = 10;
    ctx.strokeStyle = "black";
    ctx.stroke();

    // Draw constraints
    if (cur.angleConstraints && debug) {
      var rotationAngle = cur.direction.angle();
      var startAngle = rotationAngle + THREE.Math.degToRad(cur.angleConstraints[0]);
      var endAngle = rotationAngle + THREE.Math.degToRad(cur.angleConstraints[1]);

      var startConstraintPos = cur.position.clone();
      startConstraintPos.x += 30 * Math.cos(startAngle);
      startConstraintPos.y += 30 * Math.sin(startAngle);

      var endConstraintPos = cur.position.clone();
      endConstraintPos.x += 30 * Math.cos(endAngle);
      endConstraintPos.y += 30 * Math.sin(endAngle);

      ctx.beginPath();
      ctx.moveTo(cur.position.x, cur.position.y);
      ctx.lineTo(startConstraintPos.x, startConstraintPos.y);
      ctx.closePath();
      ctx.strokeStyle = "red";
      ctx.lineWidth = 2;
      ctx.stroke();

      ctx.beginPath();
      ctx.moveTo(cur.position.x, cur.position.y);
      ctx.lineTo(endConstraintPos.x, endConstraintPos.y);
      ctx.closePath();
      ctx.strokeStyle = "blue";
      ctx.lineWidth = 2;
      ctx.stroke();
    }

    if (debug) {
      var dir = cur.direction.clone().multiplyScalar(30).add(cur.position);
      ctx.beginPath();
      ctx.moveTo(cur.position.x, cur.position.y);
      ctx.lineTo(dir.x, dir.y);
      ctx.closePath();
      ctx.strokeStyle = "purple";
      ctx.lineWidth = 2;
      ctx.stroke();
    }

    // Draw Joint
    ctx.beginPath();
    ctx.arc(cur.position.x, cur.position.y, 8, 0, 2 * Math.PI);
    if (cur.fixed === true) {
      ctx.fillStyle = "red";
    } else {
      ctx.fillStyle = "gray";
    }
    ctx.fill();
    ctx.closePath();


    cur = cur.next;
  }
}

function drawTarget() {
  if (kinematics1.target) {
    ctx.beginPath();
    ctx.moveTo(kinematics1.target.x - 10, kinematics1.target.y - 10);
    ctx.lineTo(kinematics1.target.x - 10, kinematics1.target.y + 10);
    ctx.lineTo(kinematics1.target.x + 10, kinematics1.target.y + 10);
    ctx.lineTo(kinematics1.target.x + 10, kinematics1.target.y - 10);
    ctx.closePath();
    ctx.fillStyle = "green";
    ctx.fill();
  }
}

window.addEventListener('resize', resizeCanvas);
resizeCanvas();
animate();

В этой демонстрации у меня работают две кинематические модели: у одной все углы ограничены до -45 и 45, а у другой шарнир с углом поворота 0 градусов. Модель с углом 45 градусов работает хорошо, но та, которая имеет 0 степеней свободы, если вне оси локального вращения предыдущих суставов, никогда не достигнет цели, даже если это возможно. См. Иллюстрацию ниже.

Я знаю, это потому, что в настоящее время я вычисляю только ограничение поворота в передней части алгоритма. При расчете в обратном направлении последний сустав касается цели, но затем форвард фиксирует его назад, чтобы оказаться в пределах ограничения (это можно увидеть на изображении «Что происходит» выше).

Я знаю, что мне, вероятно, нужно применить ограничение поворота при обратном расчете, но я не уверен, как это сделать. Я использую локальное вращение предыдущего сустава для ограничений вращения, поскольку я пытаюсь смоделировать сервоприводы, чьи ограничения были бы относительно предыдущего сустава.

Применение ограничения угла на прямом проходе имеет смысл, поскольку у вас уже есть направление предыдущего соединения. При движении назад этот угол изменится, когда вы обновите предыдущие позиции соединений, что может вывести его за пределы ограничений.

Есть какие-нибудь идеи о том, как реализовать проверку ограничения угла наклона назад? Я не обязательно ищу код, а просто теорию, как это сделать или псевдокод. Я посмотрел несколько разных библиотек на GitHub, но не смог отследить, как они выполняют свои ограничения вращения.

Примечание: Я знаю, что мне нужно выполнять итерацию, пока я не окажусь в пределах некоторого расстояния для каждого вычисления FABRIK, но я не делаю этого, поскольку в данный момент я могу просто нажимать одно и то же место несколько раз, чтобы имитировать каждое итерация. Однако повторение несколько раз не решает мою проблему.