561 lines
21 KiB
TypeScript
561 lines
21 KiB
TypeScript
import type {
|
||
Collider,
|
||
ColliderDesc,
|
||
EventQueue,
|
||
RigidBody,
|
||
World as RapierWorld,
|
||
} from '@dimforge/rapier2d-compat';
|
||
import { isHandleEnabled, worldMatrixOf } from '../../core/graph';
|
||
import { apply, determinant, IDENTITY, invert, type Mat } from '../../core/model/matrix';
|
||
import type { Vec2 } from '../../core/model/Properties';
|
||
import { parentChain } from '../../core/model/walk';
|
||
import { type ColliderShape, isTrigger, shapesOverlap } from '../../core/properties/collider';
|
||
import { hasTransformProps } from '../../core/properties/PropertySchema';
|
||
import {
|
||
type BodyType,
|
||
colliderMaterial,
|
||
type RigidbodySettings,
|
||
rigidbodySettings,
|
||
sceneGravity,
|
||
} from '../../core/properties/rigidbody';
|
||
import type { Entity } from '../Entity';
|
||
import type { World } from '../World';
|
||
import {
|
||
bodyOf,
|
||
type CollisionPair,
|
||
type CollisionSource,
|
||
partsOf,
|
||
shapeOf,
|
||
Solids,
|
||
} from '../systems/solids';
|
||
import type { System } from '../systems/System';
|
||
import { type Rapier, toMetres, toPixels } from './engine';
|
||
|
||
/** The engine always steps this long, whatever the frame took. */
|
||
export const PHYSICS_STEP = 1 / 60;
|
||
/** After a hitch the world catches up this many steps at most, and forgets the rest. */
|
||
const MAX_STEPS_PER_TICK = 8;
|
||
/** A pose that moved less than this (world units, radians) was not moved by anyone. */
|
||
const POSE_EPSILON = 1e-6;
|
||
|
||
interface Pose {
|
||
x: number;
|
||
y: number;
|
||
angle: number;
|
||
}
|
||
|
||
/**
|
||
* One body in the engine. `rigid` is an object with a rigidbody child, carrying every collider below
|
||
* it; `free` is a collider of an object without one, a body of its own that goes where the object
|
||
* is put; `tilemap` is a placed map with a box per merged rectangle of its solid cells.
|
||
*/
|
||
interface Body {
|
||
kind: 'rigid' | 'free' | 'tilemap';
|
||
body: RigidBody;
|
||
colliders: Collider[];
|
||
/** What the colliders were built from; they are built again when it changes (a scaled object). */
|
||
shapeKey: string;
|
||
settingsKey: string;
|
||
/** Whether the engine may turn it – what `writeBack` hands on as the object's rotation. */
|
||
turns: boolean;
|
||
/** The mass it carries of its own because no collider gives it one (kg, 0 = none). */
|
||
ownMass: number;
|
||
/** Where the pose was last put, by us or by the object moving. */
|
||
placed: Pose;
|
||
/** A dynamic body's pose before and after the last step, for drawing in between. */
|
||
previous: Pose;
|
||
current: Pose;
|
||
}
|
||
|
||
/** An engine collider: the entity it stands for and its world shape at the last sync. */
|
||
interface Part {
|
||
entity: Entity;
|
||
shape: ColliderShape;
|
||
owner: Body;
|
||
}
|
||
|
||
/**
|
||
* The played game's physics on Rapier (#146): rigidbodies fall, bounce and rest, and the collision
|
||
* events come from the engine's contact pairs. Ticked first, like the collision system it stands in
|
||
* for, so the behaviors see this frame's events.
|
||
*
|
||
* What has no rigidbody keeps behaving as it did before the engine: a collider is a body of its own
|
||
* that goes wherever the object is put, and whether two such colliders touch is decided by the
|
||
* editor's own overlap test – the engine only proposes the pairs. Touching without overlapping is
|
||
* no collision, as it never was. Only where a dynamic body is involved does the engine's contact say it.
|
||
*/
|
||
export class PhysicsSystem implements System, CollisionSource {
|
||
enters: CollisionPair[] = [];
|
||
stays: CollisionPair[] = [];
|
||
exits: CollisionPair[] = [];
|
||
private engine: RapierWorld | null = null;
|
||
private queue: EventQueue | null = null;
|
||
private readonly bodies = new Map<Entity, Body>();
|
||
private readonly parts = new Map<number, Part>();
|
||
/** Collider handle pairs the engine reports in contact or intersecting, keyed `h1|h2`. */
|
||
private readonly candidates = new Map<string, [number, number]>();
|
||
private current = new Map<string, CollisionPair>();
|
||
private readonly solids = new Solids();
|
||
private backlog = 0;
|
||
|
||
constructor(private readonly rapier: Rapier) {}
|
||
|
||
start(world: World): void {
|
||
const engine = new this.rapier.World(this.gravity(world));
|
||
engine.timestep = PHYSICS_STEP;
|
||
this.engine = engine;
|
||
this.queue = new this.rapier.EventQueue(true);
|
||
}
|
||
|
||
stop(): void {
|
||
this.engine?.free();
|
||
this.queue?.free();
|
||
this.engine = null;
|
||
this.queue = null;
|
||
this.bodies.clear();
|
||
this.parts.clear();
|
||
this.candidates.clear();
|
||
this.current = new Map();
|
||
this.enters = [];
|
||
this.stays = [];
|
||
this.exits = [];
|
||
this.backlog = 0;
|
||
}
|
||
|
||
update(world: World, dt: number): void {
|
||
const engine = this.engine;
|
||
if (!engine) return;
|
||
this.solids.reset();
|
||
this.enters = [];
|
||
this.exits = [];
|
||
this.sync(world);
|
||
engine.gravity = this.gravity(world);
|
||
this.backlog += dt;
|
||
let steps = 0;
|
||
const entered = new Set<string>();
|
||
while (this.backlog >= PHYSICS_STEP - 1e-9 && steps < MAX_STEPS_PER_TICK) {
|
||
for (const body of this.bodies.values()) if (body.body.isDynamic()) body.previous = body.current;
|
||
engine.step(this.queue!);
|
||
for (const body of this.bodies.values()) if (body.body.isDynamic()) body.current = poseOf(body.body);
|
||
this.collect(world, entered);
|
||
this.backlog -= PHYSICS_STEP;
|
||
steps++;
|
||
}
|
||
this.backlog = steps === MAX_STEPS_PER_TICK ? 0 : Math.max(0, this.backlog);
|
||
this.writeBack(this.backlog / PHYSICS_STEP);
|
||
this.stays = [...this.current].filter(([key]) => !entered.has(key)).map(([, pair]) => pair);
|
||
}
|
||
|
||
/** Whether any collider of `a` is in contact with any collider of `b` as of the last step. */
|
||
touching(world: World, a: Entity, b: Entity): boolean {
|
||
for (const ca of partsOf(world, a))
|
||
for (const cb of partsOf(world, b))
|
||
if (this.current.has(`${ca.id}|${cb.id}`) || this.current.has(`${cb.id}|${ca.id}`)) return true;
|
||
return false;
|
||
}
|
||
|
||
moveAndSlide(
|
||
world: World,
|
||
entity: Entity,
|
||
dx: number,
|
||
dy: number,
|
||
): { blockedX: boolean; blockedY: boolean } {
|
||
return this.solids.moveAndSlide(world, entity, dx, dy);
|
||
}
|
||
|
||
/** A dynamic body's velocity in world units per second; null for an object the engine does not move. */
|
||
velocityOf(owner: Entity): Vec2 | null {
|
||
const body = this.bodies.get(owner);
|
||
if (!body || body.kind !== 'rigid' || !body.body.isDynamic()) return null;
|
||
const v = body.body.linvel();
|
||
return { x: toPixels(v.x), y: toPixels(v.y) };
|
||
}
|
||
|
||
/** Sets a dynamic body's velocity (world units per second); false when the object has none. */
|
||
setVelocity(owner: Entity, velocity: Vec2): boolean {
|
||
const body = this.bodies.get(owner);
|
||
if (!body || body.kind !== 'rigid' || !body.body.isDynamic()) return false;
|
||
body.body.setLinvel({ x: toMetres(velocity.x), y: toMetres(velocity.y) }, true);
|
||
return true;
|
||
}
|
||
|
||
private gravity(world: World): Vec2 {
|
||
const g = sceneGravity(world.sceneProps);
|
||
return { x: toMetres(g.x), y: toMetres(g.y) };
|
||
}
|
||
|
||
/** Makes the engine's bodies match the entities: new ones in, gone ones out, moved ones moved. */
|
||
private sync(world: World): void {
|
||
const wanted = this.wantedBodies(world);
|
||
for (const [key, body] of this.bodies) {
|
||
const want = wanted.get(key);
|
||
if (!want || want.kind !== body.kind) this.remove(key, body);
|
||
}
|
||
for (const [key, want] of wanted) {
|
||
let body = this.bodies.get(key);
|
||
if (!body) {
|
||
body = this.create(want, key);
|
||
this.bodies.set(key, body);
|
||
}
|
||
this.place(body, want, key);
|
||
this.build(body, want, key, world);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Every body the entities ask for, keyed by the entity that stands for it. A collider belongs to
|
||
* the nearest object above it with a rigidbody switched on; without one it is a body of its own.
|
||
*/
|
||
private wantedBodies(world: World): Map<Entity, Wanted> {
|
||
const owners = new Map<Entity, Entity>();
|
||
for (const e of world.entities)
|
||
if (
|
||
e.type === 'rigidbody' &&
|
||
e.parent &&
|
||
e.parent.type !== 'tilemap' &&
|
||
!owners.has(e.parent) &&
|
||
isHandleEnabled(e)
|
||
)
|
||
owners.set(e.parent, e);
|
||
const wanted = new Map<Entity, Wanted>();
|
||
for (const [owner, rigidbody] of owners)
|
||
wanted.set(owner, { kind: 'rigid', settings: rigidbodySettings(rigidbody.props), colliders: [] });
|
||
for (const e of world.entities) {
|
||
if (e.type === 'tilemap') wanted.set(e, { kind: 'tilemap', settings: null, colliders: [] });
|
||
if (e.type !== 'collider') continue;
|
||
const owner = [...parentChain(e, (x) => x.parent)].slice(1).find((x) => owners.has(x));
|
||
if (owner) wanted.get(owner)!.colliders.push(e);
|
||
else wanted.set(e, { kind: 'free', settings: null, colliders: [e] });
|
||
}
|
||
return wanted;
|
||
}
|
||
|
||
private create(want: Wanted, key: Entity): Body {
|
||
const R = this.rapier;
|
||
const pose = this.poseWanted(want, key);
|
||
const desc =
|
||
want.kind === 'tilemap'
|
||
? R.RigidBodyDesc.fixed()
|
||
: want.kind === 'free'
|
||
? R.RigidBodyDesc.kinematicPositionBased()
|
||
: want.settings!.type === 'dynamic'
|
||
? R.RigidBodyDesc.dynamic()
|
||
: want.settings!.type === 'static'
|
||
? R.RigidBodyDesc.fixed()
|
||
: R.RigidBodyDesc.kinematicPositionBased();
|
||
desc.setTranslation(toMetres(pose.x), toMetres(pose.y)).setRotation(pose.angle);
|
||
return {
|
||
kind: want.kind,
|
||
body: this.engine!.createRigidBody(desc),
|
||
colliders: [],
|
||
shapeKey: '',
|
||
settingsKey: '',
|
||
turns: false,
|
||
ownMass: 0,
|
||
placed: pose,
|
||
previous: pose,
|
||
current: pose,
|
||
};
|
||
}
|
||
|
||
private remove(key: Entity, body: Body): void {
|
||
for (const collider of body.colliders) this.parts.delete(collider.handle);
|
||
this.engine!.removeRigidBody(body.body);
|
||
this.bodies.delete(key);
|
||
}
|
||
|
||
/** Where the entities say the body is: the object's origin, a free collider's shape, a map at zero. */
|
||
private poseWanted(want: Wanted, key: Entity): Pose {
|
||
if (want.kind === 'tilemap') return { x: 0, y: 0, angle: 0 };
|
||
if (want.kind === 'free') {
|
||
const shape = shapeOf(key);
|
||
return { x: shape.x, y: shape.y, angle: shape.kind === 'box' ? shape.angle : 0 };
|
||
}
|
||
return matrixPose(worldMatrixOf(key));
|
||
}
|
||
|
||
/**
|
||
* Puts the body where its object is. A dynamic body is only moved when something else moved the
|
||
* object since we last wrote it – that is a teleport, and the body keeps its velocity (#146, 3).
|
||
*/
|
||
private place(body: Body, want: Wanted, key: Entity): void {
|
||
const settings = want.settings;
|
||
const settingsKey = settings ? JSON.stringify(settings) + hasTransformProps(key.type) : '';
|
||
if (settings && settingsKey !== body.settingsKey) {
|
||
body.settingsKey = settingsKey;
|
||
this.applySettings(body, settings, key);
|
||
}
|
||
const on = isHandleEnabled(key);
|
||
if (body.body.isEnabled() !== on) body.body.setEnabled(on);
|
||
const pose = this.poseWanted(want, key);
|
||
if (samePose(pose, body.placed)) return;
|
||
body.placed = pose;
|
||
const t = { x: toMetres(pose.x), y: toMetres(pose.y) };
|
||
if (body.body.isKinematic() && want.kind === 'rigid') {
|
||
body.body.setNextKinematicTranslation(t);
|
||
body.body.setNextKinematicRotation(pose.angle);
|
||
return;
|
||
}
|
||
body.body.setTranslation(t, true);
|
||
body.body.setRotation(pose.angle, true);
|
||
body.previous = pose;
|
||
body.current = pose;
|
||
}
|
||
|
||
private applySettings(target: Body, settings: RigidbodySettings, owner: Entity): void {
|
||
const body = target.body;
|
||
body.setBodyType(this.bodyType(settings.type), true);
|
||
body.setGravityScale(settings.gravityScale, true);
|
||
body.setLinearDamping(settings.linearDamping);
|
||
body.setAngularDamping(settings.angularDamping);
|
||
// An object without a rotation of its own could not show the body turning, so it does not turn.
|
||
target.turns = !settings.fixedRotation && hasTransformProps(owner.type);
|
||
body.lockRotations(!target.turns, true);
|
||
body.enableCcd(settings.ccd);
|
||
}
|
||
|
||
private bodyType(type: BodyType) {
|
||
const T = this.rapier.RigidBodyType;
|
||
return type === 'dynamic' ? T.Dynamic : type === 'static' ? T.Fixed : T.KinematicPositionBased;
|
||
}
|
||
|
||
/** The body's colliders, built again whenever what they were built from changed. */
|
||
private build(body: Body, want: Wanted, key: Entity, world: World): void {
|
||
const shapes = this.shapesWanted(want, key, world);
|
||
const origin = body.placed;
|
||
const local = shapes.map(({ entity, shape }) => ({ entity, shape, at: relativeTo(origin, shape) }));
|
||
const shapeKey =
|
||
JSON.stringify(
|
||
local.map(({ entity, shape, at }) => [
|
||
entity.id,
|
||
shape.kind === 'box' ? [round(shape.w), round(shape.h)] : round(shape.r),
|
||
round(at.x),
|
||
round(at.y),
|
||
round(at.angle),
|
||
entity.type === 'collider' ? [isTrigger(entity.props), colliderMaterial(entity.props)] : 0,
|
||
]),
|
||
) + JSON.stringify(want.settings?.mass ?? 0);
|
||
if (shapeKey !== body.shapeKey) {
|
||
body.shapeKey = shapeKey;
|
||
for (const collider of body.colliders) {
|
||
this.parts.delete(collider.handle);
|
||
this.engine!.removeCollider(collider, true);
|
||
}
|
||
body.colliders = [];
|
||
const mass = want.settings?.mass ?? 0;
|
||
const area = local.reduce((sum, { entity, shape }) => sum + (isSolid(entity) ? areaOf(shape) : 0), 0);
|
||
for (const { entity, shape, at } of local) {
|
||
if (areaOf(shape) <= 0) continue;
|
||
const desc = this.colliderDesc(
|
||
entity,
|
||
shape,
|
||
at,
|
||
mass > 0 && area > 0 ? (mass * areaOf(shape)) / area : 0,
|
||
);
|
||
const collider = this.engine!.createCollider(desc, body.body);
|
||
body.colliders.push(collider);
|
||
this.parts.set(collider.handle, { entity, shape, owner: body });
|
||
}
|
||
}
|
||
// Shapes are refreshed every tick even when the colliders stay: the overlap test reads them.
|
||
const built = local.filter(({ shape }) => areaOf(shape) > 0);
|
||
body.colliders.forEach((collider, i) => {
|
||
const { entity, shape } = built[i]!;
|
||
this.parts.set(collider.handle, { entity, shape, owner: body });
|
||
const on = isHandleEnabled(entity);
|
||
if (collider.isEnabled() === on) return;
|
||
collider.setEnabled(on);
|
||
// A resting body sleeps, and losing what held it up does not wake it by itself.
|
||
body.body.wakeUp();
|
||
});
|
||
if (body.kind !== 'rigid') return;
|
||
// Without a collider the engine would weigh it nothing, and a thing of no weight never falls.
|
||
const weighed = built.some(({ entity }) => isSolid(entity) && isHandleEnabled(entity));
|
||
const own = weighed ? 0 : want.settings?.mass || 1;
|
||
if (own !== body.ownMass) {
|
||
body.ownMass = own;
|
||
body.body.setAdditionalMass(own, true);
|
||
}
|
||
}
|
||
|
||
private shapesWanted(want: Wanted, key: Entity, world: World): { entity: Entity; shape: ColliderShape }[] {
|
||
if (want.kind === 'tilemap')
|
||
return this.solids.shapesOf(world, key).map((shape) => ({ entity: key, shape }));
|
||
return want.colliders.map((entity) => ({ entity, shape: shapeOf(entity) }));
|
||
}
|
||
|
||
private colliderDesc(entity: Entity, shape: ColliderShape, at: Pose, mass: number): ColliderDesc {
|
||
const R = this.rapier;
|
||
const desc =
|
||
shape.kind === 'circle'
|
||
? R.ColliderDesc.ball(toMetres(shape.r))
|
||
: R.ColliderDesc.cuboid(toMetres(shape.w / 2), toMetres(shape.h / 2));
|
||
desc.setTranslation(toMetres(at.x), toMetres(at.y)).setRotation(at.angle);
|
||
// A tile has no material of its own; it gets what a new collider starts with.
|
||
const material = colliderMaterial(entity.type === 'collider' ? entity.props : {});
|
||
desc.setFriction(material.friction).setRestitution(material.bounciness);
|
||
// The livelier side wins, as in Box2D: a ball that should bounce does so on plain ground too.
|
||
desc.setRestitutionCombineRule(R.CoefficientCombineRule.Max);
|
||
if (!isSolid(entity)) desc.setSensor(true).setDensity(0);
|
||
else if (mass > 0) desc.setMass(mass);
|
||
else desc.setDensity(material.density);
|
||
desc.setActiveEvents(R.ActiveEvents.COLLISION_EVENTS);
|
||
desc.setActiveCollisionTypes(R.ActiveCollisionTypes.ALL);
|
||
return desc;
|
||
}
|
||
|
||
/**
|
||
* The pairs in contact after a step, compared with those before it. The engine proposes the
|
||
* pairs; where no dynamic body takes part, the overlap test decides as it always did.
|
||
*/
|
||
private collect(world: World, entered: Set<string>): void {
|
||
this.queue!.drainCollisionEvents((h1, h2, started) => {
|
||
const key = h1 < h2 ? `${h1}|${h2}` : `${h2}|${h1}`;
|
||
if (started) this.candidates.set(key, [h1, h2]);
|
||
else this.candidates.delete(key);
|
||
});
|
||
const order = new Map(world.entities.map((e, i) => [e, i]));
|
||
const now = new Map<string, CollisionPair>();
|
||
const first = new Map<string, [Collider, Collider]>();
|
||
for (const [key, [h1, h2]] of this.candidates) {
|
||
let p = this.parts.get(h1);
|
||
let q = this.parts.get(h2);
|
||
if (!p || !q) {
|
||
this.candidates.delete(key);
|
||
continue;
|
||
}
|
||
const a0 = p.entity;
|
||
const b0 = q.entity;
|
||
if (!order.has(a0) || !order.has(b0) || !isHandleEnabled(a0) || !isHandleEnabled(b0)) continue;
|
||
if (bodyOf(a0) === bodyOf(b0) || (a0.type === 'tilemap' && b0.type === 'tilemap')) continue;
|
||
const engineSays = p.owner.body.isDynamic() || q.owner.body.isDynamic();
|
||
if (!engineSays && !shapesOverlap(p.shape, q.shape)) continue;
|
||
let c1 = h1;
|
||
let c2 = h2;
|
||
if (order.get(a0)! > order.get(b0)!) {
|
||
[p, q] = [q, p];
|
||
[c1, c2] = [c2, c1];
|
||
}
|
||
const pairKey = `${p.entity.id}|${q.entity.id}`;
|
||
if (now.has(pairKey)) continue;
|
||
now.set(pairKey, { a: p.entity, b: q.entity });
|
||
first.set(pairKey, [this.engine!.getCollider(c1), this.engine!.getCollider(c2)]);
|
||
}
|
||
for (const [key, pair] of now) {
|
||
if (this.current.has(key)) {
|
||
now.set(key, this.current.get(key)!);
|
||
continue;
|
||
}
|
||
const [c1, c2] = first.get(key)!;
|
||
this.enters.push(Object.assign(pair, this.contactOf(c1, c2)));
|
||
entered.add(key);
|
||
}
|
||
for (const [key, pair] of this.current) if (!now.has(key)) this.exits.push(pair);
|
||
this.current = now;
|
||
}
|
||
|
||
/** How the pair met: the normal from the second collider towards the first, and the impulse. */
|
||
private contactOf(c1: Collider, c2: Collider): { normal: Vec2; impulse: number } {
|
||
let normal = { x: 0, y: 0 };
|
||
let impulse = 0;
|
||
this.engine!.contactPair(c1, c2, (manifold, flipped) => {
|
||
if (manifold.numContacts() === 0) return;
|
||
const n = manifold.normal();
|
||
// The manifold's normal runs from its first collider to its second; ours runs back to `a`.
|
||
const sign = flipped ? 1 : -1;
|
||
if (normal.x === 0 && normal.y === 0) normal = { x: sign * n.x, y: sign * n.y };
|
||
for (let i = 0; i < manifold.numContacts(); i++) impulse += manifold.contactImpulse(i);
|
||
});
|
||
return { normal, impulse: toPixels(impulse) };
|
||
}
|
||
|
||
/**
|
||
* Hands the dynamic bodies' poses to their objects, a parent before its children, drawn `alpha`
|
||
* of the way from the step before to the last one so the picture moves evenly between steps.
|
||
*/
|
||
private writeBack(alpha: number): void {
|
||
const dynamic = [...this.bodies].filter(([, body]) => body.kind === 'rigid' && body.body.isDynamic());
|
||
dynamic.sort(([a], [b]) => depthOf(a) - depthOf(b));
|
||
for (const [owner, body] of dynamic) {
|
||
const pose = blend(body.previous, body.current, alpha);
|
||
const parent = owner.parent ? worldMatrixOf(owner.parent) : IDENTITY;
|
||
const local = apply(invert(parent), pose);
|
||
owner.x = local.x;
|
||
owner.y = local.y;
|
||
if (body.turns) {
|
||
const turned = angleBetween(matrixPose(worldMatrixOf(owner)).angle, pose.angle);
|
||
const sign = determinant(parent) < 0 ? -1 : 1;
|
||
owner.props.rotation = Number(owner.props.rotation ?? 0) + (sign * turned * 180) / Math.PI;
|
||
}
|
||
body.placed = matrixPose(worldMatrixOf(owner));
|
||
}
|
||
}
|
||
}
|
||
|
||
interface Wanted {
|
||
kind: Body['kind'];
|
||
settings: RigidbodySettings | null;
|
||
colliders: Entity[];
|
||
}
|
||
|
||
function poseOf(body: RigidBody): Pose {
|
||
const t = body.translation();
|
||
return { x: toPixels(t.x), y: toPixels(t.y), angle: body.rotation() };
|
||
}
|
||
|
||
function matrixPose(m: Mat): Pose {
|
||
return { x: m.tx, y: m.ty, angle: Math.atan2(m.b, m.a) };
|
||
}
|
||
|
||
function samePose(a: Pose, b: Pose): boolean {
|
||
return (
|
||
Math.abs(a.x - b.x) < POSE_EPSILON &&
|
||
Math.abs(a.y - b.y) < POSE_EPSILON &&
|
||
Math.abs(angleBetween(a.angle, b.angle)) < POSE_EPSILON
|
||
);
|
||
}
|
||
|
||
/** The turn from `from` to `to`, the short way round. */
|
||
function angleBetween(from: number, to: number): number {
|
||
const d = (to - from) % (2 * Math.PI);
|
||
return d > Math.PI ? d - 2 * Math.PI : d < -Math.PI ? d + 2 * Math.PI : d;
|
||
}
|
||
|
||
function blend(a: Pose, b: Pose, t: number): Pose {
|
||
return {
|
||
x: a.x + (b.x - a.x) * t,
|
||
y: a.y + (b.y - a.y) * t,
|
||
angle: a.angle + angleBetween(a.angle, b.angle) * t,
|
||
};
|
||
}
|
||
|
||
/** A world shape as seen from a body at `origin`: offset and turn in the body's own frame. */
|
||
function relativeTo(origin: Pose, shape: ColliderShape): Pose {
|
||
const dx = shape.x - origin.x;
|
||
const dy = shape.y - origin.y;
|
||
const cos = Math.cos(origin.angle);
|
||
const sin = Math.sin(origin.angle);
|
||
return {
|
||
x: dx * cos + dy * sin,
|
||
y: -dx * sin + dy * cos,
|
||
angle: (shape.kind === 'box' ? shape.angle : 0) - origin.angle,
|
||
};
|
||
}
|
||
|
||
function areaOf(shape: ColliderShape): number {
|
||
return shape.kind === 'circle' ? Math.PI * shape.r * shape.r : shape.w * shape.h;
|
||
}
|
||
|
||
/** A tilemap's cells always block; a collider unless it is a trigger. */
|
||
function isSolid(entity: Entity): boolean {
|
||
return entity.type !== 'collider' || !isTrigger(entity.props);
|
||
}
|
||
|
||
function depthOf(entity: Entity): number {
|
||
return [...parentChain(entity, (e) => e.parent)].length;
|
||
}
|
||
|
||
function round(n: number): number {
|
||
return Math.round(n * 1e4) / 1e4;
|
||
}
|