Working version of train following spline path

math/path.ts

@@ -0,0 +1,172 @@
+import { Vector } from "./vector.ts";
+
+export class ComplexPath {
+
+  points: Vector[] = [];
+
+  radius = 50;
+
+  ctx?: CanvasRenderingContext2D;
+
+  constructor(points?: Vector[]) {
+    points && (this.points = points);
+  }
+
+  setContext(ctx: CanvasRenderingContext2D) {
+    this.ctx = ctx;
+  }
+
+  draw() {
+    if (!this.ctx || !this.points.length) return;
+    const ctx = this.ctx;
+
+    ctx.save();
+    ctx.lineWidth = 2;
+    ctx.strokeStyle = 'white';
+    ctx.setLineDash([21, 6])
+
+    let last = this.points[this.points.length - 1]
+
+    for (const point of this.points) {
+      ctx.beginPath();
+      ctx.moveTo(last.x, last.y);
+      ctx.lineTo(point.x, point.y);
+      ctx.stroke();
+      last = point;
+    }
+    ctx.restore();
+  }
+}
+
+export class PathSegment {
+  points: [Vector, Vector, Vector, Vector]
+  ctx?: CanvasRenderingContext2D;
+
+  constructor(points: [Vector, Vector, Vector, Vector]) {
+    this.points = points;
+  }
+
+  setContext(ctx: CanvasRenderingContext2D) {
+    this.ctx = ctx;
+  }
+
+  draw() {
+    if (!this.ctx) return;
+    const ctx = this.ctx;
+
+    ctx.save();
+    ctx.beginPath();
+    ctx.moveTo(this.points[0].x, this.points[0].y);
+
+    ctx.bezierCurveTo(
+      this.points[1].x,
+      this.points[1].y,
+      this.points[2].x,
+      this.points[2].y,
+      this.points[3].x,
+      this.points[3].y,
+    );
+
+    ctx.strokeStyle = '#ffffff50';
+    ctx.lineWidth = 2;
+    ctx.stroke();
+    ctx.restore();
+  }
+
+  getPointAtT(t: number) {
+    const [a, b, c, d] = this.points;
+    const res = a.copy();
+
+    res.add(Vector.add(a.copy().mult(-3), b.copy().mult(3)).mult(t))
+    res.add(Vector.add(Vector.add(a.copy().mult(3), b.copy().mult(-6)), c.copy().mult(3)).mult(Math.pow(t, 2)));
+    res.add(Vector.add(Vector.add(a.copy().mult(-1), b.copy().mult(3)), Vector.add(c.copy().mult(-3), d.copy())).mult(Math.pow(t, 3)));
+
+    return res;
+  }
+
+  getClosestPoint(v: Vector): [Vector, number, number] {
+    const samples = 25;
+    const resolution = 1 / samples;
+    let closest = this.points[0];
+    let closestDistance = this.points[0].dist(v);
+    let closestT = 0;
+
+    for (let i = 0; i < samples; i++) {
+      const point = this.getPointAtT(i * resolution);
+      const distance = v.dist(point);
+      if (distance < closestDistance) {
+        closest = point;
+        closestDistance = distance;
+        closestT = i * resolution;
+      }
+    }
+
+    return [closest, closestDistance, closestT];
+  }
+
+  getPointsWithinRadius(v: Vector, r: number) {
+    const points: [number, PathSegment][] = [];
+    const samples = 25;
+    const resolution = 1 / samples;
+
+    for (let i = 0; i < samples; i++) {
+      const point = this.getPointAtT(i * resolution);
+      const distance = v.dist(point);
+      if (distance < r) {
+        points.push([i * resolution,this]);
+      }
+    }
+    return points
+  }
+
+  tangent(t: number) {
+    // dP(t) / dt =  -3(1-t)^2 * P0 + 3(1-t)^2 * P1 - 6t(1-t) * P1 - 3t^2 * P2 + 6t(1-t) * P2 + 3t^2 * P3 
+    const [a, b, c, d] = this.points;
+
+    const res = Vector.sub(b, a).mult(3 * Math.pow(1 - t, 2));
+    res.add(Vector.add(Vector.sub(c, b).mult(6 * (1 - t) * t), Vector.sub(d, c).mult(3 * Math.pow(t, 2))));
+
+    return res;
+  }
+
+  doesIntersectCircle(x: number, y: number, r: number) {
+    const v = new Vector(x, y);
+    const samples = 25;
+    const resolution = 1 / samples;
+
+    let distance = Infinity;
+    let t;
+
+    for (let i = 0; i < samples; i++) {
+      if (i !== samples - 1) {
+        const a = this.getPointAtT(i * resolution);
+        const b = this.getPointAtT((i + 1) * resolution);
+        const ac = Vector.sub(v, a);
+        const ab = Vector.sub(b, a);
+
+        const d = Vector.add(Vector.vectorProjection(ac, ab), a);
+        const ad = Vector.sub(d, a);
+
+        const k = Math.abs(ab.x) > Math.abs(ab.y) ? ad.x / ab.x : ad.y / ab.y;
+
+        let dist;
+        if (k <= 0.0) {
+          dist = Vector.hypot2(v, a)
+        } else if (k >= 1.0) {
+          dist = Vector.hypot2(v, b)
+        }
+
+        dist = Vector.hypot2(v, d)
+
+        if (dist < distance) {
+          distance = dist;
+          t = i * resolution;
+        }
+      }
+    }
+
+    if (distance < r) return t;
+
+    return false;
+  }
+}