From 9500f6dabfc4b8e7967e30990c34f428e34e6dfe Mon Sep 17 00:00:00 2001 From: Emma Date: Sat, 15 Mar 2025 15:45:44 -0600 Subject: [PATCH] Physics based car structure --- deno.json | 2 +- deno.lock | 8 +- src/physics/SpatialGrid.ts | 66 ++++++++++++++ src/physics/solver.ts | 39 ++++++++ src/state/states/RunningState.ts | 70 ++++++++------- src/track/system.ts | 46 +++++----- src/train/newTrain.ts | 147 ------------------------------- src/train/newTrain/Bogie.ts | 75 ++++++++++++++++ src/train/newTrain/Train.ts | 70 +++++++++++++++ src/train/newTrain/TrainCar.ts | 68 ++++++++++++++ src/train/newTrain/physics.ts | 138 +++++++++++++++++++++++++++++ src/train/train.ts | 10 ++- src/types.ts | 2 + 13 files changed, 529 insertions(+), 212 deletions(-) create mode 100644 src/physics/SpatialGrid.ts create mode 100644 src/physics/solver.ts delete mode 100644 src/train/newTrain.ts create mode 100644 src/train/newTrain/Bogie.ts create mode 100644 src/train/newTrain/Train.ts create mode 100644 src/train/newTrain/TrainCar.ts create mode 100644 src/train/newTrain/physics.ts diff --git a/deno.json b/deno.json index b3c33c3..4c3060f 100644 --- a/deno.json +++ b/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" }, diff --git a/deno.lock b/deno.lock index dd91014..904efae 100644 --- a/deno.lock +++ b/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": { - "integrity": "70bd19397deac3b8a2ff6641b5df99bd1881581258c1c9ef3dab1170cf348430" + "@bearmetal/doodler@0.0.5-i": { + "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" ] diff --git a/src/physics/SpatialGrid.ts b/src/physics/SpatialGrid.ts new file mode 100644 index 0000000..c11cfa5 --- /dev/null +++ b/src/physics/SpatialGrid.ts @@ -0,0 +1,66 @@ +import { Vector } from "@bearmetal/doodler"; + +declare global { + type GridItem = [Vector, Vector, Set]; +} +export class SpatialHashGrid { + private grid: Map = 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 = 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(); + } +} diff --git a/src/physics/solver.ts b/src/physics/solver.ts new file mode 100644 index 0000000..c68600f --- /dev/null +++ b/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); + } +} diff --git a/src/state/states/RunningState.ts b/src/state/states/RunningState.ts index 356532f..630f63a 100644 --- a/src/state/states/RunningState.ts +++ b/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 { const doodler = getContextItem( "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); - } + // 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 @@ -40,7 +42,7 @@ export class RunningState extends State { // 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 { // const path = track.path; // const follower = new DotFollower(path, path.points[0].copy()); // ctx.trains.push(follower); - const train = new Train(track.path, [ - new LargeLady(), - new LargeLadyTender(), + // 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 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("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("trains"); for (const train of trains) { - train.speed -= 1; + train.bogies.filter((b) => b instanceof Driver).forEach((b) => { + b.drivingForce -= 10; + }); } }); } diff --git a/src/track/system.ts b/src/track/system.ts index 51be6d0..945d52d 100644 --- a/src/track/system.ts +++ b/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 = new Map(); private doodler: Doodler; + public grid: SpatialHashGrid = new SpatialHashGrid(10); constructor(segments: TrackSegment[]) { super("track"); this.doodler = getContextItem("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("showEnds")) { - // const ends = this.findEnds(); - // for (const end of ends) { - // this.doodler.fillCircle(end.pos, 2, { - // color: "red", - // // weight: 3, - // }); - // if (getContextItem("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 = 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; diff --git a/src/train/newTrain.ts b/src/train/newTrain.ts deleted file mode 100644 index 6c3cd3d..0000000 --- a/src/train/newTrain.ts +++ /dev/null @@ -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; - - get trailingPoint() { - const desired = this.velocity.copy(); - desired.normalize(); - desired.mult(-this._trailingPoint); - - return Vector.add(this.position, desired); - } - - constructor(path: Spline, 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"); - - 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.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; -} diff --git a/src/train/newTrain/Bogie.ts b/src/train/newTrain/Bogie.ts new file mode 100644 index 0000000..482ef5e --- /dev/null +++ b/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; + } + } +} diff --git a/src/train/newTrain/Train.ts b/src/train/newTrain/Train.ts new file mode 100644 index 0000000..c026c41 --- /dev/null +++ b/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 })) + // ); + } +} diff --git a/src/train/newTrain/TrainCar.ts b/src/train/newTrain/TrainCar.ts new file mode 100644 index 0000000..d3f98aa --- /dev/null +++ b/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 { + } +} diff --git a/src/train/newTrain/physics.ts b/src/train/newTrain/physics.ts new file mode 100644 index 0000000..e57a2d0 --- /dev/null +++ b/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; + } +} diff --git a/src/train/train.ts b/src/train/train.ts index 5431bc6..4282223 100644 --- a/src/train/train.ts +++ b/src/train/train.ts @@ -432,8 +432,10 @@ export class TrainCar extends Debuggable { } } -interface ISprite { - at: Vector; - width: number; - height: number; +declare global { + interface ISprite { + at: Vector; + width: number; + height: number; + } } diff --git a/src/types.ts b/src/types.ts index f093362..514c0b4 100644 --- a/src/types.ts +++ b/src/types.ts @@ -35,6 +35,8 @@ declare global { height: number; center: Vector; }; + + type tag = string | number; } export function applyMixins(derivedCtor: any, baseCtors: any[]) {