Add collision radius

2 files changed, 77 insertions, 2 deletions

diff --git a/src/controller/gamecontext.ts b/src/controller/gamecontext.ts
index 295872d..8107b31 100644
--- a/src/controller/gamecontext.ts
+++ b/src/controller/gamecontext.ts
@@ -39,6 +39,7 @@ export class GameContext {
 	private readonly entityPos: vec2 = vec2.clone([6, 6]);
 	private readonly entityMovement: vec2 = vec2.create();
 
+	private readonly collisionRadius = 7 / 16;
 	private readonly walls: LineSegment[] = [
 		LineSegment.fromPoints(vec2.fromValues(1, 1), vec2.fromValues(11, 1)),
 
@@ -99,10 +100,14 @@ export class GameContext {
 			const move = new Movement(this.entityPos, dest);
 
 			for (const w of this.walls) {
-				if (!w.collidesMove(dest2, move))
+				if (!w.collidesMoveCircle(dest2, move, this.collisionRadius))
 					continue;
 
 				if (!vec2.exactEquals(dest, dest2)) {
+					// Ensure termination
+					if (vec2.squaredDistance(this.entityPos, dest2) >= vec2.squaredDistance(this.entityPos, dest))
+						return;
+
 					vec2.copy(dest, dest2);
 					rescan = true;
 					break;
diff --git a/src/math/line.ts b/src/math/line.ts
index 902c9b6..d071779 100644
--- a/src/math/line.ts
+++ b/src/math/line.ts
@@ -1,6 +1,9 @@
 import { mat2, vec2 } from 'gl-matrix';
 
-const rot90 = mat2.fromRotation(mat2.create(), Math.PI / 2);
+const rot90 = mat2.fromValues(
+	0, 1,
+	-1, 0,
+);
 
 export function normal(out: vec2, a: vec2): vec2 {
 	return vec2.transformMat2(out, a, rot90);
@@ -16,6 +19,10 @@ export class Line {
 		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);
@@ -55,6 +62,22 @@ export class Movement {
 		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 {
@@ -98,4 +121,51 @@ export class LineSegment extends Line {
 
 		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;
+	}
 }