@driftengine/physics 4.8.0 → 4.8.2

package/dist/ragdoll.js

@@ -45,8 +45,32 @@ export function ragdollFromBones(world, parents, worldMatrices, options = {}) {
         return value === undefined || !Number.isFinite(value) ? fallback : value;
     };
     const bodyOf = new Int32Array(jointCount).fill(-1);
-    /** `conj(bodyRotation) · jointWorldRotation`, so a pose can be recovered from a body. */
+    /**
+     * `conj(bodyRotation) · jointWorldRotation` for the joint a bone **ends** at, so a joint with no
+     * bone of its own below it — a fingertip, a head's tip — can ride the bone it hangs off.
+     */
     const bodyToJoint = new Float32Array(jointCount * 4);
+    /**
+     * `conj(bodyRotation) · jointWorldRotation` for the joint a bone **starts** at, indexed like
+     * `bodyOf` — by the bone's own joint — and naming its parent's rotation.
+     *
+     * **This is the joint a skinned limb follows.** A forearm's vertices are weighted to the elbow,
+     * so the elbow's rotation is what points the forearm, and the body standing along the forearm is
+     * what carries it. Reading each joint off the bone that *ends* at it — the upper arm for the
+     * elbow — drew every limb one bone late: a hand 0.47 m from its body in a doll synced and never
+     * stepped, reported from a game. A bone is rigid in its start joint's frame, so the relation
+     * captured at rest holds for as long as the doll does.
+     */
+    const bodyToStart = new Float32Array(jointCount * 4);
+    /** The first bone starting at each joint, by that bone's own index, or −1 where none does. */
+    const startBone = new Int32Array(jointCount).fill(-1);
+    /**
+     * Each joint's translation in its parent's frame at rest, which is what a pose holds.
+     *
+     * **In the parent's frame, not the world's.** It was the world offset between the two, which is
+     * the same number only where the parent is unrotated at rest — true of every chain this module's
+     * tests were built from, and of no rig a game loads.
+     */
     const localRest = new Float32Array(jointCount * 3);
     const restRotation = new Float32Array(jointCount * 4);
     /**
@@ -73,9 +97,7 @@ export function ragdollFromBones(world, parents, worldMatrices, options = {}) {
         const tx = worldMatrices[j * 16 + 12] ?? 0;
         const ty = worldMatrices[j * 16 + 13] ?? 0;
         const tz = worldMatrices[j * 16 + 14] ?? 0;
-        localRest[j * 3] = tx - hx;
-        localRest[j * 3 + 1] = ty - hy;
-        localRest[j * 3 + 2] = tz - hz;
+        intoParentFrame(worldMatrices, p, tx - hx, ty - hy, tz - hz, localRest, j * 3);
         const dx = tx - hx;
         const dy = ty - hy;
         const dz = tz - hz;
@@ -101,8 +123,14 @@ export function ragdollFromBones(world, parents, worldMatrices, options = {}) {
         });
         bodyOf[j] = body;
         halfOf[j] = length / 2;
-        // What takes the body's rotation back to the joint's, captured while both are at rest.
+        // What takes the body's rotation back to each of its two joints', captured at rest. Parents
+        // come before children in a rig, so the start joint's rotation is already read.
         multiplyConjugate(TMP, restRotation, j * 4, bodyToJoint, j * 4);
+        if (p >= j)
+            matrixRotation(worldMatrices, p, restRotation, p * 4);
+        multiplyConjugate(TMP, restRotation, p * 4, bodyToStart, j * 4);
+        if ((startBone[p] ?? -1) < 0)
+            startBone[p] = j;
         boneCount++;
         if (firstBone < 0) {
             firstBone = j;
@@ -210,28 +238,52 @@ export function ragdollFromBones(world, parents, worldMatrices, options = {}) {
                 world.ignorePair(body, other);
         }
     }
+    /**
+     * One joint's world rotation, from the body that speaks for it: the first bone starting at the
+     * joint, or — for a joint with none below it, a fingertip — the bone it hangs off, which it then
+     * rides rigidly. False where neither exists and nothing can be said.
+     */
+    const recoverJoint = (w, j, out, at) => {
+        const start = startBone[j] ?? -1;
+        if (start >= 0) {
+            readBodyRotation(w, bodyOf[start] ?? 0, TMP);
+            multiplyInto(TMP, bodyToStart, start * 4, out, at);
+            return true;
+        }
+        const own = bodyOf[j] ?? -1;
+        if (own < 0)
+            return false;
+        readBodyRotation(w, own, TMP);
+        multiplyInto(TMP, bodyToJoint, j * 4, out, at);
+        return true;
+    };
     const root = firstBone;
     return {
         bodyOf,
         boneCount,
         writePose(out) {
+            /*
+             * Two passes, because a joint's local rotation needs its parent's world rotation and a rig
+             * is not obliged to list parents first. The first recovers every joint a body speaks for.
+             */
+            for (let j = 0; j < jointCount; j++)
+                KNOWN[j] = recoverJoint(world, j, WORLD_ROT, j * 4) ? 1 : 0;
             for (let j = 0; j < jointCount; j++) {
-                const body = bodyOf[j] ?? -1;
-                const p = parents[j] ?? -1;
-                if (body < 0) {
-                    // No bone: leave the joint as the pose already had it.
+                if (KNOWN[j] !== 1) {
+                    // No bone at either end: leave the joint as the pose already had it.
                     continue;
                 }
-                // The joint's world rotation, recovered from the body's.
-                readBodyRotation(world, body, TMP);
-                multiplyInto(TMP, bodyToJoint, j * 4, WORLD_ROT, j * 4);
-                const parentBody = p >= 0 ? (bodyOf[p] ?? -1) : -1;
-                if (parentBody >= 0) {
+                const p = parents[j] ?? -1;
+                if (p >= 0 && KNOWN[p] === 1) {
                     conjugateMultiply(WORLD_ROT, p * 4, WORLD_ROT, j * 4, out.rotation, j * 4);
                 }
                 else {
+                    /* A root, or a joint whose parent no bone speaks for: its rotation in the frame the
+                       caller places the character's node in, which `rootX` and its two siblings answer. */
                     copy4(WORLD_ROT, j * 4, out.rotation, j * 4);
                 }
+                if (p < 0)
+                    continue;
                 // Bones are rigid, so translation stays what the rig was built with.
                 out.translation[j * 3] = localRest[j * 3] ?? 0;
                 out.translation[j * 3 + 1] = localRest[j * 3 + 1] ?? 0;
@@ -245,27 +297,32 @@ export function ragdollFromBones(world, parents, worldMatrices, options = {}) {
             if (weight <= 0)
                 return;
             const k = weight > 1 ? 1 : weight;
-            for (let j = 0; j < jointCount; j++) {
-                const body = bodyOf[j] ?? -1;
+            for (let c = 0; c < jointCount; c++) {
+                const body = bodyOf[c] ?? -1;
                 if (body < 0)
                     continue;
-                const p = parents[j] ?? -1;
-                const parentBody = p >= 0 ? (bodyOf[p] ?? -1) : -1;
+                /*
+                 * **A bone is steered by the joint it starts at**, the one whose rotation points it — the
+                 * forearm by the elbow — which is the same joint `writePose` reads it back as. Steered by
+                 * the joint it ends at, a pose's elbow bend went into the upper arm.
+                 */
+                const j = parents[c] ?? -1;
+                if (j < 0)
+                    continue;
                 /*
                  * The target is the pose's local rotation composed onto the parent's *current* world
                  * rotation, so a driven limb follows the animation relative to a body that may itself have
                  * been knocked aside — which is what a partial ragdoll is.
                  */
-                if (parentBody >= 0) {
-                    readBodyRotation(w, parentBody, TMP);
-                    multiplyInto(TMP, bodyToJoint, p * 4, PARENT_ROT, 0);
+                const p = parents[j] ?? -1;
+                if (p >= 0 && recoverJoint(w, p, PARENT_ROT, 0)) {
                     multiplyRaw(PARENT_ROT, 0, pose.rotation, j * 4, TARGET_ROT, 0);
                 }
                 else {
                     copy4(pose.rotation, j * 4, TARGET_ROT, 0);
                 }
                 readBodyRotation(w, body, TMP);
-                multiplyInto(TMP, bodyToJoint, j * 4, CURRENT_ROT, 0);
+                multiplyInto(TMP, bodyToStart, c * 4, CURRENT_ROT, 0);
                 // Twice the vector part of target · conj(current) is the rotation that closes the gap.
                 conjugateMultiply(CURRENT_ROT, 0, TARGET_ROT, 0, ERROR_ROT, 0);
                 let ex = ERROR_ROT[0] ?? 0;
@@ -301,22 +358,23 @@ export function ragdollFromBones(world, parents, worldMatrices, options = {}) {
                 w.bodies.posY[body] = (hy + ty) / 2;
                 w.bodies.posZ[body] = (hz + tz) / 2;
                 /*
-                 * The rotation is taken from the *bone*, the same way the build took it: the capsule
-                 * stands along y, so the body is turned to put its y along the bone. Taking it from the
-                 * joint's world matrix instead would be wrong wherever a rig's bind orientation is not
-                 * the bone direction, which is most rigs.
+                 * **The rotation is the start joint's, carried through the relation captured at rest**,
+                 * which is the inverse of what `writePose` reads: `jointRotation · conj(bodyToStart)`.
+                 *
+                 * It was the shortest arc from +y onto the bone, which points the capsule correctly and
+                 * throws away the bone's twist about its own length — so a joint with two bones below it, a
+                 * wrist with a thumb beside the fingers, came back turned about the first and put the
+                 * second somewhere else. The joint's world matrix alone would be wrong too, wherever a
+                 * rig's bind orientation is not the bone direction, which is most rigs; the captured
+                 * relation is what absorbs that. **What would make it wrong** is a pose that stretches a
+                 * bone, which a ragdoll's rigid bones cannot follow either way.
                  */
-                const dx = tx - hx;
-                const dy = ty - hy;
-                const dz = tz - hz;
-                const length = Math.sqrt(dx * dx + dy * dy + dz * dz);
-                if (length > 0) {
-                    alignYTo(dx / length, dy / length, dz / length, TMP);
-                    w.bodies.rotX[body] = TMP[0] ?? 0;
-                    w.bodies.rotY[body] = TMP[1] ?? 0;
-                    w.bodies.rotZ[body] = TMP[2] ?? 0;
-                    w.bodies.rotW[body] = TMP[3] ?? 1;
-                }
+                matrixRotation(worldMatrices, p, START_ROT, 0);
+                multiplyByConjugate(START_ROT, bodyToStart, j * 4, TMP);
+                w.bodies.rotX[body] = TMP[0] ?? 0;
+                w.bodies.rotY[body] = TMP[1] ?? 0;
+                w.bodies.rotZ[body] = TMP[2] ?? 0;
+                w.bodies.rotW[body] = TMP[3] ?? 1;
                 /* At rest, which is the half that makes it a reaction rather than a continuation: a body
                    carrying the speed of a two-second fall is thrown across the character the instant it
                    is written into the pose. */
@@ -341,6 +399,9 @@ export function ragdollFromBones(world, parents, worldMatrices, options = {}) {
     };
 }
 const WORLD_ROT = new Float32Array(256 * 4);
+/** Whether `writePose` recovered each joint's rotation this call: 1 if a body speaks for it. */
+const KNOWN = new Uint8Array(256);
+const START_ROT = new Float32Array(4);
 const PARENT_ROT = new Float32Array(4);
 const TARGET_ROT = new Float32Array(4);
 const CURRENT_ROT = new Float32Array(4);
@@ -454,6 +515,51 @@ function alignYTo(dx, dy, dz, out) {
     out[2] = az / len;
     out[3] = w / len;
 }
+/**
+ * A world-space offset from joint `p`, expressed in `p`'s own frame: the inverse of the matrix's
+ * upper 3x3 applied to it, so a scaled joint is undone as well as a rotated one.
+ */
+function intoParentFrame(m, p, dx, dy, dz, out, at) {
+    const o = p * 16;
+    const a = m[o] ?? 1;
+    const b = m[o + 4] ?? 0;
+    const c = m[o + 8] ?? 0;
+    const d = m[o + 1] ?? 0;
+    const e = m[o + 5] ?? 1;
+    const f = m[o + 9] ?? 0;
+    const g = m[o + 2] ?? 0;
+    const h = m[o + 6] ?? 0;
+    const i = m[o + 10] ?? 1;
+    const ei = e * i - f * h;
+    const fg = f * g - d * i;
+    const dh = d * h - e * g;
+    const det = a * ei + b * fg + c * dh;
+    if (Math.abs(det) < 1e-12) {
+        out[at] = dx;
+        out[at + 1] = dy;
+        out[at + 2] = dz;
+        return;
+    }
+    const inv = 1 / det;
+    out[at] = (ei * dx + (c * h - b * i) * dy + (b * f - c * e) * dz) * inv;
+    out[at + 1] = (fg * dx + (a * i - c * g) * dy + (c * d - a * f) * dz) * inv;
+    out[at + 2] = (dh * dx + (b * g - a * h) * dy + (a * e - b * d) * dz) * inv;
+}
+/** `a · conj(b)` into a Float64 quaternion, for turning a joint's rotation back into a body's. */
+function multiplyByConjugate(a, b, bAt, out) {
+    const ax = a[0] ?? 0;
+    const ay = a[1] ?? 0;
+    const az = a[2] ?? 0;
+    const aw = a[3] ?? 1;
+    const bx = -(b[bAt] ?? 0);
+    const by = -(b[bAt + 1] ?? 0);
+    const bz = -(b[bAt + 2] ?? 0);
+    const bw = b[bAt + 3] ?? 1;
+    out[0] = aw * bx + ax * bw + ay * bz - az * by;
+    out[1] = aw * by - ax * bz + ay * bw + az * bx;
+    out[2] = aw * bz + ax * by - ay * bx + az * bw;
+    out[3] = aw * bw - ax * bx - ay * by - az * bz;
+}
 /** `conj(a) · b` into `out`. */
 function multiplyConjugate(a, b, bAt, out, at) {
     const ax = -a[0];

package/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@driftengine/physics",
-  "version": "4.8.0",
+  "version": "4.8.2",
   "description": "Collision shapes, spatial queries and swept kinematic collision",
   "license": "Apache-2.0",
   "type": "module",

package/src/controller.ts

@@ -170,6 +170,8 @@ const SLIDE_PASSES = 4;
  * own stop margin already leaves a tenth of a millimetre of clearance, which is what a skin was for.
  */
 const SNAP_EPS = 0.001;
+/** How far past a step's edge a step that landed on the edge carries the body's axis. */
+const STEP_OVER = 0.01;
 
 export class CharacterController {
   x = 0;
@@ -451,9 +453,10 @@ export class CharacterController {
      * the same clamp and air control — only the plane is given up, not the feel.
      */
     if (input.projectMove === false) {
-      this.velX = approach(this.velX, wantX, step);
-      this.velY = approach(this.velY, wantY, step);
-      this.velZ = approach(this.velZ, wantZ, step);
+      const t = approachFraction(wantX - this.velX, wantY - this.velY, wantZ - this.velZ, step);
+      this.velX = t === 1 ? wantX : this.velX + (wantX - this.velX) * t;
+      this.velY = t === 1 ? wantY : this.velY + (wantY - this.velY) * t;
+      this.velZ = t === 1 ? wantZ : this.velZ + (wantZ - this.velZ) * t;
     } else {
       const alongUp = this.speedAlongUp;
       let tangentX = this.velX - this.upX * alongUp;
@@ -463,9 +466,15 @@ export class CharacterController {
       const wantTangentX = wantX - this.upX * wantIntoUp;
       const wantTangentY = wantY - this.upY * wantIntoUp;
       const wantTangentZ = wantZ - this.upZ * wantIntoUp;
-      tangentX = approach(tangentX, wantTangentX, step);
-      tangentY = approach(tangentY, wantTangentY, step);
-      tangentZ = approach(tangentZ, wantTangentZ, step);
+      const t = approachFraction(
+        wantTangentX - tangentX,
+        wantTangentY - tangentY,
+        wantTangentZ - tangentZ,
+        step,
+      );
+      tangentX = t === 1 ? wantTangentX : tangentX + (wantTangentX - tangentX) * t;
+      tangentY = t === 1 ? wantTangentY : tangentY + (wantTangentY - tangentY) * t;
+      tangentZ = t === 1 ? wantTangentZ : tangentZ + (wantTangentZ - tangentZ) * t;
       this.velX = tangentX + this.upX * alongUp;
       this.velY = tangentY + this.upY * alongUp;
       this.velZ = tangentZ + this.upZ * alongUp;
@@ -603,26 +612,74 @@ export class CharacterController {
     this.z += liftZ;
     this.readPose();
     if (world.shapecast(this.shape, this.pose, flatX, flatY, flatZ, this.stepHit, this.filter)) {
-      this.x = startX;
-      this.y = startY;
-      this.z = startZ;
-      return false;
+      return this.stepBack(startX, startY, startZ);
     }
     this.x += flatX;
     this.y += flatY;
     this.z += flatZ;
     this.readPose();
-    if (world.shapecast(this.shape, this.pose, -liftX, -liftY, -liftZ, this.stepHit, this.filter)) {
-      const drop = this.stepHeight * this.stepHit.fraction - SNAP_EPS;
-      this.x -= this.upX * drop;
-      this.y -= this.upY * drop;
-      this.z -= this.upZ * drop;
-      return true;
+    if (
+      !world.shapecast(this.shape, this.pose, -liftX, -liftY, -liftZ, this.stepHit, this.filter)
+    ) {
+      // Nothing beneath the lifted position, so this was a gap rather than a step.
+      return this.stepBack(startX, startY, startZ);
     }
-    // Nothing beneath the lifted position, so this was a gap rather than a step.
-    this.x = startX;
-    this.y = startY;
-    this.z = startZ;
+    let landing =
+      this.stepHit.nx * this.upX + this.stepHit.ny * this.upY + this.stepHit.nz * this.upZ;
+    if (landing < this.slopeCos) {
+      /*
+       * **Landed on the lip, so go on over it.** The forward part of a step is what was left of
+       * the tick's move when the body met the step, and at a tick's length that can be millimetres:
+       * the foot ball then comes down on the step's edge, a contact far too steep to stand on, and
+       * the body was set there airborne, slid off, and never stepped again. Measured with a 0.32 m
+       * radius and a 0.35 m step height: a 0.25 m rise climbed and a 0.28 m one did not.
+       *
+       * The edge's normal points from the edge at the ball's centre, so its part across up is how
+       * far the axis stands short of the edge. Moving that far again, plus a centimetre, puts the
+       * ball over the step's top, where the drop meets a face. The body moves up to one radius
+       * further in that tick than it was going to, which is the price of a step taken in one tick.
+       */
+      const lipX = this.stepHit.nx - this.upX * landing;
+      const lipY = this.stepHit.ny - this.upY * landing;
+      const lipZ = this.stepHit.nz - this.upZ * landing;
+      const lip = Math.sqrt(lipX * lipX + lipY * lipY + lipZ * lipZ);
+      // An edge behind the move, or beside it, is not one this step was climbing.
+      if (lip < 1e-6 || lipX * flatX + lipY * flatY + lipZ * flatZ >= 0) {
+        return this.stepBack(startX, startY, startZ);
+      }
+      const reach = (this.radius * lip + STEP_OVER) / lip;
+      const overX = -lipX * reach;
+      const overY = -lipY * reach;
+      const overZ = -lipZ * reach;
+      this.readPose();
+      if (world.shapecast(this.shape, this.pose, overX, overY, overZ, this.stepHit, this.filter)) {
+        return this.stepBack(startX, startY, startZ);
+      }
+      this.x += overX;
+      this.y += overY;
+      this.z += overZ;
+      this.readPose();
+      if (
+        !world.shapecast(this.shape, this.pose, -liftX, -liftY, -liftZ, this.stepHit, this.filter)
+      ) {
+        return this.stepBack(startX, startY, startZ);
+      }
+      landing =
+        this.stepHit.nx * this.upX + this.stepHit.ny * this.upY + this.stepHit.nz * this.upZ;
+      if (landing < this.slopeCos) return this.stepBack(startX, startY, startZ);
+    }
+    const drop = this.stepHeight * this.stepHit.fraction - SNAP_EPS;
+    this.x -= this.upX * drop;
+    this.y -= this.upY * drop;
+    this.z -= this.upZ * drop;
+    return true;
+  }
+
+  /** Put the body back where a step began, and say the step was not taken. */
+  private stepBack(x: number, y: number, z: number): false {
+    this.x = x;
+    this.y = y;
+    this.z = z;
     return false;
   }
 
@@ -808,10 +865,18 @@ export class CharacterController {
   }
 }
 
-/** Move `from` toward `to` by at most `step`. */
-function approach(from: number, to: number, step: number): number {
-  const d = to - from;
-  if (d > step) return from + step;
-  if (d < -step) return from - step;
-  return to;
+/**
+ * How much of the way to close a gap of `(dx, dy, dz)` this tick: exactly 1 when the gap is no
+ * longer than `step`, which the caller reads as "land on the target", and `step` of its length
+ * otherwise.
+ *
+ * **Toward the wanted velocity as a vector, by at most `step` in length.** It was one world axis at
+ * a time, each by the whole step, so a diagonal gained and lost speed √2 times as fast as a straight
+ * run while the clamp on the wanted vector kept its top speed honest — the faster diagonal lived in
+ * the acceleration. What it gives up is that an axis no longer reaches its target on its own
+ * schedule: turning from one direction to another now curves rather than squaring the corner.
+ */
+function approachFraction(dx: number, dy: number, dz: number, step: number): number {
+  const gap = Math.sqrt(dx * dx + dy * dy + dz * dz);
+  return gap <= step ? 1 : step / gap;
 }

package/src/manifold.ts

@@ -207,6 +207,76 @@ function roundRound(a: ConvexShape, b: ConvexShape, margin: number, out: Manifol
 
 /* ---------- sphere or capsule against a polytope ---------- */
 
+/** Where along a capsule's segment the search settles: 0.618 to the 30th of 1.2 m is under a micron. */
+const SEGMENT_STEPS = 30;
+const INV_PHI = 0.6180339887498949;
+/** Each end's direction away from the polytope, and its separation, read by the segment search. */
+const END_DIR = new Float64Array(9);
+const END_SEP = new Float64Array(2);
+/** The manifold being filled by `roundPolytope`, kept here so adding a contact allocates nothing. */
+const ROUND = { count: 0, bestSep: Infinity, nx: 0, ny: 1, nz: 0, margin: 0, flipped: false };
+
+/** Add the contact `CLOSEST` and `END_DIR[slot]` describe, at `separation`, as `feature`. */
+function emitRound(out: Manifold, slot: number, separation: number, feature: number): void {
+  if (separation > ROUND.margin || ROUND.count >= MAX_CONTACTS) return;
+  // Normal points from the round shape toward the polytope, then flipped if A was the polytope.
+  const ux = END_DIR[slot * 3] ?? 0;
+  const uy = END_DIR[slot * 3 + 1] ?? 0;
+  const uz = END_DIR[slot * 3 + 2] ?? 0;
+  if (separation < ROUND.bestSep) {
+    ROUND.bestSep = separation;
+    ROUND.nx = ROUND.flipped ? ux : -ux;
+    ROUND.ny = ROUND.flipped ? uy : -uy;
+    ROUND.nz = ROUND.flipped ? uz : -uz;
+  }
+  const at = ROUND.count * 3;
+  out.points[at] = CLOSEST[0];
+  out.points[at + 1] = CLOSEST[1];
+  out.points[at + 2] = CLOSEST[2];
+  out.separations[ROUND.count] = separation;
+  out.featureIds[ROUND.count] = feature;
+  ROUND.count++;
+}
+
+/**
+ * The signed distance from a point to the polytope, with the direction away from it left in
+ * `END_DIR[slot]`: positive outside, the deepest face's plane distance inside.
+ */
+function signedDistance(
+  poly: ConvexShape,
+  polyWorld: Float32Array,
+  faces: number,
+  px: number,
+  py: number,
+  pz: number,
+  slot: number,
+): number {
+  closestOnPolytope(poly, polyWorld, planesB, faces, px, py, pz);
+  let ux = px - CLOSEST[0];
+  let uy = py - CLOSEST[1];
+  let uz = pz - CLOSEST[2];
+  let d = Math.sqrt(ux * ux + uy * uy + uz * uz);
+  if (CLOSEST[6] < 0) {
+    // Inside: the closest face's outward normal is the direction, and depth is its plane distance.
+    ux = CLOSEST[3];
+    uy = CLOSEST[4];
+    uz = CLOSEST[5];
+    d = CLOSEST[6];
+  } else if (d === 0) {
+    ux = CLOSEST[3];
+    uy = CLOSEST[4];
+    uz = CLOSEST[5];
+  } else {
+    ux /= d;
+    uy /= d;
+    uz /= d;
+  }
+  END_DIR[slot * 3] = ux;
+  END_DIR[slot * 3 + 1] = uy;
+  END_DIR[slot * 3 + 2] = uz;
+  return d;
+}
+
 function roundPolytope(
   round: ConvexShape,
   poly: ConvexShape,
@@ -222,63 +292,92 @@ function roundPolytope(
 
   const points = round.vertices.length / 3;
   const faces = faceCount(poly);
-  let count = 0;
-  let bestSep = Infinity;
-  let nx = 0;
-  let ny = 1;
-  let nz = 0;
-
-  for (let p = 0; p < points; p++) {
-    const px = roundWorld[p * 3] ?? 0;
-    const py = roundWorld[p * 3 + 1] ?? 0;
-    const pz = roundWorld[p * 3 + 2] ?? 0;
-    closestOnPolytope(poly, polyWorld, planesB, faces, px, py, pz);
-    let ux = px - CLOSEST[0];
-    let uy = py - CLOSEST[1];
-    let uz = pz - CLOSEST[2];
-    let d = Math.sqrt(ux * ux + uy * uy + uz * uz);
-    if (CLOSEST[6] < 0) {
-      // Inside: the closest face's outward normal is the direction, and depth is its plane distance.
-      ux = CLOSEST[3];
-      uy = CLOSEST[4];
-      uz = CLOSEST[5];
-      d = CLOSEST[6];
-    } else if (d === 0) {
-      ux = CLOSEST[3];
-      uy = CLOSEST[4];
-      uz = CLOSEST[5];
-    } else {
-      ux /= d;
-      uy /= d;
-      uz /= d;
-    }
-    const separation = d - round.radius - poly.radius;
-    if (separation > margin) continue;
-    if (count >= MAX_CONTACTS) break;
-    // Normal points from the round shape toward the polytope, then flipped if A was the polytope.
-    const sx = flipped ? ux : -ux;
-    const sy = flipped ? uy : -uy;
-    const sz = flipped ? uz : -uz;
-    if (separation < bestSep) {
-      bestSep = separation;
-      nx = sx;
-      ny = sy;
-      nz = sz;
-    }
-    const at = count * 3;
-    out.points[at] = CLOSEST[0];
-    out.points[at + 1] = CLOSEST[1];
-    out.points[at + 2] = CLOSEST[2];
-    out.separations[count] = separation;
+  const radii = round.radius + poly.radius;
+  ROUND.count = 0;
+  ROUND.bestSep = Infinity;
+  ROUND.nx = 0;
+  ROUND.ny = 1;
+  ROUND.nz = 0;
+  ROUND.margin = margin;
+  ROUND.flipped = flipped;
+
+  for (let p = 0; p < points && p < 2; p++) {
+    const separation =
+      signedDistance(
+        poly,
+        polyWorld,
+        faces,
+        roundWorld[p * 3] ?? 0,
+        roundWorld[p * 3 + 1] ?? 0,
+        roundWorld[p * 3 + 2] ?? 0,
+        p,
+      ) - radii;
+    END_SEP[p] = separation;
     // The endpoint that produced it, which is stable while the capsule keeps its orientation.
-    out.featureIds[count] = 0x40000000 | p;
-    count++;
+    emitRound(out, p, separation, 0x40000000 | p);
   }
-  if (count === 0) return false;
-  out.nx = nx;
-  out.ny = ny;
-  out.nz = nz;
-  out.count = count;
+
+  /*
+   * **A capsule is a segment, and its middle can be the part that touches.** This used to ask only
+   * its two ends, which is exact for a ball and for a capsule lying on a face, and wrong wherever the
+   * segment crosses an edge: a standing capsule walked through a slab across its waist, because
+   * neither end ball came within reach of it, and a capsule lying across a rail fell through it.
+   *
+   * The distance from a point on the segment to a convex polytope is convex along the segment, so
+   * its least value is at an end exactly when the slope there points away from the other end. Each
+   * end's direction away from the polytope is that slope, so a capsule whose ends are the answer, as
+   * one standing on a floor always is, costs nothing more. Otherwise a fixed number of golden-section
+   * steps finds the interior point, and it is kept only where it is nearer than both ends.
+   */
+  if (points === 2) {
+    const ax = roundWorld[0] ?? 0;
+    const ay = roundWorld[1] ?? 0;
+    const az = roundWorld[2] ?? 0;
+    const sx = (roundWorld[3] ?? 0) - ax;
+    const sy = (roundWorld[4] ?? 0) - ay;
+    const sz = (roundWorld[5] ?? 0) - az;
+    const leavesFirst = sx * (END_DIR[0] ?? 0) + sy * (END_DIR[1] ?? 0) + sz * (END_DIR[2] ?? 0);
+    const leavesSecond = -(
+      sx * (END_DIR[3] ?? 0) +
+      sy * (END_DIR[4] ?? 0) +
+      sz * (END_DIR[5] ?? 0)
+    );
+    if (leavesFirst < 0 && leavesSecond < 0) {
+      let lo = 0;
+      let hi = 1;
+      let m1 = hi - (hi - lo) * INV_PHI;
+      let m2 = lo + (hi - lo) * INV_PHI;
+      let f1 = signedDistance(poly, polyWorld, faces, ax + sx * m1, ay + sy * m1, az + sz * m1, 2);
+      let f2 = signedDistance(poly, polyWorld, faces, ax + sx * m2, ay + sy * m2, az + sz * m2, 2);
+      for (let step = 0; step < SEGMENT_STEPS; step++) {
+        if (f1 <= f2) {
+          hi = m2;
+          m2 = m1;
+          f2 = f1;
+          m1 = hi - (hi - lo) * INV_PHI;
+          f1 = signedDistance(poly, polyWorld, faces, ax + sx * m1, ay + sy * m1, az + sz * m1, 2);
+        } else {
+          lo = m1;
+          m1 = m2;
+          f1 = f2;
+          m2 = lo + (hi - lo) * INV_PHI;
+          f2 = signedDistance(poly, polyWorld, faces, ax + sx * m2, ay + sy * m2, az + sz * m2, 2);
+        }
+      }
+      const at = (lo + hi) / 2;
+      const separation =
+        signedDistance(poly, polyWorld, faces, ax + sx * at, ay + sy * at, az + sz * at, 2) - radii;
+      if (separation < Math.min(END_SEP[0] ?? Infinity, END_SEP[1] ?? Infinity) - 1e-7) {
+        emitRound(out, 2, separation, 0x40000000 | 2);
+      }
+    }
+  }
+
+  if (ROUND.count === 0) return false;
+  out.nx = ROUND.nx;
+  out.ny = ROUND.ny;
+  out.nz = ROUND.nz;
+  out.count = ROUND.count;
   return true;
 }