walkersim/src/kinematics.js

import { LENGTHS, POINTS } from './geometry.js';
import {
  add,
  circleCircleIntersection,
  dist,
  normalizeAngleRad,
  scale,
  sub,
  vec,
} from './math.js';

const EPS = 1e-7;

function chooseLowerPoint(i) {
  if (!i) return null;
  return i.p1.y > i.p2.y ? i.p1 : i.p2;
}

function pointAlong(a, b, length) {
  const ab = sub(b, a);
  const d = dist(a, b);
  if (d < 1e-9) return vec(a.x, a.y);
  return add(a, scale(ab, length / d));
}

function scaleFromBodyCenter(p, center, bodyScale) {
  return add(center, scale(sub(p, center), bodyScale));
}

function resolveGeometry(scales = null) {
  const bodyScale = Number.isFinite(scales?.body) ? scales.body : 1;
  const bodyCenter = scale(add(POINTS.A, POINTS.B), 0.5);

  const A = scaleFromBodyCenter(POINTS.A, bodyCenter, bodyScale);
  const B = scaleFromBodyCenter(POINTS.B, bodyCenter, bodyScale);
  const O = scaleFromBodyCenter(POINTS.O, bodyCenter, bodyScale);
  const D0 = scaleFromBodyCenter(POINTS.D0, bodyCenter, bodyScale);
  const E0 = scaleFromBodyCenter(POINTS.E0, bodyCenter, bodyScale);

  return {
    A,
    B,
    O,
    D0,
    E0,
    lengths: resolveLengths(scales),
  };
}

function resolveLengths(scales = null) {
  const crankScale = Number.isFinite(scales?.crank) ? scales.crank : 1;
  const legScale = Number.isFinite(scales?.leg) ? scales.leg : 1;
  const tendonScale = Number.isFinite(scales?.tendon) ? scales.tendon : 1;

  const rOC = LENGTHS.rOC * crankScale;
  const rAD = LENGTHS.rAD * tendonScale;
  const rCD = LENGTHS.rCD;
  const rCF = LENGTHS.rCF * legScale;
  const rBE = LENGTHS.rBE * tendonScale;
  const rCE = LENGTHS.rCE;
  const rCG = LENGTHS.rCG * legScale;

  return {
    rOC,
    rAD,
    rCD,
    rCF,
    rBE,
    rCE,
    rCG,
    kL: rCF / rCD,
    kR: rCG / rCE,
  };
}

function solveOneSide(theta, geom, prevPose = null) {
  const { lengths } = geom;
  const C = vec(
    geom.O.x + lengths.rOC * Math.cos(theta),
    geom.O.y + lengths.rOC * Math.sin(theta),
  );

  const iD = circleCircleIntersection(geom.A, lengths.rAD, C, lengths.rCD);
  const iE = circleCircleIntersection(geom.B, lengths.rBE, C, lengths.rCE);

  const D = chooseLowerPoint(iD);
  const E = chooseLowerPoint(iE);

  if (!D || !E) {
    const fallbackD = pointAlong(geom.A, geom.D0, lengths.rAD);
    const fallbackE = pointAlong(geom.B, geom.E0, lengths.rBE);
    const fallbackF = add(C, scale(sub(fallbackD, C), lengths.kL));
    const fallbackG = add(C, scale(sub(fallbackE, C), lengths.kR));

    return {
      A: geom.A,
      B: geom.B,
      O: geom.O,
      C,
      D: fallbackD,
      E: fallbackE,
      F: fallbackF,
      G: fallbackG,
      valid: false,
      fallbackUsed: true,
      errD: NaN,
      errE: NaN,
    };
  }

  const F = add(C, scale(sub(D, C), lengths.kL));
  const G = add(C, scale(sub(E, C), lengths.kR));

  const errD = Math.abs(dist(D, geom.A) - lengths.rAD) + Math.abs(dist(D, C) - lengths.rCD);
  const errE = Math.abs(dist(E, geom.B) - lengths.rBE) + Math.abs(dist(E, C) - lengths.rCE);

  return {
    A: geom.A,
    B: geom.B,
    O: geom.O,
    C,
    D,
    E,
    F,
    G,
    valid: errD + errE < EPS * 20,
    fallbackUsed: false,
    errD,
    errE,
  };
}

export function solveWalker(theta, prev = null, scales = null) {
  const tNear = normalizeAngleRad(theta);
  const tFar = normalizeAngleRad(theta + Math.PI);
  const geom = resolveGeometry(scales);

  const near = solveOneSide(tNear, geom, prev?.near ?? null);
  const far = solveOneSide(tFar, geom, prev?.far ?? null);

  return {
    theta: tNear,
    thetaFar: tFar,
    near,
    far,
  };
}