138 lines
3.1 KiB
TypeScript
138 lines
3.1 KiB
TypeScript
import { mat2, vec2 } from 'gl-matrix';
|
|
import { Collidable } from './collision';
|
|
|
|
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 implements Collidable {
|
|
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 containsPoint(p2: vec2): boolean {
|
|
const d = this.projectPointDistance(p2);
|
|
return (d >= 0 && d <= this.l);
|
|
}
|
|
|
|
public collide(out: vec2, move: Movement, r: number): boolean {
|
|
if (this.distancePoint(move.p1) < 0)
|
|
return false;
|
|
|
|
const t = this.getNormal(vec2.create());
|
|
vec2.scale(t, t, -r);
|
|
|
|
const refMove = move.translate(t);
|
|
|
|
if (!this.collideRef(out, refMove))
|
|
return false;
|
|
|
|
vec2.sub(out, out, t);
|
|
return true;
|
|
}
|
|
|
|
private collideRef(out: vec2, move: Movement): boolean {
|
|
if (this.distancePoint(move.p2) >= 0)
|
|
return false;
|
|
|
|
const x = move.intersectLine(vec2.create(), this);
|
|
if (!this.containsPoint(x))
|
|
return false;
|
|
|
|
this.projectPoint(out, move.p2);
|
|
|
|
return true;
|
|
}
|
|
}
|