@flighthq/physics3d 0.4.0-edge.1908.751021c

package/src/broadphase.test.ts

@@ -0,0 +1,265 @@
+import { setSpatialIndexingGuard } from '@flighthq/spatial/contract';
+import type {
+  CollisionBuiltInShape3D,
+  Physics3DWorld,
+  RigidBody3D,
+  SpatialIndexingNotice,
+  SpatialPair,
+} from '@flighthq/types/contract';
+import { describe, expect, it } from 'vitest';
+
+import { synchronizePhysics3DBroadphase, synchronizePhysics3DSweptBroadphase } from './broadphase';
+import {
+  addPhysics3DBody,
+  addPhysics3DCollider,
+  createPhysics3DCollider,
+  createPhysics3DWorld,
+  createRigidBody3D,
+  removePhysics3DBody,
+} from './world';
+
+afterEach(() => {
+  setSpatialIndexingGuard(null);
+});
+
+function unitBox(): CollisionBuiltInShape3D {
+  return { kind: 'aabb', minX: -0.5, minY: -0.5, minZ: -0.5, maxX: 0.5, maxY: 0.5, maxZ: 0.5 };
+}
+
+function addBoxBody(world: Physics3DWorld, x: number, y: number, z: number): RigidBody3D {
+  const body = createRigidBody3D('dynamic');
+  body.x = x;
+  body.y = y;
+  body.z = z;
+  addPhysics3DBody(world, body);
+  addPhysics3DCollider(world, body, createPhysics3DCollider(unitBox()));
+  return body;
+}
+
+function pairs(world: Readonly<Physics3DWorld>): SpatialPair[] {
+  const out: SpatialPair[] = [];
+  world.index.querySpatialPairs(out);
+  return out;
+}
+
+describe('synchronizePhysics3DBroadphase', () => {
+  it('uses insert, update, and remove without false missing-id lifecycle notices', () => {
+    const notices: SpatialIndexingNotice[] = [];
+    setSpatialIndexingGuard((notice) => notices.push({ ...notice }));
+    const world = createPhysics3DWorld();
+    const body = createRigidBody3D('dynamic');
+
+    addPhysics3DBody(world, body);
+    for (let step = 0; step < 100; step += 1) synchronizePhysics3DBroadphase(world);
+    addPhysics3DCollider(world, body, createPhysics3DCollider(unitBox()));
+    synchronizePhysics3DBroadphase(world);
+    body.colliders.length = 0;
+    synchronizePhysics3DBroadphase(world);
+    synchronizePhysics3DBroadphase(world);
+    removePhysics3DBody(world, body);
+
+    expect(notices.filter((notice) => notice.reason === 'missing-id')).toEqual([]);
+  });
+
+  it('publishes a body so an overlapping neighbour becomes a candidate pair', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    addBoxBody(world, 0.25, 0, 0);
+
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(1);
+  });
+
+  it('reports no pair for bodies that are far apart', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    addBoxBody(world, 500, 0, 0);
+
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+
+  it('separates bodies along Z, not only along X and Y', () => {
+    // The dimension the 2D index cannot see. A seam that quietly dropped Z would report this pair.
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    addBoxBody(world, 0, 0, 500);
+
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+
+  it('follows a body that moves, rather than holding its bounds at insertion', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    const mover = addBoxBody(world, 500, 0, 0);
+    synchronizePhysics3DBroadphase(world);
+    expect(pairs(world)).toHaveLength(0);
+
+    mover.x = 0.25;
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(1);
+  });
+
+  it('withdraws a body that has no colliders rather than leaving stale bounds', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    const ghost = addBoxBody(world, 0.25, 0, 0);
+    synchronizePhysics3DBroadphase(world);
+    expect(pairs(world)).toHaveLength(1);
+
+    // Emptied directly rather than through `removePhysics3DCollider`, so this tests the sync's own
+    // withdrawal rather than the lifecycle helper's.
+    ghost.colliders.length = 0;
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+
+  it('withdraws a body that has diverged to a non-finite position', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    const diverged = addBoxBody(world, 0.25, 0, 0);
+
+    diverged.x = Number.NaN;
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+
+  it('withdraws a body whose bounds exceed the simulated extent', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    const huge = createRigidBody3D('dynamic');
+    addPhysics3DBody(world, huge);
+    addPhysics3DCollider(
+      world,
+      huge,
+      createPhysics3DCollider({ kind: 'aabb', minX: -1e9, minY: -1, minZ: -1, maxX: 1e9, maxY: 1, maxZ: 1 }),
+    );
+
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+
+  it('refreshes each collider world shape as a side effect, so the narrow phase reads current poses', () => {
+    const world = createPhysics3DWorld();
+    const body = addBoxBody(world, 0, 0, 0);
+
+    body.x = 7;
+    synchronizePhysics3DBroadphase(world);
+
+    expect(body.colliders[0].world.kind === 'box' && body.colliders[0].world.x).toBeCloseTo(7, 9);
+  });
+
+  it('unions a compound body into one indexed volume', () => {
+    const world = createPhysics3DWorld();
+    const compound = createRigidBody3D('dynamic');
+    addPhysics3DBody(world, compound);
+    addPhysics3DCollider(world, compound, createPhysics3DCollider(unitBox()));
+    addPhysics3DCollider(
+      world,
+      compound,
+      createPhysics3DCollider({ kind: 'aabb', minX: 9.5, minY: -0.5, minZ: -0.5, maxX: 10.5, maxY: 0.5, maxZ: 0.5 }),
+    );
+    // Sits beside the FAR collider, which is only a candidate if the body's bounds covered both.
+    addBoxBody(world, 10, 0, 0);
+
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(1);
+  });
+});
+
+describe('synchronizePhysics3DSweptBroadphase', () => {
+  it('pairs bodies that will only meet PART WAY through the interval', () => {
+    // The whole reason a swept publication exists: at the current pose these two are 20 apart, so an
+    // ordinary sync reports no candidate and a continuous pass would never even test the pair.
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    const mover = addBoxBody(world, 20, 0, 0);
+    mover.velocityX = -1200;
+    synchronizePhysics3DBroadphase(world);
+    expect(pairs(world)).toHaveLength(0);
+
+    synchronizePhysics3DSweptBroadphase(world, 1 / 60);
+
+    expect(pairs(world)).toHaveLength(1);
+  });
+
+  it('leaves a body that is not moving where it was', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    addBoxBody(world, 20, 0, 0);
+
+    synchronizePhysics3DSweptBroadphase(world, 1 / 60);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+
+  it('does not widen a STATIC body, which cannot move however fast its velocity field reads', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    const stuck = createRigidBody3D('static');
+    stuck.x = 20;
+    stuck.velocityX = -1200;
+    addPhysics3DBody(world, stuck);
+    addPhysics3DCollider(world, stuck, createPhysics3DCollider(unitBox()));
+
+    synchronizePhysics3DSweptBroadphase(world, 1 / 60);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+
+  it('does not widen a SLEEPING body', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    const asleep = addBoxBody(world, 20, 0, 0);
+    asleep.velocityX = -1200;
+    asleep.sleeping = true;
+
+    synchronizePhysics3DSweptBroadphase(world, 1 / 60);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+
+  it('covers every orientation a SPINNING body could reach, not just its translation', () => {
+    // A rotating body's colliders swing through space its straight-line sweep never enters, so the
+    // widened volume has to be the sphere its geometry can reach rather than the box it currently fills.
+    const world = createPhysics3DWorld();
+    const spinner = createRigidBody3D('dynamic');
+    addPhysics3DBody(world, spinner);
+    // A long arm reaching out along x; spun about y its far end sweeps to z.
+    addPhysics3DCollider(
+      world,
+      spinner,
+      createPhysics3DCollider({ kind: 'aabb', minX: -0.25, minY: -0.25, minZ: -0.25, maxX: 4, maxY: 0.25, maxZ: 0.25 }),
+    );
+    spinner.angularVelocityY = 30;
+    addBoxBody(world, 0, 0, 3.5);
+
+    synchronizePhysics3DBroadphase(world);
+    expect(pairs(world)).toHaveLength(0);
+    synchronizePhysics3DSweptBroadphase(world, 1 / 60);
+
+    expect(pairs(world)).toHaveLength(1);
+  });
+
+  it('restores the ordinary bounds when the plain sync runs after it', () => {
+    const world = createPhysics3DWorld();
+    addBoxBody(world, 0, 0, 0);
+    const mover = addBoxBody(world, 20, 0, 0);
+    mover.velocityX = -1200;
+
+    synchronizePhysics3DSweptBroadphase(world, 1 / 60);
+    expect(pairs(world)).toHaveLength(1);
+    synchronizePhysics3DBroadphase(world);
+
+    expect(pairs(world)).toHaveLength(0);
+  });
+});

package/src/colliderCollision.test.ts

@@ -0,0 +1,57 @@
+import {
+  createCollisionContactManifold3D,
+  createCollisionRaycastHit3D,
+  createCollisionTimeOfImpact3D,
+  createCollisionTriangleMesh3D,
+  registerBuiltInCollisionFaceQueries3D,
+  registerBuiltInCollisionSupports3D,
+} from '@flighthq/collision/contract';
+import type { CollisionShape3D } from '@flighthq/types/contract';
+import { beforeEach, describe, expect, it } from 'vitest';
+
+import {
+  collidePhysics3DColliderShapes,
+  raycastPhysics3DColliderShape,
+  sweepPhysics3DColliderShapes,
+} from './colliderCollision';
+
+beforeEach(() => {
+  registerBuiltInCollisionSupports3D();
+  registerBuiltInCollisionFaceQueries3D();
+});
+
+describe('collidePhysics3DColliderShapes', () => {
+  it('preserves argument-oriented normals when the mesh is first', () => {
+    const mesh = createFloorMesh();
+    const sphere: CollisionShape3D = { kind: 'sphere', x: 0, y: 0.4, z: 0, radius: 0.5 };
+    const manifold = createCollisionContactManifold3D();
+
+    expect(collidePhysics3DColliderShapes(mesh, sphere, manifold)).toBe(true);
+    expect(manifold.normalY).toBeLessThan(-0.99);
+    expect(collidePhysics3DColliderShapes(sphere, mesh, manifold)).toBe(true);
+    expect(manifold.normalY).toBeGreaterThan(0.99);
+  });
+});
+
+describe('raycastPhysics3DColliderShape', () => {
+  it('routes concave surfaces through their triangle query', () => {
+    const hit = createCollisionRaycastHit3D();
+    expect(raycastPhysics3DColliderShape(createFloorMesh(), 0, 2, 0, 0, -1, 0, hit, 10)).toBe(true);
+    expect(hit.fraction).toBeCloseTo(2, 12);
+  });
+});
+
+describe('sweepPhysics3DColliderShapes', () => {
+  it('preserves argument-oriented normals when the mesh is first', () => {
+    const mesh = createFloorMesh();
+    const sphere: CollisionShape3D = { kind: 'sphere', x: 0, y: 2, z: 0, radius: 0.25 };
+    const hit = createCollisionTimeOfImpact3D();
+
+    expect(sweepPhysics3DColliderShapes(mesh, 0, 0, 0, sphere, 0, -4, 0, hit, 1)).toBe(true);
+    expect(hit.normalY).toBeLessThan(-0.99);
+  });
+});
+
+function createFloorMesh() {
+  return createCollisionTriangleMesh3D([-5, 0, -5, 5, 0, -5, 5, 0, 5, -5, 0, 5], [0, 2, 1, 0, 3, 2]);
+}

package/src/colliderTransform.test.ts

@@ -0,0 +1,221 @@
+import type { CollisionBuiltInShape3D, Physics3DCollider, RigidBody3D, SpatialAabb3D } from '@flighthq/types/contract';
+import { describe, expect, it } from 'vitest';
+
+import {
+  createPhysics3DColliderWorldShape,
+  updatePhysics3DColliderWorldShape,
+  writePhysics3DColliderBounds,
+} from './colliderTransform';
+import { createPhysics3DCollider, createRigidBody3D } from './world';
+
+function bounds(): SpatialAabb3D {
+  return { minX: 0, minY: 0, minZ: 0, maxX: 0, maxY: 0, maxZ: 0 };
+}
+
+// A body spun a quarter turn about +Y, which maps local +x onto world -z.
+function spunBody(): RigidBody3D {
+  const body = createRigidBody3D('dynamic');
+  body.orientationY = Math.sin(Math.PI / 4);
+  body.orientationW = Math.cos(Math.PI / 4);
+  return body;
+}
+
+function collider(local: CollisionBuiltInShape3D): Physics3DCollider {
+  return createPhysics3DCollider(local);
+}
+
+describe('createPhysics3DColliderWorldShape', () => {
+  it('keeps the kind for shapes a rigid transform cannot change', () => {
+    expect(createPhysics3DColliderWorldShape({ kind: 'sphere', x: 1, y: 2, z: 3, radius: 4 }).kind).toBe('sphere');
+    expect(
+      createPhysics3DColliderWorldShape({ kind: 'capsule', x0: 0, y0: 0, z0: 0, x1: 0, y1: 1, z1: 0, radius: 1 }).kind,
+    ).toBe('capsule');
+    expect(createPhysics3DColliderWorldShape({ kind: 'convex', points: [0, 0, 0] }).kind).toBe('convex');
+  });
+
+  it('promotes an axis-aligned box to an ORIENTED box', () => {
+    // Rotate an aabb and it is no longer axis-aligned. Keeping the kind would silently grow the box to its
+    // bounding extent the first time the body turned.
+    const world = createPhysics3DColliderWorldShape({
+      kind: 'aabb',
+      minX: -1,
+      minY: -1,
+      minZ: -1,
+      maxX: 1,
+      maxY: 1,
+      maxZ: 1,
+    });
+    expect(world.kind).toBe('box');
+  });
+
+  it('sizes hull storage from the local shape so the transform can write in place', () => {
+    const local: CollisionBuiltInShape3D = { kind: 'convex', points: [1, 2, 3, 4, 5, 6] };
+    const world = createPhysics3DColliderWorldShape(local);
+    expect(world.kind === 'convex' && world.points).toHaveLength(6);
+  });
+
+  it('does not alias the local hull points', () => {
+    const local: CollisionBuiltInShape3D = { kind: 'convex', points: [1, 2, 3] };
+    const world = createPhysics3DColliderWorldShape(local);
+    expect(world.kind === 'convex' && world.points).not.toBe(local.points);
+  });
+});
+
+describe('updatePhysics3DColliderWorldShape', () => {
+  it('translates a sphere by the body position', () => {
+    const body = createRigidBody3D('dynamic');
+    body.x = 10;
+    body.y = 20;
+    body.z = 30;
+    const held = collider({ kind: 'sphere', x: 1, y: 0, z: 0, radius: 2 });
+
+    updatePhysics3DColliderWorldShape(held, body);
+
+    expect(held.world).toMatchObject({ kind: 'sphere', x: 11, y: 20, z: 30, radius: 2 });
+  });
+
+  it('rotates a sphere offset about the body origin', () => {
+    const held = collider({ kind: 'sphere', x: 1, y: 0, z: 0, radius: 1 });
+
+    updatePhysics3DColliderWorldShape(held, spunBody());
+
+    // A quarter turn about +Y takes local +x to world -z.
+    expect(held.world.kind === 'sphere' && held.world.x).toBeCloseTo(0, 9);
+    expect(held.world.kind === 'sphere' && held.world.z).toBeCloseTo(-1, 9);
+  });
+
+  it('carries the body orientation onto a promoted aabb', () => {
+    const body = spunBody();
+    const held = collider({ kind: 'aabb', minX: -1, minY: -2, minZ: -3, maxX: 1, maxY: 2, maxZ: 3 });
+
+    updatePhysics3DColliderWorldShape(held, body);
+
+    expect(held.world.kind === 'box' && held.world.halfX).toBeCloseTo(1, 9);
+    expect(held.world.kind === 'box' && held.world.halfZ).toBeCloseTo(3, 9);
+    expect(held.world.kind === 'box' && held.world.rotationY).toBeCloseTo(body.orientationY, 9);
+  });
+
+  it('composes body and local rotations rather than replacing one with the other', () => {
+    // Both a quarter turn about +Y: composed they are a half turn, whose quaternion is (0,1,0,0).
+    const held = collider({
+      kind: 'box',
+      x: 0,
+      y: 0,
+      z: 0,
+      halfX: 1,
+      halfY: 1,
+      halfZ: 1,
+      rotationX: 0,
+      rotationY: Math.sin(Math.PI / 4),
+      rotationZ: 0,
+      rotationW: Math.cos(Math.PI / 4),
+    });
+
+    updatePhysics3DColliderWorldShape(held, spunBody());
+
+    expect(held.world.kind === 'box' && held.world.rotationY).toBeCloseTo(1, 9);
+    expect(held.world.kind === 'box' && held.world.rotationW).toBeCloseTo(0, 9);
+  });
+
+  it('moves both capsule endpoints', () => {
+    const body = createRigidBody3D('dynamic');
+    body.y = 5;
+    const held = collider({ kind: 'capsule', x0: 0, y0: -1, z0: 0, x1: 0, y1: 1, z1: 0, radius: 0.5 });
+
+    updatePhysics3DColliderWorldShape(held, body);
+
+    expect(held.world.kind === 'capsule' && held.world.y0).toBeCloseTo(4, 9);
+    expect(held.world.kind === 'capsule' && held.world.y1).toBeCloseTo(6, 9);
+  });
+
+  it('moves every hull point', () => {
+    const body = createRigidBody3D('dynamic');
+    body.x = 100;
+    const held = collider({ kind: 'convex', points: [0, 0, 0, 1, 0, 0, 0, 1, 0] });
+
+    updatePhysics3DColliderWorldShape(held, body);
+
+    expect(held.world.kind === 'convex' && held.world.points).toEqual([100, 0, 0, 101, 0, 0, 100, 1, 0]);
+  });
+
+  it('writes in place, so a step over many colliders allocates nothing', () => {
+    const body = createRigidBody3D('dynamic');
+    const held = collider({ kind: 'sphere', x: 0, y: 0, z: 0, radius: 1 });
+    const before = held.world;
+
+    body.x = 1;
+    updatePhysics3DColliderWorldShape(held, body);
+    body.x = 2;
+    updatePhysics3DColliderWorldShape(held, body);
+
+    expect(held.world).toBe(before);
+  });
+});
+
+describe('writePhysics3DColliderBounds', () => {
+  it('bounds a sphere by its radius', () => {
+    const out = bounds();
+    const held = collider({ kind: 'sphere', x: 1, y: 2, z: 3, radius: 4 });
+    updatePhysics3DColliderWorldShape(held, createRigidBody3D('dynamic'));
+
+    writePhysics3DColliderBounds(held, out);
+
+    expect(out).toEqual({ minX: -3, minY: -2, minZ: -1, maxX: 5, maxY: 6, maxZ: 7 });
+  });
+
+  it('grows an oriented box to the extent its rotation sweeps', () => {
+    const out = bounds();
+    const held = collider({ kind: 'aabb', minX: -1, minY: -1, minZ: -1, maxX: 1, maxY: 1, maxZ: 1 });
+    const body = createRigidBody3D('dynamic');
+    // An eighth turn about +Y, where a unit box's x/z extent grows to sqrt(2).
+    body.orientationY = Math.sin(Math.PI / 8);
+    body.orientationW = Math.cos(Math.PI / 8);
+    updatePhysics3DColliderWorldShape(held, body);
+
+    writePhysics3DColliderBounds(held, out);
+
+    expect(out.maxX).toBeCloseTo(Math.SQRT2, 9);
+    expect(out.maxZ).toBeCloseTo(Math.SQRT2, 9);
+    // The rotation axis is unaffected.
+    expect(out.maxY).toBeCloseTo(1, 9);
+  });
+
+  it('leaves an unrotated box at its own extent rather than inflating it', () => {
+    const out = bounds();
+    const held = collider({ kind: 'aabb', minX: -1, minY: -2, minZ: -3, maxX: 1, maxY: 2, maxZ: 3 });
+    updatePhysics3DColliderWorldShape(held, createRigidBody3D('dynamic'));
+
+    writePhysics3DColliderBounds(held, out);
+
+    expect(out).toEqual({ minX: -1, minY: -2, minZ: -3, maxX: 1, maxY: 2, maxZ: 3 });
+  });
+
+  it('bounds a capsule by both endpoints plus the radius', () => {
+    const out = bounds();
+    const held = collider({ kind: 'capsule', x0: -2, y0: 0, z0: 0, x1: 2, y1: 0, z1: 0, radius: 0.5 });
+    updatePhysics3DColliderWorldShape(held, createRigidBody3D('dynamic'));
+
+    writePhysics3DColliderBounds(held, out);
+
+    expect(out).toEqual({ minX: -2.5, minY: -0.5, minZ: -0.5, maxX: 2.5, maxY: 0.5, maxZ: 0.5 });
+  });
+
+  it('bounds a hull by its extreme points', () => {
+    const out = bounds();
+    const held = collider({ kind: 'convex', points: [-1, 0, 0, 3, 0, 0, 0, 7, 0, 0, 0, -2] });
+    updatePhysics3DColliderWorldShape(held, createRigidBody3D('dynamic'));
+
+    writePhysics3DColliderBounds(held, out);
+
+    expect(out).toEqual({ minX: -1, minY: 0, minZ: -2, maxX: 3, maxY: 7, maxZ: 0 });
+  });
+
+  it('zeroes the bounds of an empty hull rather than reporting infinities', () => {
+    const out = bounds();
+    const held = collider({ kind: 'convex', points: [] });
+
+    writePhysics3DColliderBounds(held, out);
+
+    expect(out).toEqual({ minX: 0, minY: 0, minZ: 0, maxX: 0, maxY: 0, maxZ: 0 });
+  });
+});