Add foldable pathfinding panel and richer ETA tooltips
This commit is contained in:
ololukaszuk committed
1 parent
401dad3170
commit
e7940ae0cb
6 files changed
+1063
-3
No files matched your search
@@ -0,0 +1,484 @@
|
||||
'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,
|
||||
}
|
||||
};
|
||||
}
|
||||
Reference in new issue
Block a user