trainsim/math/path.ts

import { Vector } from "@bearmetal/doodler";

export class ComplexPath {
  points: Vector[] = [];
  segments: PathSegment[] = [];

  radius = 50;

  ctx?: CanvasRenderingContext2D;
  evenPoints: Vector[] = [];

  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();
  }

  followEvenPoints(t: number) {
    if (t < 0) t += this.evenPoints.length;
    const i = Math.floor(t);
    const a = this.evenPoints[i];
    const b = this.evenPoints[(i + 1) % this.evenPoints.length];

    return Vector.lerp(a, b, t % 1);
  }

  calculateEvenlySpacedPoints(spacing: number, resolution = 1) {
    // this.pointSpacing = 1;
    // return this.segments.flatMap(s => s.calculateEvenlySpacedPoints(spacing, resolution));
    const points: Vector[] = [];

    points.push(this.segments[0].points[0]);
    let prev = points[0];
    let distSinceLastEvenPoint = 0;
    for (const seg of this.segments) {
      let t = 0;

      const div = Math.ceil(seg.length * resolution * 10);
      while (t < 1) {
        t += 1 / div;
        const point = seg.getPointAtT(t);
        distSinceLastEvenPoint += prev.dist(point);

        if (distSinceLastEvenPoint >= spacing) {
          const overshoot = distSinceLastEvenPoint - spacing;
          const evenPoint = Vector.add(
            point,
            Vector.sub(point, prev).normalize().mult(overshoot),
          );
          distSinceLastEvenPoint = overshoot;
          points.push(evenPoint);
          prev = evenPoint;
        }

        prev = point;
      }
    }

    this.evenPoints = points;

    return points;
  }
}

export class PathSegment {
  id: string;
  points: [Vector, Vector, Vector, Vector];

  length: number;
  startingLength: number;

  next?: PathSegment;
  prev?: PathSegment;

  constructor(points: [Vector, Vector, Vector, Vector]) {
    this.id = crypto.randomUUID();
    this.points = points;
    this.length = this.calculateApproxLength(100);
    this.startingLength = Math.round(this.length);
  }

  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;
  }

  calculateApproxLength(resolution = 25) {
    const stepSize = 1 / resolution;
    const points: Vector[] = [];
    for (let i = 0; i <= resolution; i++) {
      const current = stepSize * i;
      points.push(this.getPointAtT(current));
    }
    this.length =
      points.reduce((acc: { prev?: Vector; length: number }, cur) => {
        const prev = acc.prev;
        acc.prev = cur;
        if (!prev) return acc;
        acc.length += cur.dist(prev);
        return acc;
      }, { prev: undefined, length: 0 }).length;
    return this.length;
  }

  calculateEvenlySpacedPoints(
    spacing: number,
    resolution = 1,
    targetLength?: number,
  ) {
    const points: [Vector, number][] = [];

    points.push([this.points[0], this.tangent(0).heading()]);
    let [prev] = points[0];
    let distSinceLastEvenPoint = 0;

    let t = 0;

    const div = Math.ceil(this.length * resolution * 10);
    while (t < 1) {
      t += 1 / div;
      const point = this.getPointAtT(t);
      distSinceLastEvenPoint += prev.dist(point);

      if (distSinceLastEvenPoint >= spacing) {
        const overshoot = distSinceLastEvenPoint - spacing;
        const evenPoint = Vector.add(
          point,
          Vector.sub(point, prev).normalize().mult(overshoot),
        );
        distSinceLastEvenPoint = overshoot;
        points.push([evenPoint, this.tangent(t).heading()]);
        prev = evenPoint;
      }

      prev = point;
    }

    if (targetLength && points.length < targetLength) {
      while (points.length < targetLength) {
        t += 1 / div;
        const point = this.getPointAtT(t);
        distSinceLastEvenPoint += prev.dist(point);

        if (distSinceLastEvenPoint >= spacing) {
          const overshoot = distSinceLastEvenPoint - spacing;
          const evenPoint = Vector.add(
            point,
            Vector.sub(point, prev).normalize().mult(overshoot),
          );
          distSinceLastEvenPoint = overshoot;
          points.push([evenPoint, this.tangent(t).heading()]);
          prev = evenPoint;
        }

        prev = point;
      }
    }

    return points;
  }

  calculateSubdividedPoints(numberOfPoints: number) {
    const points: Vector[] = [];

    for (let i = 0; i < numberOfPoints; i++) {
      const point = this.getPointAtT(i / numberOfPoints);
      points.push(point);
    }

    return points;
  }

  clampLength() {
    const curveLength = this.startingLength;
    const points = this.calculateEvenlySpacedPoints(1, 1, curveLength + 1);
    if (points.length >= curveLength) {
      this.points[3].set(points[curveLength][0]);
    }
  }

  draw(): void {}
}