debugable wip

.gitignore

@@ -23,3 +23,8 @@ dist-ssr
 *.njsproj
 *.sln
 *.sw?
+
+# Packed devtools
+devtools.zip
+
+temp.*

deno.json

@@ -3,7 +3,8 @@
     "dev": "deno run -A --node-modules-dir npm:vite",
     "build": "deno run -A --node-modules-dir npm:vite build",
     "preview": "deno run -A --node-modules-dir npm:vite preview",
-    "serve": "deno run --allow-net --allow-read jsr:@std/http@1/file-server dist/"
+    "serve": "deno run --allow-net --allow-read jsr:@std/http@1/file-server dist/",
+    "pack-devtools": "rm -rf devtools.zip && deno run -A npm:web-ext build -o --source-dir devtools --artifacts-dir devtools.zip"
   },
   "compilerOptions": {
     "lib": [
@@ -13,8 +14,9 @@
     ]
   },
   "imports": {
-    "@bearmetal/doodler": "jsr:@bearmetal/doodler@0.0.5-b",
+    "@bearmetal/doodler": "jsr:@bearmetal/doodler@0.0.5-i",
     "@deno/vite-plugin": "npm:@deno/vite-plugin@^1.0.4",
     "vite": "npm:vite@^6.0.1"
-  }
+  },
+  "nodeModulesDir": "auto"
 }

devtools/manifest.json

@@ -1,8 +1,9 @@
 {
   "manifest_version": 2,
-  "name": "Context Stack DevTools",
+  "name": "SNR DevTools",
   "version": "1.0",
   "description": "A devtools panel to view and edit context stack values.",
+  "author": "Emmaline Autumn",
   "devtools_page": "devtools.html",
   "background": {
     "scripts": [
@@ -24,5 +25,13 @@
     "devtools",
     "tabs",
     "*://*/*"
-  ]
+  ],
+  "icons": {
+    "48": "train icon.png"
+  },
+  "browser_specific_settings": {
+    "gecko": {
+      "id": "snrdt@cyborggrizzly.com"
+    }
+  }
 }

src/inputs.ts

@@ -2,8 +2,12 @@ import { getContextItem } from "./lib/context.ts";
 import { InputManager } from "./lib/input.ts";
 import { StateMachine } from "./state/machine.ts";
 import { States } from "./state/states/index.ts";
+import { TrackSystem } from "./track/system.ts";
 
 export function bootstrapInputs() {
+  addEventListener("keydown", (e) => {
+    e.preventDefault();
+  });
   const inputManager = getContextItem<InputManager>("inputManager");
   inputManager.onKey("e", () => {
     const state = getContextItem<StateMachine<States>>("state");
@@ -14,4 +18,22 @@ export function bootstrapInputs() {
       localStorage.removeItem("track");
     }
   });
+  inputManager.onKey("c", () => {
+    if (inputManager.getKeyState("Control")) {
+      const currentTrack = localStorage.getItem("track");
+      navigator.clipboard.writeText(currentTrack ?? "[]");
+    }
+  });
+  addEventListener("paste", async (e) => {
+    let data = e.clipboardData?.getData("text/plain");
+    if (!data) return;
+    try {
+      // data = data.trim().replace(/^"|"$/g, "").replace(/\\"/g, '"');
+      console.log(data);
+      const track = TrackSystem.deserialize(JSON.parse(data));
+      localStorage.setItem("track", track.serialize());
+    } catch (e) {
+      console.error(e);
+    }
+  });
 }

src/lib/context.ts

@@ -1,14 +1,35 @@
+import { ZoomableDoodler } from "@bearmetal/doodler";
 import { TrackSegment } from "../track/system.ts";
 import { Train, TrainCar } from "../train/train.ts";
 import { InputManager } from "./input.ts";
+import { ResourceManager } from "./resources.ts";
+import { Debuggable } from "./debuggable.ts";
+
+interface ContextMap {
+  inputManager: InputManager;
+  doodler: ZoomableDoodler;
+  resources: ResourceManager;
+  debug: Debug;
+  debuggables: Debuggable[];
+  colors: [
+    string,
+    string,
+    string,
+    string,
+    string,
+    string,
+    string,
+    string,
+    ...string[],
+  ];
+  [key: string]: unknown;
+}
 
 type ContextStore = Record<string, any>;
 
 const defaultContext: ContextStore = {};
 const contextStack: ContextStore[] = [defaultContext];
 
-const debug = JSON.parse(localStorage.getItem("debug") || "false");
-
 export function setDefaultContext(context: ContextStore) {
   Object.assign(defaultContext, context);
 }
@@ -38,6 +59,10 @@ export const ctx = new Proxy(
 export function getContext() {
   return ctx;
 }
+export function getContextItem<K extends keyof ContextMap>(
+  prop: K,
+): ContextMap[K];
+export function getContextItem<T>(prop: string): T;
 export function getContextItem<T>(prop: string): T {
   return ctx[prop] as T;
 }

src/lib/debuggable.ts

@@ -12,22 +12,24 @@ export abstract class Debuggable extends Drawable {
       if (typeof debugKeys[0] === "boolean") {
         drawFirst = debugKeys.shift() as boolean;
       }
-      const draw = this.draw.bind(this);
-      this.draw = drawFirst
-        ? (...args: unknown[]) => {
-          draw(...args);
-          const debug = getContextItem<Debug>("debug");
-          if (debugKeys.some((k) => debug[k as keyof Debug])) {
-            this.debugDraw();
-          }
-        }
-        : (...args: unknown[]) => {
-          const debug = getContextItem<Debug>("debug");
-          if (debugKeys.some((k) => debug[k as keyof Debug])) {
-            this.debugDraw();
-          }
-          draw(...args);
-        };
+      const debuggables = getContextItem("debuggables");
+      debuggables.push(this);
+      // const draw = this.draw.bind(this);
+      // this.draw = drawFirst
+      //   ? (...args: unknown[]) => {
+      //     draw(...args);
+      //     const debug = getContextItem<Debug>("debug");
+      //     if (debugKeys.some((k) => debug[k as keyof Debug])) {
+      //       this.debugDraw();
+      //     }
+      //   }
+      //   : (...args: unknown[]) => {
+      //     const debug = getContextItem<Debug>("debug");
+      //     if (debugKeys.some((k) => debug[k as keyof Debug])) {
+      //       this.debugDraw();
+      //     }
+      //     draw(...args);
+      //   };
     }
   }
 

src/lib/resources.ts

@@ -1,24 +1,64 @@
+// namespace:type/location
+type sprite = "sprite";
+type img = "img";
+type audio = "audio";
+type ResourceType = keyof ResourceMap;
+type SpriteId = `${string}:${sprite}/${string}`;
+
+interface ResourceMap {
+  sprite: HTMLImageElement;
+  img: HTMLImageElement;
+  audio: HTMLAudioElement;
+}
+type NamespacedId<T extends keyof ResourceMap> = `${string}:${T}/${string}`;
+
+/**
+ * Resources are stored in namespaces, and can be accessed by their namespaced id.
+ * Sprites are located in blob storage as a single png file.
+ * Audio is located in blob storage as a single mp3 file.
+ *
+ * Custom resources can be loaded via the public API, however they will not be loaded on other clients.
+ * Ideally, engine and car definitions should be stored in the resource manager so that custom cars can be created.
+ */
 export class ResourceManager {
   private resources: Map<string, unknown> = new Map();
   private statuses: Map<string, Promise<boolean>> = new Map();
 
-  get<T>(name: string): T {
+  get<K extends ResourceType>(name: NamespacedId<K>): ResourceMap[K];
+  get<T>(name: string): T;
+  get<T>(
+    name: string,
+  ): T {
     if (!this.resources.has(name)) {
       throw new Error(`Resource ${name} not found`);
     }
     return this.resources.get(name) as T;
   }
 
-  set(name: string, value: unknown) {
+  set<T extends keyof ResourceMap>(
+    name: NamespacedId<T>,
+    value: unknown,
+  ) {
+    const identifier = parseNamespacedId(name);
     if (typeof (value as EventSource).addEventListener === "function") {
+      if (value instanceof Image || value instanceof Audio) {
+        // During development, we can use the local file system
+        value.src =
+          `/blobs/${identifier.namespace}/${identifier.type}/${identifier.name}${
+            extensionByType(identifier.type)
+          }`;
+        console.log(value.src);
+      }
       this.statuses.set(
         name,
         new Promise((resolve) => {
           const onload = () => {
             this.resources.set(name, value);
             resolve(true);
+            (value as EventSource).removeEventListener("loadeddata", onload);
             (value as EventSource).removeEventListener("load", onload);
           };
+          (value as EventSource).addEventListener("loadeddata", onload);
           (value as EventSource).addEventListener("load", onload);
         }),
       );
@@ -36,3 +76,28 @@ export class ResourceManager {
     return Promise.all(Array.from(this.statuses.values()));
   }
 }
+
+function extensionByType(type: ResourceType) {
+  switch (type) {
+    case "img":
+      return ".png";
+    case "audio":
+      return ".mp3";
+    case "sprite":
+      return ".png";
+  }
+}
+
+type NamespaceIdentifier<T extends ResourceType> = {
+  namespace: string;
+  type: T;
+  name: string;
+};
+
+function parseNamespacedId<T extends ResourceType>(
+  id: NamespacedId<T>,
+): NamespaceIdentifier<T> {
+  const [namespace, location] = id.split(":");
+  const [type, ...name] = location.split("/");
+  return { namespace, type: type as T, name: name.join("/") };
+}

src/main.ts

@@ -20,11 +20,14 @@ const resources = new ResourceManager();
 const doodler = new ZoomableDoodler({
   fillScreen: true,
   bg: "#302040",
-});
-(doodler as any as { ctx: CanvasRenderingContext2D }).ctx
-  .imageSmoothingEnabled = false;
+  noSmooth: true,
+}, () => {});
+setTimeout(() => {
+  (doodler as any as { ctx: CanvasRenderingContext2D }).ctx
+    .imageSmoothingEnabled = false;
+}, 0);
 // doodler.minScale = 0.1;
-(doodler as any).scale = 3.14;
+// (doodler as any).scale = 3.14;
 
 const colors = [
   "red",
@@ -37,13 +40,20 @@ const colors = [
   "violet",
 ];
 
-const _debug: Debug = JSON.parse(localStorage.getItem("debug") || "0") || {
+const _fullDebug: Debug = {
   track: false,
   train: false,
   path: false,
   car: false,
+  bogies: false,
+  angles: false,
+  aabb: false,
+  segment: false,
 };
 
+const storedDebug = JSON.parse(localStorage.getItem("debug") || "0");
+const _debug: Debug = Object.assign({}, _fullDebug, storedDebug);
+
 const debug = new Proxy(_debug, {
   get: (_, prop: string) => {
     // if (prop !in _debug) {
@@ -107,3 +117,8 @@ gameLoop.start(state);
 if (import.meta.env.DEV) {
   console.log("Running in development mode");
 }
+
+globalThis.TWO_PI = Math.PI * 2;
+declare global {
+  var TWO_PI: number;
+}

src/math/angle.ts

@@ -0,0 +1,7 @@
+export function angleToRadians(angle: number) {
+  return angle / 180 * Math.PI;
+}
+
+export function angleToDegrees(angle: number) {
+  return angle * 180 / Math.PI;
+}

src/math/lerp.ts

@@ -9,3 +9,19 @@ export const map = (
   x2: number,
   y2: number,
 ) => (value - x1) * (y2 - x2) / (y1 - x1) + x2;
+
+export function lerpAngle(a: number, b: number, t: number) {
+  let diff = b - a;
+  // Wrap difference to [-PI, PI]
+  while (diff < -Math.PI) diff += 2 * Math.PI;
+  while (diff > Math.PI) diff -= 2 * Math.PI;
+  return a + diff * t;
+}
+
+export function averageAngles(angle1: number, angle2: number) {
+  // Convert angles to unit vectors
+  const x = Math.cos(angle1) + Math.cos(angle2);
+  const y = Math.sin(angle1) + Math.sin(angle2);
+  // Compute the angle of the resulting vector
+  return Math.atan2(y, x);
+}

src/physics/SpatialGrid.ts

@@ -0,0 +1,66 @@
+import { Vector } from "@bearmetal/doodler";
+
+declare global {
+  type GridItem = [Vector, Vector, Set<tag>];
+}
+export class SpatialHashGrid {
+  private grid: Map<string, GridItem[]> = new Map();
+  private cellSize: number;
+
+  constructor(cellSize: number) {
+    this.cellSize = cellSize;
+  }
+
+  private getKey(x: number, y: number): string {
+    return `${Math.floor(x / this.cellSize)},${Math.floor(y / this.cellSize)}`;
+  }
+
+  insert(segment: GridItem) {
+    const [a, b] = segment;
+    const minX = Math.min(a.x, b.x);
+    const minY = Math.min(a.y, b.y);
+    const maxX = Math.max(a.x, b.x);
+    const maxY = Math.max(a.y, b.y);
+
+    for (let x = minX; x <= maxX; x += this.cellSize) {
+      for (let y = minY; y <= maxY; y += this.cellSize) {
+        const key = this.getKey(x, y);
+        if (!this.grid.has(key)) this.grid.set(key, []);
+        this.grid.get(key)!.push(segment);
+      }
+    }
+  }
+
+  query(position: Vector, radius: number, tag?: tag): GridItem[] {
+    const minX = position.x - radius;
+    const minY = position.y - radius;
+    const maxX = position.x + radius;
+    const maxY = position.y + radius;
+
+    const segments: Set<GridItem> = new Set();
+
+    for (let x = minX; x <= maxX; x += this.cellSize) {
+      for (let y = minY; y <= maxY; y += this.cellSize) {
+        const key = this.getKey(x, y);
+        if (this.grid.has(key)) {
+          for (const segment of this.grid.get(key)!) {
+            tag
+              ? segment[2].has(tag) &&
+                segments.add(segment)
+              : segments.add(segment);
+          }
+        }
+      }
+    }
+
+    return Array.from(segments);
+  }
+
+  getAllSegments() {
+    return Array.from(this.grid.values()).flatMap((s) => s);
+  }
+
+  clear() {
+    this.grid.clear();
+  }
+}

src/physics/solver.ts

@@ -0,0 +1,39 @@
+export class PhysicsSolver {
+  tasks: SolverTask[] = [];
+  maxTickStep?: number;
+
+  addTask(c: SolverTask) {
+    c.solver = this;
+    this.tasks.push(c);
+    this.tasks.sort((a, b) => a.priority - b.priority);
+  }
+  removeTask(c: SolverTask) {
+    this.tasks = this.tasks.filter((c1) => c1 !== c);
+  }
+
+  cleanupTasks() {
+    this.tasks = this.tasks.filter((c) => c.active);
+  }
+
+  solve(dt: number) {
+    dt = Math.min(dt, this.maxTickStep ?? Infinity);
+    for (const c of this.tasks) {
+      for (let i = 0; i < c.iterations; i++) {
+        if (!c.apply(dt)) break;
+      }
+    }
+  }
+}
+
+export abstract class SolverTask {
+  iterations: number = 10;
+  solver?: PhysicsSolver;
+  priority: number = 0;
+
+  active = true;
+
+  abstract apply(dt: number): boolean;
+  remove() {
+    this.solver?.removeTask(this);
+  }
+}

src/state/states/EditTrackState.ts

@@ -14,6 +14,10 @@ import {
   SBendLeft,
   SBendRight,
   StraightTrack,
+  TightBankLeft,
+  TightBankRight,
+  WideBankLeft,
+  WideBankRight,
 } from "../../track/shapes.ts";
 import { TrackSegment } from "../../track/system.ts";
 import { clamp } from "../../math/clamp.ts";
@@ -40,18 +44,6 @@ export class EditTrackState extends State<States> {
     const track = getContextItem<TrackSystem>("track");
     const doodler = getContextItem<Doodler>("doodler");
 
-    // For moving a segment, i.e. the currently active one
-    // const segment = track.lastSegment;
-    // if (segment) {
-    //   const firstPoint = segment.points[0].copy();
-    //   const { x, y } = inputManager.getMouseLocation();
-    //   segment.points.forEach((p, i) => {
-    //     const relativePoint = Vector.sub(p, firstPoint);
-    //     p.set(x, y);
-    //     p.add(relativePoint);
-    //   });
-    // }
-
     if (this.selectedSegment) {
       const segment = this.selectedSegment;
       const firstPoint = segment.points[0].copy();
@@ -61,6 +53,7 @@ export class EditTrackState extends State<States> {
         p.set(mousePos);
         p.add(relativePoint);
       });
+      segment.update();
 
       const ends = track.findEnds();
       setContextItem("showEnds", true);
@@ -109,18 +102,21 @@ export class EditTrackState extends State<States> {
           this.ghostRotated = false;
         }
         switch (this.closestEnd.frontOrBack) {
-          case "front":
+          case "front": {
             this.ghostSegment.setPositionByPoint(
               this.closestEnd.pos,
               this.ghostSegment.points[0],
             );
             // this.ghostSegment.points[0] = this.closestEnd.pos;
+            const ghostEndTangent = this.ghostSegment.tangent(0);
+
             !this.ghostRotated && this.ghostSegment.rotateAboutPoint(
-              this.closestEnd.tangent.heading(),
+              this.closestEnd.tangent.heading() - ghostEndTangent.heading(),
               this.ghostSegment.points[0],
             );
             this.ghostRotated = true;
             break;
+          }
           case "back": {
             this.ghostSegment.setPositionByPoint(
               this.closestEnd.pos,
@@ -136,6 +132,8 @@ export class EditTrackState extends State<States> {
             break;
           }
         }
+        this.ghostSegment.update();
+
         // } else if (closestEnd) {
         //   this.closestEnd = closestEnd;
       } else if (!this.closestEnd || !closestEnd) {
@@ -249,6 +247,7 @@ export class EditTrackState extends State<States> {
 
     if (translation.x !== 0 || translation.y !== 0) {
       track.translate(translation);
+      track.recalculateAll();
     }
 
     // TODO
@@ -286,6 +285,10 @@ export class EditTrackState extends State<States> {
       new SBendRight(),
       new BankLeft(),
       new BankRight(),
+      new WideBankLeft(),
+      new WideBankRight(),
+      new TightBankLeft(),
+      new TightBankRight(),
     ]);
 
     const inputManager = getContextItem<InputManager>("inputManager");
@@ -306,14 +309,6 @@ export class EditTrackState extends State<States> {
       state.transitionTo(States.RUNNING);
     });
 
-    inputManager.onKey(" ", () => {
-      if (this.selectedSegment) {
-        this.selectedSegment = undefined;
-      } else {
-        this.selectedSegment = new StraightTrack();
-      }
-    });
-
     inputManager.onMouse("left", () => {
       const track = getContextItem<TrackSystem>("track");
       if (this.ghostSegment && this.closestEnd) {
@@ -383,6 +378,18 @@ export class EditTrackState extends State<States> {
       }
     });
 
+    inputManager.onKey("r", () => {
+      if (!this.selectedSegment) return;
+      const segment = this.selectedSegment;
+      let angle = Math.PI / 12;
+      segment.rotate(angle);
+    });
+    inputManager.onKey("R", () => {
+      if (!this.selectedSegment) return;
+      const segment = this.selectedSegment;
+      let angle = -Math.PI / 12;
+      segment.rotate(angle);
+    });
     // TODO
     // Cache trains and save
 
@@ -404,6 +411,9 @@ export class EditTrackState extends State<States> {
     const inputManager = getContextItem<InputManager>("inputManager");
     inputManager.offKey("e");
     inputManager.offKey("w");
+    inputManager.offKey("z");
+    inputManager.offKey("r");
+    inputManager.offKey("R");
     inputManager.offKey("Escape");
     inputManager.offMouse("left");
     if (this.heldEvents.size > 0) {

src/state/states/LoadState.ts

@@ -34,12 +34,16 @@ export class LoadState extends State<States> {
 
     bootstrapInputs();
 
-    resources.set("engine-sprites", new Image());
-    resources.get<HTMLImageElement>("engine-sprites")!.src =
-      "/sprites/EngineSprites.png";
+    // This should be driven by a manifest
+    resources.set("snr:sprite/engine", new Image());
+    resources.set("snr:sprite/LargeLady", new Image());
+    // resources.get<HTMLImageElement>("snr:sprite/engine")!.src =
+    //   "/sprites/EngineSprites.png";
+
+    resources.set("snr:audio/ding", new Audio());
     resources.ready().then(() => {
       this.stateMachine.transitionTo(States.RUNNING);
-    });
+    }).catch((e) => console.error(e));
 
     const doodler = getContextItem<Doodler>("doodler");
     // this.layers.push(doodler.createLayer((_, __, dTime) => {

src/state/states/RunningState.ts

@@ -1,13 +1,16 @@
-import { Doodler } from "@bearmetal/doodler";
+import { Doodler, Point, Vector, ZoomableDoodler } from "@bearmetal/doodler";
 import { getContext, getContextItem } from "../../lib/context.ts";
 import { InputManager } from "../../lib/input.ts";
 import { TrackSystem } from "../../track/system.ts";
 import { Tender } from "../../train/cars.ts";
 import { RedEngine } from "../../train/engines.ts";
-import { DotFollower } from "../../train/newTrain.ts";
-import { Train } from "../../train/train.ts";
+// import { Train } from "../../train/train.ts";
+import { Train } from "../../train/newTrain/Train.ts";
+import { Bogie, Driver } from "../../train/newTrain/Bogie.ts";
+import { TrainCar } from "../../train/newTrain/TrainCar.ts";
 import { State } from "../machine.ts";
 import { States } from "./index.ts";
+import { LargeLady, LargeLadyTender } from "../../train/LargeLady.ts";
 
 export class RunningState extends State<States> {
   override name: States = States.RUNNING;
@@ -18,8 +21,20 @@ export class RunningState extends State<States> {
 
   layers: number[] = [];
 
+  activeTrain: Train | undefined;
+
   override update(dt: number): void {
     const ctx = getContext() as { trains: Train[]; track: TrackSystem };
+    const doodler = getContextItem<ZoomableDoodler>(
+      "doodler",
+    );
+    // if (this.activeTrain) {
+    //   // (doodler as any).origin = doodler.worldToScreen(
+    //   //   doodler.width - this.activeTrain.aabb.center.x,
+    //   //   doodler.height - this.activeTrain.aabb.center.y,
+    //   // );
+    //   doodler.centerCameraOn(this.activeTrain.aabb.center);
+    // }
     // const ctx = getContext() as { trains: DotFollower[]; track: TrackSystem };
     // TODO
     // Update trains
@@ -27,7 +42,7 @@ export class RunningState extends State<States> {
     // Handle input
     // Monitor world events
     for (const train of ctx.trains) {
-      train.move(dt);
+      train.update(dt);
     }
   }
   override start(): void {
@@ -58,28 +73,44 @@ export class RunningState extends State<States> {
       // const path = track.path;
       // const follower = new DotFollower(path, path.points[0].copy());
       // ctx.trains.push(follower);
-      const train = new Train(track.path, [new RedEngine(), new Tender()]);
+      // const train = new Train(track.path, [
+      //   new LargeLady(),
+      //   new LargeLadyTender(),
+      // ]);
+      const bogies = [new Bogie(20, 20, 1), new Driver(20, 20, 1)];
+      const train = new Train(track, [
+        new TrainCar(
+          bogies,
+        ),
+      ]);
+      track.generatePath();
+      const firstPoint = track.path.points[0].copy();
+      let prevOffset = 0;
+      for (let i = 0; i < bogies.length; i++) {
+        const b = bogies[i];
+        b.position = firstPoint.add(b.leadingOffset + prevOffset, 0);
+        b.prevPos = b.position.copy();
+        prevOffset += b.trailingOffset;
+      }
       ctx.trains.push(train);
     });
-    // const trainCount = 1000;
-    // for (let i = 0; i < trainCount; i++) {
-    //   const train = new Train(track.path, [new RedEngine(), new Tender()]);
-    //   ctx.trains.push(train);
-    // }
 
     inputManager.onKey("ArrowUp", () => {
       const trains = getContextItem<Train[]>("trains");
       for (const train of trains) {
-        train.speed += 1;
+        train.bogies.filter((b) => b instanceof Driver).forEach((b) => {
+          b.drivingForce += 10;
+          if (b.dir.mag() < 0.1) b.updateDirection(new Vector(1, 0));
+          b.velocity = new Vector(10, 0);
+        });
       }
-      // for (const [i, train] of trains.entries()) {
-      //   train.speed += .01 * i;
-      // }
     });
     inputManager.onKey("ArrowDown", () => {
       const trains = getContextItem<Train[]>("trains");
       for (const train of trains) {
-        train.speed -= 1;
+        train.bogies.filter((b) => b instanceof Driver).forEach((b) => {
+          b.drivingForce -= 10;
+        });
       }
     });
   }

src/test/angleMathBench.test.ts

@@ -0,0 +1,32 @@
+/// <reference no-default-lib="true" />
+/// <reference lib="esnext" />
+/// <reference lib="dom" />
+/// <reference lib="dom.iterable" />
+/// <reference lib="dom.asynciterable" />
+/// <reference lib="deno.ns" />
+
+import { averageAngles, lerpAngle } from "../math/lerp.ts";
+import { testPerformance } from "./bench.ts";
+
+Deno.test("angle math", () => {
+  console.log("Average angles");
+  testPerformance(
+    () => {
+      const a = Math.random() * Math.PI * 2;
+      const b = Math.random() * Math.PI * 2;
+      const avg = averageAngles(a, b);
+    },
+    10000,
+    60,
+  );
+  console.log("Lerp angles");
+  testPerformance(
+    () => {
+      const a = Math.random() * Math.PI * 2;
+      const b = Math.random() * Math.PI * 2;
+      const avg = lerpAngle(a, b, .5);
+    },
+    10000,
+    60,
+  );
+});

src/track/shapes.ts

@@ -18,9 +18,9 @@ export class SBendLeft extends TrackSegment {
     start = start || new Vector(100, 100);
     super([
       start,
-      start.copy().add(60, 0),
-      start.copy().add(90, -25),
-      start.copy().add(150, -25),
+      start.copy().add(80, 0),
+      start.copy().add(120, -25),
+      start.copy().add(200, -25),
     ]);
   }
 }
@@ -29,9 +29,9 @@ export class SBendRight extends TrackSegment {
     start = start || new Vector(100, 100);
     super([
       start,
-      start.copy().add(60, 0),
-      start.copy().add(90, 25),
-      start.copy().add(150, 25),
+      start.copy().add(80, 0),
+      start.copy().add(120, 25),
+      start.copy().add(200, 25),
     ]);
   }
 }
@@ -44,7 +44,7 @@ export class BankLeft extends TrackSegment {
     const p2 = start.copy();
     const p3 = start.copy();
     const p4 = start.copy();
-    const scale = 33;
+    const scale = 66;
 
     p2.add(new Vector(1, 0).mult(scale));
     p3.set(p2);
@@ -62,6 +62,110 @@ export class BankLeft extends TrackSegment {
     ]);
   }
 }
+export class WideBankLeft extends TrackSegment {
+  constructor(start?: Vector) {
+    start = start || new Vector(100, 100);
+
+    const p1 = start.copy();
+    const p2 = start.copy();
+    const p3 = start.copy();
+    const p4 = start.copy();
+    const scale = 70.4;
+
+    p2.add(new Vector(1, 0).mult(scale));
+    p3.set(p2);
+    const dirToP3 = Vector.fromAngle(-Math.PI / 12).mult(scale);
+    p3.add(dirToP3);
+    p4.set(p3);
+    const dirToP4 = Vector.fromAngle(-Math.PI / 6).mult(scale);
+    p4.add(dirToP4);
+
+    super([
+      p1,
+      p2,
+      p3,
+      p4,
+    ]);
+  }
+}
+export class TightBankLeft extends TrackSegment {
+  constructor(start?: Vector) {
+    start = start || new Vector(100, 100);
+
+    const p1 = start.copy();
+    const p2 = start.copy();
+    const p3 = start.copy();
+    const p4 = start.copy();
+    const scale = 61.57;
+
+    p2.add(new Vector(1, 0).mult(scale));
+    p3.set(p2);
+    const dirToP3 = Vector.fromAngle(-Math.PI / 12).mult(scale);
+    p3.add(dirToP3);
+    p4.set(p3);
+    const dirToP4 = Vector.fromAngle(-Math.PI / 6).mult(scale);
+    p4.add(dirToP4);
+
+    super([
+      p1,
+      p2,
+      p3,
+      p4,
+    ]);
+  }
+}
+export class TightBankRight extends TrackSegment {
+  constructor(start?: Vector) {
+    start = start || new Vector(100, 100);
+
+    const p1 = start.copy();
+    const p2 = start.copy();
+    const p3 = start.copy();
+    const p4 = start.copy();
+    const scale = 61.57;
+
+    p2.add(new Vector(1, 0).mult(scale));
+    p3.set(p2);
+    const dirToP3 = Vector.fromAngle(Math.PI / 12).mult(scale);
+    p3.add(dirToP3);
+    p4.set(p3);
+    const dirToP4 = Vector.fromAngle(Math.PI / 6).mult(scale);
+    p4.add(dirToP4);
+
+    super([
+      p1,
+      p2,
+      p3,
+      p4,
+    ]);
+  }
+}
+export class WideBankRight extends TrackSegment {
+  constructor(start?: Vector) {
+    start = start || new Vector(100, 100);
+
+    const p1 = start.copy();
+    const p2 = start.copy();
+    const p3 = start.copy();
+    const p4 = start.copy();
+    const scale = 70.4;
+
+    p2.add(new Vector(1, 0).mult(scale));
+    p3.set(p2);
+    const dirToP3 = Vector.fromAngle(Math.PI / 12).mult(scale);
+    p3.add(dirToP3);
+    p4.set(p3);
+    const dirToP4 = Vector.fromAngle(Math.PI / 6).mult(scale);
+    p4.add(dirToP4);
+
+    super([
+      p1,
+      p2,
+      p3,
+      p4,
+    ]);
+  }
+}
 export class BankRight extends TrackSegment {
   constructor(start?: Vector) {
     start = start || new Vector(100, 100);
@@ -70,7 +174,7 @@ export class BankRight extends TrackSegment {
     const p2 = start.copy();
     const p3 = start.copy();
     const p4 = start.copy();
-    const scale = 33;
+    const scale = 66;
 
     p2.add(new Vector(1, 0).mult(scale));
     p3.set(p2);

src/track/system.ts

@@ -2,15 +2,27 @@ import { Doodler, Point, Vector } from "@bearmetal/doodler";
 import { ComplexPath, PathSegment } from "../math/path.ts";
 import { getContextItem, setDefaultContext } from "../lib/context.ts";
 import { clamp } from "../math/clamp.ts";
+import { Debuggable } from "../lib/debuggable.ts";
+import { SpatialHashGrid } from "../physics/SpatialGrid.ts";
 
-export class TrackSystem {
+export class TrackSystem extends Debuggable {
   private _segments: Map<string, TrackSegment> = new Map();
   private doodler: Doodler;
+  public grid: SpatialHashGrid = new SpatialHashGrid(10);
 
   constructor(segments: TrackSegment[]) {
+    super("track");
     this.doodler = getContextItem<Doodler>("doodler");
     for (const segment of segments) {
       this._segments.set(segment.id, segment);
+      let [prev] = segment.evenPoints[0];
+      for (const [p] of segment.evenPoints.slice(1)) {
+        const seg: GridItem = [prev, p, new Set([segment.id])];
+        segment.lineSegments ??= [];
+        segment.lineSegments.push(seg);
+        this.grid.insert(seg);
+        prev = p;
+      }
     }
   }
 
@@ -26,6 +38,19 @@ export class TrackSystem {
     return this._segments.values().toArray().pop();
   }
 
+  getNearestSegment(pos: Vector) {
+    let minDistance = Infinity;
+    let nearestSegment: TrackSegment | undefined;
+    for (const segment of this._segments.values()) {
+      const distance = segment.getDistanceTo(pos);
+      if (distance < minDistance) {
+        minDistance = distance;
+        nearestSegment = segment;
+      }
+    }
+    return nearestSegment;
+  }
+
   optimize(percent: number) {
     console.log("Optimizing track", percent * 100 / 4);
     for (const segment of this._segments.values()) {
@@ -35,7 +60,7 @@ export class TrackSystem {
 
   recalculateAll() {
     for (const segment of this._segments.values()) {
-      segment.recalculateRailPoints();
+      segment.update();
       segment.length = segment.calculateApproxLength();
     }
   }
@@ -59,30 +84,21 @@ export class TrackSystem {
     for (const [i, segment] of this._segments.entries()) {
       segment.draw(showControls);
     }
+  }
 
-    // try {
-    //   if (getContextItem<boolean>("showEnds")) {
-    //     const ends = this.findEnds();
-    //     for (const end of ends) {
-    //       this.doodler.fillCircle(end.pos, 2, {
-    //         color: "red",
-    //         // weight: 3,
-    //       });
-    //       if (getContextItem<boolean>("debug")) {
-    //         this.doodler.line(
-    //           end.pos,
-    //           end.pos.copy().add(end.tangent.copy().mult(20)),
-    //           {
-    //             color: "blue",
-    //             // weight: 3,
-    //           },
-    //         );
-    //       }
-    //     }
-    //   }
-    // } catch {
-    //   setDefaultContext({ showEnds: false });
-    // }
+  override debugDraw(): void {
+    const debug = getContextItem("debug");
+    if (debug.track) {
+      for (const segment of this._segments.values()) {
+        segment.drawAABB();
+      }
+    }
+    this.grid.getAllSegments().forEach((segment) => {
+      this.doodler.drawLine(segment.slice(0, 2) as Vector[], {
+        color: "red",
+        weight: 2,
+      });
+    });
   }
 
   ends: Map<TrackSegment, [End, End]> = new Map();
@@ -183,6 +199,11 @@ export class TrackSystem {
       const prev = arr[i - 1];
       s.points[0] = prev.points[3];
       s.prev = prev;
+      let [prevEvenPoint] = prev.evenPoints[0];
+      for (const [p] of s.evenPoints.slice(1)) {
+        this.grid.insert([prevEvenPoint, p, new Set([s.id])]);
+        prevEvenPoint = p;
+      }
       prev.next = s;
     });
     if (flags.looping) {
@@ -281,15 +302,58 @@ export class TrackSegment extends PathSegment {
   normalPoints: Vector[] = [];
   antiNormalPoints: Vector[] = [];
   evenPoints: [Vector, number][] = [];
+  lineSegments?: GridItem[];
+
+  aabb!: AABB;
+
+  private trackGuage = 12;
 
   constructor(p: VectorSet, id?: string) {
     super(p);
     this.doodler = getContextItem<Doodler>("doodler");
     this.id = id ?? crypto.randomUUID();
-    this.recalculateRailPoints();
+    this.update();
+  }
 
-    const spacing = Math.ceil(this.length / 10);
-    this.evenPoints = this.calculateEvenlySpacedPoints(this.length / spacing);
+  getDistanceTo(pos: Vector) {
+    return Vector.dist(this.aabb.center, pos);
+  }
+
+  updateAABB() {
+    let minX = Infinity;
+    let maxX = -Infinity;
+    let minY = Infinity;
+    let maxY = -Infinity;
+
+    [...this.normalPoints, ...this.antiNormalPoints].forEach((p) => {
+      minX = Math.min(minX, p.x);
+      maxX = Math.max(maxX, p.x);
+      minY = Math.min(minY, p.y);
+      maxY = Math.max(maxY, p.y);
+    });
+
+    const width = maxX - minX;
+    const height = maxY - minY;
+    if (width < this.trackGuage) {
+      const extra = (this.trackGuage - width) / 2;
+      minX -= extra;
+      maxX += extra;
+    }
+    if (height < this.trackGuage) {
+      const extra = (this.trackGuage - height) / 2;
+      minY -= extra;
+      maxY += extra;
+    }
+    this.aabb = {
+      pos: new Vector(minX, minY),
+      x: minX,
+      y: minY,
+      width: maxX - minX,
+      height: maxY - minY,
+      center: new Vector(minX, minY).add(
+        new Vector(maxX - minX, maxY - minY).div(2),
+      ),
+    };
   }
 
   recalculateRailPoints(resolution = 60) {
@@ -298,12 +362,22 @@ export class TrackSegment extends PathSegment {
     for (let i = 0; i <= resolution; i++) {
       const t = i / resolution;
       const normal = this.tangent(t).rotate(Math.PI / 2);
-      normal.setMag(6);
+      normal.setMag(this.trackGuage / 2);
       const p = this.getPointAtT(t);
       this.normalPoints.push(p.copy().add(normal));
       this.antiNormalPoints.push(p.copy().add(normal.rotate(Math.PI)));
     }
   }
+  recalculateTiePoints() {
+    const spacing = Math.ceil(this.length / 10);
+    this.evenPoints = this.calculateEvenlySpacedPoints(this.length / spacing);
+  }
+
+  update() {
+    this.recalculateRailPoints();
+    this.recalculateTiePoints();
+    this.updateAABB();
+  }
 
   setTrack(t: TrackSystem) {
     this.track = t;
@@ -389,6 +463,15 @@ export class TrackSegment extends PathSegment {
     // });
   }
 
+  drawAABB() {
+    this.doodler.drawRect(this.aabb.pos, this.aabb.width, this.aabb.height, {
+      color: "lime",
+    });
+    this.doodler.drawCircle(this.aabb.center, 2, {
+      color: "cyan",
+    });
+  }
+
   serialize(): SerializedTrackSegment {
     return {
       p: this.points.map((p) => p.array()),
@@ -431,6 +514,7 @@ export class TrackSegment extends PathSegment {
   rotate(angle: number | Vector) {
     const [p1, p2, p3, p4] = this.points;
     let newP2;
+
     if (angle instanceof Vector) {
       const tan = angle;
       angle = tan.heading() - (this.lastHeading ?? 0);
@@ -467,7 +551,7 @@ export class TrackSegment extends PathSegment {
   }
 
   rotateAboutPoint(angle: number, point: Vector) {
-    if (!this.points.includes(point)) return;
+    // if (!this.points.includes(point)) return;
     point = point.copy();
     this.points.forEach((p, i) => {
       const relativePoint = Vector.sub(p, point);
@@ -499,7 +583,14 @@ export class Spline<T extends PathSegment = PathSegment> {
   ctx?: CanvasRenderingContext2D;
 
   evenPoints: PathPoint[];
-  pointSpacing: number;
+  _pointSpacing: number;
+  get pointSpacing() {
+    return this._pointSpacing;
+  }
+  set pointSpacing(value: number) {
+    this._pointSpacing = value;
+    this.evenPoints = this.calculateEvenlySpacedPoints(value);
+  }
 
   get points() {
     return Array.from(new Set(this.segments.flatMap((s) => s.points)));
@@ -514,8 +605,8 @@ export class Spline<T extends PathSegment = PathSegment> {
     if (this.segments.at(-1)?.next === this.segments[0]) {
       this.looped = true;
     }
-    this.pointSpacing = 1;
-    this.evenPoints = this.calculateEvenlySpacedPoints(1);
+    this._pointSpacing = 1;
+    this.evenPoints = this.calculateEvenlySpacedPoints(this._pointSpacing);
     this.nodes = [];
     // for (let i = 0; i < this.points.length; i += 3) {
     //   const node: IControlNode = {
@@ -549,7 +640,7 @@ export class Spline<T extends PathSegment = PathSegment> {
   }
 
   calculateEvenlySpacedPoints(spacing: number, resolution = 1): PathPoint[] {
-    this.pointSpacing = 1;
+    // this._pointSpacing = 1;
     // return this.segments.flatMap(s => s.calculateEvenlySpacedPoints(spacing, resolution));
     const points: PathPoint[] = [];
 
@@ -589,7 +680,7 @@ export class Spline<T extends PathSegment = PathSegment> {
       }
     }
 
-    this.evenPoints = points;
+    // this.evenPoints = points;
 
     return points;
   }

src/train/LargeLady.ts

@@ -0,0 +1,194 @@
+import { Doodler, Vector } from "@bearmetal/doodler";
+import { Train, TrainCar } from "./train.ts";
+import { getContextItem } from "../lib/context.ts";
+import { ResourceManager } from "../lib/resources.ts";
+import { debug } from "node:console";
+import { averageAngles, lerpAngle } from "../math/lerp.ts";
+
+export class LargeLady extends TrainCar {
+  scale = 1;
+  constructor() {
+    const resources = getContextItem<ResourceManager>("resources");
+    const img = resources.get<HTMLImageElement>("snr:sprite/LargeLady")!;
+    super(50, 10, img, 132, 23, {
+      at: new Vector(0, 0),
+      width: 132,
+      height: 23,
+    });
+
+    this.bogies = [
+      {
+        pos: new Vector(0, 0),
+        angle: 0,
+        length: 35 * this.scale,
+        sprite: {
+          at: new Vector(0, 24),
+          width: 35,
+          height: 23,
+          offset: new Vector(-23, -11.5),
+        },
+      },
+      {
+        pos: new Vector(0, 0),
+        angle: 0,
+        length: 64 * this.scale,
+        // sprite: {
+        //   at: new Vector(0, 23),
+        //   width: 33,
+        //   height: 19,
+        //   offset: new Vector(-19, -9.5),
+        // },
+        sprite: {
+          at: new Vector(36, 24),
+          width: 60,
+          height: 23,
+          offset: new Vector(-35, -11.5),
+        },
+      },
+      {
+        pos: new Vector(0, 0),
+        angle: 0,
+        length: 35 * this.scale,
+        sprite: {
+          at: new Vector(36, 24),
+          width: 60,
+          height: 23,
+          offset: new Vector(-35, -11.5),
+        },
+      },
+      {
+        pos: new Vector(0, 0),
+        angle: 0,
+        length: 28,
+        sprite: {
+          at: new Vector(97, 24),
+          width: 22,
+          height: 23,
+          offset: new Vector(-11, -11.5),
+        },
+      },
+    ];
+
+    this.leading = 23;
+  }
+
+  drawAngle?: number;
+
+  override draw(): void {
+    const doodler = getContextItem<Doodler>("doodler");
+    for (const b of this.bogies) {
+      if (!b.sprite) continue;
+      doodler.drawRotated(b.pos, b.angle + (b.rotate ? 0 : Math.PI), () => {
+        doodler.drawSprite(
+          this.img,
+          b.sprite!.at,
+          b.sprite!.width,
+          b.sprite!.height,
+          b.pos.copy().add(b.sprite!.offset ?? new Vector(0, 0)),
+          b.sprite!.width,
+          b.sprite!.height,
+        );
+      });
+    }
+
+    const b = this.bogies[2];
+    const a = this.bogies[1];
+    // const origin = Vector.add(Vector.sub(a.pos, b.pos).div(2), b.pos);
+    // const angle = Vector.sub(b.pos, a.pos).heading();
+    const debug = getContextItem<Debug>("debug");
+    if (debug.bogies) return;
+
+    const difAngle = Vector.sub(a.pos, b.pos).heading();
+    if (this.drawAngle == undefined) this.drawAngle = b.angle + Math.PI;
+    const origin = b.pos.copy().add(new Vector(33, 0).rotate(difAngle));
+    const angle = b.angle;
+    const avgAngle = averageAngles(difAngle, angle) + Math.PI;
+    this.drawAngle = lerpAngle(this.drawAngle, avgAngle, .2);
+
+    doodler.drawRotated(origin, this.drawAngle, () => {
+      this.sprite
+        ? doodler.drawSprite(
+          this.img,
+          this.sprite.at,
+          this.sprite.width,
+          this.sprite.height,
+          origin.copy().sub(
+            this.imgWidth * this.scale / 2,
+            this.imgHeight * this.scale / 2,
+          ),
+          this.imgWidth * this.scale,
+          this.imgHeight * this.scale,
+        )
+        : doodler.drawImage(
+          this.img,
+          origin.copy().sub(this.imgWidth / 2, this.imgHeight / 2),
+        );
+    });
+    // doodler.drawCircle(origin, 4, { color: "blue" });
+
+    doodler.deferDrawing(() => {
+      doodler.drawRotated(origin, this.drawAngle! + Math.PI, () => {
+        doodler.drawSprite(
+          this.img,
+          new Vector(133, 0),
+          28,
+          23,
+          origin.copy().sub(93, this.imgHeight / 2),
+          28,
+          23,
+        );
+      });
+    });
+  }
+}
+
+export class LargeLadyTender extends TrainCar {
+  constructor() {
+    const resources = getContextItem<ResourceManager>("resources");
+    const sprite = resources.get("snr:sprite/LargeLady");
+    super(40, 39, sprite, 98, 23, {
+      at: new Vector(0, 48),
+      width: 98,
+      height: 23,
+    });
+
+    this.leading = 19;
+  }
+
+  override draw(): void {
+    const doodler = getContextItem<Doodler>("doodler");
+    const b = this.bogies[0];
+    doodler.drawRotated(b.pos, b.angle, () => {
+      doodler.drawSprite(
+        this.img,
+        new Vector(97, 24),
+        22,
+        23,
+        b.pos.copy().sub(11, 11.5),
+        22,
+        23,
+      );
+    });
+
+    const angle = Vector.sub(this.bogies[1].pos, this.bogies[0].pos).heading();
+    const origin = this.bogies[1].pos.copy().add(
+      new Vector(-11, 0).rotate(angle),
+    );
+    doodler.drawRotated(origin, angle, () => {
+      this.sprite
+        ? doodler.drawSprite(
+          this.img,
+          this.sprite.at,
+          this.sprite.width,
+          this.sprite.height,
+          origin.copy().sub(this.imgWidth / 2, this.imgHeight / 2),
+          this.imgWidth,
+          this.imgHeight,
+        )
+        : doodler.drawImage(
+          this.img,
+          origin.copy().sub(this.imgWidth / 2, this.imgHeight / 2),
+        );
+    });
+  }
+}

src/train/cars.ts

@@ -6,50 +6,85 @@ import { getContextItem } from "../lib/context.ts";
 export class Tender extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(25, resources.get<HTMLImageElement>("engine-sprites")!, 40, 20, {
-      at: new Vector(80, 0),
-      width: 40,
-      height: 20,
-    });
+    super(
+      25,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      40,
+      20,
+      {
+        at: new Vector(80, 0),
+        width: 40,
+        height: 20,
+      },
+    );
   }
 }
 export class Tank extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(50, resources.get<HTMLImageElement>("engine-sprites")!, 70, 20, {
-      at: new Vector(80, 20),
-      width: 70,
-      height: 20,
-    });
+    super(
+      50,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      70,
+      20,
+      {
+        at: new Vector(80, 20),
+        width: 70,
+        height: 20,
+      },
+    );
   }
 }
 export class YellowDumpCar extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(50, resources.get<HTMLImageElement>("engine-sprites")!, 70, 20, {
-      at: new Vector(80, 40),
-      width: 70,
-      height: 20,
-    });
+    super(
+      50,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      70,
+      20,
+      {
+        at: new Vector(80, 40),
+        width: 70,
+        height: 20,
+      },
+    );
   }
 }
 export class GrayDumpCar extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(50, resources.get<HTMLImageElement>("engine-sprites")!, 70, 20, {
-      at: new Vector(80, 60),
-      width: 70,
-      height: 20,
-    });
+    super(
+      50,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      70,
+      20,
+      {
+        at: new Vector(80, 60),
+        width: 70,
+        height: 20,
+      },
+    );
   }
 }
 export class NullCar extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(50, resources.get<HTMLImageElement>("engine-sprites")!, 70, 20, {
-      at: new Vector(80, 80),
-      width: 70,
-      height: 20,
-    });
+    super(
+      50,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      70,
+      20,
+      {
+        at: new Vector(80, 80),
+        width: 70,
+        height: 20,
+      },
+    );
   }
 }

src/train/engines.ts

@@ -6,50 +6,85 @@ import { ResourceManager } from "../lib/resources.ts";
 export class RedEngine extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(55, resources.get<HTMLImageElement>("engine-sprites")!, 80, 20, {
-      at: new Vector(0, 60),
-      width: 80,
-      height: 20,
-    });
+    super(
+      55,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      80,
+      20,
+      {
+        at: new Vector(0, 60),
+        width: 80,
+        height: 20,
+      },
+    );
   }
 }
 export class PurpleEngine extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(55, resources.get<HTMLImageElement>("engine-sprites")!, 80, 20, {
-      at: new Vector(0, 60),
-      width: 80,
-      height: 20,
-    });
+    super(
+      55,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      80,
+      20,
+      {
+        at: new Vector(0, 60),
+        width: 80,
+        height: 20,
+      },
+    );
   }
 }
 export class GreenEngine extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(55, resources.get<HTMLImageElement>("engine-sprites")!, 80, 20, {
-      at: new Vector(0, 40),
-      width: 80,
-      height: 20,
-    });
+    super(
+      55,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      80,
+      20,
+      {
+        at: new Vector(0, 40),
+        width: 80,
+        height: 20,
+      },
+    );
   }
 }
 export class GrayEngine extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(55, resources.get<HTMLImageElement>("engine-sprites")!, 80, 20, {
-      at: new Vector(0, 20),
-      width: 80,
-      height: 20,
-    });
+    super(
+      55,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      80,
+      20,
+      {
+        at: new Vector(0, 20),
+        width: 80,
+        height: 20,
+      },
+    );
   }
 }
 export class BlueEngine extends TrainCar {
   constructor() {
     const resources = getContextItem<ResourceManager>("resources");
-    super(55, resources.get<HTMLImageElement>("engine-sprites")!, 80, 20, {
-      at: new Vector(0, 0),
-      width: 80,
-      height: 20,
-    });
+    super(
+      55,
+      10,
+      resources.get<HTMLImageElement>("snr:sprite/engine")!,
+      80,
+      20,
+      {
+        at: new Vector(0, 0),
+        width: 80,
+        height: 20,
+      },
+    );
   }
 }

src/train/newTrain.ts

@@ -1,147 +0,0 @@
-import { Doodler, Vector } from "@bearmetal/doodler";
-import { getContextItem } from "../lib/context.ts";
-import { Spline, TrackSegment } from "../track/system.ts";
-
-export class DotFollower {
-  position: Vector;
-  velocity: Vector;
-  acceleration: Vector;
-  maxSpeed: number;
-  maxForce: number;
-  _trailingPoint: number;
-  protected _leadingPoint: number;
-
-  path: Spline<TrackSegment>;
-
-  get trailingPoint() {
-    const desired = this.velocity.copy();
-    desired.normalize();
-    desired.mult(-this._trailingPoint);
-
-    return Vector.add(this.position, desired);
-  }
-
-  constructor(path: Spline<TrackSegment>, pos: Vector) {
-    this.path = path;
-    this.position = pos;
-    this.velocity = new Vector();
-    this.acceleration = new Vector();
-    this.maxSpeed = 3;
-    this.maxForce = 0.3;
-
-    this._trailingPoint = 0;
-    this._leadingPoint = 0;
-
-    this.init();
-  }
-
-  init() {
-  }
-
-  move(dt: number) {
-    dt *= 10;
-    const force = calculatePathForce(this, this.path.points);
-    this.applyForce(force.mult(dt));
-    this.velocity.limit(this.maxSpeed);
-    this.acceleration.limit(this.maxForce);
-    this.velocity.add(this.acceleration.copy().mult(dt));
-    this.position.add(this.velocity.copy().mult(dt));
-    this.edges();
-  }
-
-  edges() {
-    const doodler = getContextItem<Doodler>("doodler");
-
-    if (this.position.x > doodler.width) this.position.x = 0;
-    if (this.position.y > doodler.height) this.position.y = 0;
-    if (this.position.x < 0) this.position.x = doodler.width;
-    if (this.position.y < 0) this.position.y = doodler.height;
-  }
-
-  draw() {
-    const doodler = getContextItem<Doodler>("doodler");
-    doodler.drawRotated(this.position, this.velocity.heading() || 0, () => {
-      doodler.fillCenteredRect(this.position, 20, 20, { fillColor: "white" });
-    });
-    for (const point of this.path.points) {
-      doodler.drawCircle(point, 4, { color: "red", weight: 3 });
-    }
-  }
-
-  applyForce(force: Vector) {
-    this.velocity.add(force);
-  }
-
-  static edges(point: Vector, width: number, height: number) {
-    if (point.x > width) point.x = 0;
-    if (point.y > height) point.y = 0;
-    if (point.x < 0) point.x = width;
-    if (point.y < 0) point.y = height;
-  }
-}
-
-function closestPointOnLineSegment(p: Vector, a: Vector, b: Vector): Vector {
-  // Vector AB
-  // const AB = { x: b.x - a.x, y: b.y - a.y };
-  const AB = Vector.sub(b, a);
-  // Vector AP
-  // const AP = { x: p.x - a.x, y: p.y - a.y };
-  const AP = Vector.sub(p, a);
-  // Dot product of AP and AB
-  // const AB_AB = AB.x * AB.x + AB.y * AB.y;
-  const AB_AB = Vector.dot(AB, AB);
-  // const AP_AB = AP.x * AB.x + AP.y * AB.y;
-  const AP_AB = Vector.dot(AP, AB);
-  // Project AP onto AB
-  const t = AP_AB / AB_AB;
-
-  // Clamp t to the range [0, 1] to restrict to the segment
-  const tClamped = Math.max(0, Math.min(1, t));
-
-  // Closest point on the segment
-  return new Vector({ x: a.x + AB.x * tClamped, y: a.y + AB.y * tClamped });
-}
-
-function calculatePathForce(f: DotFollower, path: Vector[]) {
-  let closestPoint: Vector = path[0];
-  let minDistance = Infinity;
-
-  // Loop through each segment to find the closest point on the path
-  for (let i = 0; i < path.length - 1; i++) {
-    const segmentStart = path[i];
-    const segmentEnd = path[i + 1];
-
-    // Find the closest point on the segment
-    const closest = closestPointOnLineSegment(
-      f.position,
-      segmentStart,
-      segmentEnd,
-    );
-
-    // Calculate the distance from the follower to the closest point
-    // const distance = Math.sqrt(
-    //   Math.pow(follower.position.x - closest.x, 2) +
-    //     Math.pow(follower.position.y - closest.y, 2),
-    // );
-
-    const distance = Vector.dist(f.position, closest);
-
-    // Track the closest point
-    if (distance < minDistance) {
-      minDistance = distance;
-      closestPoint = closest;
-    }
-  }
-
-  // Calculate the force to apply toward the closest point
-  // const force = {
-  //   x: closestPoint.x - f.position.x,
-  //   y: closestPoint.y - f.position.y,
-  // };
-  const force = Vector.sub(closestPoint, f.position);
-
-  // Normalize the force and apply a magnitude (this will depend on your desired strength)
-  const magnitude = 100; // Adjust this based on your needs
-  force.setMag(magnitude);
-  return force;
-}

src/train/newTrain/Bogie.ts

@@ -0,0 +1,75 @@
+import { Vector } from "@bearmetal/doodler";
+import { getContextItem } from "../../lib/context.ts";
+import { Debuggable } from "../../lib/debuggable.ts";
+
+export class Bogie extends Debuggable {
+  private pos: Vector = new Vector(0, 0);
+  private prevPos: Vector = this.pos.copy();
+  public dir: Vector = new Vector(0, 0);
+  public velocity: Vector = new Vector(0, 0);
+  private acceleration: Vector = new Vector(0, 0);
+  private damping: number = 0.999;
+
+  constructor(
+    public leadingOffset: number,
+    public trailingOffset: number,
+    public mass: number = 1,
+  ) {
+    super("bogies");
+  }
+
+  get position() {
+    return this.pos;
+  }
+  set position(v: Vector) {
+    this.pos.set(v);
+  }
+
+  applyForce(force: Vector) {
+    this.acceleration.add(force.copy().div(this.mass));
+  }
+
+  update(dt: number) {
+    // Compute velocity from previous position
+    this.velocity = this.pos.copy().sub(this.prevPos);
+
+    // Apply friction to the velocity instead of velocity storage
+    this.velocity.mult(this.damping);
+
+    // Apply Verlet integration with forces
+    const temp = this.pos.copy();
+    this.pos.add(this.velocity.add(this.acceleration.copy().mult(dt * dt)));
+    this.prevPos = temp; // Store previous position
+    this.acceleration.set(0, 0); // Reset acceleration after update
+  }
+
+  draw() {}
+  debugDraw() {
+    const d = getContextItem("doodler");
+    d.deferDrawing(() => {
+      d.dot(this.pos, {
+        color: this instanceof Driver ? "red" : "white",
+        weight: 3,
+      });
+    });
+  }
+}
+
+export class Driver extends Bogie {
+  public drivingForce: number = 0;
+
+  override update(dt: number): void {
+    const force = this.dir.copy();
+    force.setMag(this.drivingForce);
+    this.applyForce(force);
+    super.update(dt);
+  }
+  updateDirection(dir: Vector) {
+    if (this.dir.dot(dir) < -0.5) {
+      // If new dir is nearly opposite, blend gradually
+      this.dir = this.dir.mult(0.9).add(dir.mult(0.1)).normalize();
+    } else {
+      this.dir = dir;
+    }
+  }
+}

src/train/newTrain/Train.ts

@@ -0,0 +1,70 @@
+import { getContextItem } from "../../lib/context.ts";
+import { TrackSystem } from "../../track/system.ts";
+import { Debuggable } from "../../lib/debuggable.ts";
+import { PhysicsSolver } from "../../physics/solver.ts";
+import { MoveTask, TrackConstraint, TrainTask } from "./physics.ts";
+import { TrainCar } from "./TrainCar.ts";
+import { Bogie } from "./Bogie.ts";
+
+export class Train extends Debuggable {
+  private _bogies: Bogie[];
+  get bogies() {
+    return this._bogies;
+  }
+
+  private solver: PhysicsSolver;
+
+  constructor(private track: TrackSystem, private cars: TrainCar[] = []) {
+    super("train");
+    this._bogies = this.recalculateBogies();
+    this.solver = new PhysicsSolver();
+    this._bogies.forEach((b) =>
+      this.solver.addTask(new TrackConstraint(b, this.track))
+    );
+    let prev: Bogie | null = null;
+    for (const car of this.cars) {
+      for (const b of car.bogies) {
+        if (prev) {
+          this.solver.addTask(
+            new TrainTask(prev, b, prev.trailingOffset + b.leadingOffset),
+          );
+        }
+        prev = b;
+      }
+    }
+    this.solver.addTask(new MoveTask(this));
+    this.solver.maxTickStep = 1 / 30;
+  }
+
+  addCar(car: TrainCar, toFront = false) {
+    this.cars.push(car);
+    for (const b of toFront ? car.bogies.reverse() : car.bogies) {
+      this.solver.addTask(new TrackConstraint(b, this.track));
+    }
+    this._bogies = this.recalculateBogies();
+  }
+  removeCar(car: TrainCar) {
+    this.cars = this.cars.filter((c) => c !== car);
+  }
+
+  recalculateBogies() {
+    return this.cars.flatMap((c) => c.bogies);
+  }
+
+  update(dt: number) {
+    this.solver.solve(dt);
+  }
+
+  draw() {
+    for (const car of this.cars) {
+      car.draw();
+    }
+  }
+
+  debugDraw(): void {
+    const d = getContextItem("doodler");
+    // this.track._segments.forEach((s) =>
+    //   s.evenPoints.forEach((p) => d.dot(p[0], { color: "lime", weight: 2 }))
+    // );
+  }
+}

src/train/newTrain/TrainCar.ts

@@ -0,0 +1,68 @@
+import { Vector } from "@bearmetal/doodler";
+import { getContextItem } from "../../lib/context.ts";
+import { Debuggable } from "../../lib/debuggable.ts";
+import { Bogie } from "./Bogie.ts";
+
+export class TrainCar extends Debuggable {
+  private spriteImg: HTMLImageElement;
+  private sprite?: ISprite;
+  constructor(
+    public bogies: Bogie[] = [],
+  ) {
+    super("car");
+    const res = getContextItem("resources");
+    this.spriteImg = res.get("snr:sprite/engine");
+    this.sprite = {
+      at: new Vector(80, 20),
+      width: 70,
+      height: 20,
+    };
+  }
+
+  draw(): void {
+    const doodler = getContextItem("doodler");
+    for (const b of this.bogies) {
+      doodler.drawCircle(b.position, 4, { color: "blue" });
+      doodler.fillText(
+        b.velocity.mag().toFixed(1).toString(),
+        b.position.copy().add(10, 10),
+        100,
+        {
+          color: "white",
+        },
+      );
+    }
+    const angle = Vector.sub(this.bogies[1].position, this.bogies[0].position)
+      .heading();
+    const origin = Vector.lerp(
+      this.bogies[0].position,
+      this.bogies[1].position,
+      .5,
+    );
+    doodler.drawRotated(origin, angle, () => {
+      this.sprite
+        ? doodler.drawSprite(
+          this.spriteImg,
+          this.sprite.at,
+          this.sprite.width,
+          this.sprite.height,
+          origin.copy().sub(
+            this.sprite.width / 2,
+            this.sprite.height / 2,
+          ),
+          this.sprite.width,
+          this.sprite.height,
+        )
+        : doodler.drawImage(
+          this.spriteImg,
+          origin.copy().sub(
+            this.spriteImg.width / 2,
+            this.spriteImg.height / 2,
+          ),
+        );
+    });
+  }
+
+  debugDraw(): void {
+  }
+}

src/train/newTrain/physics.ts

@@ -0,0 +1,138 @@
+import { Vector } from "@bearmetal/doodler";
+import { SolverTask } from "../../physics/solver.ts";
+import { Bogie, Driver } from "./Bogie.ts";
+import { TrackSystem } from "../../track/system.ts";
+import { clamp } from "../../math/clamp.ts";
+import { Train } from "./Train.ts";
+
+export class TrackConstraint extends SolverTask {
+  detached: boolean = false;
+  detachmentThreshold: number = 1;
+  reattachmentThreshold: number = 0.1;
+  pathTag?: tag;
+  constructor(
+    private node: Bogie,
+    private track: TrackSystem,
+  ) {
+    super();
+    this.priority = 1;
+    this.iterations = 1;
+  }
+  override apply(): boolean {
+    if (this.detached) {
+      if (this.node.velocity.mag() < this.reattachmentThreshold) {
+        this.detached = false;
+      } else {
+        this.active = false;
+        return false;
+      }
+    }
+
+    // if (this.node.velocity.mag() < 0) {
+    //   return false;
+    // }
+
+    const searchRadius = 10;
+    const nearbySegments = this.track.grid.query(
+      this.node.position,
+      searchRadius,
+      this.pathTag,
+    );
+
+    let closestSegment: GridItem | null = null;
+    let closestDistance = Infinity;
+    let projectedPoint: Vector | null = null;
+    for (const seg of nearbySegments) {
+      const candidate = this.closestPointOnSegment(seg, this.node.position);
+      const distance = this.node.position.dist(candidate);
+      if (distance < closestDistance) {
+        closestSegment = seg;
+        closestDistance = distance;
+        projectedPoint = candidate;
+      }
+    }
+
+    if (closestSegment && projectedPoint) {
+      const correctionForce = Vector.sub(projectedPoint, this.node.position);
+      const correctionMag = correctionForce.mag() * this.node.mass;
+
+      if (correctionMag > this.detachmentThreshold) {
+        this.detached = true;
+        return false;
+      }
+
+      this.node.position = projectedPoint;
+      const pathDir = Vector.sub(closestSegment[1], closestSegment[0])
+        .normalize();
+      this.node.velocity.set(pathDir.mult(this.node.velocity.dot(pathDir)));
+      if (this.node instanceof Driver) {
+        this.node.updateDirection(pathDir);
+      }
+    }
+
+    return true;
+  }
+
+  closestPointOnSegment([a, b]: GridItem, point: Vector): Vector {
+    const ab = Vector.sub(b, a);
+    const ap = Vector.sub(point, a);
+    const t = clamp(ab.dot(ap) / ab.magSq(), 0, 1);
+
+    return Vector.add(a, ab.mult(t));
+  }
+}
+
+export class MoveTask extends SolverTask {
+  constructor(
+    private train: Train,
+  ) {
+    super();
+    this.priority = 2;
+    this.iterations = 1;
+  }
+
+  override apply(dt: number): boolean {
+    for (const bogie of this.train.bogies) {
+      bogie.update(dt);
+    }
+    return true;
+  }
+}
+
+export class TrainTask extends SolverTask {
+  constructor(
+    private a: Bogie,
+    private b: Bogie,
+    private restLength: number,
+    private tolerance = 0,
+    private solid = false,
+    private breakThreshold = 10,
+  ) {
+    super();
+    this.iterations = 100;
+  }
+  override apply(): boolean {
+    const delta = Vector.sub(this.b.position, this.a.position);
+    const currentLength = delta.mag();
+    if (currentLength <= this.tolerance) return false;
+
+    const correction = currentLength - this.restLength;
+    const correctionVector = delta.normalize().mult(correction / currentLength);
+
+    const requiredForce = correction / this.restLength;
+
+    if (!this.solid && Math.abs(requiredForce) > this.breakThreshold) {
+      this.active = false;
+      return false;
+    }
+
+    const totalMass = this.a.mass + this.b.mass;
+    const aWeight = this.b.mass / totalMass;
+    const bWeight = this.b.mass / totalMass;
+
+    this.a.position.add(correctionVector.copy().mult(aWeight));
+    this.b.position.sub(correctionVector.copy().mult(bWeight));
+
+    return true;
+  }
+}

src/train/train.ts

@@ -2,7 +2,7 @@ import { getContextItem } from "../lib/context.ts";
 import { Doodler, Vector } from "@bearmetal/doodler";
 import { Spline, TrackSegment, TrackSystem } from "../track/system.ts";
 import { Debuggable } from "../lib/debuggable.ts";
-import { map } from "../math/lerp.ts";
+import { lerp, lerpAngle, map } from "../math/lerp.ts";
 
 export class Train extends Debuggable {
   nodes: Vector[] = [];
@@ -12,64 +12,91 @@ export class Train extends Debuggable {
   path: Spline<TrackSegment>;
   t: number;
 
-  spacing = 20;
+  spacing = 0;
 
-  speed = 10;
+  speed = 5;
+
+  aabb!: AABB;
 
   get segments() {
-    return Array.from(new Set(this.cars.flatMap((c) => c.segments)));
+    return Array.from(
+      new Set(this.cars.flatMap((c) => c.segments.values().toArray())),
+    );
   }
 
-  constructor(track: Spline<TrackSegment>, cars: TrainCar[]) {
+  constructor(track: Spline<TrackSegment>, cars: TrainCar[], t = 0) {
     super("train", "path");
     this.path = track;
-    this.t = 0;
+    this.path.pointSpacing = 4;
     this.cars = cars;
+    this.t = this.cars.reduce((acc, c) => acc + c.length, 0) +
+      (this.cars.length - 1) * this.spacing;
+    this.t = this.t / this.path.pointSpacing;
 
     let currentOffset = 0;
-    try {
-      for (const car of this.cars) {
-        currentOffset += this.spacing;
-        const a = this.path.followEvenPoints(this.t - currentOffset);
-        currentOffset += car.length;
-        const b = this.path.followEvenPoints(this.t - currentOffset);
-        car.points = [a.p, b.p];
-        this.nodes.push(a.p, b.p);
-        car.segments = [a.segmentId, b.segmentId];
-      }
-    } catch {
-      currentOffset = 0;
-      for (const car of this.cars.toReversed()) {
-        currentOffset += this.spacing;
-        const a = this.path.followEvenPoints(this.t - currentOffset);
-        currentOffset += car.length;
-        const b = this.path.followEvenPoints(this.t - currentOffset);
-        car.points = [a.p, b.p];
-        this.nodes.push(a.p, b.p);
-        car.segments = [a.segmentId, b.segmentId];
-      }
+    // try {
+    for (const car of this.cars) {
+      car.train = this;
+      currentOffset += car.moveAlongPath(this.t - currentOffset, true) +
+        this.spacing / this.path.pointSpacing;
     }
+    // } catch {
+    //   currentOffset = 0;
+    //   console.log("Reversed");
+    //   for (const car of this.cars.toReversed()) {
+    //     for (const [i, bogie] of car.bogies.entries().toArray().reverse()) {
+    //       currentOffset += bogie.length;
+    //       const a = this.path.followEvenPoints(this.t - currentOffset);
+    //       car.setBogiePosition(a.p, i);
+    //       this.nodes.push(a.p);
+    //       car.segments.add(a.segmentId);
+    //     }
+    //   }
+    // }
+
+    this.updateAABB();
   }
 
   move(dTime: number) {
     if (!this.speed) return;
-    this.t = this.t + this.speed * dTime * 10;
+    this.t = this.t + (this.speed / this.path.pointSpacing) * dTime * 10;
     // % this.path.evenPoints.length; // This should probably be on the track system
     let currentOffset = 0;
     for (const car of this.cars) {
       // This needs to be moved to the car itself
-      if (!car.points) return;
-      const [a, b] = car.points;
-      const nA = this.path.followEvenPoints(this.t - currentOffset);
-      a.set(nA.p);
-      currentOffset += car.length;
-      const nB = this.path.followEvenPoints(this.t - currentOffset);
-      b.set(nB.p);
-      currentOffset += this.spacing;
-      car.segments = [nA.segmentId, nB.segmentId];
+      // if (!car.points) return;
+      // const [a, b] = car.points;
+      // const nA = this.path.followEvenPoints(this.t - currentOffset);
+      // a.set(nA.p);
+      // currentOffset += car.length;
+      // const nB = this.path.followEvenPoints(this.t - currentOffset);
+      // b.set(nB.p);
+      // currentOffset += this.spacing;
+      // car.segments = [nA.segmentId, nB.segmentId];
       // car.draw();
+
+      currentOffset += car.moveAlongPath(this.t - currentOffset) +
+        (this.spacing / this.path.pointSpacing);
     }
     // this.draw();
+    this.updateAABB();
+  }
+
+  updateAABB() {
+    const minX = Math.min(...this.cars.map((c) => c.aabb.x));
+    const maxX = Math.max(...this.cars.map((c) => c.aabb.x + c.aabb.width));
+    const minY = Math.min(...this.cars.map((c) => c.aabb.y));
+    const maxY = Math.max(...this.cars.map((c) => c.aabb.y + c.aabb.height));
+    this.aabb = {
+      pos: new Vector(minX, minY),
+      x: minX,
+      y: minY,
+      width: maxX - minX,
+      height: maxY - minY,
+      center: new Vector(minX, minY).add(
+        new Vector(maxX - minX, maxY - minY).div(2),
+      ),
+    };
   }
 
   // draw() {
@@ -118,7 +145,7 @@ export class Train extends Debuggable {
       colors.push(colors.shift()!);
       colors.push(colors.shift()!);
       colors.push(colors.shift()!);
-      for (const [i, segmentId] of this.segments.entries()) {
+      for (const [i, segmentId] of this.segments.entries().toArray()) {
         const segment = track.getSegment(segmentId);
         segment &&
           doodler.drawBezier(...segment.points, {
@@ -127,6 +154,16 @@ export class Train extends Debuggable {
           });
       }
     }
+    if (debug.aabb) {
+      doodler.deferDrawing(() => {
+        doodler.drawRect(this.aabb.pos, this.aabb.width, this.aabb.height, {
+          color: "orange",
+        });
+        doodler.drawCircle(this.aabb.center, 2, {
+          color: "lime",
+        });
+      });
+    }
   }
 
   real2Track(length: number) {
@@ -134,6 +171,14 @@ export class Train extends Debuggable {
   }
 }
 
+interface Bogie {
+  pos: Vector;
+  angle: number;
+  length: number;
+  sprite?: ISprite & { offset?: Vector };
+  rotate?: boolean;
+}
+
 export class TrainCar extends Debuggable {
   img: HTMLImageElement;
   imgWidth: number;
@@ -141,31 +186,141 @@ export class TrainCar extends Debuggable {
   sprite?: ISprite;
 
   points?: [Vector, Vector, ...Vector[]];
-  length: number;
+  _length: number;
+  leading: number = 0;
 
-  segments: string[] = [];
+  bogies: Bogie[] = [];
+
+  segments: Set<string> = new Set();
+
+  train?: Train;
+
+  aabb!: AABB;
 
   constructor(
     length: number,
+    trailing: number,
     img: HTMLImageElement,
     w: number,
     h: number,
     sprite?: ISprite,
   ) {
-    super(true, "car");
+    super(true, "car", "bogies", "angles");
     this.img = img;
     this.sprite = sprite;
     this.imgWidth = w;
     this.imgHeight = h;
-    this.length = length;
+    this._length = length;
+
+    this.bogies = [
+      {
+        pos: new Vector(0, 0),
+        angle: 0,
+        length: length,
+      },
+      {
+        pos: new Vector(0, 0),
+        angle: 0,
+        length: trailing,
+      },
+    ];
+
+    this.updateAABB();
+  }
+
+  get length() {
+    return this.bogies.reduce((acc, b) => acc + b.length, 0) + this.leading;
+  }
+
+  setBogiePosition(pos: Vector, idx: number) {
+    this.bogies[idx].pos.set(pos);
+  }
+
+  update(dTime: number, t: number) {
+    if (this.train) {
+      for (const [i, bogie] of this.bogies.entries()) {
+        const a = this.train.path.followEvenPoints(t - this._length * i);
+      }
+    }
+  }
+
+  updateAABB() {
+    let minX = Infinity;
+    let maxX = -Infinity;
+    let minY = Infinity;
+    let maxY = -Infinity;
+
+    this.bogies.forEach((bogie, index) => {
+      // Unit vector in the direction the bogie is facing.
+      const u = new Vector(Math.cos(bogie.angle), Math.sin(bogie.angle));
+      // Perpendicular vector (to thicken the rectangle).
+      const v = new Vector(-Math.sin(bogie.angle), Math.cos(bogie.angle));
+
+      // For the first bogie, extend in the opposite direction by this.leading.
+      let front = bogie.pos.copy();
+      if (index === 0) {
+        front = front.sub(u.copy().rotate(Math.PI).mult(this.leading));
+      }
+      // Rear point is at bogie.pos plus the bogie length.
+      const rear = bogie.pos.copy().add(
+        u.copy().rotate(Math.PI).mult(bogie.length),
+      );
+
+      // Calculate half the height to offset from the center line.
+      const halfHeight = this.imgHeight / 2;
+
+      // Calculate the four corners of the rectangle.
+      const corners = [
+        front.copy().add(v.copy().mult(halfHeight)),
+        front.copy().add(v.copy().mult(-halfHeight)),
+        rear.copy().add(v.copy().mult(halfHeight)),
+        rear.copy().add(v.copy().mult(-halfHeight)),
+      ];
+
+      // Update the overall AABB limits.
+      corners.forEach((corner) => {
+        minX = Math.min(minX, corner.x);
+        minY = Math.min(minY, corner.y);
+        maxX = Math.max(maxX, corner.x);
+        maxY = Math.max(maxY, corner.y);
+      });
+    });
+    this.aabb = {
+      pos: new Vector(minX, minY),
+      center: new Vector(minX, minY).add(
+        new Vector(maxX - minX, maxY - minY).div(2),
+      ),
+      x: minX,
+      y: minY,
+      width: maxX - minX,
+      height: maxY - minY,
+    };
+  }
+
+  moveAlongPath(t: number, initial = false): number {
+    if (!this.train) return 0;
+    let offset = this.leading / this.train.path.pointSpacing;
+    this.segments.clear();
+    for (const [i, bogie] of this.bogies.entries()) {
+      const a = this.train.path.followEvenPoints(t - offset);
+      a.tangent.rotate(TWO_PI);
+      offset += bogie.length / this.train.path.pointSpacing;
+      this.setBogiePosition(a.p, i);
+      if (initial) bogie.angle = a.tangent.heading();
+      else {
+        bogie.angle = lerpAngle(a.tangent.heading(), bogie.angle, .1);
+      }
+      this.segments.add(a.segmentId);
+    }
+    this.updateAABB();
+    return offset;
   }
 
   draw() {
-    if (!this.points) return;
     const doodler = getContextItem<Doodler>("doodler");
-    const [a, b] = this.points;
-    const origin = Vector.add(Vector.sub(a, b).div(2), b);
-    const angle = Vector.sub(b, a).heading();
+    const [a, b] = this.bogies;
+    const origin = Vector.add(Vector.sub(a.pos, b.pos).div(2), b.pos);
+    const angle = Vector.sub(b.pos, a.pos).heading();
 
     doodler.drawCircle(origin, 4, { color: "blue" });
 
@@ -187,17 +342,100 @@ export class TrainCar extends Debuggable {
     });
   }
   override debugDraw(...args: unknown[]): void {
-    if (!this.points) return;
     const doodler = getContextItem<Doodler>("doodler");
-    doodler.drawLine(this.points, {
-      color: "blue",
-      weight: 3,
-    });
+    const debug = getContextItem<Debug>("debug");
+    if (debug.bogies) {
+      doodler.deferDrawing(() => {
+        for (const [i, b] of this.bogies.entries()) {
+          const next = this.bogies[i + 1];
+          if (!next) continue;
+          const dist = Vector.dist(b.pos, next.pos);
+          doodler.drawCircle(b.pos, 5, { color: "red" });
+          doodler.fillText(
+            dist.toFixed(1).toString(),
+            b.pos.copy().add(10, 10),
+            100,
+            {
+              color: "white",
+            },
+          );
+        }
+      });
+    }
+    if (debug.car) {
+      doodler.deferDrawing(() => {
+        doodler.drawLine(this.bogies.map((b) => b.pos), {
+          color: "blue",
+          weight: 2,
+        });
+        doodler.deferDrawing(() => {
+          const colors = getContextItem<string[]>("colors");
+          for (const [i, b] of this.bogies.entries()) {
+            doodler.drawCircle(b.pos, 5, { color: colors[i % colors.length] });
+            doodler.fillText(
+              b.length.toString(),
+              b.pos.copy().add(10, 0),
+              100,
+              {
+                color: "white",
+              },
+            );
+          }
+        });
+      });
+
+      if (debug.aabb) {
+        doodler.deferDrawing(() => {
+          doodler.drawRect(this.aabb.pos, this.aabb.width, this.aabb.height, {
+            color: "white",
+            weight: .5,
+          });
+          doodler.drawCircle(this.aabb.center, 2, {
+            color: "yellow",
+          });
+          doodler.fillText(
+            this.aabb.width.toFixed(1).toString(),
+            this.aabb.center.copy().add(10, 10),
+            100,
+            {
+              color: "white",
+            },
+          );
+        });
+      }
+    }
+
+    if (debug.angles) {
+      doodler.deferDrawing(() => {
+        const ps: { pos: Vector; angle: number }[] = [];
+        for (const [i, b] of this.bogies.entries()) {
+          ps.push({ pos: b.pos, angle: b.angle });
+          const next = this.bogies[i + 1];
+          if (!next) continue;
+          const heading = Vector.sub(next.pos, b.pos);
+          const p = b.pos.copy().add(heading.mult(.5));
+          ps.push({ pos: p, angle: heading.heading() });
+        }
+        for (const p of ps) {
+          doodler.dot(p.pos, { color: "green" });
+          doodler.fillText(
+            p.angle.toFixed(2).toString(),
+            p.pos.copy().add(0, 20),
+            100,
+            {
+              color: "white",
+            },
+          );
+        }
+      });
+    }
   }
 }
 
-interface ISprite {
-  at: Vector;
-  width: number;
-  height: number;
+declare global {
+  interface ISprite {
+    at: Vector;
+    width: number;
+    height: number;
+  }
 }

src/types.ts

@@ -18,8 +18,35 @@ declare global {
 
   type Debug = {
     track: boolean;
+    segment: boolean;
     train: boolean;
     car: boolean;
     path: boolean;
+    bogies: boolean;
+    angles: boolean;
+    aabb: boolean;
   };
+
+  type AABB = {
+    pos: Vector;
+    x: number;
+    y: number;
+    width: number;
+    height: number;
+    center: Vector;
+  };
+
+  type tag = string | number;
+}
+
+export function applyMixins(derivedCtor: any, baseCtors: any[]) {
+  baseCtors.forEach((baseCtor) => {
+    Object.getOwnPropertyNames(baseCtor.prototype).forEach((name) => {
+      Object.defineProperty(
+        derivedCtor.prototype,
+        name,
+        Object.getOwnPropertyDescriptor(baseCtor.prototype, name) ?? {},
+      );
+    });
+  });
 }