feat: add participant runtime simulation with traffic signal scheduling
- Implement deterministic participant simulation in cesium-preview.js with fixed-seed arrival, IDM car-following, stop-line constraints, and geometric conflict occupancy detection - Build native traffic simulation participants from road package data including signal phase mapping, crosswalk centerlines, and lane routing - Add participant runtime regression test covering 600 s simulation, green-only releases, and 2D bounding box overlap checks - Update model URI prefix to _preview/ so simulation JSON resolves assets relative to area output directory - Sync Trellis spec and task docs for participant scheduling contract
This commit is contained in:
1 parent
c1277af16a
commit
7ce412e52e
10 files changed
+1587
-43
No files matched your search
@@ -4,7 +4,20 @@ const crypto = require("crypto");
|
||||
const fs = require("fs");
|
||||
const path = require("path");
|
||||
|
||||
const SCHEMA = "native-preview-traffic-simulation/v1";
|
||||
const SCHEMA = "native-preview-traffic-simulation/v2";
|
||||
// Local preview signal plan. Each green is preceded by a 2 s all-red clearance
|
||||
// so the previous movement can leave the junction before conflicting traffic.
|
||||
const DEFAULT_SIGNAL_PLAN = {
|
||||
cycleSeconds: 60,
|
||||
// Start with every approach red so the first rendered frame is a controlled
|
||||
// queue, not a vehicle released into a phase before the scene is visible.
|
||||
initialAllRedSeconds: 5,
|
||||
groups: {
|
||||
0: [{ from: 0, to: 25, state: "green" }, { from: 25, to: 28, state: "yellow" }, { from: 28, to: 60, state: "red" }],
|
||||
1: [{ from: 0, to: 32, state: "red" }, { from: 32, to: 57, state: "green" }, { from: 57, to: 60, state: "yellow" }],
|
||||
},
|
||||
pedestrianRule: "cross only while the paired approach group is red; finish crossing after the phase changes",
|
||||
};
|
||||
const MAX_ROUTES = 5;
|
||||
const MIN_ROAD_COUNT = 3;
|
||||
const MAX_ROAD_COUNT = 7;
|
||||
@@ -17,6 +30,49 @@ const DEFAULT_SETTINGS = {
|
||||
vehicleLengthMeters: 4.6,
|
||||
minimumGapMeters: 7,
|
||||
};
|
||||
const DEFAULT_FLEET = {
|
||||
vehiclesPerThroughRoute: 1,
|
||||
vehiclesPerTurnRoute: 1,
|
||||
motorcycles: 1,
|
||||
bicyclesPerRoute: 1,
|
||||
pedestriansPerCrossing: 1,
|
||||
};
|
||||
// Dashboard cross-model catalog (`${type}${subType}.glb`): type 1 motor
|
||||
// vehicles with subType 1 small / 2 bus / 3 special / 5 mid / 6 large /
|
||||
// 7 oversize, 4 motorcycle, 21/22 bicycles, 31 pedestrian.
|
||||
const VEHICLE_MODEL_CATALOG = [
|
||||
{ modelName: "v2x-vehicles/11.glb", vehicleLengthMeters: 4.6, weight: 5 },
|
||||
{ modelName: "v2x-vehicles/15.glb", vehicleLengthMeters: 6.5, weight: 2 },
|
||||
{ modelName: "v2x-vehicles/16.glb", vehicleLengthMeters: 8.5, weight: 1.5 },
|
||||
{ modelName: "v2x-vehicles/17.glb", vehicleLengthMeters: 10.5, weight: 0.75 },
|
||||
{ modelName: "v2x-vehicles/12.glb", vehicleLengthMeters: 11.5, weight: 0.75 },
|
||||
{ modelName: "v2x-vehicles/13.glb", vehicleLengthMeters: 5.8, weight: 0.5 },
|
||||
];
|
||||
const MOTORCYCLE_MODEL = { modelName: "v2x-vehicles/14.glb", vehicleLengthMeters: 2.0 };
|
||||
const BICYCLE_MODELS = ["v2x-vehicles/221.glb", "v2x-vehicles/222.glb"];
|
||||
const PEDESTRIAN_MODEL = "v2x-vehicles/31.glb";
|
||||
const KIND_DYNAMICS = {
|
||||
vehicle: { desiredSpeedMetersPerSecond: 8, accelerationMetersPerSecondSquared: 1.8, decelerationMetersPerSecondSquared: 3.5 },
|
||||
motorcycle: { desiredSpeedMetersPerSecond: 9, accelerationMetersPerSecondSquared: 2.4, decelerationMetersPerSecondSquared: 3.8 },
|
||||
bicycle: { desiredSpeedMetersPerSecond: 5.0, accelerationMetersPerSecondSquared: 1.8, decelerationMetersPerSecondSquared: 2.8 },
|
||||
pedestrian: { desiredSpeedMetersPerSecond: 2.0, accelerationMetersPerSecondSquared: 1.4, decelerationMetersPerSecondSquared: 2.4 },
|
||||
};
|
||||
const KIND_PLACEMENT = {
|
||||
vehicle: { modelScale: 1.0, heightMeters: 0.4 },
|
||||
motorcycle: { modelScale: 1.0, heightMeters: 0.4 },
|
||||
bicycle: { modelScale: 1.0, heightMeters: 0.05 },
|
||||
pedestrian: { modelScale: 1.0, heightMeters: 0.05 },
|
||||
};
|
||||
// The dashboard glb set models face +X (AMap three.js applies a 90° heading
|
||||
// offset); keep the same correction so models face along the route.
|
||||
const MODEL_HEADING_OFFSET_DEGREES = -90;
|
||||
// Lane indices run 1 (median) -> 3 (curb) on the modelled travel direction.
|
||||
const CANONICAL_LANE_BY_TURN = { right: 3, through: 2, left: 1 };
|
||||
const CROSSWALK_PAD_METERS = 2.5;
|
||||
// Participant routes may extend up to this far into the upstream/downstream
|
||||
// corridor roads so vehicles enter and leave the scene instead of popping in
|
||||
// beside the stop line.
|
||||
const PARTICIPANT_CORRIDOR_CAP_METERS = 250;
|
||||
|
||||
function buildNativeTrafficSimulation(area, options = {}) {
|
||||
const root = options.nativePackage?.rootDir;
|
||||
@@ -34,29 +90,56 @@ function buildNativeTrafficSimulation(area, options = {}) {
|
||||
const connectors = readFeatureCollection(connectorPath, "native connectors");
|
||||
const laneCenterlines = fs.existsSync(laneCenterlinePath) ? readFeatureCollection(laneCenterlinePath, "native lane centerlines") : null;
|
||||
const stopLines = readFeatureCollection(stopLinePath, "native stop lines");
|
||||
const crosswalkPath = path.join(root, "layers", "crosswalks.geojson");
|
||||
const crosswalks = fs.existsSync(crosswalkPath) ? readFeatureCollection(crosswalkPath, "native crosswalks") : null;
|
||||
const settings = { ...DEFAULT_SETTINGS, ...(options.settings || {}) };
|
||||
validateSettings(settings);
|
||||
const fleet = { ...DEFAULT_FLEET, ...(options.fleet || {}) };
|
||||
const modelPrefix = options.modelPrefix || "";
|
||||
const routeBuild = buildRoutes(compiled, connectors, signals.signals || [], stopLines.features, laneCenterlines);
|
||||
const participantBuild = buildParticipants({
|
||||
compiled,
|
||||
connectorCollection: connectors,
|
||||
laneCenterlines,
|
||||
stopLineFeatures: stopLines.features,
|
||||
crosswalks,
|
||||
signals: signals.signals || [],
|
||||
fleet,
|
||||
modelPrefix,
|
||||
});
|
||||
const participants = participantBuild.participants;
|
||||
return {
|
||||
schema: SCHEMA,
|
||||
areaId: area.id,
|
||||
coordinateSystem: { route: "WGS84", model: "ENU", units: "meters", axes: "X east / Y north / Z up" },
|
||||
generatedAt: new Date().toISOString(),
|
||||
speedMetersPerSecond: settings.desiredSpeedMetersPerSecond,
|
||||
source: sourceRecords(area, [compiledPath, signalPath, connectorPath, stopLinePath, ...(laneCenterlines ? [laneCenterlinePath] : [])]),
|
||||
source: sourceRecords(area, [
|
||||
compiledPath,
|
||||
signalPath,
|
||||
connectorPath,
|
||||
stopLinePath,
|
||||
...(laneCenterlines ? [laneCenterlinePath] : []),
|
||||
...(crosswalks ? [crosswalkPath] : []),
|
||||
]),
|
||||
settings,
|
||||
fleet,
|
||||
signalPlan: DEFAULT_SIGNAL_PLAN,
|
||||
routes: routeBuild.routes,
|
||||
participants,
|
||||
signals: (signals.signals || []).filter((signal) => signal && signal.id).map(signalDescriptor),
|
||||
diagnostics: [
|
||||
...routeBuild.diagnostics,
|
||||
...(routeBuild.routes.length ? [] : [{ severity: "warning", reason: "no_native_cruise_route", message: "No deterministic native route crossed three or more directional roads." }]),
|
||||
...participantBuild.diagnostics,
|
||||
...(routeBuild.routes.length || participants.length ? [] : [{ severity: "warning", reason: "no_native_cruise_route", message: "No deterministic native route or open intersection participant was generated." }]),
|
||||
],
|
||||
rejectedRoutes: routeBuild.rejectedRoutes,
|
||||
migration: {
|
||||
schema: SCHEMA,
|
||||
updateLoop: "advance route distance using settings, stop before red/yellow signal, then apply leader minimum gap",
|
||||
updateLoop: "advance route distance using settings, stop before red/yellow signal, then apply leader minimum gap; pedestrians advance only while their crossing phase is red",
|
||||
signalIdentity: "signals[].id is the existing traffic runtime signal_uid",
|
||||
coordinates: "routes[].coordinates are WGS84; static package remains local ENU placed by package manifest",
|
||||
coordinates: "routes[]/participants[] coordinates are WGS84; static package remains local ENU placed by package manifest",
|
||||
participants: "participants[] are open route templates (approach to exit, or crosswalk spans) with per-kind models and fleet counts",
|
||||
legacyDependency: false,
|
||||
},
|
||||
};
|
||||
@@ -102,6 +185,661 @@ function buildRoutes(compiled, connectorCollection, signals, stopLineFeatures, l
|
||||
return { routes, diagnostics, rejectedRoutes };
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Intersection participants (v2)
|
||||
//
|
||||
// A single-intersection clip has no closed loop, so cruise routes stay empty.
|
||||
// Instead the descriptor carries open route templates ("participants"): each
|
||||
// runs from an approach entry, through one movement connector, onto an exit
|
||||
// (vehicles, motorcycles, bicycles), or across a crosswalk span (pedestrians).
|
||||
// Signal phases come from pairing signal stop points with native stop lines,
|
||||
// so no way id is hardcoded.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
function buildParticipants({ compiled, connectorCollection, laneCenterlines, stopLineFeatures, crosswalks, signals, fleet, modelPrefix }) {
|
||||
const diagnostics = [];
|
||||
const roads = new Map((compiled.model?.roads || []).map((road) => [road.id, road]));
|
||||
const allMovements = (compiled.movements || []).filter((movement) => movement.geometryPublished !== false && roads.has(movement.fromRoadId) && roads.has(movement.toRoadId));
|
||||
const connectorByMovement = new Map(connectorCollection.features.map((feature) => [feature.properties?.movement_id, feature.geometry?.coordinates || []]));
|
||||
const movementHasConnector = (movement) => (connectorByMovement.get(movement.id) || []).length >= 2;
|
||||
// Intersection movements are the cluster lane-paths; endpoint-connection
|
||||
// movements are corridor continuations used only to extend participant
|
||||
// routes toward the clip boundary. Fall back to all movements when the
|
||||
// package predates cluster naming.
|
||||
let movements = allMovements.filter((movement) => String(movement.id).includes(":cluster:") && movementHasConnector(movement));
|
||||
if (!movements.length) movements = allMovements.filter(movementHasConnector);
|
||||
const laneCenterlineById = indexLaneCenterlines(laneCenterlines);
|
||||
const context = {
|
||||
roads,
|
||||
connectorByMovement,
|
||||
laneCenterlineById,
|
||||
movements: allMovements,
|
||||
diagnostics,
|
||||
};
|
||||
const crossings = collectMainCrosswalks(crosswalks, stopLineFeatures, diagnostics);
|
||||
const crossingByRoad = new Map(crossings.map((crossing) => [crossing.roadId, crossing]));
|
||||
const participants = [];
|
||||
|
||||
const movementsByApproach = new Map();
|
||||
for (const movement of movements) {
|
||||
if (!movementsByApproach.has(movement.fromRoadId)) movementsByApproach.set(movement.fromRoadId, []);
|
||||
movementsByApproach.get(movement.fromRoadId).push(movement);
|
||||
}
|
||||
const { phaseByRoad, signalIdByRoad } = mapApproachPhases(signals, movementsByApproach, context, diagnostics);
|
||||
|
||||
let vehicleTemplateIndex = 0;
|
||||
const throughApproaches = [];
|
||||
let approachIndex = 0;
|
||||
for (const approachRoadId of [...movementsByApproach.keys()].sort()) {
|
||||
const byTurn = groupByTurn(movementsByApproach.get(approachRoadId));
|
||||
// Keep one through stream on every arm. Add a single alternating turn
|
||||
// stream so the intersection remains readable and does not spawn three
|
||||
// vehicles into the same stop line at once.
|
||||
const allowedTurns = approachIndex % 2 === 0 ? ["through", "left"] : ["through"];
|
||||
for (const turn of allowedTurns) {
|
||||
const movement = pickMovementByCanonicalLane(byTurn[turn] || [], CANONICAL_LANE_BY_TURN[turn]);
|
||||
if (!movement) continue;
|
||||
const path = buildMovementPath(movement, context);
|
||||
if (!path) continue;
|
||||
const isThrough = turn === "through";
|
||||
const fleetCount = isThrough ? fleet.vehiclesPerThroughRoute : fleet.vehiclesPerTurnRoute;
|
||||
const model = VEHICLE_MODEL_CATALOG[vehicleTemplateIndex % VEHICLE_MODEL_CATALOG.length];
|
||||
vehicleTemplateIndex += 1;
|
||||
participants.push(makeParticipantTemplate({
|
||||
id: `participant:vehicle:${shortRoadId(approachRoadId)}:${turn}`,
|
||||
kind: "vehicle",
|
||||
movement,
|
||||
path,
|
||||
fleetCount,
|
||||
modelPrefix,
|
||||
model: model.modelName,
|
||||
vehicleLengthMeters: model.vehicleLengthMeters,
|
||||
approachRoadId,
|
||||
signalIdByRoad,
|
||||
signals,
|
||||
stopLineFeatures,
|
||||
crossing: crossingByRoad.get(approachRoadId) || null,
|
||||
crossings,
|
||||
}));
|
||||
if (isThrough) throughApproaches.push({ approachRoadId, movement, path });
|
||||
}
|
||||
approachIndex += 1;
|
||||
}
|
||||
|
||||
if (fleet.motorcycles > 0 && throughApproaches.length) {
|
||||
// Deterministic: ride the alphabetically first through corridor.
|
||||
const template = throughApproaches[0];
|
||||
participants.push(makeParticipantTemplate({
|
||||
id: `participant:motorcycle:${shortRoadId(template.approachRoadId)}:through`,
|
||||
kind: "motorcycle",
|
||||
movement: template.movement,
|
||||
path: template.path,
|
||||
fleetCount: fleet.motorcycles,
|
||||
modelPrefix,
|
||||
model: MOTORCYCLE_MODEL.modelName,
|
||||
vehicleLengthMeters: MOTORCYCLE_MODEL.vehicleLengthMeters,
|
||||
approachRoadId: template.approachRoadId,
|
||||
signalIdByRoad,
|
||||
signals,
|
||||
stopLineFeatures,
|
||||
crossing: crossingByRoad.get(template.approachRoadId) || null,
|
||||
crossings,
|
||||
}));
|
||||
}
|
||||
|
||||
if (fleet.pedestriansPerCrossing > 0) {
|
||||
for (const crossing of crossings) {
|
||||
const phaseGroup = phaseByRoad.get(crossing.roadId);
|
||||
if (!Number.isFinite(phaseGroup)) {
|
||||
diagnostics.push({ severity: "warning", reason: "crosswalk_phase_unresolved", message: `Crosswalk ${crossing.id} has no paired approach signal; pedestrians omitted.`, crosswalkId: crossing.id });
|
||||
continue;
|
||||
}
|
||||
const coordinates = crossing.coordinates;
|
||||
const distances = cumulativeDistances(coordinates);
|
||||
const dynamics = KIND_DYNAMICS.pedestrian;
|
||||
participants.push({
|
||||
id: `participant:pedestrian:${crossing.id}`,
|
||||
kind: "pedestrian",
|
||||
crossingId: crossing.id,
|
||||
fleet: fleet.pedestriansPerCrossing,
|
||||
modelName: `${modelPrefix}${PEDESTRIAN_MODEL}`,
|
||||
modelScale: KIND_PLACEMENT.pedestrian.modelScale,
|
||||
headingOffsetDegrees: MODEL_HEADING_OFFSET_DEGREES,
|
||||
heightMeters: KIND_PLACEMENT.pedestrian.heightMeters,
|
||||
phaseGroup,
|
||||
coordinates,
|
||||
distances,
|
||||
lengthMeters: round(distances.at(-1)),
|
||||
speedMetersPerSecond: dynamics.desiredSpeedMetersPerSecond,
|
||||
accelerationMetersPerSecondSquared: dynamics.accelerationMetersPerSecondSquared,
|
||||
decelerationMetersPerSecondSquared: dynamics.decelerationMetersPerSecondSquared,
|
||||
stops: [],
|
||||
});
|
||||
if (fleet.bicyclesPerRoute > 0) {
|
||||
participants.push({
|
||||
id: `participant:bicycle:${crossing.id}`,
|
||||
kind: "bicycle",
|
||||
fleet: fleet.bicyclesPerRoute,
|
||||
modelName: `${modelPrefix}${BICYCLE_MODELS[crossings.indexOf(crossing) % BICYCLE_MODELS.length]}`,
|
||||
modelScale: KIND_PLACEMENT.bicycle.modelScale,
|
||||
headingOffsetDegrees: MODEL_HEADING_OFFSET_DEGREES,
|
||||
heightMeters: KIND_PLACEMENT.bicycle.heightMeters,
|
||||
phaseGroup,
|
||||
crossingId: crossing.id,
|
||||
coordinates,
|
||||
distances,
|
||||
lengthMeters: round(distances.at(-1)),
|
||||
speedMetersPerSecond: KIND_DYNAMICS.bicycle.desiredSpeedMetersPerSecond,
|
||||
accelerationMetersPerSecondSquared: KIND_DYNAMICS.bicycle.accelerationMetersPerSecondSquared,
|
||||
decelerationMetersPerSecondSquared: KIND_DYNAMICS.bicycle.decelerationMetersPerSecondSquared,
|
||||
vehicleLengthMeters: 1.8,
|
||||
stops: [],
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const withFleet = participants.filter((participant) => participant.fleet > 0 && participant.lengthMeters > 5);
|
||||
return { participants: withFleet, diagnostics };
|
||||
}
|
||||
|
||||
function makeParticipantTemplate({ id, kind, movement, path, fleetCount, modelPrefix, model, vehicleLengthMeters, approachRoadId, signalIdByRoad, signals, crossing, crossings }) {
|
||||
const coordinates = path.coordinates;
|
||||
const distances = cumulativeDistances(coordinates);
|
||||
const crossingConflicts = kind === "vehicle" || kind === "motorcycle"
|
||||
? (crossings || []).map((candidate) => {
|
||||
let best = { distance: Infinity, routeDistance: 0 };
|
||||
for (let index = 0; index < coordinates.length; index += 1) {
|
||||
for (const point of candidate.coordinates || []) {
|
||||
const distance = haversine(coordinates[index], point);
|
||||
if (distance < best.distance) best = { distance, routeDistance: distances[index] };
|
||||
}
|
||||
}
|
||||
return best.distance <= 7 ? { crossingId: candidate.id, distance: round(best.routeDistance), matchDistanceMeters: round(best.distance) } : null;
|
||||
}).filter(Boolean)
|
||||
: [];
|
||||
const dynamics = KIND_DYNAMICS[kind];
|
||||
// Attach the paired approach signal directly by projecting its stop point
|
||||
// onto the route: raw proximity matching grabs neighbouring signals in this
|
||||
// compact intersection, and the curb lane can sit outside their tolerance.
|
||||
const ownSignal = signals.find((signal) => signal && signal.id === (signalIdByRoad.get(approachRoadId) || null));
|
||||
const stops = [];
|
||||
if (ownSignal && Number.isFinite(ownSignal.stopLongitude) && Number.isFinite(ownSignal.stopLatitude)) {
|
||||
let best = { distance: Infinity, routeDistance: 0 };
|
||||
// The controlling zebra is on the approach arm. Never match a nearby
|
||||
// crossing after the turn connector, otherwise a turning vehicle receives
|
||||
// a stop point inside the junction.
|
||||
const approachLimit = distances.at(-1) * 0.48;
|
||||
for (let index = 0; index < coordinates.length && distances[index] <= approachLimit; index += 1) {
|
||||
const distance = haversine(coordinates[index], [ownSignal.stopLongitude, ownSignal.stopLatitude]);
|
||||
if (distance < best.distance) best = { distance, routeDistance: distances[index] };
|
||||
}
|
||||
if (best.distance <= 30) {
|
||||
// Route distance tracks the model origin, while the legal stop line is
|
||||
// constrained by the front bumper. Keep half a vehicle length plus a
|
||||
// small safety margin behind the signal's stop-line point.
|
||||
const centerStopDistance = Math.max(0, best.routeDistance - vehicleLengthMeters / 2 - 1.0);
|
||||
stops.push({ signalId: ownSignal.id, phaseGroup: ownSignal.phaseGroup, distance: round(centerStopDistance), stopLineDistance: round(best.routeDistance), matchDistanceMeters: round(best.distance), stopReason: "traffic-signal" });
|
||||
}
|
||||
}
|
||||
let crossingStopDistance = null;
|
||||
if (crossing?.coordinates?.length) {
|
||||
let nearest = { distance: Infinity, routeDistance: null };
|
||||
const approachLimit = distances.at(-1) * 0.48;
|
||||
for (let index = 0; index < coordinates.length && distances[index] <= approachLimit; index += 1) {
|
||||
for (const point of crossing.coordinates) {
|
||||
const distance = haversine(coordinates[index], point);
|
||||
if (distance < nearest.distance) nearest = { distance, routeDistance: distances[index] };
|
||||
}
|
||||
}
|
||||
if (Number.isFinite(nearest.routeDistance) && nearest.distance <= 20) {
|
||||
// GLB origins are not guaranteed to be geometric centers. Keep the
|
||||
// complete vehicle length plus a buffer behind the zebra so long buses
|
||||
// and trucks cannot nose over the stop line.
|
||||
crossingStopDistance = Math.max(0, nearest.routeDistance - (vehicleLengthMeters + 4.0));
|
||||
if (ownSignal) {
|
||||
const existing = stops[0];
|
||||
if (!existing || crossingStopDistance < existing.distance) {
|
||||
stops.splice(0, 1, { signalId: ownSignal.id, phaseGroup: ownSignal.phaseGroup, distance: round(crossingStopDistance), matchDistanceMeters: round(nearest.distance), stopReason: "crosswalk-control" });
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return {
|
||||
id,
|
||||
kind,
|
||||
fleet: fleetCount,
|
||||
modelName: `${modelPrefix}${model}`,
|
||||
modelScale: KIND_PLACEMENT[kind].modelScale,
|
||||
headingOffsetDegrees: MODEL_HEADING_OFFSET_DEGREES,
|
||||
heightMeters: KIND_PLACEMENT[kind].heightMeters,
|
||||
movementId: movement.id,
|
||||
approachRoadId,
|
||||
approachLaneId: movement.fromLaneId || null,
|
||||
turn: movement.turn,
|
||||
exitRoadId: movement.toRoadId,
|
||||
coordinates,
|
||||
distances,
|
||||
lengthMeters: round(distances.at(-1)),
|
||||
speedMetersPerSecond: dynamics.desiredSpeedMetersPerSecond,
|
||||
accelerationMetersPerSecondSquared: dynamics.accelerationMetersPerSecondSquared,
|
||||
decelerationMetersPerSecondSquared: dynamics.decelerationMetersPerSecondSquared,
|
||||
vehicleLengthMeters,
|
||||
crossingId: crossing?.id || null,
|
||||
crossingConflicts,
|
||||
crossingStopDistance: crossingStopDistance === null ? null : round(crossingStopDistance),
|
||||
stops,
|
||||
};
|
||||
}
|
||||
|
||||
// Bind each approach to the signal whose *facing direction* controls its
|
||||
// inbound movement. approachId/node equality and nearest-stop-point matching
|
||||
// are not sufficient here: this junction's identifiers pair several signals
|
||||
// with perpendicular stop lines, which releases cars on the cross street's
|
||||
// green. The route tangent is the vehicle's actual inbound heading.
|
||||
function mapApproachPhases(signals, movementsByApproach, context, diagnostics) {
|
||||
const phaseByRoad = new Map();
|
||||
const signalIdByRoad = new Map();
|
||||
const unmatched = [];
|
||||
for (const [roadId, movements] of movementsByApproach) {
|
||||
let best = null;
|
||||
for (const movement of movements) {
|
||||
const path = buildMovementPath(movement, context);
|
||||
if (!path || path.coordinates.length < 2) continue;
|
||||
const heading = bearingDegrees(path.coordinates[0], path.coordinates[1]);
|
||||
let nearestSignal = null;
|
||||
for (const signal of signals) {
|
||||
if (!Number.isFinite(signal.stopLongitude) || !Number.isFinite(signal.stopLatitude)) continue;
|
||||
const travelHeading = signalTravelHeading(signal);
|
||||
if (!Number.isFinite(travelHeading)) continue;
|
||||
const headingDelta = angleDeltaDegrees(heading, travelHeading);
|
||||
if (headingDelta > 45) continue;
|
||||
let distance = Infinity;
|
||||
for (const point of path.coordinates) {
|
||||
distance = Math.min(distance, haversine(point, [signal.stopLongitude, signal.stopLatitude]));
|
||||
}
|
||||
if (distance > 55) continue;
|
||||
const score = headingDelta * 2 + distance;
|
||||
if (!nearestSignal || score < nearestSignal.score) nearestSignal = { signal, distance, headingDelta, score };
|
||||
}
|
||||
if (nearestSignal && (!best || nearestSignal.score < best.score)) best = nearestSignal;
|
||||
}
|
||||
if (best) {
|
||||
phaseByRoad.set(roadId, best.signal.phaseGroup);
|
||||
signalIdByRoad.set(roadId, best.signal.id);
|
||||
} else unmatched.push(roadId);
|
||||
}
|
||||
if (!phaseByRoad.size) {
|
||||
diagnostics.push({ severity: "warning", reason: "signal_approach_heading_pairing_failed", message: "No signal could be paired to an inbound route by travel heading; participants remain controlled only where a signal match exists." });
|
||||
}
|
||||
for (const roadId of unmatched) diagnostics.push({ severity: "warning", reason: "signal_approach_unmatched", message: `No direction-compatible signal was found for approach ${roadId}.`, approachRoadId: roadId });
|
||||
return { phaseByRoad, signalIdByRoad };
|
||||
}
|
||||
|
||||
function signalTravelHeading(signal) {
|
||||
const face = Number(signal.faceHeadingDegrees);
|
||||
if (Number.isFinite(face)) return (face + 180) % 360;
|
||||
const mast = Number(signal.mastHeadingDegrees ?? signal.headingDegrees);
|
||||
return Number.isFinite(mast) ? (mast + 90 + 360) % 360 : NaN;
|
||||
}
|
||||
|
||||
function bearingDegrees(a, b) {
|
||||
const radians = Math.PI / 180;
|
||||
const dLon = (b[0] - a[0]) * radians;
|
||||
const lat1 = a[1] * radians;
|
||||
const lat2 = b[1] * radians;
|
||||
const y = Math.sin(dLon) * Math.cos(lat2);
|
||||
const x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(dLon);
|
||||
return (Math.atan2(y, x) / radians + 360) % 360;
|
||||
}
|
||||
|
||||
function angleDeltaDegrees(a, b) {
|
||||
const delta = Math.abs((a - b) % 360);
|
||||
return Math.min(delta, 360 - delta);
|
||||
}
|
||||
|
||||
function groupByTurn(movements) {
|
||||
const byTurn = {};
|
||||
for (const movement of movements) {
|
||||
const turn = movement.turn || "through";
|
||||
(byTurn[turn] = byTurn[turn] || []).push(movement);
|
||||
}
|
||||
for (const list of Object.values(byTurn)) list.sort((a, b) => String(a.id).localeCompare(String(b.id)));
|
||||
return byTurn;
|
||||
}
|
||||
|
||||
function movementLaneIndex(movement, side) {
|
||||
const laneId = side === "from" ? movement.fromLaneId : movement.toLaneId;
|
||||
const match = /:(\d+)$/.exec(String(laneId || ""));
|
||||
return match ? Number(match[1]) : null;
|
||||
}
|
||||
|
||||
function pickMovementByCanonicalLane(movements, canonicalLane) {
|
||||
if (!movements.length) return null;
|
||||
if (Number.isFinite(canonicalLane)) {
|
||||
const preferred = movements.find((movement) => movementLaneIndex(movement, "from") === canonicalLane);
|
||||
if (preferred) return preferred;
|
||||
}
|
||||
return movements[0];
|
||||
}
|
||||
|
||||
function pickCurbLaneMovement(movements) {
|
||||
if (!movements.length) return null;
|
||||
const withLanes = movements.filter((movement) => Number.isFinite(movementLaneIndex(movement, "from")));
|
||||
if (withLanes.length) {
|
||||
// Highest-numbered approach lane rides nearest the curb.
|
||||
withLanes.sort((a, b) => movementLaneIndex(b, "from") - movementLaneIndex(a, "from"));
|
||||
return withLanes[0];
|
||||
}
|
||||
return movements[0];
|
||||
}
|
||||
|
||||
// Assemble approach lane -> connector -> exit lane with travel-direction
|
||||
// continuity: each lane line is oriented so it flows into/out of its
|
||||
// connector, then trimmed to a cap so vehicles spawn near the junction
|
||||
// instead of at the far clip boundary.
|
||||
function buildMovementPath(movement, context) {
|
||||
const { roads, connectorByMovement, laneCenterlineById, diagnostics } = context;
|
||||
const connector = cleanLine(connectorByMovement.get(movement.id));
|
||||
if (!connector || connector.length < 2) return null;
|
||||
const approachRoad = roads.get(movement.fromRoadId);
|
||||
const exitRoad = roads.get(movement.toRoadId);
|
||||
const approachLine = laneLineForRoad(laneCenterlineById, approachRoad, movementLaneIndex(movement, "from"));
|
||||
const exitLine = laneLineForRoad(laneCenterlineById, exitRoad, movementLaneIndex(movement, "to"));
|
||||
if (!approachLine || !exitLine) return null;
|
||||
|
||||
const elements = [
|
||||
{ type: "line", role: "approach", coords: approachLine },
|
||||
{ type: "connector", coords: connector },
|
||||
{ type: "line", role: "exit", coords: exitLine },
|
||||
];
|
||||
const coordinates = [];
|
||||
let anchored = false;
|
||||
for (let index = 0; index < elements.length; index += 1) {
|
||||
const element = elements[index];
|
||||
const anchor = coordinates.at(-1) || null;
|
||||
if (element.type === "line") {
|
||||
const nextConnector = elements[index + 1]?.type === "connector" ? elements[index + 1] : null;
|
||||
const previousConnector = elements[index - 1]?.type === "connector" ? elements[index - 1] : null;
|
||||
let oriented;
|
||||
if (!anchored) {
|
||||
// Head of the route: align the line end with the first connector.
|
||||
const direction = nextConnector ? segmentDirection(nextConnector.coords, 0) : segmentDirection(element.coords, element.coords.length - 2);
|
||||
oriented = orientLine(element.coords, nextConnector?.coords[0] || element.coords.at(-1), direction, true);
|
||||
} else {
|
||||
// Continue out of the previous connector.
|
||||
const direction = previousConnector ? segmentDirection(previousConnector.coords, previousConnector.coords.length - 2) : segmentDirection(element.coords, 0);
|
||||
oriented = orientLine(element.coords, anchor, direction, false);
|
||||
}
|
||||
if (!oriented) {
|
||||
diagnostics.push({ severity: "warning", reason: "participant_path_orientation_failed", message: `Movement ${movement.id} lane geometry could not be oriented against its connector.`, movementId: movement.id });
|
||||
return null;
|
||||
}
|
||||
const startGap = anchor ? distanceMeters(anchor, oriented[0]) : 0;
|
||||
if (anchored && startGap > MAX_CONNECTION_GAP_METERS) {
|
||||
diagnostics.push({ severity: "warning", reason: "participant_path_gap", message: `Movement ${movement.id} path gap ${round(startGap)}m exceeds ${MAX_CONNECTION_GAP_METERS}m at element ${index}.`, movementId: movement.id, elementIndex: index });
|
||||
return null;
|
||||
}
|
||||
append(coordinates, trimLine(oriented, PARTICIPANT_CORRIDOR_CAP_METERS, element.role === "approach" ? "end" : "start"));
|
||||
} else {
|
||||
const startGap = anchor ? distanceMeters(anchor, element.coords[0]) : 0;
|
||||
if (anchored && startGap > MAX_CONNECTION_GAP_METERS) {
|
||||
diagnostics.push({ severity: "warning", reason: "participant_path_gap", message: `Movement ${movement.id} path gap ${round(startGap)}m exceeds ${MAX_CONNECTION_GAP_METERS}m at element ${index}.`, movementId: movement.id, elementIndex: index });
|
||||
return null;
|
||||
}
|
||||
append(coordinates, element.coords);
|
||||
}
|
||||
anchored = true;
|
||||
}
|
||||
if (coordinates.length < 4) return null;
|
||||
return { coordinates };
|
||||
}
|
||||
|
||||
function laneLineForRoad(laneCenterlineById, road, laneIndex) {
|
||||
if (!road) return null;
|
||||
if (Number.isFinite(laneIndex)) {
|
||||
const lane = laneCenterlineById.get(`${road.id}:${laneIndex}`);
|
||||
if (lane) return lane;
|
||||
}
|
||||
return laneCenterlineById.get(`${road.id}:1`) || road.centerline || null;
|
||||
}
|
||||
|
||||
// Keep only the corridor portion within `capMeters` of the junction end.
|
||||
function trimLine(line, capMeters, attachEnd) {
|
||||
const cleaned = cleanLine(line);
|
||||
if (!cleaned || cleaned.length < 2) return cleaned;
|
||||
const distances = cumulativeDistances(cleaned);
|
||||
const total = distances.at(-1);
|
||||
if (total <= capMeters) return cleaned;
|
||||
const kept = [];
|
||||
if (attachEnd === "end") {
|
||||
const cut = total - capMeters;
|
||||
for (let index = 0; index < cleaned.length; index += 1) {
|
||||
if (distances[index] >= cut) kept.push(cleaned[index]);
|
||||
}
|
||||
} else {
|
||||
for (let index = 0; index < cleaned.length; index += 1) {
|
||||
if (distances[index] <= capMeters) kept.push(cleaned[index]);
|
||||
}
|
||||
}
|
||||
return kept.length >= 2 ? kept : cleaned;
|
||||
}
|
||||
|
||||
function cleanLine(line) {
|
||||
if (!Array.isArray(line)) return null;
|
||||
const cleaned = line.map((point) => [Number(point[0]), Number(point[1])]).filter((point) => point.every(Number.isFinite));
|
||||
return cleaned.length >= 2 ? cleaned : null;
|
||||
}
|
||||
|
||||
// Orient `line` so the end that should meet `attachPoint` does, preferring the
|
||||
// orientation whose initial travel direction continues `directionSample`.
|
||||
function orientLine(line, attachPoint, directionSample, attachAtEnd) {
|
||||
const forward = line.map((point) => point.slice(0, 2).map(Number)).filter((point) => point.every(Number.isFinite));
|
||||
if (forward.length < 2) return null;
|
||||
const reverse = [...forward].reverse();
|
||||
const candidates = attachAtEnd
|
||||
? [
|
||||
{ line: forward, attach: forward.at(-1), direction: segmentDirection(forward, forward.length - 2) },
|
||||
{ line: reverse, attach: reverse.at(-1), direction: segmentDirection(reverse, reverse.length - 2) },
|
||||
]
|
||||
: [
|
||||
{ line: forward, attach: forward[0], direction: segmentDirection(forward, 0) },
|
||||
{ line: reverse, attach: reverse[0], direction: segmentDirection(reverse, 0) },
|
||||
];
|
||||
const desired = Math.atan2(directionSample[1], directionSample[0]);
|
||||
let best = null;
|
||||
let bestScore = Infinity;
|
||||
for (const candidate of candidates) {
|
||||
const proximity = distanceMeters(candidate.attach, attachPoint);
|
||||
if (!Number.isFinite(proximity)) continue;
|
||||
const actual = Math.atan2(candidate.direction[1], candidate.direction[0]);
|
||||
let turn = Math.abs(actual - desired);
|
||||
while (turn > Math.PI) turn = 2 * Math.PI - turn;
|
||||
// Direction continuity dominates; proximity only breaks near-ties.
|
||||
const score = turn * 1000 + proximity;
|
||||
if (score < bestScore) {
|
||||
bestScore = score;
|
||||
best = candidate.line;
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
function segmentDirection(points, index) {
|
||||
const a = points[index];
|
||||
const b = points[index + 1] || a;
|
||||
const metersPerDegreeLatitude = 111320;
|
||||
const metersPerDegreeLongitude = Math.max(1, Math.cos(((a[1] + b[1]) / 2) * Math.PI / 180) * metersPerDegreeLatitude);
|
||||
return [(b[0] - a[0]) * metersPerDegreeLongitude, (b[1] - a[1]) * metersPerDegreeLatitude];
|
||||
}
|
||||
|
||||
function indexLaneCenterlines(laneCenterlineCollection) {
|
||||
const byId = new Map();
|
||||
for (const feature of laneCenterlineCollection?.features || []) {
|
||||
const properties = feature.properties || {};
|
||||
const coordinates = feature.geometry?.coordinates || [];
|
||||
if (properties.native_id) byId.set(properties.native_id, coordinates);
|
||||
if (properties.road_id && Number.isFinite(Number(properties.lane_index))) byId.set(`${properties.road_id}:${properties.lane_index}`, coordinates);
|
||||
}
|
||||
return byId;
|
||||
}
|
||||
|
||||
// Main crosswalk spans: one band per intersection arm. Zebra stripes are
|
||||
// grouped by crossing_key; the band phase comes from the paired stop line on
|
||||
// the same arm (nearest stop line wins as a geometry-level fallback).
|
||||
function collectMainCrosswalks(crosswalks, stopLineFeatures, diagnostics) {
|
||||
if (!crosswalks?.features?.length) return [];
|
||||
const groups = new Map();
|
||||
for (const feature of crosswalks.features) {
|
||||
const properties = feature.properties || {};
|
||||
const crossingKey = properties.crossing_key || "";
|
||||
if (!crossingKey || crossingKey.includes(":corner:")) continue;
|
||||
if (!groups.has(crossingKey)) groups.set(crossingKey, []);
|
||||
groups.get(crossingKey).push(feature);
|
||||
}
|
||||
const stopLines = stopLineFeatures.map((feature) => ({
|
||||
roadId: feature.properties?.road_id,
|
||||
armKey: feature.properties?.arm_key || "",
|
||||
points: flattenCoordinates(feature.geometry),
|
||||
})).filter((entry) => entry.roadId && entry.points.length);
|
||||
const crossings = [];
|
||||
for (const [crossingKey, features] of groups) {
|
||||
const centroids = features.map((feature) => ringCentroid(feature.geometry)).filter(Boolean);
|
||||
if (centroids.length < 2) continue;
|
||||
const roadId = resolveCrosswalkRoad(crossingKey, features, stopLines);
|
||||
if (!roadId) {
|
||||
diagnostics.push({ severity: "warning", reason: "crosswalk_road_unresolved", message: `Crosswalk ${crossingKey} could not be tied to a stop line.`, crosswalkId: crossingKey });
|
||||
continue;
|
||||
}
|
||||
crossings.push({
|
||||
id: crossingIdFromKey(crossingKey),
|
||||
roadId,
|
||||
coordinates: crosswalkCenterline(centroids),
|
||||
});
|
||||
}
|
||||
return crossings.sort((a, b) => a.id.localeCompare(b.id));
|
||||
}
|
||||
|
||||
function resolveCrosswalkRoad(crossingKey, features, stopLines) {
|
||||
const decodedArm = safeDecode(crossingKey);
|
||||
const armPrefix = decodedArm.split("|")[0];
|
||||
for (const stopLine of stopLines) {
|
||||
const decodedStopArm = safeDecode(stopLine.armKey);
|
||||
if (decodedStopArm.split("|").some((part) => part === armPrefix)) return stopLine.roadId;
|
||||
}
|
||||
// Geometry fallback: the stop line nearest the band centroid.
|
||||
const centroid = features.map((feature) => ringCentroid(feature.geometry)).filter(Boolean)[0];
|
||||
if (!centroid) return null;
|
||||
let best = null;
|
||||
let bestDistance = Infinity;
|
||||
for (const stopLine of stopLines) {
|
||||
for (const point of stopLine.points) {
|
||||
const distance = haversine(centroid, point);
|
||||
if (distance < bestDistance) {
|
||||
bestDistance = distance;
|
||||
best = stopLine.roadId;
|
||||
}
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
function crosswalkCenterline(centroids) {
|
||||
const meanLon = centroids.reduce((sum, point) => sum + point[0], 0) / centroids.length;
|
||||
const meanLat = centroids.reduce((sum, point) => sum + point[1], 0) / centroids.length;
|
||||
const metersPerDegreeLongitude = Math.max(1, Math.cos(meanLat * Math.PI / 180) * 111320);
|
||||
// Local meter frame centred on the cloud mean.
|
||||
const points = centroids.map((point) => [(point[0] - meanLon) * metersPerDegreeLongitude, (point[1] - meanLat) * 111320]);
|
||||
let sxx = 0;
|
||||
let syy = 0;
|
||||
let sxy = 0;
|
||||
for (const [x, y] of points) {
|
||||
sxx += x * x;
|
||||
syy += y * y;
|
||||
sxy += x * y;
|
||||
}
|
||||
// Zebra stripes repeat along the walking direction, so the centroid cloud's
|
||||
// major axis IS the walking line (its long side spans the road width).
|
||||
const theta = 0.5 * Math.atan2(2 * sxy, sxx - syy);
|
||||
const axis = [Math.cos(theta), Math.sin(theta)];
|
||||
let minProjection = Infinity;
|
||||
let maxProjection = -Infinity;
|
||||
let minPoint = null;
|
||||
let maxPoint = null;
|
||||
for (const [x, y] of points) {
|
||||
const projection = x * axis[0] + y * axis[1];
|
||||
if (projection < minProjection) {
|
||||
minProjection = projection;
|
||||
minPoint = [x, y];
|
||||
}
|
||||
if (projection > maxProjection) {
|
||||
maxProjection = projection;
|
||||
maxPoint = [x, y];
|
||||
}
|
||||
}
|
||||
// Extend past the outermost stripes onto the sidewalk ramps.
|
||||
const start = [minPoint[0] - axis[0] * CROSSWALK_PAD_METERS, minPoint[1] - axis[1] * CROSSWALK_PAD_METERS];
|
||||
const end = [maxPoint[0] + axis[0] * CROSSWALK_PAD_METERS, maxPoint[1] + axis[1] * CROSSWALK_PAD_METERS];
|
||||
const toDegrees = ([x, y]) => [meanLon + x / metersPerDegreeLongitude, meanLat + y / 111320];
|
||||
return [
|
||||
toDegrees(start),
|
||||
toDegrees(end),
|
||||
];
|
||||
}
|
||||
|
||||
function ringCentroid(geometry) {
|
||||
const rings = geometry?.type === "Polygon" ? geometry.coordinates : null;
|
||||
if (!rings?.length || !rings[0]?.length) return null;
|
||||
const ring = rings[0];
|
||||
let lon = 0;
|
||||
let lat = 0;
|
||||
for (const point of ring) {
|
||||
lon += point[0];
|
||||
lat += point[1];
|
||||
}
|
||||
return [lon / ring.length, lat / ring.length];
|
||||
}
|
||||
|
||||
function flattenCoordinates(geometry) {
|
||||
const points = [];
|
||||
const walk = (value) => {
|
||||
if (!Array.isArray(value)) return;
|
||||
if (value.every((item) => typeof item === "number")) {
|
||||
points.push([value[0], value[1]]);
|
||||
return;
|
||||
}
|
||||
for (const item of value) walk(item);
|
||||
};
|
||||
walk(geometry?.coordinates);
|
||||
return points.filter((point) => point.every(Number.isFinite));
|
||||
}
|
||||
|
||||
function crossingIdFromKey(crossingKey) {
|
||||
const decoded = safeDecode(crossingKey);
|
||||
const match = /segment:way\/(\d+)\/(\d+)/.exec(decoded);
|
||||
return match ? `crossing-${match[1]}-${match[2]}` : `crossing-${Math.abs(hashString(decoded)) % 100000}`;
|
||||
}
|
||||
|
||||
function shortRoadId(roadId) {
|
||||
return String(roadId || "").replace(/^road:way\//, "").replace(/:forward$/, "");
|
||||
}
|
||||
|
||||
function safeDecode(value) {
|
||||
try {
|
||||
return decodeURIComponent(String(value || ""));
|
||||
} catch (error) {
|
||||
return String(value || "");
|
||||
}
|
||||
}
|
||||
|
||||
function hashString(value) {
|
||||
let hash = 0;
|
||||
for (let index = 0; index < value.length; index += 1) {
|
||||
hash = (hash * 31 + value.charCodeAt(index)) | 0;
|
||||
}
|
||||
return hash;
|
||||
}
|
||||
|
||||
function findCycles(start, current, roadIds, movements, outgoing, seen, candidates) {
|
||||
const nextRoadIds = [...roadIds, current];
|
||||
if (nextRoadIds.length > MAX_ROAD_COUNT) return;
|
||||
@@ -257,4 +995,4 @@ function validateSettings(settings) {
|
||||
function sha256(file) { return crypto.createHash("sha256").update(fs.readFileSync(file)).digest("hex"); }
|
||||
function round(value) { return Math.round(value * 100) / 100; }
|
||||
|
||||
module.exports = { SCHEMA, DEFAULT_SETTINGS, buildNativeTrafficSimulation };
|
||||
module.exports = { SCHEMA, DEFAULT_SETTINGS, DEFAULT_SIGNAL_PLAN, buildNativeTrafficSimulation };
|
||||
Reference in new issue
Block a user