@driftengine/physics 3.61.0

package/src/cylinder.ts

@@ -0,0 +1,340 @@
+import type { ShapePose } from './manifold.ts';
+import type { ConvexShape } from './shape.ts';
+
+/**
+ * A cylinder resolved into world space, and the three questions contact generation asks of it.
+ *
+ * **This module is the whole of what `sideRadius` means geometrically**, kept apart from
+ * `cylinderContact.ts` so the arithmetic can be read and tested without a manifold in the way. A
+ * cylinder is the Minkowski sum of a segment and a disc perpendicular to it: its support along a
+ * direction `d` is the segment's support plus `radius` times the part of `d` that is perpendicular
+ * to the axis, which is one square root and no trigonometry.
+ *
+ * The three questions:
+ *
+ * - **How far does it reach along `d`** — `cylinderSupport`, which is what a separating-axis test
+ *   needs and what makes the axis list exact for a curved surface it never enumerates.
+ * - **What is touching along `d`** — `cylinderFeature`, which answers with a cap disc, a side
+ *   segment or a single rim point, because those are the only three a cylinder has.
+ * - **Where is the nearest surface point to `p`** — `closestOnCylinder`, which is exact in all
+ *   four regions and is how a sphere, a capsule or a cloth particle meets one.
+ *
+ * **No `Math.cos` anywhere**, which is not a style choice: `scripts/determinism.test.mjs` refuses
+ * every function ECMAScript declines to specify precisely, and a cap disc generated by trigonometry
+ * per tick would be a different set of points on two engines. The eight directions come from a
+ * literal table instead, and everything else is arithmetic and `Math.sqrt`.
+ */
+
+/** A cylinder in world space: centre, unit axis, half length, radius, and a spin-tracking basis. */
+export const CYLINDER_FLOATS = 11;
+
+/** How many points a cap disc contributes. Eight holds a disc flat without reducing to a square. */
+export const CAP_POINTS = 8;
+
+/**
+ * The eight unit directions of a cap disc, as literals.
+ *
+ * `Math.cos(k * Math.PI / 4)` is what these are, evaluated once by hand rather than per tick —
+ * `0.7071067811865476` is the double nearest √2/2. Written out because the determinism gate bans
+ * the call and because eight pairs are shorter than the loop that would derive them.
+ */
+const CIRCLE = new Float64Array([
+  1, 0, 0.7071067811865476, 0.7071067811865476, 0, 1, -0.7071067811865476, 0.7071067811865476, -1,
+  0, -0.7071067811865476, -0.7071067811865476, 0, -1, 0.7071067811865476, -0.7071067811865476,
+]);
+
+/**
+ * How parallel a direction must be to the axis before the whole cap is the touching feature, and
+ * how perpendicular before the whole side line is.
+ *
+ * Three degrees each way. **Generous on purpose**: a cylinder tilted by half a degree touches a
+ * plane at exactly one rim point, geometrically, and reporting that single point is what makes a
+ * resting wheel jitter. Two points get their separations measured against the reference plane
+ * individually, so the tilt is still resolved correctly — the constraint sees one contact deeper
+ * than the other, which is the truth — and the pair holds the shape steady meanwhile. Box-against-
+ * box clipping makes the same trade for the same reason.
+ */
+const CAP_DOT = 0.99862953;
+const SIDE_DOT = 0.05233596;
+
+/** Whether a shape is a cylinder, which is the one question every dispatch above asks. */
+export function isCylinder(shape: ConvexShape): boolean {
+  return shape.sideRadius > 0;
+}
+
+/**
+ * The pose a shape already in its own coordinates has.
+ *
+ * Exported because two callers work in a shape's local frame and would otherwise each declare
+ * their own identity quaternion — `readCylinder` costs nothing extra for one, since a rotation by
+ * the identity is the arithmetic it was going to do anyway.
+ */
+export const LOCAL_POSE: ShapePose = { x: 0, y: 0, z: 0, qx: 0, qy: 0, qz: 0, qw: 1 };
+
+/**
+ * Resolve a cylinder shape and its pose into `out`: centre, axis, half length, radius, basis.
+ *
+ * **The basis is the part worth explaining.** A cap disc's eight points have to be *the same eight
+ * points* from one tick to the next or their accumulated impulses are handed to the wrong contact,
+ * and a basis derived from the world axis alone stands still while the body spins about it. So it
+ * comes from the body's own local x, rotated by the pose and orthogonalised against the axis: it
+ * turns with the body, which is what a feature id has to do.
+ *
+ * A shape whose axis *is* local x — a cylinder baked into world space along that direction by a
+ * collider — falls back to local z, which cannot also be parallel to it.
+ */
+export function readCylinder(shape: ConvexShape, pose: ShapePose, out: Float64Array): void {
+  rotate(pose, shape.vertices[0] ?? 0, shape.vertices[1] ?? 0, shape.vertices[2] ?? 0, END, 0);
+  rotate(pose, shape.vertices[3] ?? 0, shape.vertices[4] ?? 0, shape.vertices[5] ?? 0, END, 3);
+  out[0] = (END[0] + END[3]) / 2 + pose.x;
+  out[1] = (END[1] + END[4]) / 2 + pose.y;
+  out[2] = (END[2] + END[5]) / 2 + pose.z;
+  const dx = END[3] - END[0];
+  const dy = END[4] - END[1];
+  const dz = END[5] - END[2];
+  const length = Math.sqrt(dx * dx + dy * dy + dz * dz);
+  const inv = length > 1e-9 ? 1 / length : 0;
+  out[3] = length > 1e-9 ? dx * inv : 0;
+  out[4] = length > 1e-9 ? dy * inv : 1;
+  out[5] = length > 1e-9 ? dz * inv : 0;
+  out[6] = length / 2;
+  out[7] = shape.sideRadius;
+
+  rotate(pose, 1, 0, 0, END, 6);
+  let bx = END[6];
+  let by = END[7];
+  let bz = END[8];
+  let along = bx * out[3] + by * out[4] + bz * out[5];
+  if (Math.abs(along) > 0.9) {
+    rotate(pose, 0, 0, 1, END, 6);
+    bx = END[6];
+    by = END[7];
+    bz = END[8];
+    along = bx * out[3] + by * out[4] + bz * out[5];
+  }
+  bx -= out[3] * along;
+  by -= out[4] * along;
+  bz -= out[5] * along;
+  const blen = Math.sqrt(bx * bx + by * by + bz * bz) || 1;
+  out[8] = bx / blen;
+  out[9] = by / blen;
+  out[10] = bz / blen;
+}
+
+/** Scratch for the two endpoints and the candidate basis, so `readCylinder` allocates nothing. */
+const END = new Float64Array(9);
+
+/** How far the cylinder reaches along a unit direction: the highest `p · n` over its surface. */
+export function cylinderSupport(cyl: Float64Array, nx: number, ny: number, nz: number): number {
+  const along = (cyl[3] ?? 0) * nx + (cyl[4] ?? 0) * ny + (cyl[5] ?? 0) * nz;
+  const across = Math.sqrt(Math.max(0, 1 - along * along));
+  return (
+    (cyl[0] ?? 0) * nx +
+    (cyl[1] ?? 0) * ny +
+    (cyl[2] ?? 0) * nz +
+    (cyl[6] ?? 0) * Math.abs(along) +
+    (cyl[7] ?? 0) * across
+  );
+}
+
+/** The lowest `p · n` over its surface, which is the support in the opposite direction. */
+export function cylinderMin(cyl: Float64Array, nx: number, ny: number, nz: number): number {
+  const along = (cyl[3] ?? 0) * nx + (cyl[4] ?? 0) * ny + (cyl[5] ?? 0) * nz;
+  const across = Math.sqrt(Math.max(0, 1 - along * along));
+  return (
+    (cyl[0] ?? 0) * nx +
+    (cyl[1] ?? 0) * ny +
+    (cyl[2] ?? 0) * nz -
+    (cyl[6] ?? 0) * Math.abs(along) -
+    (cyl[7] ?? 0) * across
+  );
+}
+
+/**
+ * The feature touching along unit `n`, written xyz-packed into `out` with an index per point.
+ *
+ * Returns how many points were written, and **the count is the kind**: eight is a cap disc, two is
+ * the side line, one is a rim point. Nothing else can touch, which is why no separate tag is
+ * carried and why the caller reads the count instead.
+ *
+ * The indices are what a feature id is built from, and they are stable while the cylinder keeps its
+ * orientation: a cap point is its position around the basis, a side point is which end it is.
+ */
+export function cylinderFeature(
+  cyl: Float64Array,
+  nx: number,
+  ny: number,
+  nz: number,
+  out: Float64Array,
+  ids: Int32Array,
+): number {
+  const cx = cyl[0] ?? 0;
+  const cy = cyl[1] ?? 0;
+  const cz = cyl[2] ?? 0;
+  const ax = cyl[3] ?? 0;
+  const ay = cyl[4] ?? 0;
+  const az = cyl[5] ?? 0;
+  const h = cyl[6] ?? 0;
+  const r = cyl[7] ?? 0;
+  const along = ax * nx + ay * ny + az * nz;
+
+  if (Math.abs(along) >= CAP_DOT) {
+    const side = along >= 0 ? 1 : -1;
+    const ux = cyl[8] ?? 0;
+    const uy = cyl[9] ?? 0;
+    const uz = cyl[10] ?? 0;
+    // The third basis vector, so the eight directions sweep the cap plane rather than a line.
+    const vx = ay * uz - az * uy;
+    const vy = az * ux - ax * uz;
+    const vz = ax * uy - ay * ux;
+    const ox = cx + ax * h * side;
+    const oy = cy + ay * h * side;
+    const oz = cz + az * h * side;
+    for (let i = 0; i < CAP_POINTS; i++) {
+      const c = CIRCLE[i * 2] ?? 0;
+      const s = CIRCLE[i * 2 + 1] ?? 0;
+      out[i * 3] = ox + (ux * c + vx * s) * r;
+      out[i * 3 + 1] = oy + (uy * c + vy * s) * r;
+      out[i * 3 + 2] = oz + (uz * c + vz * s) * r;
+      ids[i] = (side > 0 ? 0x10 : 0x20) | i;
+    }
+    return CAP_POINTS;
+  }
+
+  // The radial part of `n`, which is where the curved side reaches furthest.
+  let wx = nx - ax * along;
+  let wy = ny - ay * along;
+  let wz = nz - az * along;
+  const wlen = Math.sqrt(wx * wx + wy * wy + wz * wz) || 1;
+  wx /= wlen;
+  wy /= wlen;
+  wz /= wlen;
+
+  if (Math.abs(along) <= SIDE_DOT) {
+    out[0] = cx - ax * h + wx * r;
+    out[1] = cy - ay * h + wy * r;
+    out[2] = cz - az * h + wz * r;
+    out[3] = cx + ax * h + wx * r;
+    out[4] = cy + ay * h + wy * r;
+    out[5] = cz + az * h + wz * r;
+    ids[0] = 0x30;
+    ids[1] = 0x31;
+    return 2;
+  }
+
+  const side = along >= 0 ? 1 : -1;
+  out[0] = cx + ax * h * side + wx * r;
+  out[1] = cy + ay * h * side + wy * r;
+  out[2] = cz + az * h * side + wz * r;
+  ids[0] = side > 0 ? 0x40 : 0x41;
+  return 1;
+}
+
+/**
+ * The nearest point on a cylinder's surface to `p`, written as
+ * `[x, y, z, nx, ny, nz, signedDistance]` — the point, the outward direction there, and how far
+ * `p` sits from it. Negative distance means `p` is inside, and the direction is then the cheapest
+ * way out.
+ *
+ * Four regions, and each of them is exact: beside the side, past a cap, past a rim, or inside. The
+ * inside case picks the nearer of the side and the cap, which is what "cheapest way out" means and
+ * what keeps a deeply-buried particle from being pushed the long way through the solid.
+ */
+export function closestOnCylinder(
+  cyl: Float64Array,
+  px: number,
+  py: number,
+  pz: number,
+  out: Float64Array,
+): void {
+  const cx = cyl[0] ?? 0;
+  const cy = cyl[1] ?? 0;
+  const cz = cyl[2] ?? 0;
+  const ax = cyl[3] ?? 0;
+  const ay = cyl[4] ?? 0;
+  const az = cyl[5] ?? 0;
+  const h = cyl[6] ?? 0;
+  const r = cyl[7] ?? 0;
+
+  const dx = px - cx;
+  const dy = py - cy;
+  const dz = pz - cz;
+  const t = dx * ax + dy * ay + dz * az;
+  let wx = dx - ax * t;
+  let wy = dy - ay * t;
+  let wz = dz - az * t;
+  const rho = Math.sqrt(wx * wx + wy * wy + wz * wz);
+  if (rho > 1e-9) {
+    wx /= rho;
+    wy /= rho;
+    wz /= rho;
+  } else {
+    // On the axis: any radial direction is as good as another, and the basis is the stable one.
+    wx = cyl[8] ?? 1;
+    wy = cyl[9] ?? 0;
+    wz = cyl[10] ?? 0;
+  }
+
+  if (rho <= r && Math.abs(t) <= h) {
+    const toSide = r - rho;
+    const toCap = h - Math.abs(t);
+    if (toSide <= toCap) {
+      out[0] = cx + ax * t + wx * r;
+      out[1] = cy + ay * t + wy * r;
+      out[2] = cz + az * t + wz * r;
+      out[3] = wx;
+      out[4] = wy;
+      out[5] = wz;
+      out[6] = -toSide;
+      return;
+    }
+    const side = t >= 0 ? 1 : -1;
+    out[0] = cx + ax * h * side + wx * rho;
+    out[1] = cy + ay * h * side + wy * rho;
+    out[2] = cz + az * h * side + wz * rho;
+    out[3] = ax * side;
+    out[4] = ay * side;
+    out[5] = az * side;
+    out[6] = -toCap;
+    return;
+  }
+
+  const tc = t < -h ? -h : t > h ? h : t;
+  const rc = rho < r ? rho : r;
+  const qx = cx + ax * tc + wx * rc;
+  const qy = cy + ay * tc + wy * rc;
+  const qz = cz + az * tc + wz * rc;
+  const ex = px - qx;
+  const ey = py - qy;
+  const ez = pz - qz;
+  const d = Math.sqrt(ex * ex + ey * ey + ez * ez);
+  out[0] = qx;
+  out[1] = qy;
+  out[2] = qz;
+  if (d > 1e-9) {
+    out[3] = ex / d;
+    out[4] = ey / d;
+    out[5] = ez / d;
+  } else {
+    out[3] = wx;
+    out[4] = wy;
+    out[5] = wz;
+  }
+  out[6] = d;
+}
+
+function rotate(
+  p: ShapePose,
+  x: number,
+  y: number,
+  z: number,
+  out: Float64Array,
+  at: number,
+): void {
+  const tx = 2 * (p.qy * z - p.qz * y);
+  const ty = 2 * (p.qz * x - p.qx * z);
+  const tz = 2 * (p.qx * y - p.qy * x);
+  out[at] = x + p.qw * tx + (p.qy * tz - p.qz * ty);
+  out[at + 1] = y + p.qw * ty + (p.qz * tx - p.qx * tz);
+  out[at + 2] = z + p.qw * tz + (p.qx * ty - p.qy * tx);
+}