@flighthq/physics2d 0.3.0-edge.1458.0c01b63

package/src/massProperties.test.ts

@@ -0,0 +1,169 @@
+import type { CollisionShape, Physics2DCollider, Physics2DMassData, RigidBody2D } from '@flighthq/types/contract';
+import { describe, expect, it } from 'vitest';
+
+import { computePhysics2DColliderMassData, updateRigidBody2DMassData } from './massProperties';
+import { createRigidBody2D } from './world';
+
+function collider(local: CollisionShape, density = 1): Physics2DCollider {
+  return {
+    local,
+    world: local,
+    material: { density, friction: 0.2, restitution: 0 },
+    sensor: false,
+  };
+}
+
+// The constructor, not a literal that happens to match the fields: a body grows fields (sleep state, for
+// one) and a literal silently goes stale the moment it does.
+function body(colliders: Physics2DCollider[], type: RigidBody2D['type'] = 'dynamic'): RigidBody2D {
+  const created = createRigidBody2D(type, 0, 0);
+  created.index = 0;
+  created.colliders.push(...colliders);
+  return created;
+}
+
+function massData(): Physics2DMassData {
+  return { mass: 0, inertia: 0, centerX: 0, centerY: 0 };
+}
+
+describe('computePhysics2DColliderMassData', () => {
+  it('derives a disc from its area and half its mass-radius-squared', () => {
+    const out = massData();
+    computePhysics2DColliderMassData(collider({ kind: 'circle', x: 3, y: -2, radius: 2 }, 4), out);
+    expect(out.mass).toBeCloseTo(Math.PI * 4 * 4);
+    expect(out.inertia).toBeCloseTo(0.5 * Math.PI * 4 * 4 * 4);
+    expect(out.centerX).toBe(3);
+    expect(out.centerY).toBe(-2);
+  });
+
+  it('derives a box from its extents and centres it on the box', () => {
+    const out = massData();
+    computePhysics2DColliderMassData(collider({ kind: 'aabb', minX: 0, minY: 0, maxX: 4, maxY: 2 }, 3), out);
+    expect(out.mass).toBeCloseTo(24);
+    expect(out.inertia).toBeCloseTo((24 * (16 + 4)) / 12);
+    expect(out.centerX).toBeCloseTo(2);
+    expect(out.centerY).toBeCloseTo(1);
+  });
+
+  it('gives an oriented box the same inertia as the axis-aligned box of equal extents', () => {
+    // Rotation about z does not change a rectangle's second moment about its own centre. Pinned
+    // because assuming otherwise is a natural mistake, and it would make a tilted crate swing wrong.
+    const upright = massData();
+    const tilted = massData();
+    computePhysics2DColliderMassData(collider({ kind: 'aabb', minX: -2, minY: -1, maxX: 2, maxY: 1 }), upright);
+    computePhysics2DColliderMassData(collider({ kind: 'obb', x: 0, y: 0, halfW: 2, halfH: 1, rotation: 0.7 }), tilted);
+    expect(tilted.mass).toBeCloseTo(upright.mass);
+    expect(tilted.inertia).toBeCloseTo(upright.inertia);
+  });
+
+  it('agrees with the box formula when the same square is given as a polygon', () => {
+    // Cross-validation between two independent derivations: the closed-form rectangle expression and
+    // the general polygon accumulation. Agreement is evidence neither is quietly wrong.
+    const asBox = massData();
+    const asPolygon = massData();
+    computePhysics2DColliderMassData(
+      collider({ kind: 'aabb', minX: -1.5, minY: -0.5, maxX: 1.5, maxY: 0.5 }, 2),
+      asBox,
+    );
+    computePhysics2DColliderMassData(
+      collider({ kind: 'polygon', points: [-1.5, -0.5, 1.5, -0.5, 1.5, 0.5, -1.5, 0.5] }, 2),
+      asPolygon,
+    );
+    expect(asPolygon.mass).toBeCloseTo(asBox.mass);
+    expect(asPolygon.inertia).toBeCloseTo(asBox.inertia);
+    expect(asPolygon.centerX).toBeCloseTo(asBox.centerX);
+    expect(asPolygon.centerY).toBeCloseTo(asBox.centerY);
+  });
+
+  it('gives a polygon the same mass whichever winding it is given in', () => {
+    const counterClockwise = massData();
+    const clockwise = massData();
+    computePhysics2DColliderMassData(collider({ kind: 'polygon', points: [0, 0, 4, 0, 4, 2, 0, 2] }), counterClockwise);
+    computePhysics2DColliderMassData(collider({ kind: 'polygon', points: [0, 0, 0, 2, 4, 2, 4, 0] }), clockwise);
+    expect(clockwise.mass).toBeCloseTo(counterClockwise.mass);
+    expect(clockwise.inertia).toBeCloseTo(counterClockwise.inertia);
+    expect(clockwise.centerX).toBeCloseTo(counterClockwise.centerX);
+    expect(clockwise.centerY).toBeCloseTo(counterClockwise.centerY);
+  });
+
+  it('offsets a polygon centroid without changing its inertia about that centroid', () => {
+    // The parallel-axis subtraction is the step most easily got wrong, and the wrongness is invisible
+    // in a shape centred on the origin. Translating the same square must move the centroid and leave
+    // the inertia untouched.
+    const atOrigin = massData();
+    const shifted = massData();
+    computePhysics2DColliderMassData(collider({ kind: 'polygon', points: [-1, -1, 1, -1, 1, 1, -1, 1] }), atOrigin);
+    computePhysics2DColliderMassData(collider({ kind: 'polygon', points: [9, 19, 11, 19, 11, 21, 9, 21] }), shifted);
+    expect(shifted.centerX).toBeCloseTo(10);
+    expect(shifted.centerY).toBeCloseTo(20);
+    expect(shifted.inertia).toBeCloseTo(atOrigin.inertia);
+  });
+
+  it('gives area-less and degenerate shapes no mass', () => {
+    const out = massData();
+    computePhysics2DColliderMassData(collider({ kind: 'segment', x0: 0, y0: 0, x1: 5, y1: 5 }), out);
+    expect(out.mass).toBe(0);
+    computePhysics2DColliderMassData(collider({ kind: 'point', x: 1, y: 1 }), out);
+    expect(out.mass).toBe(0);
+    // Collinear vertices enclose no area; dividing the centroid by it would be a NaN body.
+    computePhysics2DColliderMassData(collider({ kind: 'polygon', points: [0, 0, 1, 1, 2, 2] }), out);
+    expect(out.mass).toBe(0);
+    expect(Number.isNaN(out.centerX)).toBe(false);
+  });
+});
+
+describe('updateRigidBody2DMassData', () => {
+  it('combines two colliders into one centre of mass and shifts their inertia onto it', () => {
+    const left = collider({ kind: 'aabb', minX: -3, minY: -1, maxX: -1, maxY: 1 });
+    const right = collider({ kind: 'aabb', minX: 1, minY: -1, maxX: 3, maxY: 1 });
+    const target = body([left, right]);
+    updateRigidBody2DMassData(target);
+
+    expect(target.mass).toBeCloseTo(8);
+    expect(target.centerX).toBeCloseTo(0);
+    expect(target.centerY).toBeCloseTo(0);
+    // Each 2x2 box contributes its own inertia plus mass times the square of its 2-unit offset.
+    const own = (4 * (4 + 4)) / 12;
+    expect(target.inertia).toBeCloseTo(2 * (own + 4 * 4));
+    expect(target.inverseMass).toBeCloseTo(1 / 8);
+  });
+
+  it('gives a static body zero inverse mass and inertia while keeping its centre', () => {
+    // Zero inverse mass is the arithmetic that makes a static body immovable without a branch in the
+    // solver, so it must hold even though the shape has real area.
+    const ground = body([collider({ kind: 'aabb', minX: -5, minY: -1, maxX: 5, maxY: 0 })], 'static');
+    updateRigidBody2DMassData(ground);
+    expect(ground.inverseMass).toBe(0);
+    expect(ground.inverseInertia).toBe(0);
+    expect(ground.mass).toBe(0);
+    expect(ground.centerX).toBeCloseTo(0);
+    expect(ground.centerY).toBeCloseTo(-0.5);
+  });
+
+  it('gives a kinematic body zero inverse mass even though it moves', () => {
+    const platform = body([collider({ kind: 'aabb', minX: 0, minY: 0, maxX: 4, maxY: 1 })], 'kinematic');
+    updateRigidBody2DMassData(platform);
+    expect(platform.inverseMass).toBe(0);
+    expect(platform.inverseInertia).toBe(0);
+  });
+
+  it('leaves a dynamic body with no area finite rather than dividing by its zero mass', () => {
+    // A body whose only collider is a sensor point has no mass. Inverting it would seed NaN into the
+    // velocity of everything it later touches, which is unrecoverable rather than merely wrong.
+    const ghost = body([collider({ kind: 'point', x: 0, y: 0 })]);
+    updateRigidBody2DMassData(ghost);
+    expect(ghost.inverseMass).toBe(0);
+    expect(ghost.inverseInertia).toBe(0);
+    expect(Number.isFinite(ghost.centerX)).toBe(true);
+  });
+
+  it('scales mass with density but leaves the centre of mass where it was', () => {
+    const light = body([collider({ kind: 'aabb', minX: 0, minY: 0, maxX: 2, maxY: 2 }, 1)]);
+    const heavy = body([collider({ kind: 'aabb', minX: 0, minY: 0, maxX: 2, maxY: 2 }, 7)]);
+    updateRigidBody2DMassData(light);
+    updateRigidBody2DMassData(heavy);
+    expect(heavy.mass).toBeCloseTo(light.mass * 7);
+    expect(heavy.inertia).toBeCloseTo(light.inertia * 7);
+    expect(heavy.centerX).toBeCloseTo(light.centerX);
+  });
+});

package/src/solver.test.ts

@@ -0,0 +1,186 @@
+import type { RigidBody2D } from '@flighthq/types/contract';
+import { describe, expect, it } from 'vitest';
+
+import {
+  applyPhysics2DImpulse,
+  solvePhysics2DContactsOnce,
+  relativeNormalVelocity,
+  solvePhysics2DContacts,
+  warmStartPhysics2DContacts,
+} from './solver';
+import { stepPhysics2D } from './step';
+import { addPhysics2DBody, createPhysics2DCollider, createPhysics2DWorld, createRigidBody2D } from './world';
+
+const STONE = { density: 1, friction: 0.3, restitution: 0 };
+
+function body(type: RigidBody2D['type'], x: number, y: number): RigidBody2D {
+  const made = createRigidBody2D(type, x, y);
+  made.colliders.push(createPhysics2DCollider({ kind: 'aabb', minX: -0.5, minY: -0.5, maxX: 0.5, maxY: 0.5 }, STONE));
+  return made;
+}
+
+function restingWorld() {
+  const world = createPhysics2DWorld();
+  const floor = createRigidBody2D('static', 0, 0);
+  floor.colliders.push(createPhysics2DCollider({ kind: 'aabb', minX: -50, minY: -1, maxX: 50, maxY: 0 }, STONE));
+  addPhysics2DBody(world, floor);
+  const crate = addPhysics2DBody(world, body('dynamic', 0, 0.499));
+  return { world, crate };
+}
+
+describe('applyPhysics2DImpulse', () => {
+  it('pushes A along the impulse and B against it, matching the manifold normal direction', () => {
+    // The normal separates A out of B, so a positive impulse must move A along it. Reversed, the solver
+    // drives bodies together and a resting box settles below the floor rather than on it.
+    const world = createPhysics2DWorld();
+    const a = addPhysics2DBody(world, body('dynamic', 0, 0));
+    const b = addPhysics2DBody(world, body('dynamic', 0, 1));
+    applyPhysics2DImpulse(a, b, 0, 0, 0, 0, 0, 1);
+    expect(a.velocityY).toBeGreaterThan(0);
+    expect(b.velocityY).toBeLessThan(0);
+  });
+
+  it('spins a body when the impulse acts off its centre of mass', () => {
+    const world = createPhysics2DWorld();
+    const a = addPhysics2DBody(world, body('dynamic', 0, 0));
+    const b = addPhysics2DBody(world, body('dynamic', 0, 1));
+    applyPhysics2DImpulse(a, b, 0.5, 0, 0, 0, 0, 1);
+    expect(a.angularVelocity).not.toBe(0);
+  });
+
+  it('leaves a static body untouched through its zero inverse mass, with no branch', () => {
+    const world = createPhysics2DWorld();
+    const floor = addPhysics2DBody(world, body('static', 0, 0));
+    const crate = addPhysics2DBody(world, body('dynamic', 0, 1));
+    applyPhysics2DImpulse(floor, crate, 0.3, 0.2, 0, 0, 5, 7);
+    expect(floor.velocityX).toBe(0);
+    expect(floor.velocityY).toBe(0);
+    expect(floor.angularVelocity).toBe(0);
+  });
+});
+
+describe('relativeNormalVelocity', () => {
+  const point = {
+    x: 0,
+    y: 0,
+    depth: 0,
+    featureId: 0,
+    rAX: 0,
+    rAY: 0,
+    rBX: 0,
+    rBY: 0,
+    normalImpulse: 0,
+    tangentImpulse: 0,
+    normalMass: 0,
+    tangentMass: 0,
+    bias: 0,
+  };
+
+  it('is negative while the pair is closing along the separating normal', () => {
+    const world = createPhysics2DWorld();
+    const a = addPhysics2DBody(world, body('dynamic', 0, 0));
+    const b = addPhysics2DBody(world, body('dynamic', 0, 1));
+    b.velocityY = -1; // B falling toward A
+    expect(relativeNormalVelocity(a, b, point, 0, 1)).toBeGreaterThan(0);
+    b.velocityY = 1; // B moving away
+    expect(relativeNormalVelocity(a, b, point, 0, 1)).toBeLessThan(0);
+  });
+
+  it('includes the angular contribution at the lever arm', () => {
+    const world = createPhysics2DWorld();
+    const a = addPhysics2DBody(world, body('dynamic', 0, 0));
+    const b = addPhysics2DBody(world, body('dynamic', 0, 1));
+    const spinning = { ...point, rAX: 1, rAY: 0 };
+    a.angularVelocity = 2;
+    expect(relativeNormalVelocity(a, b, spinning, 0, 1)).toBeCloseTo(2);
+  });
+});
+
+describe('solvePhysics2DContacts', () => {
+  it('removes the closing velocity at a resting contact', () => {
+    const { world, crate } = restingWorld();
+    crate.velocityY = -5;
+    stepPhysics2D(world, 1 / 60);
+    expect(crate.velocityY).toBeGreaterThan(-1);
+  });
+
+  it('accumulates a non-negative normal impulse rather than pulling the pair together', () => {
+    // The clamp is on the ACCUMULATED impulse, not the increment: a contact may never pull.
+    const { world } = restingWorld();
+    for (let i = 0; i < 30; i++) stepPhysics2D(world, 1 / 60);
+    for (const contact of world.contacts) {
+      for (let i = 0; i < contact.pointCount; i++) {
+        expect(contact.points[i].normalImpulse).toBeGreaterThanOrEqual(0);
+      }
+    }
+  });
+
+  it('bounds friction by the Coulomb limit against the normal impulse', () => {
+    const { world, crate } = restingWorld();
+    crate.velocityX = 4;
+    for (let i = 0; i < 20; i++) stepPhysics2D(world, 1 / 60);
+    for (const contact of world.contacts) {
+      for (let i = 0; i < contact.pointCount; i++) {
+        const point = contact.points[i];
+        expect(Math.abs(point.tangentImpulse)).toBeLessThanOrEqual(contact.friction * point.normalImpulse + 1e-9);
+      }
+    }
+  });
+
+  it('slows a sliding box through friction instead of letting it glide forever', () => {
+    const { world, crate } = restingWorld();
+    crate.velocityX = 4;
+    for (let i = 0; i < 60; i++) stepPhysics2D(world, 1 / 60);
+    expect(crate.velocityX).toBeLessThan(4);
+    expect(crate.velocityX).toBeGreaterThan(0);
+  });
+
+  it('does nothing for a world with no contacts', () => {
+    const world = createPhysics2DWorld();
+    expect(() => solvePhysics2DContacts(world)).not.toThrow();
+  });
+});
+
+describe('solvePhysics2DContactsOnce', () => {
+  it('applies one pass, so the step can interleave contacts with joints inside a single iteration', () => {
+    // Joints and contacts constrain the same bodies. Giving either a whole pass to itself lets it undo
+    // what the other just corrected, which is why the step alternates them rather than running one solver
+    // to convergence and then the other.
+    const { world, crate } = restingWorld();
+    crate.velocityY = -5;
+    stepPhysics2D(world, 1 / 60);
+    const before = crate.velocityY;
+    solvePhysics2DContactsOnce(world);
+    expect(crate.velocityY).not.toBe(before);
+  });
+
+  it('does nothing for a world with no contacts', () => {
+    expect(() => solvePhysics2DContactsOnce(createPhysics2DWorld())).not.toThrow();
+  });
+});
+
+describe('warmStartPhysics2DContacts', () => {
+  it('reapplies the cached impulse, leaving a converged resting contact deeper than a cold start', () => {
+    // The measurable effect of warm starting: a settled stack keeps its impulses and stays put, where a
+    // cold-started one gives them up each step and sinks into its own slop.
+    const warm = createPhysics2DWorld();
+    const cold = createPhysics2DWorld();
+    cold.config.warmStarting = false;
+    for (const world of [warm, cold]) {
+      const floor = createRigidBody2D('static', 0, 0);
+      floor.colliders.push(createPhysics2DCollider({ kind: 'aabb', minX: -50, minY: -1, maxX: 50, maxY: 0 }, STONE));
+      addPhysics2DBody(world, floor);
+      addPhysics2DBody(world, body('dynamic', 0, 0.5));
+      addPhysics2DBody(world, body('dynamic', 0, 1.5));
+      addPhysics2DBody(world, body('dynamic', 0, 2.5));
+      for (let i = 0; i < 120; i++) stepPhysics2D(world, 1 / 60);
+    }
+    const warmTop = warm.bodies[3].y;
+    const coldTop = cold.bodies[3].y;
+    expect(warmTop).toBeGreaterThanOrEqual(coldTop - 1e-9);
+  });
+
+  it('does nothing for a world with no contacts', () => {
+    expect(() => warmStartPhysics2DContacts(createPhysics2DWorld())).not.toThrow();
+  });
+});