Files
road-compiler/src/traffic-signals.js
T

530 lines
21 KiB
JavaScript

'use strict';
const fs = require('fs');
const crypto = require('crypto');
const { parseOsm } = require('./osm');
const EARTH_RADIUS = 6371008.8;
const CURB_OFFSET_METERS = 5.2;
const MAX_SIGNAL_POLE_DISTANCE_FROM_STOP_METERS = 40;
const MAST_REACH_METERS = 4.5;
const SIGNAL_LAYOUT = Object.freeze({
poleHeightMeters: 6.7,
poleRadiusMeters: 0.13,
armWidthMeters: 0.21,
mastHeightMeters: 6.25,
headCenterHeightMeters: 6.25,
headWidthMeters: 0.68,
headDepthMeters: 0.3,
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,
countdownVerticalOffsetMeters: 0.0,
});
function buildTrafficSignalFeatures(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 clusteredStops = new Map();
for (const feature of stopLines.features || []) {
const clusterId = feature.properties?.cluster_id;
const point = polygonCenter(feature.geometry);
if (!clusterId || !point) continue;
if (!clusteredStops.has(clusterId)) clusteredStops.set(clusterId, []);
clusteredStops.get(clusterId).push(point);
}
for (const [clusterId, points] of clusteredStops) {
if (points.length < 3) continue;
const point = points.reduce(
(sum, item) => [sum[0] + item[0] / points.length, sum[1] + item[1] / points.length],
[0, 0],
);
centers.push({
id: `cluster-${clusterId}`,
clusterId,
point,
radius: Math.max(...points.map((item) => metersBetween(point, item))),
});
}
const candidates = [];
for (const feature of stopLines.features || []) {
const center = polygonCenter(feature.geometry);
if (!center) continue;
const clusterId = feature.properties?.cluster_id;
const intersection = clusterId
? centers.find((entry) => entry.clusterId === clusterId)
: nearestCenter(center, centers);
if (!intersection || metersBetween(center, intersection.point) > 32) continue;
const axis = roadAxis(feature.geometry, center, intersection.point);
if (!axis) continue;
const right = [axis[1], -axis[0]];
candidates.push({
intersectionId: intersection.id,
center,
axis,
point: intersection.clusterId
? moveMeters(center, right, CURB_OFFSET_METERS)
: moveMeters(moveMeters(intersection.point, axis, intersection.radius + 3.2), right, CURB_OFFSET_METERS),
headingDegrees: normalizeDegrees((Math.atan2(axis[0], axis[1]) * 180) / Math.PI),
matchHeadingDegrees: intersection.clusterId
? normalizeDegrees((Math.atan2(-axis[0], -axis[1]) * 180) / Math.PI)
: null,
});
}
const features = [];
for (const control of controls) {
const controlPoint = [Number(control.longitude), Number(control.latitude)];
if (!controlPoint.every(Number.isFinite) || !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((item) => item.intersectionId === intersection.id),
controlPoint,
control.arms,
);
const groups = phaseGroups(arms);
arms.forEach((candidate, index) => {
const fallbackArmId = `heading-${Math.round(normalizeDegrees(candidate.osmArm?.headingDegrees || 0) * 1000)}`;
const sourceWayId = String(candidate.osmArm?.wayId || 'legacy');
const neighborNodeId = String(candidate.osmArm?.neighborNodeId || fallbackArmId);
const approachId = `${sourceWayId}:${neighborNodeId}`;
const signalUid = `osm-${String(control.id)}-${sourceWayId}-${neighborNodeId}`;
features.push({
type: 'Feature',
geometry: { type: 'Point', coordinates: candidate.point.slice() },
properties: {
signal_uid: signalUid,
display_id: signalUid,
control_id: String(control.id),
approach_id: approachId,
source_way_id: sourceWayId,
// These are independent assembly controls. heading_deg remains a
// migration hint for older native documents only.
mast_heading_deg: normalizeDegrees(candidate.headingDegrees - 90),
face_heading_deg: normalizeDegrees(candidate.headingDegrees + 180),
phase_group: groups[index],
mast_reach_m: MAST_REACH_METERS,
stop_lon: candidate.center[0],
stop_lat: candidate.center[1],
enabled: true,
z_offset_m: 0,
},
});
});
}
return validateTrafficSignalFeatures({ type: 'FeatureCollection', features });
}
function validateTrafficSignalFeatures(collection) {
if (collection?.type !== 'FeatureCollection' || !Array.isArray(collection.features)) {
throw new Error('Traffic signal assemblies must be a FeatureCollection');
}
const uids = new Set();
const displayIds = new Set();
const features = collection.features.map((feature, index) => {
const label = `traffic signal feature ${index + 1}`;
if (
feature?.geometry?.type !== 'Point' ||
!Array.isArray(feature.geometry.coordinates) ||
feature.geometry.coordinates.length < 2 ||
!feature.geometry.coordinates.slice(0, 2).every(Number.isFinite)
) {
throw new Error(`${label}: geometry must be a finite Point`);
}
const input = feature.properties || {};
const text = (key, required = true) => {
const value = input[key] == null ? '' : String(input[key]).trim();
if (required && !value) throw new Error(`${label}: missing ${key}`);
return value;
};
const number = (key, options = {}) => {
if (input[key] === null || input[key] === undefined || input[key] === '') {
throw new Error(`${label}: missing ${key}`);
}
const value = Number(input[key]);
if (
!Number.isFinite(value) ||
(options.min != null && value < options.min) ||
(options.max != null && value > options.max)
) {
throw new Error(`${label}: invalid ${key} '${input[key]}'`);
}
return value;
};
const signalUid = text('signal_uid');
if (!/^osm-[A-Za-z0-9_.:-]+$/.test(signalUid)) throw new Error(`${label}: invalid signal_uid '${signalUid}'`);
if (uids.has(signalUid)) throw new Error(`Duplicate signal_uid '${signalUid}'`);
uids.add(signalUid);
const displayId = text('display_id', false);
if (displayId && displayIds.has(displayId)) throw new Error(`Duplicate display_id '${displayId}'`);
if (displayId) displayIds.add(displayId);
const phaseGroup = number('phase_group', { min: 0, max: 1 });
if (!Number.isInteger(phaseGroup)) throw new Error(`${label}: phase_group must be 0 or 1`);
const enabled = normalizeBoolean(input.enabled, label);
const controlId = text('control_id');
const approachId = text('approach_id');
const sourceWayId = text('source_way_id');
if (!approachId.startsWith(`${sourceWayId}:`))
throw new Error(`${label}: approach_id does not match source_way_id`);
const expectedUid = `osm-${controlId}-${approachId.replace(':', '-')}`;
if (signalUid !== expectedUid)
throw new Error(`${label}: signal_uid does not match source identity (expected '${expectedUid}')`);
const legacyHeading =
input.heading_deg == null || input.heading_deg === '' ? null : normalizeDegrees(number('heading_deg'));
if (legacyHeading == null && (input.mast_heading_deg == null || input.mast_heading_deg === '')) {
throw new Error(`${label}: missing mast_heading_deg`);
}
if (legacyHeading == null && (input.face_heading_deg == null || input.face_heading_deg === '')) {
throw new Error(`${label}: missing face_heading_deg`);
}
const mastHeading =
input.mast_heading_deg == null || input.mast_heading_deg === ''
? normalizeDegrees((legacyHeading == null ? 0 : legacyHeading) - 90)
: normalizeDegrees(number('mast_heading_deg'));
const faceHeading =
input.face_heading_deg == null || input.face_heading_deg === ''
? normalizeDegrees((legacyHeading == null ? 0 : legacyHeading) + 180)
: normalizeDegrees(number('face_heading_deg'));
const stopLon = number('stop_lon', { min: -180, max: 180 });
const stopLat = number('stop_lat', { min: -90, max: 90 });
const poleDistance = metersBetween(feature.geometry.coordinates, [stopLon, stopLat]);
if (poleDistance > MAX_SIGNAL_POLE_DISTANCE_FROM_STOP_METERS)
throw new Error(
`${label}: signal pole is ${Math.round(poleDistance)}m from its stop line (maximum ${MAX_SIGNAL_POLE_DISTANCE_FROM_STOP_METERS}m)`,
);
return {
type: 'Feature',
geometry: { type: 'Point', coordinates: feature.geometry.coordinates.slice(0, 2).map(Number) },
properties: {
...input,
signal_uid: signalUid,
display_id: displayId,
control_id: controlId,
approach_id: approachId,
source_way_id: sourceWayId,
// Retain the legacy value only for migration compatibility. Runtime
// geometry is entirely defined by mast_heading_deg and face_heading_deg.
heading_deg: legacyHeading,
mast_heading_deg: mastHeading,
face_heading_deg: faceHeading,
phase_group: phaseGroup,
mast_reach_m: number('mast_reach_m', { min: 0.1, max: 30 }),
stop_lon: stopLon,
stop_lat: stopLat,
enabled,
z_offset_m: number('z_offset_m', { min: -20, max: 100 }),
},
};
});
return { type: 'FeatureCollection', features };
}
function buildTrafficSignalsFromFeatures(collection) {
const normalized = validateTrafficSignalFeatures(collection);
const signals = normalized.features
.filter((feature) => feature.properties.enabled)
.map((feature) => {
const p = feature.properties;
const point = feature.geometry.coordinates;
const mastAxis = headingVector(p.mast_heading_deg);
return {
id: p.signal_uid,
signalUid: p.signal_uid,
displayId: p.display_id,
nodeKey: signalNodeKey(p.signal_uid),
controlId: p.control_id,
approachId: p.approach_id,
sourceWayId: p.source_way_id,
phaseGroup: p.phase_group,
longitude: point[0],
latitude: point[1],
stopLongitude: p.stop_lon,
stopLatitude: p.stop_lat,
// Existing Blender readers require headingDegrees. It is a compatibility
// alias only; the independent mast/face fields below define all geometry.
headingDegrees: p.heading_deg == null ? p.mast_heading_deg : p.heading_deg,
mastHeadingDegrees: p.mast_heading_deg,
faceHeadingDegrees: p.face_heading_deg,
mastReachMeters: p.mast_reach_m,
zOffsetMeters: p.z_offset_m,
pose: buildSignalPose(point, mastAxis, p.face_heading_deg, p.mast_reach_m, p.z_offset_m),
};
});
return { version: 3, layout: SIGNAL_LAYOUT, signals };
}
function signalNodeKey(signalUid) {
return `ts_${crypto.createHash('sha256').update(signalUid).digest('hex').slice(0, 16)}`;
}
function reconcileTrafficSignalSourceReferences(collection, controls) {
const normalized = validateTrafficSignalFeatures(collection);
const approachesByControl = new Map(
(controls || []).map((control) => [
String(control.id),
new Set((control.arms || []).map((arm) => `${String(arm.wayId)}:${String(arm.neighborNodeId)}`)),
]),
);
const kept = [];
const dropped = [];
for (const [index, feature] of normalized.features.entries()) {
const { control_id: controlId, approach_id: approachId, signal_uid: signalUid } = feature.properties;
const approaches = approachesByControl.get(controlId);
if (!approaches) {
dropped.push({
index: index + 1,
signalUid,
controlId,
approachId,
reason: 'missing-control',
message: `control_id '${controlId}' is not present in the current OSM`,
});
continue;
}
if (!approaches.has(approachId)) {
dropped.push({
index: index + 1,
signalUid,
controlId,
approachId,
reason: 'missing-approach',
message: `approach_id '${approachId}' is not present on OSM control '${controlId}'`,
});
continue;
}
kept.push(feature);
}
return { collection: { ...normalized, features: kept }, dropped };
}
function validateTrafficSignalSourceReferences(collection, controls) {
const { collection: reconciled, dropped } = reconcileTrafficSignalSourceReferences(collection, controls);
if (dropped.length) throw new Error(`traffic signal feature ${dropped[0].index}: ${dropped[0].message}`);
return reconciled;
}
function buildTrafficSignals(stopLines, intersections, controls = []) {
return buildTrafficSignalsFromFeatures(buildTrafficSignalFeatures(stopLines, intersections, controls));
}
function readTrafficSignalFeatures(stopLinePath, intersectionPath, osmPath) {
const controls = parseOsm(fs.readFileSync(osmPath, 'utf8')).trafficSignalControls;
return buildTrafficSignalFeatures(
JSON.parse(fs.readFileSync(stopLinePath, 'utf8')),
JSON.parse(fs.readFileSync(intersectionPath, 'utf8')),
controls,
);
}
function readTrafficSignals(editablePath, osmPath = null) {
const collection = JSON.parse(fs.readFileSync(editablePath, 'utf8'));
if (osmPath) {
const controls = parseOsm(fs.readFileSync(osmPath, 'utf8')).trafficSignalControls;
validateTrafficSignalSourceReferences(collection, controls);
}
return buildTrafficSignalsFromFeatures(collection);
}
function normalizeBoolean(value, label) {
if (
value === true ||
value === 1 ||
value === '1' ||
String(value).toLowerCase() === 'true' ||
String(value).toLowerCase() === 'yes'
)
return true;
if (
value === false ||
value === 0 ||
value === '0' ||
String(value).toLowerCase() === 'false' ||
String(value).toLowerCase() === 'no'
)
return false;
throw new Error(`${label}: invalid enabled '${value}'`);
}
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)
if (!arms.some((arm) => angularDistance(arm.armHeading, candidate.armHeading) <= 25)) arms.push(candidate);
return arms;
}
function matchOsmArms(candidates, controlPoint, osmArms) {
const remaining = candidates.map((candidate) => ({
...candidate,
armHeading: candidate.matchHeadingDegrees ?? normalizeDegrees(headingBetween(controlPoint, candidate.center)),
}));
if (!osmArms.length) return uniqueApproachArms(remaining, controlPoint);
return osmArms.map((osmArm) => {
let bestIndex = -1;
let bestDistance = Infinity;
remaining.forEach((item, index) => {
const directedDistance = angularDistance(item.armHeading, osmArm.headingDegrees);
const distance =
item.matchHeadingDegrees == null
? directedDistance
: Math.min(
angularDistance(item.matchHeadingDegrees, osmArm.headingDegrees),
angularDistance(item.matchHeadingDegrees + 180, osmArm.headingDegrees),
);
if (distance < bestDistance) {
bestDistance = distance;
bestIndex = index;
}
});
const candidate =
bestIndex >= 0 && bestDistance <= 45
? remaining.splice(bestIndex, 1)[0]
: fallbackCandidate(controlPoint, osmArm);
return { ...candidate, osmArm };
});
}
function fallbackCandidate(controlPoint, osmArm) {
const outward = headingVector(osmArm.headingDegrees);
const axis = [-outward[0], -outward[1]];
const center = moveMeters(controlPoint, outward, 8);
const farSide = moveMeters(controlPoint, axis, 3.2);
return {
center,
axis,
point: moveMeters(farSide, [axis[1], -axis[0]], CURB_OFFSET_METERS),
armHeading: normalizeDegrees(osmArm.headingDegrees),
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 best = -1;
for (let a = 0; a < arms.length; a += 1)
for (let b = a + 1; b < arms.length; b += 1) {
const opposition = angularDistance(arms[a].armHeading, arms[b].armHeading);
if (opposition > best) {
best = opposition;
main = [a, b];
}
}
groups[main[0]] = 0;
groups[main[1]] = 0;
return groups;
}
function buildSignalPose(pole, mastAxis, faceHeadingDegrees, mastReach, zOffset = 0) {
const face = headingVector(faceHeadingDegrees);
const head = moveMeters(pole, mastAxis, mastReach);
const position = (point, height) => ({ longitude: point[0], latitude: point[1], height: height + zOffset });
const lensPoint = moveMeters(head, face, SIGNAL_LAYOUT.lensFaceOffsetMeters);
const faceRight = [-face[1], face[0]];
const board = moveMeters(
moveMeters(head, faceRight, 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 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((s, p) => s + p[0], 0) / points.length, points.reduce((s, p) => s + p[1], 0) / points.length];
}
function polygonRadius(geometry, center) {
const ring = geometry?.type === 'Polygon' ? geometry.coordinates?.[0] : null;
return ring && center ? Math.max(...ring.slice(0, -1).map((point) => metersBetween(center, point)), 0) : 0;
}
function roadAxis(geometry, center, target) {
const ring = geometry?.coordinates?.[0];
if (!ring || ring.length < 3) return null;
let longest;
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(degrees) {
const radians = (degrees * 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,
signalNodeKey,
buildTrafficSignalFeatures,
validateTrafficSignalFeatures,
validateTrafficSignalSourceReferences,
buildTrafficSignalsFromFeatures,
buildTrafficSignals,
readTrafficSignalFeatures,
readTrafficSignals,
};