944 lines
35 KiB
JavaScript
944 lines
35 KiB
JavaScript
// Angry Birds — rigid-body physics. Zero imports, deterministic, Node-testable.
|
|
//
|
|
// Generic 2D rigid-body solver: it knows about convex polygons and circles and
|
|
// nothing at all about birds, pigs or scoring. AngryBirdsLogic.js layers the
|
|
// game on top.
|
|
//
|
|
// ── WHY A BESPOKE SOLVER ────────────────────────────────────────────────────
|
|
// Matter.js ships inside the CDN Phaser build and would have worked in the
|
|
// browser, but it cannot be imported in bare Node and is not reproducible.
|
|
// This repo's whole quality apparatus — tools/verifyAngryBirds.js, the level
|
|
// generator's MEASURED star thresholds, and the editor's winnability gate —
|
|
// depends on replaying a shot headlessly and getting the same answer twice.
|
|
//
|
|
// Neither existing engine could be extended into this. PeggleLogic collides a
|
|
// moving circle against STATIC pegs with no rotation and no persistent
|
|
// contacts; GooTowerLogic has no angular state anywhere, because its rigidity
|
|
// is emergent from triangulation. A toppling tower of boxes needs both
|
|
// rotation and lasting contacts, so this is new code.
|
|
//
|
|
// ── THE THREE THINGS THAT MAKE STACKS STAND UP ──────────────────────────────
|
|
// 1. TWO-POINT MANIFOLDS. A single contact point cannot resist rotation, so a
|
|
// box resting on a box rocks forever. Reference-face clipping yields up to
|
|
// 2 points per pair, which is what makes a face-to-face rest contact rigid.
|
|
// 2. WARM STARTING. Each contact point remembers last substep's accumulated
|
|
// impulse — keyed by a FEATURE ID that survives across frames, not by array
|
|
// position — and reapplies it before iterating. Without it a 6-box tower
|
|
// visibly sags every frame because the solver spends all its iterations
|
|
// rediscovering gravity from zero.
|
|
// 3. ISLAND SLEEPING. Bodies are grouped by contact into islands; an island
|
|
// sleeps only when EVERY member has been slow for SLEEP_TIME. Per-body
|
|
// sleeping would let half a tower freeze while the other half moves.
|
|
// Sleeping is load-bearing three times over: stack stability, perf, and the
|
|
// "shot is finished" test the rules layer waits on.
|
|
//
|
|
// ── UNITS AND CONVENTIONS ───────────────────────────────────────────────────
|
|
// Pixels, seconds, radians. Y IS DOWN (screen convention). Polygon winding is
|
|
// normalized on insert so the shoelace area is positive; the outward normal of
|
|
// edge v[i]->v[i+1] is then (e.y, -e.x). Contact normals point from A to B.
|
|
//
|
|
// Determinism rules, all load-bearing:
|
|
// * bodies and contacts are always iterated in ascending id order;
|
|
// * no Math.random() anywhere — callers pass a seeded rng if they need one;
|
|
// * no Date/performance reads;
|
|
// * nothing that affects the result may depend on Map/Set iteration order —
|
|
// broadphase pairs are sorted by (aId, bId) and the solver walks that list.
|
|
|
|
export const PHYS = {
|
|
SUBSTEP_DT: 1 / 240,
|
|
VEL_ITERS: 8, // sequential-impulse passes per substep
|
|
GRAVITY: 1400, // px/s²
|
|
BAUMGARTE: 0.25, // fraction of excess penetration removed per substep
|
|
SLOP: 0.5, // allowed penetration, px — stops resting contacts buzzing
|
|
REST_THRESHOLD: 120, // approach speed below which restitution is ignored
|
|
MAX_SPEED: 2400, // px/s
|
|
MAX_OMEGA: 40, // rad/s
|
|
LINEAR_DAMPING: 0.002,
|
|
ANGULAR_DAMPING: 0.004,
|
|
SLEEP_LIN: 14, // px/s
|
|
SLEEP_ANG: 0.14, // rad/s
|
|
SLEEP_TIME: 0.5, // s below both thresholds before an island sleeps
|
|
GRID_CELL: 96, // broadphase cell size, px
|
|
MIN_HALF_EXTENT: 12, // smallest half-extent any body may have (anti-tunnel)
|
|
};
|
|
|
|
// ── Small math helpers ──────────────────────────────────────────────────────
|
|
|
|
const cross = (ax, ay, bx, by) => ax * by - ay * bx;
|
|
const clamp = (v, lo, hi) => (v < lo ? lo : v > hi ? hi : v);
|
|
|
|
// Numeric composite keys. Body ids are dense and small, so this is exact well
|
|
// past any level we'd author, and it keeps Map lookups off the string path.
|
|
const PAIR_STRIDE = 1 << 20;
|
|
const pairKey = (aId, bId) => aId * PAIR_STRIDE + bId;
|
|
// Grid cells can be negative, so bias into non-negative space before packing.
|
|
const GRID_BIAS = 1 << 14;
|
|
const cellKey = (gx, gy) => (gx + GRID_BIAS) * (1 << 15) + (gy + GRID_BIAS);
|
|
|
|
// ── Mass properties ─────────────────────────────────────────────────────────
|
|
|
|
// Signed shoelace area. Positive means the winding matches our normal formula.
|
|
function signedArea(verts) {
|
|
let a = 0;
|
|
for (let i = 0; i < verts.length; i += 1) {
|
|
const [x0, y0] = verts[i];
|
|
const [x1, y1] = verts[(i + 1) % verts.length];
|
|
a += x0 * y1 - x1 * y0;
|
|
}
|
|
return a * 0.5;
|
|
}
|
|
|
|
// Area, centroid and second moment of a convex polygon, integrated over the
|
|
// triangle fan from the origin. Inertia comes out about the ORIGIN, so it is
|
|
// shifted to the centroid with the parallel-axis theorem before returning.
|
|
function polyMassData(verts, density) {
|
|
let area = 0;
|
|
let cx = 0;
|
|
let cy = 0;
|
|
let inertia = 0;
|
|
for (let i = 0; i < verts.length; i += 1) {
|
|
const [x0, y0] = verts[i];
|
|
const [x1, y1] = verts[(i + 1) % verts.length];
|
|
const c = x0 * y1 - x1 * y0;
|
|
area += c;
|
|
cx += (x0 + x1) * c;
|
|
cy += (y0 + y1) * c;
|
|
inertia += c * (x0 * x0 + x0 * x1 + x1 * x1 + y0 * y0 + y0 * y1 + y1 * y1);
|
|
}
|
|
area *= 0.5;
|
|
const inv6A = 1 / (6 * area);
|
|
cx *= inv6A;
|
|
cy *= inv6A;
|
|
const mass = density * Math.abs(area);
|
|
let I = density * Math.abs(inertia) / 12;
|
|
I -= mass * (cx * cx + cy * cy); // parallel axis: origin -> centroid
|
|
return { mass, cx, cy, I: Math.max(I, 1e-6) };
|
|
}
|
|
|
|
// ── World ───────────────────────────────────────────────────────────────────
|
|
|
|
export function createWorld(opts = {}) {
|
|
return {
|
|
bodies: [],
|
|
byId: new Map(),
|
|
// Keyed by pairKey(a,b) — a NUMBER, not a string. Contact lookup runs a few
|
|
// thousand times a substep and string keys dominated the profile.
|
|
contacts: new Map(),
|
|
// Rebuilt in pair order each substep; the solver iterates this, never the
|
|
// Map, so nothing depends on Map insertion history.
|
|
contactList: [],
|
|
nextId: 1,
|
|
gravity: opts.gravity ?? PHYS.GRAVITY,
|
|
accum: 0,
|
|
time: 0,
|
|
};
|
|
}
|
|
|
|
function addBody(world, body) {
|
|
body.id = world.nextId;
|
|
world.nextId += 1;
|
|
world.bodies.push(body);
|
|
world.byId.set(body.id, body);
|
|
return body;
|
|
}
|
|
|
|
function baseBody(o) {
|
|
return {
|
|
id: 0,
|
|
x: o.x, y: o.y, angle: o.angle ?? 0,
|
|
vx: 0, vy: 0, omega: 0,
|
|
// Pseudo-velocity for the split-impulse position solve. Accumulated during
|
|
// the bias pass, added into the position integration, then zeroed. It never
|
|
// survives a substep, so it can't inject energy into the real velocity.
|
|
psx: 0, psy: 0, psw: 0,
|
|
friction: o.friction ?? 0.5,
|
|
restitution: o.restitution ?? 0.1,
|
|
isStatic: !!o.isStatic,
|
|
sleeping: false,
|
|
sleepTimer: 0,
|
|
// Free-form pointer the rules layer hangs blocks/pigs/birds off. The solver
|
|
// never reads it, which is what keeps this file game-agnostic.
|
|
userData: o.userData ?? null,
|
|
aabb: { minx: 0, miny: 0, maxx: 0, maxy: 0 },
|
|
};
|
|
}
|
|
|
|
/** Convex polygon from local-space verts (any winding, recentred on centroid). */
|
|
export function addPoly(world, o) {
|
|
let verts = o.verts.map(([x, y]) => [x, y]);
|
|
if (signedArea(verts) < 0) verts.reverse();
|
|
const md = polyMassData(verts, o.density ?? 1);
|
|
// Recentre so the body origin IS the centre of mass; the solver assumes it.
|
|
verts = verts.map(([x, y]) => [x - md.cx, y - md.cy]);
|
|
|
|
const b = baseBody(o);
|
|
b.kind = 'poly';
|
|
b.verts = verts;
|
|
b.normals = verts.map(([x0, y0], i) => {
|
|
const [x1, y1] = verts[(i + 1) % verts.length];
|
|
const ex = x1 - x0;
|
|
const ey = y1 - y0;
|
|
const len = Math.hypot(ex, ey) || 1;
|
|
return [ey / len, -ex / len];
|
|
});
|
|
b.wverts = verts.map(() => [0, 0]);
|
|
b.wnormals = verts.map(() => [0, 0]);
|
|
b.radius = Math.max(...verts.map(([x, y]) => Math.hypot(x, y)));
|
|
b.invMass = o.isStatic ? 0 : 1 / md.mass;
|
|
b.invI = o.isStatic ? 0 : 1 / md.I;
|
|
b.mass = o.isStatic ? Infinity : md.mass;
|
|
addBody(world, b);
|
|
syncTransform(b);
|
|
return b;
|
|
}
|
|
|
|
/** Axis-aligned-at-rest box helper; `angle` still rotates it. */
|
|
export function addBox(world, o) {
|
|
const hw = o.w / 2;
|
|
const hh = o.h / 2;
|
|
return addPoly(world, {
|
|
...o,
|
|
verts: [[-hw, -hh], [hw, -hh], [hw, hh], [-hw, hh]],
|
|
});
|
|
}
|
|
|
|
export function addCircle(world, o) {
|
|
const b = baseBody(o);
|
|
b.kind = 'circle';
|
|
b.radius = o.r;
|
|
const mass = (o.density ?? 1) * Math.PI * o.r * o.r;
|
|
b.invMass = o.isStatic ? 0 : 1 / mass;
|
|
b.invI = o.isStatic ? 0 : 1 / (0.5 * mass * o.r * o.r);
|
|
b.mass = o.isStatic ? Infinity : mass;
|
|
addBody(world, b);
|
|
syncTransform(b);
|
|
return b;
|
|
}
|
|
|
|
export function removeBody(world, body) {
|
|
const i = world.bodies.indexOf(body);
|
|
if (i >= 0) world.bodies.splice(i, 1);
|
|
world.byId.delete(body.id);
|
|
for (const [key, c] of [...world.contacts]) {
|
|
if (c.aId === body.id || c.bId === body.id) world.contacts.delete(key);
|
|
}
|
|
world.contactList = world.contactList.filter((c) => c.aId !== body.id && c.bId !== body.id);
|
|
}
|
|
|
|
// Recompute world-space verts/normals and the AABB after a transform change.
|
|
function syncTransform(b) {
|
|
const c = Math.cos(b.angle);
|
|
const s = Math.sin(b.angle);
|
|
if (b.kind === 'poly') {
|
|
let minx = Infinity; let miny = Infinity; let maxx = -Infinity; let maxy = -Infinity;
|
|
for (let i = 0; i < b.verts.length; i += 1) {
|
|
const [lx, ly] = b.verts[i];
|
|
const wx = b.x + lx * c - ly * s;
|
|
const wy = b.y + lx * s + ly * c;
|
|
b.wverts[i][0] = wx;
|
|
b.wverts[i][1] = wy;
|
|
const [nx, ny] = b.normals[i];
|
|
b.wnormals[i][0] = nx * c - ny * s;
|
|
b.wnormals[i][1] = nx * s + ny * c;
|
|
if (wx < minx) minx = wx;
|
|
if (wy < miny) miny = wy;
|
|
if (wx > maxx) maxx = wx;
|
|
if (wy > maxy) maxy = wy;
|
|
}
|
|
b.aabb.minx = minx; b.aabb.miny = miny; b.aabb.maxx = maxx; b.aabb.maxy = maxy;
|
|
} else {
|
|
b.aabb.minx = b.x - b.radius; b.aabb.miny = b.y - b.radius;
|
|
b.aabb.maxx = b.x + b.radius; b.aabb.maxy = b.y + b.radius;
|
|
}
|
|
}
|
|
|
|
export function wakeBody(b) {
|
|
if (b.isStatic) return;
|
|
b.sleeping = false;
|
|
b.sleepTimer = 0;
|
|
}
|
|
|
|
export function applyImpulse(b, ix, iy, px, py) {
|
|
if (b.isStatic) return;
|
|
wakeBody(b);
|
|
b.vx += ix * b.invMass;
|
|
b.vy += iy * b.invMass;
|
|
if (px !== undefined) b.omega += cross(px - b.x, py - b.y, ix, iy) * b.invI;
|
|
}
|
|
|
|
// ── Broadphase ──────────────────────────────────────────────────────────────
|
|
|
|
// Uniform grid over AABBs, rebuilt each substep. Bodies here are large relative
|
|
// to their per-substep travel, so a rebuild is cheaper than incremental upkeep.
|
|
function broadphase(world) {
|
|
const cell = PHYS.GRID_CELL;
|
|
const grid = new Map();
|
|
const bodies = world.bodies;
|
|
for (let i = 0; i < bodies.length; i += 1) {
|
|
const b = bodies[i];
|
|
const x0 = Math.floor(b.aabb.minx / cell);
|
|
const x1 = Math.floor(b.aabb.maxx / cell);
|
|
const y0 = Math.floor(b.aabb.miny / cell);
|
|
const y1 = Math.floor(b.aabb.maxy / cell);
|
|
for (let gx = x0; gx <= x1; gx += 1) {
|
|
for (let gy = y0; gy <= y1; gy += 1) {
|
|
const key = cellKey(gx, gy);
|
|
let bucket = grid.get(key);
|
|
if (!bucket) { bucket = []; grid.set(key, bucket); }
|
|
bucket.push(b);
|
|
}
|
|
}
|
|
}
|
|
|
|
const pairs = [];
|
|
const seen = new Set();
|
|
for (const bucket of grid.values()) {
|
|
for (let i = 0; i < bucket.length; i += 1) {
|
|
for (let j = i + 1; j < bucket.length; j += 1) {
|
|
let a = bucket[i];
|
|
let b = bucket[j];
|
|
if (a.id > b.id) { const t = a; a = b; b = t; }
|
|
// A pair where neither side can move produces no useful contact. A
|
|
// sleeping body against a static one is already at rest by definition.
|
|
if (a.isStatic && b.isStatic) continue;
|
|
if (a.sleeping && b.sleeping) continue;
|
|
if (a.sleeping && b.isStatic) continue;
|
|
if (b.sleeping && a.isStatic) continue;
|
|
const key = pairKey(a.id, b.id);
|
|
if (seen.has(key)) continue;
|
|
seen.add(key);
|
|
if (a.aabb.maxx < b.aabb.minx || b.aabb.maxx < a.aabb.minx) continue;
|
|
if (a.aabb.maxy < b.aabb.miny || b.aabb.maxy < a.aabb.miny) continue;
|
|
pairs.push(a, b); // flat, to avoid a per-pair array allocation
|
|
}
|
|
}
|
|
}
|
|
// Sorted so the solve order never depends on Map/Set iteration order.
|
|
const order = [];
|
|
for (let i = 0; i < pairs.length; i += 2) order.push(i);
|
|
order.sort((p, q) => (pairs[p].id - pairs[q].id) || (pairs[p + 1].id - pairs[q + 1].id));
|
|
const sorted = [];
|
|
for (const i of order) sorted.push(pairs[i], pairs[i + 1]);
|
|
return sorted;
|
|
}
|
|
|
|
// ── Narrowphase ─────────────────────────────────────────────────────────────
|
|
|
|
// Largest separation of B's verts from any of A's faces. Negative => overlap.
|
|
function maxSeparation(a, b) {
|
|
let best = -Infinity;
|
|
let bestFace = 0;
|
|
for (let i = 0; i < a.wverts.length; i += 1) {
|
|
const [nx, ny] = a.wnormals[i];
|
|
const [vx, vy] = a.wverts[i];
|
|
// Support point of B in direction -n: the deepest vert against this face.
|
|
let lowest = Infinity;
|
|
for (let j = 0; j < b.wverts.length; j += 1) {
|
|
const d = (b.wverts[j][0] - vx) * nx + (b.wverts[j][1] - vy) * ny;
|
|
if (d < lowest) lowest = d;
|
|
}
|
|
if (lowest > best) { best = lowest; bestFace = i; }
|
|
}
|
|
return { sep: best, face: bestFace };
|
|
}
|
|
|
|
// The face of `inc` most anti-parallel to the reference normal.
|
|
function incidentFace(inc, refNx, refNy) {
|
|
let best = Infinity;
|
|
let bestFace = 0;
|
|
for (let i = 0; i < inc.wnormals.length; i += 1) {
|
|
const d = inc.wnormals[i][0] * refNx + inc.wnormals[i][1] * refNy;
|
|
if (d < best) { best = d; bestFace = i; }
|
|
}
|
|
return bestFace;
|
|
}
|
|
|
|
// Clip a segment against a half-plane, keeping the portion where
|
|
// dot(n, p) - offset <= 0. Feature ids ride along so warm starting can match
|
|
// points across frames even when clipping order changes.
|
|
function clipSegment(pts, nx, ny, offset) {
|
|
const out = [];
|
|
const d0 = pts[0].x * nx + pts[0].y * ny - offset;
|
|
const d1 = pts[1].x * nx + pts[1].y * ny - offset;
|
|
if (d0 <= 0) out.push(pts[0]);
|
|
if (d1 <= 0) out.push(pts[1]);
|
|
if (d0 * d1 < 0) {
|
|
const t = d0 / (d0 - d1);
|
|
out.push({
|
|
x: pts[0].x + t * (pts[1].x - pts[0].x),
|
|
y: pts[0].y + t * (pts[1].y - pts[0].y),
|
|
fid: d0 > 0 ? pts[1].fid : pts[0].fid,
|
|
});
|
|
}
|
|
return out;
|
|
}
|
|
|
|
function collidePolyPoly(a, b) {
|
|
const sa = maxSeparation(a, b);
|
|
if (sa.sep > 0) return null;
|
|
const sb = maxSeparation(b, a);
|
|
if (sb.sep > 0) return null;
|
|
|
|
// Prefer A as reference unless B is clearly deeper — the small bias keeps the
|
|
// choice stable frame to frame, which keeps feature ids (and warm starting)
|
|
// stable for a resting contact.
|
|
let ref = a; let inc = b; let refFace = sa.face; let flip = false;
|
|
if (sb.sep > sa.sep + 0.1 * Math.abs(sa.sep) + 0.01) {
|
|
ref = b; inc = a; refFace = sb.face; flip = true;
|
|
}
|
|
|
|
const [rnx, rny] = ref.wnormals[refFace];
|
|
const rv0 = ref.wverts[refFace];
|
|
const rv1 = ref.wverts[(refFace + 1) % ref.wverts.length];
|
|
|
|
const incFace = incidentFace(inc, rnx, rny);
|
|
const iv0 = inc.wverts[incFace];
|
|
const iv1 = inc.wverts[(incFace + 1) % inc.wverts.length];
|
|
|
|
// Tangent of the reference face; clip the incident edge to its side planes.
|
|
const tx = rv1[0] - rv0[0];
|
|
const ty = rv1[1] - rv0[1];
|
|
const tlen = Math.hypot(tx, ty) || 1;
|
|
const utx = tx / tlen;
|
|
const uty = ty / tlen;
|
|
|
|
let pts = [
|
|
{ x: iv0[0], y: iv0[1], fid: (refFace << 8) | incFace },
|
|
{ x: iv1[0], y: iv1[1], fid: (refFace << 8) | ((incFace + 1) % inc.wverts.length) },
|
|
];
|
|
pts = clipSegment(pts, -utx, -uty, -(rv0[0] * utx + rv0[1] * uty));
|
|
if (pts.length < 2) return null;
|
|
pts = clipSegment(pts, utx, uty, rv1[0] * utx + rv1[1] * uty);
|
|
if (pts.length < 2) return null;
|
|
|
|
const offset = rv0[0] * rnx + rv0[1] * rny;
|
|
const points = [];
|
|
for (const p of pts) {
|
|
const sep = p.x * rnx + p.y * rny - offset;
|
|
if (sep <= 0) points.push({ x: p.x, y: p.y, sep, fid: p.fid | (flip ? 0x10000 : 0) });
|
|
}
|
|
if (!points.length) return null;
|
|
|
|
// Normal must always point A -> B.
|
|
return { nx: flip ? -rnx : rnx, ny: flip ? -rny : rny, points };
|
|
}
|
|
|
|
function collideCirclePoly(c, p, circleIsA) {
|
|
// Work in the polygon's local frame so the face tests are cheap.
|
|
const cs = Math.cos(-p.angle);
|
|
const sn = Math.sin(-p.angle);
|
|
const dx = c.x - p.x;
|
|
const dy = c.y - p.y;
|
|
const lx = dx * cs - dy * sn;
|
|
const ly = dx * sn + dy * cs;
|
|
|
|
let best = -Infinity;
|
|
let bestFace = 0;
|
|
for (let i = 0; i < p.verts.length; i += 1) {
|
|
const [nx, ny] = p.normals[i];
|
|
const [vx, vy] = p.verts[i];
|
|
const d = (lx - vx) * nx + (ly - vy) * ny;
|
|
if (d > c.radius) return null;
|
|
if (d > best) { best = d; bestFace = i; }
|
|
}
|
|
|
|
const [v0x, v0y] = p.verts[bestFace];
|
|
const [v1x, v1y] = p.verts[(bestFace + 1) % p.verts.length];
|
|
let nlx; let nly; let sep;
|
|
|
|
if (best < 1e-6) {
|
|
// Centre is inside the polygon — push straight out along the closest face.
|
|
[nlx, nly] = p.normals[bestFace];
|
|
sep = best - c.radius;
|
|
} else {
|
|
// Voronoi regions: nearest to v0, to v1, or to the face interior.
|
|
const ex = v1x - v0x;
|
|
const ey = v1y - v0y;
|
|
const t = clamp(((lx - v0x) * ex + (ly - v0y) * ey) / (ex * ex + ey * ey), 0, 1);
|
|
const px = v0x + ex * t;
|
|
const py = v0y + ey * t;
|
|
const ddx = lx - px;
|
|
const ddy = ly - py;
|
|
const dist = Math.hypot(ddx, ddy);
|
|
if (dist > c.radius) return null;
|
|
if (dist < 1e-9) { [nlx, nly] = p.normals[bestFace]; } else { nlx = ddx / dist; nly = ddy / dist; }
|
|
sep = dist - c.radius;
|
|
}
|
|
|
|
// Back to world space. Local normal points polygon -> circle.
|
|
const wc = Math.cos(p.angle);
|
|
const ws = Math.sin(p.angle);
|
|
const nwx = nlx * wc - nly * ws;
|
|
const nwy = nlx * ws + nly * wc;
|
|
const contactX = c.x - nwx * (c.radius + sep * 0.5);
|
|
const contactY = c.y - nwy * (c.radius + sep * 0.5);
|
|
|
|
// Caller's A is `circleIsA ? c : p`; normal must run A -> B.
|
|
const sign = circleIsA ? -1 : 1;
|
|
return {
|
|
nx: nwx * sign, ny: nwy * sign,
|
|
points: [{ x: contactX, y: contactY, sep, fid: 1 }],
|
|
};
|
|
}
|
|
|
|
function collideCircleCircle(a, b) {
|
|
const dx = b.x - a.x;
|
|
const dy = b.y - a.y;
|
|
const r = a.radius + b.radius;
|
|
const d2 = dx * dx + dy * dy;
|
|
if (d2 >= r * r) return null;
|
|
const d = Math.sqrt(d2);
|
|
let nx; let ny;
|
|
if (d < 1e-9) { nx = 0; ny = -1; } else { nx = dx / d; ny = dy / d; }
|
|
return {
|
|
nx, ny,
|
|
points: [{ x: a.x + nx * a.radius, y: a.y + ny * a.radius, sep: d - r, fid: 1 }],
|
|
};
|
|
}
|
|
|
|
function collide(a, b) {
|
|
if (a.kind === 'circle' && b.kind === 'circle') return collideCircleCircle(a, b);
|
|
if (a.kind === 'circle') return collideCirclePoly(a, b, true);
|
|
if (b.kind === 'circle') return collideCirclePoly(b, a, false);
|
|
return collidePolyPoly(a, b);
|
|
}
|
|
|
|
// ── Contact bookkeeping ─────────────────────────────────────────────────────
|
|
|
|
// Rebuild manifolds, carrying accumulated impulses across from last substep by
|
|
// feature id. This is the warm-starting half of stack stability; matching by
|
|
// array index instead would scramble impulses the moment a clip order flips.
|
|
function updateContacts(world, pairs) {
|
|
const live = new Set();
|
|
const list = [];
|
|
for (let pi = 0; pi < pairs.length; pi += 2) {
|
|
const a = pairs[pi];
|
|
const b = pairs[pi + 1];
|
|
const m = collide(a, b);
|
|
const key = pairKey(a.id, b.id);
|
|
if (!m) { world.contacts.delete(key); continue; }
|
|
live.add(key);
|
|
|
|
const prev = world.contacts.get(key);
|
|
const points = m.points.map((p) => {
|
|
const old = prev?.points.find((q) => q.fid === p.fid);
|
|
return {
|
|
x: p.x, y: p.y, sep: p.sep, fid: p.fid,
|
|
pn: old?.pn ?? 0, pt: old?.pt ?? 0,
|
|
pnBias: 0, // never warm-started: position error is transient
|
|
massNormal: 0, massTangent: 0, bias: 0, restTarget: 0,
|
|
rax: 0, ray: 0, rbx: 0, rby: 0,
|
|
};
|
|
});
|
|
const contact = {
|
|
aId: a.id, bId: b.id, a, b,
|
|
nx: m.nx, ny: m.ny,
|
|
friction: Math.sqrt(a.friction * b.friction),
|
|
restitution: Math.max(a.restitution, b.restitution),
|
|
points,
|
|
maxImpulse: 0, // peak accumulated |pn| — includes static load
|
|
impactImpulse: 0, // peak approach momentum — THIS is the damage signal
|
|
};
|
|
world.contacts.set(key, contact);
|
|
list.push(contact);
|
|
}
|
|
for (const [key] of [...world.contacts]) {
|
|
if (!live.has(key)) world.contacts.delete(key);
|
|
}
|
|
// Pairs arrive pre-sorted by (aId, bId), so this list is already in the
|
|
// canonical solve order and needs no further sorting.
|
|
world.contactList = list;
|
|
}
|
|
|
|
// The solver's contact iteration order. Never iterate world.contacts directly:
|
|
// Map order depends on insertion history, which would make the sim depend on
|
|
// the order bodies happened to be created in.
|
|
function orderedContacts(world) {
|
|
return world.contactList;
|
|
}
|
|
|
|
function prestep(world, h) {
|
|
const invH = 1 / h;
|
|
for (const c of orderedContacts(world)) {
|
|
const a = c.a;
|
|
const b = c.b;
|
|
const { nx, ny } = c;
|
|
const tx = -ny;
|
|
const ty = nx;
|
|
let peakImpact = 0;
|
|
|
|
for (const p of c.points) {
|
|
p.rax = p.x - a.x; p.ray = p.y - a.y;
|
|
p.rbx = p.x - b.x; p.rby = p.y - b.y;
|
|
|
|
const rnA = cross(p.rax, p.ray, nx, ny);
|
|
const rnB = cross(p.rbx, p.rby, nx, ny);
|
|
p.massNormal = 1 / (a.invMass + b.invMass + a.invI * rnA * rnA + b.invI * rnB * rnB);
|
|
|
|
const rtA = cross(p.rax, p.ray, tx, ty);
|
|
const rtB = cross(p.rbx, p.rby, tx, ty);
|
|
p.massTangent = 1 / (a.invMass + b.invMass + a.invI * rtA * rtA + b.invI * rtB * rtB);
|
|
|
|
// Position error is corrected by a SEPARATE pseudo-velocity pass (split
|
|
// impulse), not by biasing the real velocity. With plain Baumgarte the
|
|
// steady-state penetration is proportional to the load, so the bottom
|
|
// contact of a 10-box tower sinks ~10x deeper than the top one and the
|
|
// stack visibly sags. Driving position through pseudo-velocities removes
|
|
// a fixed fraction of the excess per substep regardless of load, and the
|
|
// correction never leaks into the real velocity as bounce energy.
|
|
p.bias = -PHYS.BAUMGARTE * invH * Math.min(0, p.sep + PHYS.SLOP);
|
|
p.pnBias = 0;
|
|
|
|
// Restitution, sampled once from the APPROACH velocity. Sampling it every
|
|
// iteration would let a resting box slowly bounce itself apart.
|
|
const rvx = (b.vx - p.rby * b.omega) - (a.vx - p.ray * a.omega);
|
|
const rvy = (b.vy + p.rbx * b.omega) - (a.vy + p.rax * a.omega);
|
|
const vn = rvx * nx + rvy * ny;
|
|
p.restTarget = vn < -PHYS.REST_THRESHOLD ? -c.restitution * vn : 0;
|
|
|
|
// IMPACT momentum: the impulse it would take to just stop the approach.
|
|
//
|
|
// This is the damage signal, and it must NOT be the solver's accumulated
|
|
// normal impulse. That value includes the static load a contact carries,
|
|
// which grows with stack height — measured 3.4e5 at the base of a
|
|
// 10-block stone tower, above the threshold that should shatter wood. A
|
|
// tower would quietly crush itself while nothing was happening. Approach
|
|
// velocity is zero at rest by definition, so this separates "being leaned
|
|
// on" from "being hit" for free.
|
|
const impact = vn < 0 ? p.massNormal * -vn : 0;
|
|
if (impact > peakImpact) peakImpact = impact;
|
|
}
|
|
c.impactImpulse = peakImpact;
|
|
}
|
|
for (const b of world.bodies) { b.psx = 0; b.psy = 0; b.psw = 0; }
|
|
}
|
|
|
|
function warmStart(world) {
|
|
for (const c of orderedContacts(world)) {
|
|
const a = c.a;
|
|
const b = c.b;
|
|
const { nx, ny } = c;
|
|
const tx = -ny;
|
|
const ty = nx;
|
|
for (const p of c.points) {
|
|
const ix = p.pn * nx + p.pt * tx;
|
|
const iy = p.pn * ny + p.pt * ty;
|
|
if (!a.isStatic) {
|
|
a.vx -= ix * a.invMass; a.vy -= iy * a.invMass;
|
|
a.omega -= cross(p.rax, p.ray, ix, iy) * a.invI;
|
|
}
|
|
if (!b.isStatic) {
|
|
b.vx += ix * b.invMass; b.vy += iy * b.invMass;
|
|
b.omega += cross(p.rbx, p.rby, ix, iy) * b.invI;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
function solveVelocities(world) {
|
|
const contacts = orderedContacts(world);
|
|
for (let iter = 0; iter < PHYS.VEL_ITERS; iter += 1) {
|
|
for (const c of contacts) {
|
|
const a = c.a;
|
|
const b = c.b;
|
|
const { nx, ny } = c;
|
|
const tx = -ny;
|
|
const ty = nx;
|
|
|
|
for (const p of c.points) {
|
|
// Normal impulse. Accumulate then clamp to >= 0, applying only the
|
|
// delta — clamping the per-iteration impulse instead would let a
|
|
// contact pull bodies together.
|
|
let rvx = (b.vx - p.rby * b.omega) - (a.vx - p.ray * a.omega);
|
|
let rvy = (b.vy + p.rbx * b.omega) - (a.vy + p.rax * a.omega);
|
|
const vn = rvx * nx + rvy * ny;
|
|
let dPn = p.massNormal * (-vn + p.restTarget);
|
|
const pn0 = p.pn;
|
|
p.pn = Math.max(pn0 + dPn, 0);
|
|
dPn = p.pn - pn0;
|
|
|
|
let ix = dPn * nx;
|
|
let iy = dPn * ny;
|
|
if (!a.isStatic) {
|
|
a.vx -= ix * a.invMass; a.vy -= iy * a.invMass;
|
|
a.omega -= cross(p.rax, p.ray, ix, iy) * a.invI;
|
|
}
|
|
if (!b.isStatic) {
|
|
b.vx += ix * b.invMass; b.vy += iy * b.invMass;
|
|
b.omega += cross(p.rbx, p.rby, ix, iy) * b.invI;
|
|
}
|
|
|
|
// Split-impulse position pass, run on pseudo-velocities only.
|
|
const pvx = (b.psx - p.rby * b.psw) - (a.psx - p.ray * a.psw);
|
|
const pvy = (b.psy + p.rbx * b.psw) - (a.psy + p.rax * a.psw);
|
|
const pvn = pvx * nx + pvy * ny;
|
|
let dPb = p.massNormal * (-pvn + p.bias);
|
|
const pb0 = p.pnBias;
|
|
p.pnBias = Math.max(pb0 + dPb, 0);
|
|
dPb = p.pnBias - pb0;
|
|
|
|
const bx = dPb * nx;
|
|
const by = dPb * ny;
|
|
if (!a.isStatic) {
|
|
a.psx -= bx * a.invMass; a.psy -= by * a.invMass;
|
|
a.psw -= cross(p.rax, p.ray, bx, by) * a.invI;
|
|
}
|
|
if (!b.isStatic) {
|
|
b.psx += bx * b.invMass; b.psy += by * b.invMass;
|
|
b.psw += cross(p.rbx, p.rby, bx, by) * b.invI;
|
|
}
|
|
|
|
// Coulomb friction, clamped against the ACCUMULATED normal impulse.
|
|
rvx = (b.vx - p.rby * b.omega) - (a.vx - p.ray * a.omega);
|
|
rvy = (b.vy + p.rbx * b.omega) - (a.vy + p.rax * a.omega);
|
|
const vt = rvx * tx + rvy * ty;
|
|
let dPt = p.massTangent * -vt;
|
|
const maxPt = c.friction * p.pn;
|
|
const pt0 = p.pt;
|
|
p.pt = clamp(pt0 + dPt, -maxPt, maxPt);
|
|
dPt = p.pt - pt0;
|
|
|
|
ix = dPt * tx;
|
|
iy = dPt * ty;
|
|
if (!a.isStatic) {
|
|
a.vx -= ix * a.invMass; a.vy -= iy * a.invMass;
|
|
a.omega -= cross(p.rax, p.ray, ix, iy) * a.invI;
|
|
}
|
|
if (!b.isStatic) {
|
|
b.vx += ix * b.invMass; b.vy += iy * b.invMass;
|
|
b.omega += cross(p.rbx, p.rby, ix, iy) * b.invI;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
for (const c of contacts) {
|
|
let peak = 0;
|
|
for (const p of c.points) if (p.pn > peak) peak = p.pn;
|
|
c.maxImpulse = peak;
|
|
}
|
|
}
|
|
|
|
// ── Islands and sleeping ────────────────────────────────────────────────────
|
|
|
|
// Union-find over contacts between dynamic bodies. An island sleeps only when
|
|
// every member has been slow for SLEEP_TIME — per-body sleeping would freeze
|
|
// half a tower while the rest still moves.
|
|
function updateSleeping(world, h) {
|
|
const parent = new Map();
|
|
const find = (x) => {
|
|
let r = x;
|
|
while (parent.get(r) !== r) r = parent.get(r);
|
|
while (parent.get(x) !== r) { const nx = parent.get(x); parent.set(x, r); x = nx; }
|
|
return r;
|
|
};
|
|
const union = (x, y) => {
|
|
const rx = find(x);
|
|
const ry = find(y);
|
|
if (rx !== ry) parent.set(rx > ry ? rx : ry, rx > ry ? ry : rx);
|
|
};
|
|
|
|
for (const b of world.bodies) if (!b.isStatic) parent.set(b.id, b.id);
|
|
for (const c of orderedContacts(world)) {
|
|
const a = c.a;
|
|
const b = c.b;
|
|
if (!a.isStatic && !b.isStatic) union(a.id, b.id);
|
|
}
|
|
|
|
// Per-body slow timer.
|
|
for (const b of world.bodies) {
|
|
if (b.isStatic || b.sleeping) continue;
|
|
const slow = Math.hypot(b.vx, b.vy) < PHYS.SLEEP_LIN && Math.abs(b.omega) < PHYS.SLEEP_ANG;
|
|
b.sleepTimer = slow ? b.sleepTimer + h : 0;
|
|
}
|
|
|
|
// An island's timer is its slowest member's.
|
|
const islandTimer = new Map();
|
|
for (const b of world.bodies) {
|
|
if (b.isStatic) continue;
|
|
const root = find(b.id);
|
|
const t = b.sleeping ? Infinity : b.sleepTimer;
|
|
const cur = islandTimer.get(root);
|
|
if (cur === undefined || t < cur) islandTimer.set(root, t);
|
|
}
|
|
|
|
for (const b of world.bodies) {
|
|
if (b.isStatic) continue;
|
|
const t = islandTimer.get(find(b.id)) ?? 0;
|
|
if (t >= PHYS.SLEEP_TIME) {
|
|
b.sleeping = true;
|
|
b.vx = 0; b.vy = 0; b.omega = 0;
|
|
} else if (t < PHYS.SLEEP_TIME && b.sleeping) {
|
|
// A neighbour woke up; the whole island comes with it.
|
|
b.sleeping = false;
|
|
b.sleepTimer = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// ── Integration ─────────────────────────────────────────────────────────────
|
|
|
|
function integrateVelocities(world, h) {
|
|
for (const b of world.bodies) {
|
|
if (b.isStatic || b.sleeping) continue;
|
|
b.vy += world.gravity * h;
|
|
b.vx *= 1 - PHYS.LINEAR_DAMPING;
|
|
b.vy *= 1 - PHYS.LINEAR_DAMPING;
|
|
b.omega *= 1 - PHYS.ANGULAR_DAMPING;
|
|
}
|
|
}
|
|
|
|
function integratePositions(world, h) {
|
|
for (const b of world.bodies) {
|
|
if (b.isStatic || b.sleeping) continue;
|
|
const sp = Math.hypot(b.vx, b.vy);
|
|
if (sp > PHYS.MAX_SPEED) { const k = PHYS.MAX_SPEED / sp; b.vx *= k; b.vy *= k; }
|
|
b.omega = clamp(b.omega, -PHYS.MAX_OMEGA, PHYS.MAX_OMEGA);
|
|
// Pseudo-velocity moves the body but is then discarded, so position error
|
|
// is repaired without the correction showing up as momentum next substep.
|
|
b.x += (b.vx + b.psx) * h;
|
|
b.y += (b.vy + b.psy) * h;
|
|
b.angle += (b.omega + b.psw) * h;
|
|
b.psx = 0; b.psy = 0; b.psw = 0;
|
|
syncTransform(b);
|
|
}
|
|
}
|
|
|
|
// ── The frame ───────────────────────────────────────────────────────────────
|
|
|
|
export function substep(world, h) {
|
|
integrateVelocities(world, h);
|
|
const pairs = broadphase(world);
|
|
updateContacts(world, pairs);
|
|
prestep(world, h);
|
|
warmStart(world);
|
|
solveVelocities(world);
|
|
integratePositions(world, h);
|
|
updateSleeping(world, h);
|
|
world.time += h;
|
|
}
|
|
|
|
/**
|
|
* Advance by `dt` seconds in whole fixed substeps. Any leftover is carried in
|
|
* world.accum, which is what makes 1/60 and 1/120 frames produce identical
|
|
* trajectories — the property tools/verifyAngryBirds.js asserts.
|
|
*/
|
|
export function step(world, dt) {
|
|
world.accum += Math.min(dt, 0.05); // a backgrounded tab must not teleport
|
|
let steps = 0;
|
|
while (world.accum >= PHYS.SUBSTEP_DT && steps < 16) {
|
|
substep(world, PHYS.SUBSTEP_DT);
|
|
world.accum -= PHYS.SUBSTEP_DT;
|
|
steps += 1;
|
|
}
|
|
return steps;
|
|
}
|
|
|
|
/** True when nothing is moving — the rules layer's "shot is over" test. */
|
|
export function isSettled(world) {
|
|
for (const b of world.bodies) {
|
|
if (b.isStatic) continue;
|
|
if (!b.sleeping) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/** Peak normal impulse seen on each body this substep, keyed by body id. */
|
|
export function contactImpulses(world) {
|
|
const out = new Map();
|
|
for (const c of world.contacts.values()) {
|
|
if (c.maxImpulse <= 0) continue;
|
|
for (const id of [c.aId, c.bId]) {
|
|
const cur = out.get(id) ?? 0;
|
|
if (c.maxImpulse > cur) out.set(id, c.maxImpulse);
|
|
}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
/** Radial impulse + falloff, used by Bomb and TNT. Returns bodies affected. */
|
|
export function applyExplosion(world, x, y, radius, power) {
|
|
const hit = [];
|
|
for (const b of world.bodies) {
|
|
if (b.isStatic) continue;
|
|
const dx = b.x - x;
|
|
const dy = b.y - y;
|
|
const d = Math.hypot(dx, dy);
|
|
if (d > radius) continue;
|
|
const falloff = 1 - d / radius;
|
|
const nx = d < 1e-6 ? 0 : dx / d;
|
|
const ny = d < 1e-6 ? -1 : dy / d;
|
|
const j = power * falloff * b.mass;
|
|
applyImpulse(b, nx * j, ny * j, b.x, b.y);
|
|
hit.push({ body: b, falloff, dist: d });
|
|
}
|
|
return hit;
|
|
}
|
|
|
|
// ── Determinism helpers ─────────────────────────────────────────────────────
|
|
|
|
export function cloneWorld(world) {
|
|
const copy = createWorld({ gravity: world.gravity });
|
|
copy.nextId = world.nextId;
|
|
copy.accum = world.accum;
|
|
copy.time = world.time;
|
|
for (const b of world.bodies) {
|
|
const nb = { ...b, aabb: { ...b.aabb } };
|
|
if (b.kind === 'poly') {
|
|
nb.verts = b.verts.map((v) => [v[0], v[1]]);
|
|
nb.normals = b.normals.map((v) => [v[0], v[1]]);
|
|
nb.wverts = b.wverts.map((v) => [v[0], v[1]]);
|
|
nb.wnormals = b.wnormals.map((v) => [v[0], v[1]]);
|
|
}
|
|
copy.bodies.push(nb);
|
|
copy.byId.set(nb.id, nb);
|
|
}
|
|
// Contacts cache direct body references for speed, so a shallow spread would
|
|
// leave the clone's contacts pointing at the ORIGINAL world's bodies — the
|
|
// clone would silently drive the thing it was supposed to leave untouched.
|
|
// Rewire every reference through the copy's id map.
|
|
for (const [key, c] of world.contacts) {
|
|
copy.contacts.set(key, {
|
|
...c,
|
|
a: copy.byId.get(c.aId),
|
|
b: copy.byId.get(c.bId),
|
|
points: c.points.map((p) => ({ ...p })),
|
|
});
|
|
}
|
|
copy.contactList = world.contactList.map((c) => copy.contacts.get(pairKey(c.aId, c.bId))).filter(Boolean);
|
|
return copy;
|
|
}
|
|
|
|
// FNV-1a over the Float64 bit patterns of every body's transform and velocity.
|
|
// Hashing the bits rather than rounded values means a 1-ULP divergence still
|
|
// shows up, which is the point.
|
|
export function hashWorld(world) {
|
|
const buf = new ArrayBuffer(8);
|
|
const f64 = new Float64Array(buf);
|
|
const u32 = new Uint32Array(buf);
|
|
let h = 0x811c9dc5;
|
|
const mix = (v) => {
|
|
f64[0] = v;
|
|
for (let i = 0; i < 2; i += 1) {
|
|
h ^= u32[i];
|
|
h = Math.imul(h, 0x01000193) >>> 0;
|
|
}
|
|
};
|
|
const sorted = [...world.bodies].sort((a, b) => a.id - b.id);
|
|
for (const b of sorted) {
|
|
mix(b.x); mix(b.y); mix(b.angle);
|
|
mix(b.vx); mix(b.vy); mix(b.omega);
|
|
mix(b.sleeping ? 1 : 0);
|
|
}
|
|
return h >>> 0;
|
|
}
|
|
|
|
/** Run until everything sleeps (or `maxSeconds` elapses). Used by the editor. */
|
|
export function settle(world, maxSeconds = 10) {
|
|
const limit = Math.ceil(maxSeconds / PHYS.SUBSTEP_DT);
|
|
for (let i = 0; i < limit; i += 1) {
|
|
substep(world, PHYS.SUBSTEP_DT);
|
|
if (isSettled(world)) return true;
|
|
}
|
|
return isSettled(world);
|
|
}
|