261 lines
10 KiB
JavaScript
261 lines
10 KiB
JavaScript
"use strict";
|
|
|
|
const fs = require("fs");
|
|
const { parseOsm } = require("./osm");
|
|
|
|
const EARTH_RADIUS = 6371008.8;
|
|
const CURB_OFFSET_METERS = 5.2;
|
|
const MAST_REACH_METERS = 4.5;
|
|
// This layout is serialized with the anchors so Blender's static structure and
|
|
// Cesium's dynamic overlay cannot independently drift in size or handedness.
|
|
// Lateral offsets use the approach travel direction: positive is the driver's
|
|
// right. The countdown board therefore sits at +1.15m from the signal head.
|
|
const SIGNAL_LAYOUT = Object.freeze({
|
|
poleHeightMeters: 6.7,
|
|
poleRadiusMeters: 0.13,
|
|
armWidthMeters: 0.21,
|
|
// The mast arm and the signal head share this centre elevation.
|
|
mastHeightMeters: 6.25,
|
|
headCenterHeightMeters: 6.25,
|
|
headWidthMeters: 0.68,
|
|
headDepthMeters: 0.30,
|
|
headBodyHeightMeters: 1.62,
|
|
lensRadiusMeters: 0.22,
|
|
lensDepthMeters: 0.07,
|
|
lensFaceOffsetMeters: 0.18,
|
|
lensVerticalOffsetsMeters: [0.49, -0.01, -0.51],
|
|
countdownLateralMeters: 1.15,
|
|
countdownFaceOffsetMeters: 0.05,
|
|
countdownWidthMeters: 0.82,
|
|
countdownDepthMeters: 0.14,
|
|
countdownHeightMeters: 0.56,
|
|
// The countdown board is fixed on the mast arm, not hung below it.
|
|
countdownVerticalOffsetMeters: 0.0,
|
|
});
|
|
|
|
function buildTrafficSignals(stopLines, intersections, controls = []) {
|
|
const centers = (intersections.features || []).map((feature, index) => {
|
|
const point = polygonCenter(feature.geometry);
|
|
return { id: `intersection-${index + 1}`, point, radius: polygonRadius(feature.geometry, point) };
|
|
}).filter((entry) => entry.point);
|
|
const candidates = [];
|
|
for (const feature of stopLines.features || []) {
|
|
const center = polygonCenter(feature.geometry);
|
|
if (!center) continue;
|
|
const intersection = nearestCenter(center, centers);
|
|
if (!intersection || metersBetween(center, intersection.point) > 32) continue;
|
|
const axis = roadAxis(feature.geometry, center, intersection.point);
|
|
if (!axis) continue;
|
|
// A vehicle signal belongs beyond the junction, facing back toward the
|
|
// approaching stop line. Use the far edge of the intersection, never the
|
|
// near-side stop-line area where it would read as a pedestrian signal.
|
|
const right = [axis[1], -axis[0]];
|
|
const farSide = moveMeters(intersection.point, axis, intersection.radius + 3.2);
|
|
// The pole is on the far-side sidewalk, not at the stop line or inside
|
|
// the intersection. Its mast then reaches back above the approach lanes.
|
|
const point = moveMeters(farSide, right, CURB_OFFSET_METERS);
|
|
candidates.push({
|
|
intersectionId: intersection.id,
|
|
center,
|
|
axis,
|
|
point,
|
|
headingDegrees: normalizeDegrees(Math.atan2(axis[0], axis[1]) * 180 / Math.PI),
|
|
});
|
|
}
|
|
const signals = [];
|
|
for (const control of controls) {
|
|
const controlPoint = [Number(control.longitude), Number(control.latitude)];
|
|
if (!controlPoint.every(Number.isFinite)) continue;
|
|
// A traffic-signal node on a through road is not a controlled vehicle
|
|
// junction. Its connected motor-road arms are the source of truth.
|
|
if (!Array.isArray(control.arms) || control.arms.length < 3) continue;
|
|
const intersection = nearestCenter(controlPoint, centers);
|
|
if (!intersection || metersBetween(controlPoint, intersection.point) > 32) continue;
|
|
const arms = matchOsmArms(candidates.filter((candidate) => candidate.intersectionId === intersection.id), controlPoint, control.arms);
|
|
const groups = phaseGroups(arms);
|
|
for (const [index, candidate] of arms.entries()) {
|
|
signals.push({
|
|
id: `signal-${signals.length + 1}`,
|
|
controlId: String(control.id || ""),
|
|
intersectionId: intersection.id,
|
|
phaseGroup: groups[index],
|
|
longitude: candidate.point[0],
|
|
latitude: candidate.point[1],
|
|
stopLongitude: candidate.center[0],
|
|
stopLatitude: candidate.center[1],
|
|
headingDegrees: candidate.headingDegrees,
|
|
mastReachMeters: MAST_REACH_METERS,
|
|
pose: buildSignalPose(candidate.point, candidate.axis, MAST_REACH_METERS),
|
|
});
|
|
}
|
|
}
|
|
return { version: 3, layout: SIGNAL_LAYOUT, signals };
|
|
}
|
|
|
|
function uniqueApproachArms(candidates, controlPoint) {
|
|
const sorted = candidates.map((candidate) => ({
|
|
...candidate,
|
|
armHeading: normalizeDegrees(headingBetween(controlPoint, candidate.center)),
|
|
controlDistance: metersBetween(controlPoint, candidate.center),
|
|
})).sort((a, b) => a.armHeading - b.armHeading || a.controlDistance - b.controlDistance);
|
|
const arms = [];
|
|
for (const candidate of sorted) {
|
|
const duplicate = arms.find((arm) => angularDistance(arm.armHeading, candidate.armHeading) <= 25);
|
|
if (!duplicate) arms.push(candidate);
|
|
}
|
|
return arms;
|
|
}
|
|
|
|
function matchOsmArms(candidates, controlPoint, osmArms) {
|
|
const withHeadings = candidates.map((candidate) => ({
|
|
...candidate,
|
|
armHeading: normalizeDegrees(headingBetween(controlPoint, candidate.center)),
|
|
}));
|
|
if (!Array.isArray(osmArms) || !osmArms.length) return uniqueApproachArms(withHeadings, controlPoint);
|
|
const remaining = withHeadings.slice();
|
|
const matched = [];
|
|
for (const osmArm of osmArms) {
|
|
let bestIndex = -1;
|
|
let bestDistance = Infinity;
|
|
for (let index = 0; index < remaining.length; index += 1) {
|
|
const distance = angularDistance(remaining[index].armHeading, osmArm.headingDegrees);
|
|
if (distance < bestDistance) { bestDistance = distance; bestIndex = index; }
|
|
}
|
|
if (bestIndex >= 0 && bestDistance <= 45) {
|
|
matched.push(remaining.splice(bestIndex, 1)[0]);
|
|
} else {
|
|
matched.push(fallbackCandidate(controlPoint, osmArm));
|
|
}
|
|
}
|
|
return matched;
|
|
}
|
|
|
|
function fallbackCandidate(controlPoint, osmArm) {
|
|
const outward = headingVector(osmArm.headingDegrees);
|
|
const axis = [-outward[0], -outward[1]];
|
|
const stopDistance = 8.0;
|
|
const stop = moveMeters(controlPoint, outward, stopDistance);
|
|
const farSide = moveMeters(controlPoint, axis, 3.2);
|
|
return {
|
|
center: stop,
|
|
axis,
|
|
point: moveMeters(farSide, [axis[1], -axis[0]], CURB_OFFSET_METERS),
|
|
headingDegrees: normalizeDegrees(Math.atan2(axis[0], axis[1]) * 180 / Math.PI),
|
|
fallback: true,
|
|
};
|
|
}
|
|
|
|
function phaseGroups(arms) {
|
|
const groups = Array(arms.length).fill(1);
|
|
if (arms.length < 2) return groups;
|
|
let main = [0, 1];
|
|
let bestOpposition = -1;
|
|
for (let left = 0; left < arms.length; left += 1) {
|
|
for (let right = left + 1; right < arms.length; right += 1) {
|
|
const opposition = angularDistance(arms[left].armHeading, arms[right].armHeading);
|
|
if (opposition > bestOpposition) {
|
|
bestOpposition = opposition;
|
|
main = [left, right];
|
|
}
|
|
}
|
|
}
|
|
groups[main[0]] = 0;
|
|
groups[main[1]] = 0;
|
|
return groups;
|
|
}
|
|
|
|
function buildSignalPose(pole, axis, mastReach) {
|
|
const lateral = [axis[1], -axis[0]];
|
|
const face = [-axis[0], -axis[1]];
|
|
const head = moveMeters(pole, lateral, -mastReach);
|
|
const faceHeadingDegrees = Math.atan2(face[0], face[1]) * 180 / Math.PI;
|
|
const position = (point, height) => ({ longitude: point[0], latitude: point[1], height });
|
|
const lensPoint = moveMeters(head, face, SIGNAL_LAYOUT.lensFaceOffsetMeters);
|
|
const board = moveMeters(
|
|
moveMeters(head, lateral, SIGNAL_LAYOUT.countdownLateralMeters),
|
|
face, SIGNAL_LAYOUT.countdownFaceOffsetMeters,
|
|
);
|
|
return {
|
|
pole: position(pole, 0),
|
|
arm: {
|
|
from: position(pole, SIGNAL_LAYOUT.mastHeightMeters),
|
|
to: position(head, SIGNAL_LAYOUT.mastHeightMeters),
|
|
},
|
|
head: { ...position(head, SIGNAL_LAYOUT.headCenterHeightMeters), faceHeadingDegrees },
|
|
lenses: ["red", "yellow", "green"].map((state, index) => ({
|
|
state,
|
|
...position(lensPoint, SIGNAL_LAYOUT.headCenterHeightMeters + SIGNAL_LAYOUT.lensVerticalOffsetsMeters[index]),
|
|
})),
|
|
countdown: { ...position(board, SIGNAL_LAYOUT.mastHeightMeters), faceHeadingDegrees },
|
|
};
|
|
}
|
|
|
|
function readTrafficSignals(stopLinePath, intersectionPath, osmPath) {
|
|
const controls = osmPath ? parseOsm(fs.readFileSync(osmPath, "utf8")).trafficSignalControls : [];
|
|
return buildTrafficSignals(JSON.parse(fs.readFileSync(stopLinePath, "utf8")), JSON.parse(fs.readFileSync(intersectionPath, "utf8")), controls);
|
|
}
|
|
|
|
function polygonCenter(geometry) {
|
|
const ring = geometry?.type === "Polygon" ? geometry.coordinates?.[0] : null;
|
|
if (!ring || ring.length < 4) return null;
|
|
const points = ring.slice(0, -1);
|
|
return [points.reduce((sum, point) => sum + point[0], 0) / points.length, points.reduce((sum, point) => sum + point[1], 0) / points.length];
|
|
}
|
|
|
|
function polygonRadius(geometry, center) {
|
|
const ring = geometry?.type === "Polygon" ? geometry.coordinates?.[0] : null;
|
|
if (!ring || !center) return 0;
|
|
return Math.max(...ring.slice(0, -1).map((point) => metersBetween(center, point)), 0);
|
|
}
|
|
|
|
function roadAxis(geometry, center, target) {
|
|
const ring = geometry?.coordinates?.[0];
|
|
if (!ring || ring.length < 3) return null;
|
|
let longest = null;
|
|
for (let i = 0; i < ring.length - 1; i += 1) {
|
|
const dx = (ring[i + 1][0] - ring[i][0]) * Math.cos(center[1] * Math.PI / 180);
|
|
const dy = ring[i + 1][1] - ring[i][1];
|
|
const length = Math.hypot(dx, dy);
|
|
if (!longest || length > longest.length) longest = { dx, dy, length };
|
|
}
|
|
if (!longest?.length) return null;
|
|
let axis = [-longest.dy / longest.length, longest.dx / longest.length];
|
|
const toward = [(target[0] - center[0]) * Math.cos(center[1] * Math.PI / 180), target[1] - center[1]];
|
|
if (axis[0] * toward[0] + axis[1] * toward[1] < 0) axis = [-axis[0], -axis[1]];
|
|
return axis;
|
|
}
|
|
|
|
function nearestCenter(point, centers) {
|
|
return centers.map((entry) => ({ ...entry, distance: metersBetween(point, entry.point) })).sort((a, b) => a.distance - b.distance)[0] || null;
|
|
}
|
|
|
|
function metersBetween(a, b) {
|
|
const lat = (a[1] + b[1]) / 2 * Math.PI / 180;
|
|
return Math.hypot((a[0] - b[0]) * Math.cos(lat), a[1] - b[1]) * Math.PI / 180 * EARTH_RADIUS;
|
|
}
|
|
|
|
function moveMeters(point, vector, meters) {
|
|
const scale = 180 / Math.PI / EARTH_RADIUS;
|
|
return [point[0] + vector[0] * meters * scale / Math.cos(point[1] * Math.PI / 180), point[1] + vector[1] * meters * scale];
|
|
}
|
|
|
|
function headingBetween(from, to) {
|
|
const latitude = (from[1] + to[1]) / 2 * Math.PI / 180;
|
|
return Math.atan2((to[0] - from[0]) * Math.cos(latitude), to[1] - from[1]) * 180 / Math.PI;
|
|
}
|
|
|
|
function headingVector(headingDegrees) {
|
|
const radians = headingDegrees * Math.PI / 180;
|
|
return [Math.sin(radians), Math.cos(radians)];
|
|
}
|
|
|
|
function normalizeDegrees(value) {
|
|
return ((value % 360) + 360) % 360;
|
|
}
|
|
|
|
function angularDistance(a, b) {
|
|
return Math.abs(((a - b + 540) % 360) - 180);
|
|
}
|
|
|
|
module.exports = { SIGNAL_LAYOUT, buildTrafficSignals, readTrafficSignals };
|