rpgedit/src/math/line.ts

import { mat2, vec2 } from 'gl-matrix';

const rot90 = mat2.fromValues(
	0, 1,
	-1, 0,
);

export function normal(out: vec2, a: vec2): vec2 {
	return vec2.transformMat2(out, a, rot90);
}

export function crossz(a: vec2, b: vec2): number {
	return a[0] * b[1] - a[1] * b[0];
}

export class Line {
	constructor(
		public readonly p: vec2,
		public readonly v: vec2,
	) {}

	public getNormal(out: vec2): vec2 {
		return normal(out, this.v);
	}

	public projectPointDistance(p2: vec2): number {
		const v2 = vec2.sub(vec2.create(), p2, this.p);
		return vec2.dot(this.v, v2);
	}

	public projectPoint(out: vec2, p2: vec2): vec2 {
		const d = this.projectPointDistance(p2);
		return vec2.scaleAndAdd(out, this.p, this.v, d);
	}

	public distancePoint(p2: vec2): number {
		const v2 = vec2.sub(vec2.create(), p2, this.p);
		return crossz(this.v, v2);
	}

	public intersectLine(out: vec2, l2: Line): vec2 {
		const vp = vec2.sub(vec2.create(), l2.p, this.p);
		const d = crossz(vp, this.v);
		const d2 = d / crossz(this.v, l2.v);
		return vec2.scaleAndAdd(out, l2.p, l2.v, d2);
	}
}

export class Movement {
	public readonly v: vec2;

	constructor(
		public readonly p1: vec2,
		public readonly p2: vec2,
	) {
		this.v = vec2.sub(vec2.create(), p2, p1);
	}

	public intersectLine(out: vec2, l: Line): vec2 {
		const vp = vec2.sub(vec2.create(), l.p, this.p1);
		const d = crossz(vp, this.v);
		const d2 = d / crossz(this.v, l.v);
		return vec2.scaleAndAdd(out, l.p, l.v, d2);
	}

	public passes(p: vec2): boolean {
		const vp = vec2.sub(vec2.create(), p, this.p1);
		const d = vec2.dot(this.v, vp);
		return d >= 0 && d <= vec2.sqrLen(this.v);
	}

	public toLineSegment(): LineSegment {
		return LineSegment.fromPoints(this.p1, this.p2);
	}

	public translate(t: vec2): Movement {
		const p1 = vec2.add(vec2.create(), this.p1, t);
		const p2 = vec2.add(vec2.create(), this.p2, t);
		return new Movement(p1, p2);
	}
}

export class LineSegment extends Line {
	public static fromPoints(p1: vec2, p2: vec2): LineSegment {
		const d = vec2.dist(p1, p2);
		const v = vec2.sub(vec2.create(), p2, p1);
		vec2.scale(v, v, 1 / d);

		return new LineSegment(p1, v, d);
	}

	constructor(
		p: vec2,
		v: vec2,
		public readonly l: number,
	) {
		super(p, v);
	}

	public getP2(out: vec2): vec2 {
		return vec2.scaleAndAdd(out, this.p, this.v, this.l);
	}

	public collidesPoint(p2: vec2): boolean {
		const d = this.projectPointDistance(p2);
		return (d >= 0 && d <= this.l);
	}

	public collidesMove(out: vec2, move: Movement): boolean {
		if (this.distancePoint(move.p1) < 0)
			return false;

		if (this.distancePoint(move.p2) >= 0)
			return false;

		const x = move.intersectLine(vec2.create(), this);
		if (!this.collidesPoint(x))
			return false;

		this.projectPoint(out, move.p2);

		return true;
	}

	public collidesMoveCircle(out: vec2, move: Movement, r: number): boolean {
		const t = this.getNormal(vec2.create());
		vec2.scale(t, t, -r);

		const refMove = move.translate(t);
		const refOut = vec2.create();

		if (this.collidesMove(refOut, refMove)) {
			vec2.sub(out, refOut, t);
			return true;
		}

		return this.collidesPointMoveCircle(out, move, r);
	}

	private collidesPointMoveCircle(out: vec2, move: Movement, r: number): boolean {
		const moveLine = move.toLineSegment();

		if (moveLine.projectPointDistance(this.p) < 0)
			return false;

		const d = moveLine.distancePoint(this.p) / r;
		if (Math.abs(d) >= 1)
			return false;

		const e = Math.sqrt(1 - d * d);

		const t = moveLine.getNormal(vec2.create());
		vec2.scale(t, t, d);
		vec2.scaleAndAdd(t, t, moveLine.v, e);

		const tr = vec2.scale(vec2.create(), t, r);

		const refMove = move.translate(tr);

		if (vec2.sqrDist(this.p, move.p1) > r * r && !refMove.passes(this.p))
			return false;

		normal(t, t);

		const tang = new Line(this.p, t);
		tang.projectPoint(out, refMove.p2);
		vec2.sub(out, out, tr);

		return true;
	}
}