@flighthq/physics3d 0.4.0-next.1694.598ef6f

package/src/jointRegistry.test.ts

@@ -0,0 +1,229 @@
+import type { Physics3DJointSolver, Physics3DWorld, RigidBody3D } from '@flighthq/types/contract';
+import { describe, expect, it } from 'vitest';
+
+import { createPhysics3DBallAndSocketJoint, createPhysics3DHingeJoint } from './jointFactories';
+import {
+  addPhysics3DJoint,
+  getPhysics3DJointSolver,
+  invalidatePhysics3DJoint,
+  isPhysics3DPairOrdered,
+  registerPhysics3DJointSolver,
+  removePhysics3DJoint,
+} from './jointRegistry';
+import { physics3DBallAndSocketJointSolver, physics3DHingeJointSolver, Physics3DHingeJointKind } from './joints';
+import { steppingPhysics3DWorlds } from './ownership';
+import { addPhysics3DBody, createPhysics3DWorld, createRigidBody3D } from './world';
+
+describe('addPhysics3DJoint', () => {
+  it('returns the joint it was given and records it on the world', () => {
+    const world = createTestWorld();
+    const joint = createPhysics3DBallAndSocketJoint({ bodyA: 0, bodyB: 1 });
+
+    expect(addPhysics3DJoint(world, joint)).toBe(joint);
+    expect(world.joints).toEqual([joint]);
+  });
+
+  it('refuses a joint that already belongs to a world', () => {
+    const world = createTestWorld();
+    const joint = createPhysics3DBallAndSocketJoint({ bodyA: 0, bodyB: 1 });
+    addPhysics3DJoint(world, joint);
+
+    expect(() => addPhysics3DJoint(world, joint)).toThrow();
+    expect(() => addPhysics3DJoint(createTestWorld(), joint)).toThrow();
+  });
+
+  it('exchanges out-of-order ends when the kind has a solver to consult', () => {
+    const world = createTestWorld();
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, physics3DHingeJointSolver);
+    const joint = createPhysics3DHingeJoint({
+      bodyA: 1,
+      bodyB: 0,
+      localAnchorAX: 5,
+      localAnchorBX: 7,
+      lowerAngle: -0.25,
+      upperAngle: 1.5,
+    });
+
+    addPhysics3DJoint(world, joint);
+
+    expect(joint.bodyA).toBe(0);
+    expect(joint.bodyB).toBe(1);
+    expect(joint.localAnchorAX).toBe(7);
+    expect(joint.localAnchorBX).toBe(5);
+    expect(joint.lowerAngle).toBe(-1.5);
+  });
+
+  it('leaves an unknown kind in its authored order', () => {
+    const world = createTestWorld();
+    const joint = createPhysics3DHingeJoint({ bodyA: 1, bodyB: 0, lowerAngle: -0.25, upperAngle: 1.5 });
+
+    addPhysics3DJoint(world, joint);
+
+    // Exchanging the ends without the kind's consent would move the bodies and anchors while the limit
+    // interval stayed as authored — the generic half of a swap with the kind's half missing, which no later
+    // registration repairs.
+    expect(joint.bodyA).toBe(1);
+    expect(joint.lowerAngle).toBe(-0.25);
+  });
+
+  it('wakes both bodies when the constraint is live', () => {
+    const world = createTestWorld();
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, physics3DHingeJointSolver);
+    world.bodies[0].sleeping = true;
+    world.bodies[1].sleeping = true;
+
+    addPhysics3DJoint(world, createPhysics3DHingeJoint({ bodyA: 0, bodyB: 1 }));
+
+    expect(world.bodies[0].sleeping).toBe(false);
+    expect(world.bodies[1].sleeping).toBe(false);
+  });
+
+  it('leaves a sleeping pair asleep for a kind that constrains nothing yet', () => {
+    const world = createTestWorld();
+    world.bodies[0].sleeping = true;
+
+    addPhysics3DJoint(world, createPhysics3DHingeJoint({ bodyA: 0, bodyB: 1 }));
+
+    expect(world.bodies[0].sleeping).toBe(true);
+  });
+
+  it('rejects a mutation while the world is stepping', () => {
+    const world = createTestWorld();
+    steppingPhysics3DWorlds.add(world);
+
+    expect(() => addPhysics3DJoint(world, createPhysics3DBallAndSocketJoint({ bodyA: 0, bodyB: 1 }))).toThrow();
+
+    steppingPhysics3DWorlds.delete(world);
+  });
+});
+
+describe('getPhysics3DJointSolver', () => {
+  it('returns null for a kind nobody registered', () => {
+    expect(getPhysics3DJointSolver(createTestWorld(), 'acme.Suspension')).toBeNull();
+  });
+
+  it('returns the registered solver', () => {
+    const world = createTestWorld();
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, physics3DHingeJointSolver);
+
+    expect(getPhysics3DJointSolver(world, Physics3DHingeJointKind)).toBe(physics3DHingeJointSolver);
+  });
+});
+
+describe('invalidatePhysics3DJoint', () => {
+  it('clears every common accumulator and the kind-specific ones', () => {
+    const world = createTestWorld();
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, physics3DHingeJointSolver);
+    const joint = addPhysics3DJoint(world, createPhysics3DHingeJoint({ bodyA: 0, bodyB: 1 }));
+    joint.impulse0 = 1;
+    joint.impulse5 = 2;
+    (joint as ReturnType<typeof createPhysics3DHingeJoint>).motorImpulse = 3;
+
+    expect(invalidatePhysics3DJoint(world, joint)).toBe(true);
+    expect(joint.impulse0).toBe(0);
+    expect(joint.impulse5).toBe(0);
+    expect((joint as ReturnType<typeof createPhysics3DHingeJoint>).motorImpulse).toBe(0);
+  });
+
+  it('returns false for a joint this world does not hold', () => {
+    const world = createTestWorld();
+    const joint = createPhysics3DBallAndSocketJoint({ bodyA: 0, bodyB: 1 });
+
+    expect(invalidatePhysics3DJoint(world, joint)).toBe(false);
+  });
+});
+
+describe('isPhysics3DPairOrdered', () => {
+  it('orders by persistent index and admits a pair already in order', () => {
+    expect(isPhysics3DPairOrdered(0, 1)).toBe(true);
+    expect(isPhysics3DPairOrdered(1, 1)).toBe(true);
+    expect(isPhysics3DPairOrdered(1, 0)).toBe(false);
+  });
+});
+
+describe('registerPhysics3DJointSolver', () => {
+  it('canonicalizes joints that were added before their solver existed', () => {
+    const world = createTestWorld();
+    const joint = addPhysics3DJoint(
+      world,
+      createPhysics3DHingeJoint({ bodyA: 1, bodyB: 0, lowerAngle: -0.25, upperAngle: 1.5 }),
+    );
+
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, physics3DHingeJointSolver);
+
+    // The outcome no longer depends on whether the scene or the solver arrived first.
+    expect(joint.bodyA).toBe(0);
+    expect(joint.lowerAngle).toBe(-1.5);
+    expect(joint.upperAngle).toBe(0.25);
+  });
+
+  it('lets a later registration replace an earlier one', () => {
+    const world = createTestWorld();
+    const replacement: Physics3DJointSolver = { prepare(): void {}, solve(): void {} };
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, physics3DHingeJointSolver);
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, replacement);
+
+    expect(getPhysics3DJointSolver(world, Physics3DHingeJointKind)).toBe(replacement);
+  });
+
+  it('wakes the bodies of a joint the registration just brought to life', () => {
+    const world = createTestWorld();
+    addPhysics3DJoint(world, createPhysics3DHingeJoint({ bodyA: 0, bodyB: 1 }));
+    world.bodies[0].sleeping = true;
+    world.bodies[1].sleeping = true;
+
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, physics3DHingeJointSolver);
+
+    expect(world.bodies[0].sleeping).toBe(false);
+    expect(world.bodies[1].sleeping).toBe(false);
+  });
+});
+
+describe('removePhysics3DJoint', () => {
+  it('drops the joint and reports it', () => {
+    const world = createTestWorld();
+    const joint = addPhysics3DJoint(world, createPhysics3DBallAndSocketJoint({ bodyA: 0, bodyB: 1 }));
+
+    expect(removePhysics3DJoint(world, joint)).toBe(true);
+    expect(world.joints).toEqual([]);
+  });
+
+  it('returns false for a joint it never held', () => {
+    const world = createTestWorld();
+
+    expect(removePhysics3DJoint(world, createPhysics3DBallAndSocketJoint({ bodyA: 0, bodyB: 1 }))).toBe(false);
+  });
+
+  it('releases ownership so the joint can be added again', () => {
+    const world = createTestWorld();
+    const joint = addPhysics3DJoint(world, createPhysics3DBallAndSocketJoint({ bodyA: 0, bodyB: 1 }));
+    removePhysics3DJoint(world, joint);
+
+    expect(() => addPhysics3DJoint(world, joint)).not.toThrow();
+  });
+
+  it('wakes both bodies when a live constraint is taken away', () => {
+    const world = createTestWorld();
+    registerPhysics3DJointSolver(world, Physics3DHingeJointKind, physics3DHingeJointSolver);
+    const joint = addPhysics3DJoint(world, createPhysics3DHingeJoint({ bodyA: 0, bodyB: 1 }));
+    world.bodies[0].sleeping = true;
+
+    removePhysics3DJoint(world, joint);
+
+    expect(world.bodies[0].sleeping).toBe(false);
+  });
+});
+
+function createTestWorld(): Physics3DWorld {
+  const world = createPhysics3DWorld();
+  addPhysics3DBody(world, createBody());
+  addPhysics3DBody(world, createBody());
+  // The ball-and-socket solver is imported so this file exercises a kind with no `swapEnds` of its own; the
+  // registry has to reach the generic swap for it without consulting one.
+  expect(physics3DBallAndSocketJointSolver.swapEnds).toBeUndefined();
+  return world;
+}
+
+function createBody(): RigidBody3D {
+  return createRigidBody3D('dynamic');
+}

package/src/jointRows.test.ts

@@ -0,0 +1,484 @@
+import type { Physics3DFixedJoint, Physics3DJoint, RigidBody3D } from '@flighthq/types/contract';
+import { describe, expect, it } from 'vitest';
+
+import { refreshRigidBody3DWorldInertia } from './integrate';
+import { createPhysics3DFixedJoint } from './jointFactories';
+import {
+  applyRow,
+  beginJointSolve,
+  clearJointSolve,
+  frameABasis,
+  frameARotation,
+  frameBBasis,
+  frameBRotation,
+  getJointSolveState,
+  getRowMass,
+  getRowVelocity,
+  POINT_BIAS,
+  POINT_LENGTH,
+  POINT_MASS,
+  prepareAngularBlock,
+  preparePointBlock,
+  readFrameBases,
+  readFrameRotations,
+  readJointImpulse,
+  ROW_LENGTH,
+  solveAngularBlock,
+  solveEqualityRow,
+  solveLowerLimitRow,
+  solveMotorRow,
+  solvePointBlock,
+  solveUpperLimitRow,
+  warmStartAngularBlock,
+  warmStartPointBlock,
+  writeAngularRow,
+  writeJointImpulse,
+  writeRow,
+} from './jointRows';
+import { createPhysics3DMassData, setRigidBody3DMassData } from './massProperties';
+import { createRigidBody3D } from './world';
+
+describe('applyRow', () => {
+  it('drives a row toward rest when scaled by the row mass', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.velocityX = 2;
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    applyRow(bodyA, bodyB, state, 0, -getRowVelocity(bodyA, bodyB, state, 0) * getRowMass(bodyA, bodyB, state, 0));
+
+    expect(getRowVelocity(bodyA, bodyB, state, 0)).toBeCloseTo(0, 12);
+  });
+
+  it('does nothing for a zero impulse', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    applyRow(bodyA, bodyB, state, 0, 0);
+
+    expect(bodyB.velocityX).toBe(0);
+  });
+});
+
+describe('beginJointSolve', () => {
+  it('sizes the scratch and hands the same array back next time', () => {
+    const joint = createTestJoint();
+
+    const first = beginJointSolve(joint, 5);
+    first[0] = 9;
+    const second = beginJointSolve(joint, 8);
+
+    expect(second).toBe(first);
+    expect(second.length).toBe(8);
+  });
+
+  it('establishes accumulators a reconstructed joint reached the solver without', () => {
+    const joint = createTestJoint();
+    // A joint deserialized from a saved world satisfies the required type at compile time only; at runtime the
+    // fields are simply absent, and the first `undefined + x` would send NaN out through both bodies.
+    (joint as Partial<Physics3DJoint>).impulse0 = undefined;
+    (joint as Partial<Physics3DJoint>).impulse5 = undefined;
+
+    beginJointSolve(joint, 4);
+
+    expect(joint.impulse0).toBe(0);
+    expect(joint.impulse5).toBe(0);
+  });
+});
+
+describe('clearJointSolve', () => {
+  it('leaves a solve with nothing to read', () => {
+    const joint = createTestJoint();
+    beginJointSolve(joint, 4);
+
+    clearJointSolve(joint);
+
+    expect(getJointSolveState(joint)).toBeUndefined();
+  });
+});
+
+describe('frameABasis', () => {
+  it('holds nine numbers, three per axis', () => {
+    expect(frameABasis.length).toBe(9);
+    expect(frameBBasis.length).toBe(9);
+  });
+});
+
+describe('frameARotation', () => {
+  it('holds a quaternion, and starts as the identity', () => {
+    expect(frameARotation.length).toBe(4);
+    expect(frameBRotation.length).toBe(4);
+  });
+});
+
+describe('frameBBasis', () => {
+  it('is a distinct array from the frame A basis', () => {
+    expect(frameBBasis).not.toBe(frameABasis);
+  });
+});
+
+describe('frameBRotation', () => {
+  it('is a distinct array from the frame A rotation', () => {
+    expect(frameBRotation).not.toBe(frameARotation);
+  });
+});
+
+describe('getJointSolveState', () => {
+  it('is undefined until a prepare has run', () => {
+    expect(getJointSolveState(createTestJoint())).toBeUndefined();
+  });
+});
+
+describe('getRowMass', () => {
+  it('reads the row at the given offset rather than the head of the state', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const state = newState();
+    writeRow(state, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+    writeRow(state, ROW_LENGTH, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    expect(getRowMass(bodyA, bodyB, state, 0)).toBe(0);
+    expect(getRowMass(bodyA, bodyB, state, ROW_LENGTH)).toBeCloseTo(0.5, 12);
+  });
+});
+
+describe('getRowVelocity', () => {
+  it('measures B relative to A along the row', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.velocityY = 3;
+    const state = newState();
+    writeRow(state, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0);
+
+    expect(getRowVelocity(bodyA, bodyB, state, 0)).toBeCloseTo(3, 12);
+  });
+});
+
+describe('POINT_BIAS', () => {
+  it('follows the six-number mass block', () => {
+    expect(POINT_BIAS).toBe(POINT_MASS + 6);
+    expect(POINT_LENGTH).toBe(POINT_BIAS + 3);
+  });
+});
+
+describe('POINT_LENGTH', () => {
+  it('covers the whole point block', () => {
+    expect(POINT_LENGTH).toBe(9);
+  });
+});
+
+describe('POINT_MASS', () => {
+  it('sits at the head of the per-step state', () => {
+    expect(POINT_MASS).toBe(0);
+  });
+});
+
+describe('prepareAngularBlock', () => {
+  it('reads the frames left by readFrameRotations', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    setAxisAngle(bodyB, 0, 0, 1, 0.4);
+    const joint = createPhysics3DFixedJoint({ bodyA: 0, bodyB: 1 });
+    const state = beginJointSolve(joint, 32);
+
+    readFrameRotations(bodyA, bodyB, joint);
+    prepareAngularBlock(bodyA, bodyB, state, 0, 6, 1 / 60);
+
+    // The bias is the rotation error scaled by BAUMGARTE / dt: 0.4 * 0.2 * 60.
+    expect(state[8]).toBeCloseTo(0.4 * 0.2 * 60, 9);
+  });
+});
+
+describe('preparePointBlock', () => {
+  it('writes the lever arms and the inverted mass block', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.x = 3;
+    const joint = createTestJoint();
+    joint.localAnchorAX = 1;
+    const state = newState();
+
+    preparePointBlock(bodyA, bodyB, joint, state, 1 / 60);
+
+    expect(joint.rAX).toBeCloseTo(1, 12);
+    // Both anchors are on their centres for B and one unit out on A, and the inverse of the diagonal 2 that a
+    // zero-arm pair would give is 0.5.
+    expect(state[POINT_MASS]).toBeCloseTo(0.5, 12);
+    expect(state[POINT_BIAS]).toBeCloseTo(2 * 0.2 * 60, 9);
+  });
+});
+
+describe('readFrameBases', () => {
+  it('expands both frames into world axes', () => {
+    const bodyA = createUnitBody();
+    setAxisAngle(bodyA, 0, 0, 1, Math.PI / 2);
+    const bodyB = createUnitBody();
+
+    readFrameBases(bodyA, bodyB, createPhysics3DFixedJoint({ bodyA: 0, bodyB: 1 }));
+
+    expect(frameABasis[0]).toBeCloseTo(0, 12);
+    expect(frameABasis[1]).toBeCloseTo(1, 12);
+    expect(frameBBasis[0]).toBeCloseTo(1, 12);
+  });
+});
+
+describe('readFrameRotations', () => {
+  it('establishes an absent frame as the identity', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const joint = createPhysics3DFixedJoint({ bodyA: 0, bodyB: 1 });
+    (joint as Partial<Physics3DFixedJoint>).localRotationAW = undefined;
+    (joint as Partial<Physics3DFixedJoint>).localRotationAX = undefined;
+
+    readFrameRotations(bodyA, bodyB, joint);
+
+    expect(joint.localRotationAW).toBe(1);
+    expect(frameARotation[3]).toBeCloseTo(1, 12);
+  });
+});
+
+describe('readJointImpulse', () => {
+  it('reads each of the six slots', () => {
+    const joint = createTestJoint();
+    joint.impulse0 = 10;
+    joint.impulse3 = 13;
+    joint.impulse5 = 15;
+
+    expect(readJointImpulse(joint, 0)).toBe(10);
+    expect(readJointImpulse(joint, 3)).toBe(13);
+    expect(readJointImpulse(joint, 5)).toBe(15);
+  });
+});
+
+describe('ROW_LENGTH', () => {
+  it('is a direction plus two angular arms', () => {
+    expect(ROW_LENGTH).toBe(9);
+  });
+});
+
+describe('solveAngularBlock', () => {
+  it('cancels relative rotation and accumulates into the upper three slots', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.angularVelocityY = 2;
+    const joint = createPhysics3DFixedJoint({ bodyA: 0, bodyB: 1 });
+    const state = beginJointSolve(joint, 32);
+    readFrameRotations(bodyA, bodyB, joint);
+    prepareAngularBlock(bodyA, bodyB, state, 0, 6, 1 / 60);
+
+    solveAngularBlock(bodyA, bodyB, joint, state, 0, 6);
+
+    expect(bodyB.angularVelocityY - bodyA.angularVelocityY).toBeCloseTo(0, 12);
+    expect(joint.impulse4).not.toBe(0);
+  });
+});
+
+describe('solveEqualityRow', () => {
+  it('drives the row to the bias velocity and accumulates into its slot', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.velocityX = 2;
+    const joint = createTestJoint();
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    solveEqualityRow(bodyA, bodyB, joint, 2, state, 0, getRowMass(bodyA, bodyB, state, 0), 0, 0);
+
+    expect(getRowVelocity(bodyA, bodyB, state, 0)).toBeCloseTo(0, 12);
+    expect(joint.impulse2).toBeCloseTo(-1, 12);
+  });
+});
+
+describe('solveLowerLimitRow', () => {
+  it('pushes a crossed bound back up', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    solveLowerLimitRow(bodyA, bodyB, state, 0, getRowMass(bodyA, bodyB, state, 0), -1, 60, ROW_LENGTH);
+
+    expect(getRowVelocity(bodyA, bodyB, state, 0)).toBeGreaterThan(0);
+  });
+
+  it('never pulls a coordinate down through its bound', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.velocityX = 5;
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    solveLowerLimitRow(bodyA, bodyB, state, 0, getRowMass(bodyA, bodyB, state, 0), 3, 60, ROW_LENGTH);
+
+    expect(bodyB.velocityX).toBe(5);
+  });
+});
+
+describe('solveMotorRow', () => {
+  it('returns the accumulated impulse clamped to its budget', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    const total = solveMotorRow(bodyA, bodyB, state, 0, getRowMass(bodyA, bodyB, state, 0), 100, 0.25, 0);
+
+    expect(total).toBeCloseTo(0.25, 12);
+  });
+
+  it('reaches its target speed when the budget allows', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    solveMotorRow(bodyA, bodyB, state, 0, getRowMass(bodyA, bodyB, state, 0), 4, 1000, 0);
+
+    expect(getRowVelocity(bodyA, bodyB, state, 0)).toBeCloseTo(4, 12);
+  });
+});
+
+describe('solvePointBlock', () => {
+  it('cancels the velocity separating the anchors', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.velocityX = 2;
+    bodyB.velocityZ = -1;
+    const joint = createTestJoint();
+    const state = newState();
+    preparePointBlock(bodyA, bodyB, joint, state, 1 / 60);
+
+    solvePointBlock(bodyA, bodyB, joint, state);
+
+    expect(bodyB.velocityX - bodyA.velocityX).toBeCloseTo(0, 12);
+    expect(bodyB.velocityZ - bodyA.velocityZ).toBeCloseTo(0, 12);
+  });
+});
+
+describe('solveUpperLimitRow', () => {
+  it('pushes a crossed bound back down', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    solveUpperLimitRow(bodyA, bodyB, state, 0, getRowMass(bodyA, bodyB, state, 0), -1, 60, ROW_LENGTH);
+
+    expect(getRowVelocity(bodyA, bodyB, state, 0)).toBeLessThan(0);
+  });
+
+  it('never pushes a coordinate up through its bound', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.velocityX = -5;
+    const state = newState();
+    writeRow(state, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
+
+    solveUpperLimitRow(bodyA, bodyB, state, 0, getRowMass(bodyA, bodyB, state, 0), 3, 60, ROW_LENGTH);
+
+    expect(bodyB.velocityX).toBe(-5);
+  });
+});
+
+describe('warmStartAngularBlock', () => {
+  it('reapplies the angular accumulators', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const joint = createTestJoint();
+    joint.impulse4 = 2;
+
+    warmStartAngularBlock(bodyA, bodyB, joint);
+
+    expect(bodyB.angularVelocityY).toBeCloseTo(2, 12);
+    expect(bodyA.angularVelocityY).toBeCloseTo(-2, 12);
+  });
+});
+
+describe('warmStartPointBlock', () => {
+  it('reapplies the point accumulators', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    const joint = createTestJoint();
+    joint.impulse0 = 3;
+
+    warmStartPointBlock(bodyA, bodyB, joint);
+
+    expect(bodyB.velocityX).toBeCloseTo(3, 12);
+    expect(bodyA.velocityX).toBeCloseTo(-3, 12);
+  });
+});
+
+describe('writeAngularRow', () => {
+  it('writes a row with no linear direction and the axis as both arms', () => {
+    const state = newState();
+
+    writeAngularRow(state, 0, 0, 1, 0);
+
+    expect(state.slice(0, 9)).toEqual([0, 0, 0, 0, 1, 0, 0, 1, 0]);
+  });
+
+  it('measures the relative spin about its axis', () => {
+    const bodyA = createUnitBody();
+    const bodyB = createUnitBody();
+    bodyB.angularVelocityZ = 1.5;
+    const state = newState();
+
+    writeAngularRow(state, 0, 0, 0, 1);
+
+    expect(getRowVelocity(bodyA, bodyB, state, 0)).toBeCloseTo(1.5, 12);
+  });
+});
+
+describe('writeJointImpulse', () => {
+  it('writes each of the six slots', () => {
+    const joint = createTestJoint();
+
+    for (let slot = 0; slot < 6; slot += 1) writeJointImpulse(joint, slot, slot + 1);
+
+    expect([joint.impulse0, joint.impulse1, joint.impulse2, joint.impulse3, joint.impulse4, joint.impulse5]).toEqual([
+      1, 2, 3, 4, 5, 6,
+    ]);
+  });
+});
+
+describe('writeRow', () => {
+  it('lays the direction and the two arms out in one block', () => {
+    const state = newState();
+
+    writeRow(state, ROW_LENGTH, 1, 2, 3, 4, 5, 6, 7, 8, 9);
+
+    expect(state.slice(ROW_LENGTH, ROW_LENGTH * 2)).toEqual([1, 2, 3, 4, 5, 6, 7, 8, 9]);
+  });
+});
+
+function createTestJoint(): Physics3DJoint {
+  return createPhysics3DFixedJoint({ bodyA: 0, bodyB: 1 });
+}
+
+function createUnitBody(): RigidBody3D {
+  const body = createRigidBody3D('dynamic');
+  const data = createPhysics3DMassData();
+  data.mass = 1;
+  data.inertiaXX = 1;
+  data.inertiaYY = 1;
+  data.inertiaZZ = 1;
+  setRigidBody3DMassData(body, data);
+  refreshRigidBody3DWorldInertia(body);
+  return body;
+}
+
+function newState(): number[] {
+  return new Array<number>(32).fill(0);
+}
+
+function setAxisAngle(body: RigidBody3D, x: number, y: number, z: number, angle: number): void {
+  const sine = Math.sin(angle / 2);
+  body.orientationX = x * sine;
+  body.orientationY = y * sine;
+  body.orientationZ = z * sine;
+  body.orientationW = Math.cos(angle / 2);
+  refreshRigidBody3DWorldInertia(body);
+}