debugable wip

deno.json

@@ -14,7 +14,7 @@
     ]
   },
   "imports": {
-    "@bearmetal/doodler": "jsr:@bearmetal/doodler@0.0.5-e",
+    "@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"
   },

deno.lock

@@ -1,7 +1,7 @@
 {
   "version": "4",
   "specifiers": {
-    "jsr:@bearmetal/doodler@0.0.5-e": "0.0.5-e",
+    "jsr:@bearmetal/doodler@0.0.5-i": "0.0.5-i",
     "jsr:@std/assert@*": "1.0.10",
     "jsr:@std/assert@^1.0.10": "1.0.10",
     "jsr:@std/internal@^1.0.5": "1.0.5",
@@ -13,8 +13,8 @@
     "npm:web-ext@*": "8.4.0"
   },
   "jsr": {
-    "@bearmetal/doodler@0.0.5-e": {
+    "@bearmetal/doodler@0.0.5-i": {
-      "integrity": "70bd19397deac3b8a2ff6641b5df99bd1881581258c1c9ef3dab1170cf348430"
+      "integrity": "5aa20e3d838218f0934a268639f7c2afe706aed7f87f59570a26650b968f8c8b"
     },
     "@std/assert@1.0.10": {
       "integrity": "59b5cbac5bd55459a19045d95cc7c2ff787b4f8527c0dd195078ff6f9481fbb3",
@@ -2103,7 +2103,7 @@
   },
   "workspace": {
     "dependencies": [
-      "jsr:@bearmetal/doodler@0.0.5-e",
+      "jsr:@bearmetal/doodler@0.0.5-i",
       "npm:@deno/vite-plugin@^1.0.4",
       "npm:vite@^6.0.1"
     ]

src/lib/context.ts

@@ -3,12 +3,14 @@ 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,

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);
+      const debuggables = getContextItem("debuggables");
-      this.draw = drawFirst
+      debuggables.push(this);
-        ? (...args: unknown[]) => {
+      // const draw = this.draw.bind(this);
-          draw(...args);
+      // this.draw = drawFirst
-          const debug = getContextItem<Debug>("debug");
+      //   ? (...args: unknown[]) => {
-          if (debugKeys.some((k) => debug[k as keyof Debug])) {
+      //     draw(...args);
-            this.debugDraw();
+      //     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])) {
+      //   : (...args: unknown[]) => {
-            this.debugDraw();
+      //     const debug = getContextItem<Debug>("debug");
-          }
+      //     if (debugKeys.some((k) => debug[k as keyof Debug])) {
-          draw(...args);
+      //       this.debugDraw();
-        };
+      //     }
+      //     draw(...args);
+      //   };
     }
   }
 

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/RunningState.ts

@@ -4,8 +4,10 @@ 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";
@@ -26,13 +28,13 @@ export class RunningState extends State<States> {
     const doodler = getContextItem<ZoomableDoodler>(
       "doodler",
     );
-    if (this.activeTrain) {
+    // if (this.activeTrain) {
-      // (doodler as any).origin = doodler.worldToScreen(
+    //   // (doodler as any).origin = doodler.worldToScreen(
-      //   doodler.width - this.activeTrain.aabb.center.x,
+    //   //   doodler.width - this.activeTrain.aabb.center.x,
-      //   doodler.height - this.activeTrain.aabb.center.y,
+    //   //   doodler.height - this.activeTrain.aabb.center.y,
-      // );
+    //   // );
-      doodler.centerCameraOn(this.activeTrain.aabb.center);
+    //   doodler.centerCameraOn(this.activeTrain.aabb.center);
-    }
+    // }
     // const ctx = getContext() as { trains: DotFollower[]; track: TrackSystem };
     // TODO
     // Update trains
@@ -40,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 {
@@ -71,40 +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, [
+      // const train = new Train(track.path, [
-        new LargeLady(),
+      //   new LargeLady(),
-        new LargeLadyTender(),
+      //   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 train = new Train(track.path, [
-    //   new LargeLady(),
-    //   new LargeLadyTender(),
-    // ]);
-    // ctx.trains.push(train);
-    // this.activeTr0ain = train;
-    // const trainCount = 1000;
-    // for (let i = 0; i < trainCount; i++) {
-    //   const train = new Train(track.path, [
-    //     new LargeLady(),
-    //     new LargeLadyTender(),
-    //   ]);
-    //   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/track/system.ts

@@ -3,16 +3,26 @@ 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 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;
+      }
     }
   }
 
@@ -74,30 +84,6 @@ export class TrackSystem extends Debuggable {
     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 {
@@ -107,6 +93,12 @@ export class TrackSystem extends Debuggable {
         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();
@@ -207,6 +199,11 @@ export class TrackSystem extends Debuggable {
       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) {
@@ -305,6 +302,7 @@ export class TrackSegment extends PathSegment {
   normalPoints: Vector[] = [];
   antiNormalPoints: Vector[] = [];
   evenPoints: [Vector, number][] = [];
+  lineSegments?: GridItem[];
 
   aabb!: AABB;
 

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

@@ -432,8 +432,10 @@ export class TrainCar extends Debuggable {
   }
 }
 
-interface ISprite {
+declare global {
-  at: Vector;
+  interface ISprite {
-  width: number;
+    at: Vector;
-  height: number;
+    width: number;
+    height: number;
+  }
 }

src/types.ts

@@ -35,6 +35,8 @@ declare global {
     height: number;
     center: Vector;
   };
+
+  type tag = string | number;
 }
 
 export function applyMixins(derivedCtor: any, baseCtors: any[]) {