Vehicle swept paths

/** Move a follower at distance L behind a leader that goes in a straight line from A to B: the exact tractrix. */
function tractrix(F, A, B, L) {
  const dx = B.x - A.x, dy = B.y - A.y, d = Math.hypot(dx, dy);
  if (d < 1e-15) return F;
  const wx = dx / d, wy = dy / d;
  // α: from the travel direction w to the line leader → follower, measured as the follower trails behind (α = 0 straight behind)
  const bx = F.x - A.x, by = F.y - A.y, bl = Math.hypot(bx, by) || 1;
  const cosA = -(bx * wx + by * wy) / bl, sinA = (bx * wy - by * wx) / bl;   // behind = −w
  let a = Math.atan2(sinA, cosA);
  const k = Math.exp(-d / L);
  if (Math.abs(Math.abs(a) - Math.PI) < 1e-12) a = Math.sign(a || 1) * (Math.PI - 1e-9);   // exactly ahead: unstable point
  const a1 = 2 * Math.atan(Math.tan(a / 2) * k);
  // follower = B + L·(rotation of −w by a1)
  const c = Math.cos(a1), s = Math.sin(a1);
  return { x: B.x + L * (-wx * c + wy * s), y: B.y + L * (-wy * c - wx * s) };
}

/** steady state: the front axle on a circle of radius R; each rear axle's radius about the same centre. */
export function steadyRadii(vehicle, R) {
  const U = unitDims(vehicle), rear = [], hitch = [];
  let lead = R;   // radius of the point that pulls the unit
  for (let i = 0; i < U.length; i++) {
    const r = Math.sqrt(Math.max(0, lead * lead - U[i].L * U[i].L));
    rear.push(r);
    if (i + 1 < U.length) { const c = U[i + 1].hitch; const h = Math.sqrt(r * r + c * c); hitch.push(h); lead = h; }
  }
  return { rear, hitch };
}

const ds = step > 0 ? step : Math.max(1e-3, Math.min(v.wheelbase, ...v.trailers.map((tr) => tr.wheelbase)) / 40);
const walk = [{ ...pointOnSeg(segs[0].a, segs[0].b, segs[0].a.b, 0), s: 0 }];
let s = 0;
for (const q of segs) {
  const k = Math.max(1, Math.ceil(q.L / ds));
  for (let j = 1; j <= k; j++) walk.push({ ...pointOnSeg(q.a, q.b, q.a.b, j / k), s: s + (q.L * j) / k });
  s += q.L;
}

const prev0 = { ...units[0] };
if (mode === 'forward') {
  // the front axle moves A → B; the tractor's rear axle follows it; each hitch drags the next trailer
  const r0 = tractrix(units[0], A, B, U[0].L);
  const th0 = Math.atan2(B.y - r0.y, B.x - r0.x);
  let hOld = null, hNew = null;
  const before = units.map((u) => ({ ...u }));
  units[0] = { ...r0, th: th0 };
  for (let i = 1; i < n; i++) {
    const c = U[i].hitch, p = before[i - 1], q = units[i - 1];
    hOld = { x: p.x - c * Math.cos(p.th), y: p.y - c * Math.sin(p.th) };
    hNew = { x: q.x - c * Math.cos(q.th), y: q.y - c * Math.sin(q.th) };
    const r = tractrix(before[i], hOld, hNew, U[i].L);
    units[i] = { ...r, th: Math.atan2(hNew.y - r.y, hNew.x - r.x) };
  }
} else {
  // reversing: the last axle moves A → B (the unit faces backwards); each unit ahead hangs from the hitch it pushes
  const before = units.map((u) => ({ ...u }));
  units[n - 1] = { x: B.x, y: B.y, th: B.dir + Math.PI };
  for (let i = n - 1; i > 0; i--) {
    const L = U[i].L, c = U[i].hitch, p = before[i], q = units[i];
    const hOld = { x: p.x + L * Math.cos(p.th), y: p.y + L * Math.sin(p.th) }, hNew = { x: q.x + L * Math.cos(q.th), y: q.y + L * Math.sin(q.th) };
    let r, th;
    if (c < -1e-6 && !said.hitch) { problems.push({ kind: 'hitch', unit: i, s: B.s, value: c, limit: 0 }); said.hitch = true; }
    if (c < 1e-6) {   // a hitch on or ahead of the axle: reversing, the unit turns with the hitch's path
      r = hNew;
      th = Math.hypot(hNew.x - hOld.x, hNew.y - hOld.y) > 1e-12 ? Math.atan2(hOld.y - hNew.y, hOld.x - hNew.x) : before[i - 1].th;
    } else {
      // the unit's rear axle sits c ahead of the hitch (c > 0: the hitch is behind the axle): a tractrix led by the hitch
      r = tractrix(before[i - 1], hOld, hNew, c);
      th = Math.atan2(r.y - hNew.y, r.x - hNew.x);
    }
    units[i - 1] = { ...r, th };
  }
}

  const q = units[0], dl = Math.hypot(q.x - prev0.x, q.y - prev0.y), along = (q.x - prev0.x) * Math.cos(q.th) + (q.y - prev0.y) * Math.sin(q.th);
  if (dl > 1e-12) steer = Math.atan((U[0].L * angDiff(q.th, prev0.th)) / (along < 0 ? -dl : dl));
}
const sd = Math.abs(steer) / D2R;
if (sd > maxSteer) maxSteer = sd;
if (sd > v.maxSteer + 1e-9 && !said.steer) { problems.push({ kind: 'steer', s: B.s, value: sd, limit: v.maxSteer }); said.steer = true; }
rec(B.s, steer);
const fr = frames[frames.length - 1];
fr.art.forEach((a, i) => {
  const x = Math.abs(a);
  if (x > maxArt[i]) maxArt[i] = x;
  if (x > U[i + 1].maxArt + 1e-9 && !said.art[i]) { problems.push({ kind: 'articulation', unit: i + 1, s: B.s, value: x, limit: U[i + 1].maxArt }); said.art[i] = true; }
});

/** The four corners of unit i's body at a frame (anticlockwise from rear right). */
export function bodyOutline(v, frame, i) {
  const u = unitDims(v)[i], q = frame.units[i], c = Math.cos(q.th), s = Math.sin(q.th);
  const back = -u.rear, fwd = i === 0 ? u.L + u.front : u.L + u.front;   // a trailer's front = ahead of its hitch, which is L ahead of its axle
  return [[back, -u.width / 2], [fwd, -u.width / 2], [fwd, u.width / 2], [back, u.width / 2]].map(([a, b]) => ({ x: q.x + a * c - b * s, y: q.y + a * s + b * c }));
}

/** The area the bodies sweep: every frame's body rectangles filled into a grid of `cell`, the filled cells' outline. */
export function envelope(sim, v, { cell = null, maxCells = 4e6 } = {}) {
  if (!sim.frames.length) return { loops: [], area: 0, cell: 0, cells: 0 };
  const n = 1 + v.trailers.length;
  const polys = [];
  for (const f of sim.frames) for (let i = 0; i < n; i++) polys.push(bodyOutline(v, f, i));
  let minX = Infinity, minY = Infinity, maxX = -Infinity, maxY = -Infinity;
  for (const P of polys) for (const p of P) { minX = Math.min(minX, p.x); minY = Math.min(minY, p.y); maxX = Math.max(maxX, p.x); maxY = Math.max(maxY, p.y); }
  let h = cell > 0 ? cell : Math.min(v.width, ...v.trailers.map((tr) => tr.width)) / 40;
  while (((maxX - minX) / h + 3) * ((maxY - minY) / h + 3) > maxCells) h *= 1.5;
  const W = Math.ceil((maxX - minX) / h) + 3, H = Math.ceil((maxY - minY) / h) + 3, x0 = minX - h, y0 = minY - h;
  const g = new Uint8Array(W * H);
  // a cell is in when its centre is inside a rectangle; consecutive frames overlap, so the sweep has no gaps
  for (const P of polys) {
    let a = Infinity, b = -Infinity;
    for (const p of P) { a = Math.min(a, p.y); b = Math.max(b, p.y); }
    const j0 = Math.max(0, Math.ceil((a - y0) / h - 0.5)), j1 = Math.min(H - 1, Math.floor((b - y0) / h - 0.5));
    for (let j = j0; j <= j1; j++) {
      const yc = y0 + (j + 0.5) * h, xs = [];
      for (let k = 0; k < 4; k++) {
        const p = P[k], q = P[(k + 1) % 4];
        if ((p.y <= yc) !== (q.y <= yc)) xs.push(p.x + ((yc - p.y) * (q.x - p.x)) / (q.y - p.y));
      }
      if (xs.length < 2) continue;
      xs.sort((m, o) => m - o);
      const i0 = Math.max(0, Math.ceil((xs[0] - x0) / h - 0.5)), i1 = Math.min(W - 1, Math.floor((xs[xs.length - 1] - x0) / h - 0.5));
      for (let i = i0; i <= i1; i++) g[j * W + i] = 1;
    }
  }
  let cells = 0;
  for (let i = 0; i < g.length; i++) cells += g[i];
  // the outline: every cell edge between in and out, joined into loops (corners kept: a grid polygon), then thinned
  const loops = traceLoops(g, W, H).map((L) => simplify(L.map(([i, j]) => ({ x: x0 + i * h, y: y0 + j * h })), h * 0.5));
  return { loops, area: cells * h * h, cell: h, cells };
}

/** An alignment's lines and arcs as a path with bulges (+ = left). */
export function alignmentPath(al) {
  if (!al?.elements?.length) return [];
  const out = [];
  for (const e of al.pieces()) {   // 0.7 D7.35: a spiral comes as short straight pieces
    out.push(e.type === 'arc' ? { x: e.p0.x, y: e.p0.y, b: (e.ccw ? 1 : -1) * Math.tan(e.delta / 4) } : { x: e.p0.x, y: e.p0.y });
  }
  const e = al.pieces()[al.pieces().length - 1];
  out.push({ x: e.p1.x, y: e.p1.y });
  return out;
}