485 lines
15 KiB
JavaScript
485 lines
15 KiB
JavaScript
'use strict';
|
|
|
|
import * as Bezier from './bezierlib.js';
|
|
import * as Utils from './utillib.js';
|
|
import * as Config from './../config.js';
|
|
|
|
const defaults = {
|
|
maxSpeed: Config.routing.maxSpeed,
|
|
maxGrade: Config.routing.maxGrade,
|
|
gradePenalty: Config.routing.gradePenalty,
|
|
minGradeFactor: Config.routing.minGradeFactor,
|
|
snapSearchSteps: Config.routing.snapSearchSteps,
|
|
polylineStepMeters: Config.routing.polylineStepMeters,
|
|
maxSnapDistance: Config.routing.maxSnapDistance,
|
|
assumedUnpostedSpeed: Config.routing.assumedUnpostedSpeed,
|
|
unpostedSpeedLimit: Config.routing.unpostedSpeedLimit,
|
|
tightCurveRadius: Config.routing.tightCurveRadius,
|
|
tightCurveSpeed: Config.routing.tightCurveSpeed,
|
|
accel: Config.routing.accel,
|
|
decel: Config.routing.decel,
|
|
};
|
|
|
|
function distance2D(a, b){
|
|
const dx = a.x - b.x;
|
|
const dz = a.z - b.z;
|
|
return Math.sqrt(dx*dx + dz*dz);
|
|
}
|
|
|
|
function distance3D(a, b){
|
|
const dx = a.x - b.x;
|
|
const dy = (a.y ?? 0) - (b.y ?? 0);
|
|
const dz = a.z - b.z;
|
|
return Math.sqrt(dx*dx + dy*dy + dz*dz);
|
|
}
|
|
|
|
function boundsDistance2D(bounds, point){
|
|
let dx = 0;
|
|
let dz = 0;
|
|
if(point.x < bounds.min.x) dx = bounds.min.x - point.x;
|
|
else if(point.x > bounds.max.x) dx = point.x - bounds.max.x;
|
|
if(point.z < bounds.min.z) dz = bounds.min.z - point.z;
|
|
else if(point.z > bounds.max.z) dz = point.z - bounds.max.z;
|
|
return Math.sqrt(dx*dx + dz*dz);
|
|
}
|
|
|
|
function approximateCurveLength(curve, t0, t1, steps){
|
|
if(t0 === t1) return 0;
|
|
const start = Math.min(t0, t1);
|
|
const end = Math.max(t0, t1);
|
|
const segments = Math.max(2, steps ?? 16);
|
|
let length = 0;
|
|
let prev = Bezier.evaluatePoint(curve.start, curve.h1, curve.h2, curve.end, start);
|
|
for(let i=1; i<=segments; i++){
|
|
const t = Utils.lerp(start, end, i/segments);
|
|
const cur = Bezier.evaluatePoint(curve.start, curve.h1, curve.h2, curve.end, t);
|
|
length += distance3D(prev, cur);
|
|
prev = cur;
|
|
}
|
|
return length;
|
|
}
|
|
|
|
export function buildGraph(railTracks, precision = 2){
|
|
const nodes = [];
|
|
const edges = [];
|
|
const curves = [];
|
|
const nodeIndex = new Map();
|
|
|
|
const keyFor = pos => `${pos.x.toFixed(precision)}|${pos.y.toFixed(precision)}|${pos.z.toFixed(precision)}`;
|
|
|
|
const getNodeId = pos => {
|
|
const key = keyFor(pos);
|
|
let id = nodeIndex.get(key);
|
|
if(id == null){
|
|
id = nodes.length;
|
|
nodeIndex.set(key, id);
|
|
nodes.push({
|
|
id,
|
|
position: {x:pos.x, y:pos.y, z:pos.z},
|
|
});
|
|
edges[id] = [];
|
|
}
|
|
return id;
|
|
};
|
|
|
|
const addEdge = (from, to, edge) => {
|
|
edges[from].push(edge);
|
|
};
|
|
|
|
for(const railTrack of railTracks){
|
|
for(const curve of railTrack.curves){
|
|
const startId = getNodeId(curve.start);
|
|
const endId = getNodeId(curve.end);
|
|
curve._routeStartId = startId;
|
|
curve._routeEndId = endId;
|
|
curve._routeTrackName = railTrack.name;
|
|
const edgeBase = {
|
|
curve,
|
|
length: curve.length,
|
|
grade: curve.grade,
|
|
speed: curve.postedSpeed,
|
|
};
|
|
addEdge(startId, endId, {
|
|
...edgeBase,
|
|
to: endId,
|
|
t0: 0,
|
|
t1: 1,
|
|
});
|
|
addEdge(endId, startId, {
|
|
...edgeBase,
|
|
to: startId,
|
|
t0: 1,
|
|
t1: 0,
|
|
});
|
|
curves.push(curve);
|
|
}
|
|
}
|
|
|
|
return {nodes, edges, curves};
|
|
}
|
|
|
|
export function snapToNetwork(graph, position, options = {}){
|
|
const opts = {...defaults, ...options};
|
|
let best = null;
|
|
let bestDist = Infinity;
|
|
let bestT = 0;
|
|
let bestPoint = null;
|
|
for(const curve of graph.curves){
|
|
const boundDist = boundsDistance2D(curve.bounds, position);
|
|
if(boundDist > bestDist) continue;
|
|
const steps = opts.snapSearchSteps;
|
|
let localBestDist = Infinity;
|
|
let localBestT = 0;
|
|
let localBestPoint = null;
|
|
for(let i=0; i<=steps; i++){
|
|
const t = i/steps;
|
|
const p = Bezier.evaluatePoint(curve.start, curve.h1, curve.h2, curve.end, t);
|
|
const d = distance2D(p, position);
|
|
if(d < localBestDist){
|
|
localBestDist = d;
|
|
localBestT = t;
|
|
localBestPoint = p;
|
|
}
|
|
}
|
|
if(localBestDist < bestDist){
|
|
bestDist = localBestDist;
|
|
bestT = localBestT;
|
|
bestPoint = localBestPoint;
|
|
best = curve;
|
|
}
|
|
}
|
|
if(best == null) return null;
|
|
return {
|
|
curve: best,
|
|
t: bestT,
|
|
point: {x:bestPoint.x, y:bestPoint.y, z:bestPoint.z},
|
|
distance: bestDist,
|
|
};
|
|
}
|
|
|
|
class MinHeap{
|
|
constructor(){
|
|
this.data = [];
|
|
}
|
|
push(item){
|
|
this.data.push(item);
|
|
this.bubbleUp(this.data.length - 1);
|
|
}
|
|
pop(){
|
|
if(this.data.length === 0) return null;
|
|
const root = this.data[0];
|
|
const last = this.data.pop();
|
|
if(this.data.length > 0){
|
|
this.data[0] = last;
|
|
this.bubbleDown(0);
|
|
}
|
|
return root;
|
|
}
|
|
bubbleUp(index){
|
|
while(index > 0){
|
|
const parent = Math.floor((index - 1) / 2);
|
|
if(this.data[parent].priority <= this.data[index].priority) break;
|
|
[this.data[parent], this.data[index]] = [this.data[index], this.data[parent]];
|
|
index = parent;
|
|
}
|
|
}
|
|
bubbleDown(index){
|
|
const length = this.data.length;
|
|
while(true){
|
|
let left = index * 2 + 1;
|
|
let right = left + 1;
|
|
let smallest = index;
|
|
if(left < length && this.data[left].priority < this.data[smallest].priority) smallest = left;
|
|
if(right < length && this.data[right].priority < this.data[smallest].priority) smallest = right;
|
|
if(smallest === index) break;
|
|
[this.data[smallest], this.data[index]] = [this.data[index], this.data[smallest]];
|
|
index = smallest;
|
|
}
|
|
}
|
|
get size(){
|
|
return this.data.length;
|
|
}
|
|
}
|
|
|
|
function effectivePostedSpeed(edge, options){
|
|
let speed = edge.speed;
|
|
if(options.assumedUnpostedSpeed != null && speed >= options.assumedUnpostedSpeed){
|
|
speed = Math.min(speed, options.unpostedSpeedLimit);
|
|
}
|
|
if(edge.curve && edge.curve.curvature && options.tightCurveRadius != null){
|
|
const radius = 1 / edge.curve.curvature;
|
|
if(Number.isFinite(radius) && radius < options.tightCurveRadius){
|
|
speed = Math.min(speed, options.tightCurveSpeed);
|
|
}
|
|
}
|
|
return speed;
|
|
}
|
|
|
|
function effectiveSpeedUsed(edge, options){
|
|
let speed = effectivePostedSpeed(edge, options);
|
|
speed = Math.min(speed, options.maxSpeed ?? speed);
|
|
if(speed <= 0) return 0;
|
|
const gradePct = Math.abs(edge.grade) * 100;
|
|
if(options.maxGrade != null && gradePct > options.maxGrade) return 0;
|
|
const gradeFactor = Math.max(options.minGradeFactor, 1 - options.gradePenalty * gradePct);
|
|
return speed * gradeFactor;
|
|
}
|
|
|
|
function edgeCost(edge, options){
|
|
const speedKmh = effectiveSpeedUsed(edge, options);
|
|
if(speedKmh <= 0.01) return Infinity;
|
|
const speedMps = speedKmh * 1000 / 3600;
|
|
return edge.length / speedMps;
|
|
}
|
|
|
|
function segmentTravelTime(length, v0, v1, vMax, accel, decel){
|
|
if(vMax <= 0.01) return Infinity;
|
|
const a = Math.max(accel, 0.01);
|
|
const d = Math.max(decel, 0.01);
|
|
v0 = Math.min(v0, vMax);
|
|
v1 = Math.min(v1, vMax);
|
|
const accelDist = Math.max(0, (vMax*vMax - v0*v0) / (2 * a));
|
|
const decelDist = Math.max(0, (vMax*vMax - v1*v1) / (2 * d));
|
|
if(accelDist + decelDist <= length){
|
|
const cruise = length - accelDist - decelDist;
|
|
return (vMax - v0)/a + (vMax - v1)/d + cruise / vMax;
|
|
}
|
|
const numerator = 2 * length + (v0*v0)/a + (v1*v1)/d;
|
|
const denom = (1/a + 1/d);
|
|
const vPeak = Math.sqrt(Math.max(numerator / denom, 0));
|
|
return Math.max(0, (vPeak - v0)/a) + Math.max(0, (vPeak - v1)/d);
|
|
}
|
|
|
|
function estimateRouteTime(edges, speedLimits, options){
|
|
if(edges.length === 0) return 0;
|
|
const speeds = speedLimits.map(limit => Math.max(0, limit) * 1000 / 3600);
|
|
const boundaries = new Array(edges.length + 1);
|
|
boundaries[0] = 0;
|
|
boundaries[edges.length] = 0;
|
|
for(let i=1; i<edges.length; i++){
|
|
boundaries[i] = Math.min(speeds[i-1], speeds[i]);
|
|
}
|
|
let time = 0;
|
|
for(let i=0; i<edges.length; i++){
|
|
time += segmentTravelTime(
|
|
edges[i].length,
|
|
boundaries[i],
|
|
boundaries[i+1],
|
|
speeds[i],
|
|
options.accel,
|
|
options.decel
|
|
);
|
|
}
|
|
return time;
|
|
}
|
|
|
|
export function computeRouteStats(edges, options = {}){
|
|
const opts = {...defaults, ...options};
|
|
let totalLength = 0;
|
|
let maxGrade = 0;
|
|
let maxSpeedLimit = 0;
|
|
let maxSpeed = 0;
|
|
const usedLimits = [];
|
|
for(const edge of edges){
|
|
totalLength += edge.length;
|
|
maxGrade = Math.max(maxGrade, Math.abs(edge.grade) * 100);
|
|
const posted = effectivePostedSpeed(edge, opts);
|
|
const used = effectiveSpeedUsed(edge, opts);
|
|
maxSpeedLimit = Math.max(maxSpeedLimit, posted);
|
|
maxSpeed = Math.max(maxSpeed, used);
|
|
usedLimits.push(used);
|
|
}
|
|
const time = estimateRouteTime(edges, usedLimits, opts);
|
|
return {
|
|
length: totalLength,
|
|
time,
|
|
maxGrade,
|
|
maxSpeed,
|
|
maxSpeedLimit,
|
|
};
|
|
}
|
|
|
|
function buildPolyline(edges, options){
|
|
const points = [];
|
|
const stepMeters = options.polylineStepMeters;
|
|
for(const edge of edges){
|
|
const curve = edge.curve;
|
|
if(!curve) continue;
|
|
const steps = Math.max(2, Math.ceil(edge.length / stepMeters));
|
|
for(let i=0; i<=steps; i++){
|
|
const t = Utils.lerp(edge.t0, edge.t1, i/steps);
|
|
const p = Bezier.evaluatePoint(curve.start, curve.h1, curve.h2, curve.end, t);
|
|
const last = points[points.length - 1];
|
|
if(!last || last.x !== p.x || last.z !== p.z){
|
|
points.push({x:p.x, z:p.z});
|
|
}
|
|
}
|
|
}
|
|
return points;
|
|
}
|
|
|
|
export function findRoute(graph, startPos, endPos, options = {}){
|
|
const opts = {...defaults, ...options};
|
|
const startSnap = snapToNetwork(graph, startPos, opts);
|
|
const endSnap = snapToNetwork(graph, endPos, opts);
|
|
if(!startSnap || !endSnap) return null;
|
|
if(opts.maxSnapDistance != null){
|
|
if(startSnap.distance > opts.maxSnapDistance || endSnap.distance > opts.maxSnapDistance) return null;
|
|
}
|
|
|
|
const baseCount = graph.nodes.length;
|
|
const startId = baseCount;
|
|
const endId = baseCount + 1;
|
|
const tempPositions = [];
|
|
tempPositions[startId] = startSnap.point;
|
|
tempPositions[endId] = endSnap.point;
|
|
|
|
const extraEdges = [];
|
|
const addTempEdge = (from, to, edge) => {
|
|
if(!extraEdges[from]) extraEdges[from] = [];
|
|
extraEdges[from].push(edge);
|
|
};
|
|
|
|
const connectSnap = (snap, nodeId) => {
|
|
const curve = snap.curve;
|
|
const startNode = curve._routeStartId;
|
|
const endNode = curve._routeEndId;
|
|
const t = snap.t;
|
|
const steps = Math.max(6, Math.ceil(curve.length / 80));
|
|
const lenToStart = approximateCurveLength(curve, 0, t, steps);
|
|
const lenToEnd = approximateCurveLength(curve, t, 1, steps);
|
|
const edgeBase = {
|
|
curve,
|
|
grade: curve.grade,
|
|
speed: curve.postedSpeed,
|
|
};
|
|
addTempEdge(nodeId, startNode, {
|
|
...edgeBase,
|
|
to: startNode,
|
|
length: lenToStart,
|
|
t0: t,
|
|
t1: 0,
|
|
});
|
|
addTempEdge(nodeId, endNode, {
|
|
...edgeBase,
|
|
to: endNode,
|
|
length: lenToEnd,
|
|
t0: t,
|
|
t1: 1,
|
|
});
|
|
addTempEdge(startNode, nodeId, {
|
|
...edgeBase,
|
|
to: nodeId,
|
|
length: lenToStart,
|
|
t0: 0,
|
|
t1: t,
|
|
});
|
|
addTempEdge(endNode, nodeId, {
|
|
...edgeBase,
|
|
to: nodeId,
|
|
length: lenToEnd,
|
|
t0: 1,
|
|
t1: t,
|
|
});
|
|
};
|
|
|
|
connectSnap(startSnap, startId);
|
|
connectSnap(endSnap, endId);
|
|
|
|
if(startSnap.curve === endSnap.curve){
|
|
const curve = startSnap.curve;
|
|
const steps = Math.max(6, Math.ceil(curve.length / 80));
|
|
const lenBetween = approximateCurveLength(curve, startSnap.t, endSnap.t, steps);
|
|
const edgeBase = {
|
|
curve,
|
|
grade: curve.grade,
|
|
speed: curve.postedSpeed,
|
|
};
|
|
addTempEdge(startId, endId, {
|
|
...edgeBase,
|
|
to: endId,
|
|
length: lenBetween,
|
|
t0: startSnap.t,
|
|
t1: endSnap.t,
|
|
});
|
|
addTempEdge(endId, startId, {
|
|
...edgeBase,
|
|
to: startId,
|
|
length: lenBetween,
|
|
t0: endSnap.t,
|
|
t1: startSnap.t,
|
|
});
|
|
}
|
|
|
|
const totalNodes = baseCount + 2;
|
|
const dist = new Array(totalNodes).fill(Infinity);
|
|
const prev = new Array(totalNodes).fill(-1);
|
|
const prevEdge = new Array(totalNodes).fill(null);
|
|
const visited = new Array(totalNodes).fill(false);
|
|
const heap = new MinHeap();
|
|
|
|
const nodePosition = id => {
|
|
if(id < baseCount) return graph.nodes[id].position;
|
|
return tempPositions[id];
|
|
};
|
|
|
|
const maxSpeedMps = (opts.maxSpeed * 1000 / 3600);
|
|
const heuristic = id => {
|
|
const pos = nodePosition(id);
|
|
const target = tempPositions[endId];
|
|
return distance2D(pos, target) / Math.max(maxSpeedMps, 0.1);
|
|
};
|
|
|
|
dist[startId] = 0;
|
|
heap.push({id:startId, priority:heuristic(startId)});
|
|
|
|
const getNeighbors = id => {
|
|
const baseEdges = id < baseCount ? graph.edges[id] : [];
|
|
const extra = extraEdges[id] ?? [];
|
|
return baseEdges.concat(extra);
|
|
};
|
|
|
|
while(heap.size){
|
|
const current = heap.pop();
|
|
if(!current) break;
|
|
const id = current.id;
|
|
if(visited[id]) continue;
|
|
visited[id] = true;
|
|
if(id === endId) break;
|
|
for(const edge of getNeighbors(id)){
|
|
const cost = edgeCost(edge, opts);
|
|
if(cost === Infinity) continue;
|
|
const next = edge.to;
|
|
const alt = dist[id] + cost;
|
|
if(alt < dist[next]){
|
|
dist[next] = alt;
|
|
prev[next] = id;
|
|
prevEdge[next] = edge;
|
|
heap.push({id:next, priority:alt + heuristic(next)});
|
|
}
|
|
}
|
|
}
|
|
|
|
if(!Number.isFinite(dist[endId])) return null;
|
|
|
|
const pathEdges = [];
|
|
let cur = endId;
|
|
while(cur !== startId && cur !== -1){
|
|
const edge = prevEdge[cur];
|
|
if(!edge) break;
|
|
pathEdges.push(edge);
|
|
cur = prev[cur];
|
|
}
|
|
pathEdges.reverse();
|
|
|
|
return {
|
|
edges: pathEdges,
|
|
polyline: buildPolyline(pathEdges, opts),
|
|
stats: computeRouteStats(pathEdges, opts),
|
|
snaps: {
|
|
start: startSnap,
|
|
end: endSnap,
|
|
}
|
|
};
|
|
}
|