nage/engine/system/physics.ts

import type { Circle } from "#/engine/Circle.ts";
import type { Polygon } from "#/engine/Polygon.ts";
import type { Rect } from "#/engine/Rect.ts";
import $ from "#/engine/Scene.ts";
import { createVector, invScale, type Vector, ZERO } from "#/engine/Vector.ts";

export enum Shape {
	Circle,
	Rect,
	Polygon,
}

const CIRCLE = Shape.Circle;
const RECT = Shape.Rect;
const POLYGON = Shape.Polygon;

const EPSILON = 0.0001;

type CollisionBuffer = {
	size: number;
	mask: number;
	cursor: number;
	pool: Array<Collision>;
	slice: Array<Collision>;
};

type Collide = (
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) => void;

export type Collider = {
	shape: Shape;
	body: Circle | Rect | Polygon;
	buffer: CollisionBuffer;
	offset: Vector;
	width: number;
	height: number;
};

export type Collision = {
	cid: string;
	normal: Vector;
	time: number;
};

export function createCollider(
	shape: Shape,
	body: Circle | Rect | Polygon,
	offset?: Vector,
	bufferSize: number = 8,
): Collider {
	let width = 0;
	let height = 0;

	switch (shape) {
		case CIRCLE: {
			const radius = (body as Circle).radius;
			const size = radius * 2;

			width = size;
			height = size;
			if (offset === undefined) {
				offset = createVector(-radius, -radius);
			}
			break;
		}
		case RECT: {
			width = (body as Rect).width;
			height = (body as Rect).height;
			if (offset === undefined) {
				offset = createVector(-(width / 2), -(height / 2));
			}
			break;
		}
		case POLYGON: {
			// TODO
			if (offset === undefined) {
				offset = createVector();
			}
			break;
		}
	}

	return {
		shape,
		body,
		buffer: createCollisionBuffer(bufferSize),
		offset,
		width,
		height,
	};
}

function castOnCircle(
	dx: number,
	dy: number,
	vx: number,
	vy: number,
	radius: number,
): number | null {
	const a = vx * vx + vy * vy;
	const b = 2 * (dx * vx + dy * vy);
	const c = dx * dx + dy * dy - radius * radius;

	let disc = b * b - 4 * a * c;
	if (disc < 0) {
		return null;
	}

	disc = Math.sqrt(disc);
	const time = (-b - disc) / (2 * a);
	if (time >= 0 && time <= 1) {
		return time;
	}

	return null;
}

const inflAABB = {
	x: 0,
	y: 0,
	width: 0,
	height: 0,
};
function sweptAABB(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
): null | number {
	// setup the "real" position for collider a
	const ax = positionA.x + colliderA.offset.x; // calc the x position of the collider a
	const ay = positionA.y + colliderA.offset.y; // calc the y position of the collider a
	// setup the "real" position for collider b
	const bx = positionB.x + colliderB.offset.x; // calc the x positoin of the collider b
	const by = positionB.y + colliderB.offset.y; // calc the y position of the collider b

	// setup the inflated AABB
	inflAABB.x = bx - colliderA.width; // calc the x position of the inflated AABB
	inflAABB.y = by - colliderA.height; // calc the y position of the inflated AABB
	inflAABB.width = colliderB.width + colliderA.width; // calc the width of the inflated AABB
	inflAABB.height = colliderB.height + colliderA.height; // calc the height of the inflated AABB

	// full-frame displacement for collider a
	const vx = velocityA.x * delta; // scale x by delta for collider a
	const vy = velocityA.y * delta; // scale y by delta for collider a

	let txNear = (inflAABB.x - ax) / vx;
	let txFar = (inflAABB.x + inflAABB.width - ax) / vx;

	let tyNear = (inflAABB.y - ay) / vy;
	let tyFar = (inflAABB.y + inflAABB.height - ay) / vy;

	if (txNear > txFar) {
		[txNear, txFar] = [txFar, txNear];
	}
	if (tyNear > tyFar) {
		[tyNear, tyFar] = [tyFar, tyNear];
	}

	// no collision at all
	if (txNear > tyFar || tyNear > txFar) {
		return null;
	}

	const thNear = Math.max(txNear, tyNear);
	const thFar = Math.min(txFar, tyFar);

	// thFar < 0, collision is behind a frame
	// thNear > 1, collision is ahead a frame
	if (thFar < 0 || thNear > 1) {
		return null;
	}

	collision.normal.x = 0;
	collision.normal.y = 0;
	if (txNear > tyNear) {
		collision.normal.x = vx > 0 ? -1 : 1;
	} else {
		collision.normal.y = vy > 0 ? -1 : 1;
	}

	// thNear == -Infinity if velocity is 0
	const time = Math.max(0, thNear);
	collision.time = time;

	return time;
}

function circleOnCircle(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) {
	const ax = positionA.x + colliderA.offset.x;
	const ay = positionA.y + colliderA.offset.y;
	const bx = positionB.x + colliderB.offset.x;
	const by = positionB.y + colliderB.offset.y;

	const dx = ax - bx;
	const dy = ay - by;
	const vx = velocityA.x * delta;
	const vy = velocityA.y * delta;
	const radius =
		(colliderA.body as Circle).radius + (colliderB.body as Circle).radius;

	const time = castOnCircle(dx, dy, vx, vy, radius);
	if (time === null) {
		collision.cid = "";
		return;
	}

	collision.time = time;
	invScale(collision.normal, dx + vx * time, dy + vy * time);
}

function circleOnRect(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) {
	const radius = (colliderA.body as Circle).radius;
	const cWidth = (colliderB.body as Rect).width / 2;
	const cHeight = (colliderB.body as Rect).height / 2;

	const ax = positionA.x + colliderA.offset.x + radius;
	const ay = positionA.y + colliderA.offset.y + radius;
	const bx = positionB.x + colliderB.offset.x + cWidth;
	const by = positionB.y + colliderB.offset.y + cHeight;

	const vx = velocityA.x * delta;
	const vy = velocityA.y * delta;

	const left = bx - cWidth;
	const right = bx + cWidth;
	const hitX = ax + vx * collision.time;
	if (hitX >= left && hitX <= right) {
		return;
	}

	const top = by - cHeight;
	const bottom = by + cHeight;
	const hitY = ay + vy * collision.time;
	if (hitY >= top && hitY <= bottom) {
		return;
	}

	const dx = ax - (hitX < left ? left : right);
	const dy = ay - (hitY < top ? top : bottom);
	const time = castOnCircle(dx, dy, vx, vy, radius);
	if (time === null) {
		collision.cid = "";
		return;
	}

	collision.time = time;
	invScale(collision.normal, dx + vx * time, dy + vy * time);
}

function circleOnPolygon(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) {}

function rectOnCircle(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) {
	const cWidth = (colliderA.body as Rect).width / 2;
	const cHeight = (colliderA.body as Rect).height / 2;
	const radius = (colliderB.body as Circle).radius;

	const ax = positionA.x + colliderB.offset.x + cWidth;
	const ay = positionA.y + colliderB.offset.y + cHeight;
	const bx = positionB.x + colliderA.offset.x + radius;
	const by = positionB.y + colliderA.offset.y + radius;

	const vx = velocityA.x * delta;
	const vy = velocityA.y * delta;

	const left = bx - cWidth;
	const right = bx + cWidth;
	const hitX = ax + vx * collision.time;
	if (hitX >= left && hitX <= right) {
		return;
	}

	const top = by - cHeight;
	const bottom = by + cHeight;
	const hitY = ay + vy * collision.time;
	if (hitY >= top && hitY <= bottom) {
		return;
	}

	const dx = ax - (hitX < left ? left : right);
	const dy = ay - (hitY < top ? top : bottom);
	const time = castOnCircle(dx, dy, vx, vy, radius);
	if (time === null) {
		collision.cid = "";
		return;
	}

	collision.time = time;
	invScale(collision.normal, dx + vx * time, dy + vy * time);
}

function rectOnPolygon(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) {}

function polygonOnCircle(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) {}

function polygonOnRect(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) {}

function polygonOnPolygon(
	collision: Collision,
	colliderA: Collider,
	positionA: Vector,
	velocityA: Vector,
	colliderB: Collider,
	positionB: Vector,
	delta: number,
) {}

const circleCollide: Array<Collide> = [
	circleOnCircle,
	circleOnRect,
	circleOnPolygon,
];

const rectCollide: Array<Collide> = [rectOnCircle, () => {}, rectOnPolygon];

const polygonCollide: Array<Collide> = [
	polygonOnCircle,
	polygonOnRect,
	polygonOnPolygon,
];

function createCollisionBuffer(size: number): CollisionBuffer {
	if ((size & (size - 1)) !== 0) {
		throw new Error("size must be a power of two");
	}

	const pool: Array<Collision> = [];
	for (let i = 0; i < size; i++) {
		pool.push(createCollision());
	}

	return {
		size,
		mask: size - 1,
		cursor: 0,
		pool,
		slice: [],
	};
}

function next(buffer: CollisionBuffer): Collision {
	return buffer.pool[buffer.cursor++ & buffer.mask];
}

function createCollision(): Collision {
	return {
		cid: "",
		normal: createVector(),
		time: 0,
	};
}

const relativeVel = createVector();
export function moveAndCollide(
	id: string,
	velocity: Vector,
	delta: number,
): Array<Collision> {
	const colliders = $<Collider>("collider")!;
	const positions = $<Vector>("position")!;
	const velocities = $<Vector>("velocity")!;

	const selfColl = colliders.get(id)!.value;
	const selfPos = positions.get(id)!.value;

	const slice = selfColl.buffer.slice;
	slice.length = 0;

	for (const [cid, collider] of colliders.entries()) {
		if (cid === id) {
			continue;
		}

		const otherColl = collider.value;
		const otherPos = positions.get(cid)!.value;
		const otherVel = velocities.get(cid)?.value ?? ZERO;

		relativeVel.x = velocity.x - otherVel.x;
		relativeVel.y = velocity.y - otherVel.y;

		if (relativeVel.x === 0 && relativeVel.y === 0) {
			continue;
		}

		const collision = next(selfColl.buffer);
		const tHit = sweptAABB(
			collision,
			selfColl,
			selfPos,
			relativeVel,
			otherColl,
			otherPos,
			delta,
		);

		if (tHit === null) {
			collision.cid = "";
			continue;
		}
		collision.cid = cid;

		switch (selfColl.shape) {
			case CIRCLE: {
				circleCollide[otherColl.shape](
					collision,
					selfColl,
					selfPos,
					relativeVel,
					otherColl,
					otherPos,
					delta,
				);
				break;
			}
			case RECT: {
				rectCollide[otherColl.shape](
					collision,
					selfColl,
					selfPos,
					relativeVel,
					otherColl,
					otherPos,
					delta,
				);
				break;
			}
			case POLYGON: {
				polygonCollide[otherColl.shape](
					collision,
					selfColl,
					selfPos,
					relativeVel,
					otherColl,
					otherPos,
					delta,
				);
				break;
			}
		}

		if (collision.cid.length > 0) {
			slice.push(collision);
		}
	}

	return slice;
}

export function moveAndSlide(
	id: string,
	velocity: Vector,
	delta: number,
): void {
	let timeLeft = 1;
	const position = $<Vector>("position").get(id)!.value;
	const velocities = $<Vector>("velocity");

	let i = 0;
	while (i++ < 4 && timeLeft > 0) {
		const deltaLeft = delta * timeLeft;
		const collisions = moveAndCollide(id, velocity, delta);
		if (collisions.length < 1) {
			position.x += velocity.x * deltaLeft;
			position.y += velocity.y * deltaLeft;

			break;
		}

		let closest = collisions.pop()!;
		for (const collision of collisions) {
			if (collision.time < closest.time) {
				closest = collision;
			}
		}

		const time = closest.time;
		const scalar = Math.max(0, time - EPSILON) * deltaLeft;
		position.x += velocity.x * scalar;
		position.y += velocity.y * scalar;

		// a stationary entity might not have a velocity compnent
		const otherVel = velocities.get(closest.cid)?.value ?? ZERO;
		const rvx = velocity.x - otherVel.x;
		const rvy = velocity.y - otherVel.y;
		const dot = rvx * closest.normal.x + rvy * closest.normal.y;
		velocity.x -= closest.normal.x * dot;
		velocity.y -= closest.normal.y * dot;

		timeLeft *= 1 - time;
	}
}