trainsim/physics/follower.ts

import { Constants } from "../math/constants.ts";
import { map } from "../math/lerp.ts";
import { ComplexPath, PathSegment } from "../math/path.ts";
import { Vector } from "doodler";
import { Mover } from "./mover.ts";

export class
  Follower extends Mover {
  debug = true;

  follow(toFollow: ComplexPath |  PathSegment) {
    if (toFollow instanceof ComplexPath) {
      const predict = this.velocity.copy();
      predict.normalize();
      predict.mult(25);
      const predictpos = Vector.add(this.position, predict)
  
      if (this.ctx)
        Mover.edges(predict, this.ctx.canvas.width, this.ctx.canvas.height)
      let normal = null;
      let target = null;
      let worldRecord = 1000000;
  
      for (let i = 0; i < toFollow.points.length; i++) {
        // Look at a line segment
        let a = toFollow.points[i];
        let b = toFollow.points[(i + 1) % toFollow.points.length]; // Note Path has to wraparound
  
        // Get the normal point to that line
        let normalPoint = getNormalPoint(predictpos, a, b);
  
        // Check if normal is on line segment
        let dir = Vector.sub(b, a);
        // If it's not within the line segment, consider the normal to just be the end of the line segment (point b)
        //if (da + db > line.mag()+1) {
        if (
          normalPoint.x < Math.min(a.x, b.x) ||
          normalPoint.x > Math.max(a.x, b.x) ||
          normalPoint.y < Math.min(a.y, b.y) ||
          normalPoint.y > Math.max(a.y, b.y)
        ) {
          normalPoint = b.copy();
          // If we're at the end we really want the next line segment for looking ahead
          a = toFollow.points[(i + 1) % toFollow.points.length];
          b = toFollow.points[(i + 2) % toFollow.points.length]; // Path wraps around
          dir = Vector.sub(b, a);
        }
  
        // How far away are we from the path?
        const d = Vector.dist(predictpos, normalPoint);
        // Did we beat the worldRecord and find the closest line segment?
        if (d < worldRecord) {
          worldRecord = d;
          normal = normalPoint;
  
          // Look at the direction of the line segment so we can seek a little bit ahead of the normal
          dir.normalize();
          // This is an oversimplification
          // Should be based on distance to path & velocity
          dir.mult(25);
          target = normal.copy();
          target.add(dir);
        }
  
        if (worldRecord > toFollow.radius) {
          return this.seek(target!);
        }
      }
  
      if (this.debug && this.ctx) {
        // Draw predicted future position
        this.ctx.strokeStyle = 'red';
        this.ctx.fillStyle = 'pink';
        this.ctx.beginPath();
        this.ctx.moveTo(this.position.x, this.position.y)
        this.ctx.lineTo(predictpos.x, predictpos.y);
        this.ctx.stroke();
  
        this.ctx.beginPath();
        this.ctx.arc(predictpos.x, predictpos.y, 4, 0, Constants.TWO_PI);
        this.ctx.fill();
        this.ctx.stroke();
  
        // Draw normal position
        this.ctx.beginPath();
        this.ctx.arc(normal!.x, normal!.y, 4, 0, Constants.TWO_PI);
        this.ctx.fill();
        this.ctx.stroke();
  
        // Draw actual target (red if steering towards it)
        this.ctx.beginPath();
        this.ctx.moveTo(predictpos.x, predictpos.y)
        this.ctx.lineTo(target!.x, target!.y);
        this.ctx.stroke();
  
        // if (worldRecord > toFollow.radius) fill(255, 0, 0);
        // noStroke();
  
        this.ctx.beginPath();
        this.ctx.arc(target!.x, target!.y, 8, 0, Constants.TWO_PI);
        this.ctx.fill();
        this.ctx.stroke();
      }
    } 
  }

  seek(target: Vector, strength: number = 1) {
    const desired = Vector.sub(target, this.position);
    desired.normalize();
    desired.mult(this.maxSpeed);

    const steer = Vector.sub(desired, this.velocity);
    steer.limit(this.maxForce);

    this.applyForce(steer.mult(strength));
  }

  link(target: Mover) {
    // const desired = target.velocity.copy();
    // desired.normalize();
    // desired.mult(-distance);

    // const predicted = Vector.add(target.position, desired);
    this.position = target.trailingPoint;

    // const lastVel = this.velocity.copy();
    this.seek(target.trailingPoint);
    // this.velocity = target.velocity;
  }

  arrive(target: Vector) {
    // const predicted = Vector.add(this.position, this.velocity.copy().normalize().mult(25));
    const desired = Vector.sub(target, this.position);
    const d = desired.mag();
    let speed = this.maxSpeed;
    if (d < 10) {
      speed = map(d, 0, 100, 0, this.maxSpeed);
    }
    desired.setMag(speed);

    const steer = Vector.sub(desired, this.velocity);
    steer.limit(this.maxForce);

    this.applyForce(steer);
  }
}

function getNormalPoint(p: Vector, a: Vector, b: Vector) {
  // Vector from a to p
  const ap = Vector.sub(p, a);
  // Vector from a to b
  const ab = Vector.sub(b, a);
  ab.normalize(); // Normalize the line
  // Project vector "diff" onto line by using the dot product
  ab.mult(ap.dot(ab));
  const normalPoint = Vector.add(a, ab);
  return normalPoint;
}