@drawtonomy/sdk 0.8.0 → 0.10.0

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (46) hide show
  1. package/dist/exporter/index.d.ts +5 -2
  2. package/dist/exporter/index.d.ts.map +1 -1
  3. package/dist/exporter/index.js +3 -0
  4. package/dist/exporter/index.js.map +1 -1
  5. package/dist/exporter/lanelet2.d.ts.map +1 -1
  6. package/dist/exporter/lanelet2.js +434 -2
  7. package/dist/exporter/lanelet2.js.map +1 -1
  8. package/dist/exporter/odrCarryThrough.d.ts +89 -0
  9. package/dist/exporter/odrCarryThrough.d.ts.map +1 -0
  10. package/dist/exporter/odrCarryThrough.js +186 -0
  11. package/dist/exporter/odrCarryThrough.js.map +1 -0
  12. package/dist/exporter/odrGeometry.d.ts +64 -0
  13. package/dist/exporter/odrGeometry.d.ts.map +1 -0
  14. package/dist/exporter/odrGeometry.js +324 -0
  15. package/dist/exporter/odrGeometry.js.map +1 -0
  16. package/dist/exporter/odrGeometryFit.d.ts +37 -0
  17. package/dist/exporter/odrGeometryFit.d.ts.map +1 -0
  18. package/dist/exporter/odrGeometryFit.js +476 -0
  19. package/dist/exporter/odrGeometryFit.js.map +1 -0
  20. package/dist/exporter/odrToShapes.d.ts +53 -0
  21. package/dist/exporter/odrToShapes.d.ts.map +1 -0
  22. package/dist/exporter/odrToShapes.js +1696 -0
  23. package/dist/exporter/odrToShapes.js.map +1 -0
  24. package/dist/exporter/opendrive.d.ts +14 -1
  25. package/dist/exporter/opendrive.d.ts.map +1 -1
  26. package/dist/exporter/opendrive.js +1813 -163
  27. package/dist/exporter/opendrive.js.map +1 -1
  28. package/dist/exporter/opendriveParser.d.ts +194 -0
  29. package/dist/exporter/opendriveParser.d.ts.map +1 -0
  30. package/dist/exporter/opendriveParser.js +429 -0
  31. package/dist/exporter/opendriveParser.js.map +1 -0
  32. package/dist/exporter/osmToShapes.d.ts +52 -3
  33. package/dist/exporter/osmToShapes.d.ts.map +1 -1
  34. package/dist/exporter/osmToShapes.js +208 -5
  35. package/dist/exporter/osmToShapes.js.map +1 -1
  36. package/dist/exporter/projection.d.ts +1 -1
  37. package/dist/exporter/projection.d.ts.map +1 -1
  38. package/dist/exporter/projection.js +2 -3
  39. package/dist/exporter/projection.js.map +1 -1
  40. package/dist/exporter/units.d.ts +7 -0
  41. package/dist/exporter/units.d.ts.map +1 -1
  42. package/dist/exporter/units.js +11 -0
  43. package/dist/exporter/units.js.map +1 -1
  44. package/dist/types.d.ts +27 -1
  45. package/dist/types.d.ts.map +1 -1
  46. package/package.json +1 -1
@@ -1,15 +1,31 @@
1
1
  // OpenDRIVE 1.8 (.xodr) exporter — emits a road network from a snapshot.
2
2
  // No external library dependencies.
3
3
  //
4
- // Design (Phase 1):
5
- // - Each lane is emitted as an independent <road> (junctions are out of scope)
6
- // - Reference line is the midpoints of the left/right boundary samples
7
- // - Each segment is a <line> geometry; lane width is a per-sample <width>
8
- // - Lane connectivity (next/prev) is written into <link>
4
+ // Design:
5
+ // - Laterally adjacent same-direction lanes (detected through shared boundary
6
+ // linestrings) are grouped into one road bundle and emitted as a single
7
+ // <road> with lanes -1, -2, ... (inner to outer)
8
+ // - The road reference line is the bundle's leftmost boundary (the left edge
9
+ // in travel direction), so lane 0 sits on it and no laneOffset is needed
10
+ // - The reference polyline is fitted into analytic primitives (<line>, <arc>,
11
+ // <paramPoly3>) by odrGeometryFit; lane widths are piecewise-linear <width>
12
+ // records measured along the fitted reference normals (the exact inverse of
13
+ // the importer's offset-along-normal reconstruction)
14
+ // - Lane connectivity (next/prev) is written into road/lane <link> records;
15
+ // branch/merge edges that road links cannot express are synthesized into
16
+ // <junction> elements with short connecting roads (one per lane edge, each
17
+ // carrying predecessor/successor road links) so the standard
18
+ // incoming -> connecting -> outgoing structure holds (see planConnectivity)
19
+ // - Lanelet-only lane tags are stashed per lane in
20
+ // <userData code="laneAttributes"> and restored on import; signal validity
21
+ // uses <validity fromLane toLane> lane ranges within a road and
22
+ // <signalReference> only when a signal spans several roads
9
23
  // - Coordinate frame: canvas (x right, y down) → ENU (x right, y up); y is flipped
10
- import { computeCenterlineWithWidth } from './laneCenterline';
24
+ import { evalGeometry } from './odrGeometry';
25
+ import { fitPlanView } from './odrGeometryFit';
11
26
  import { originToProjString } from './projection';
12
- import { escapeXml, fmt, pxToEnuX, pxToEnuY, pxToMeter } from './units';
27
+ import { escapeXml, fmt, fmtPrecise, pxToEnuX, pxToEnuY, pxToMeter } from './units';
28
+ import { extractOdrDocument, hashRoadState, rewriteRoadLinkTargets, } from './odrCarryThrough';
13
29
  /** O(1) shape lookup by id. */
14
30
  function buildShapeMap(shapes) {
15
31
  const map = new Map();
@@ -17,36 +33,6 @@ function buildShapeMap(shapes) {
17
33
  map.set(s.id, s);
18
34
  return map;
19
35
  }
20
- /**
21
- * Build the centerline + width samples for a lane from its two boundaries.
22
- * If pointOverrides is provided, those point ids are read from the override
23
- * map instead of from the shape map (used for boundary alignment snapping).
24
- */
25
- function buildRoadGeometry(shapeMap, lane, pointOverrides) {
26
- const leftId = lane.props.leftBoundaryId;
27
- const rightId = lane.props.rightBoundaryId;
28
- if (!leftId || !rightId)
29
- return null;
30
- const left = shapeMap.get(leftId);
31
- const right = shapeMap.get(rightId);
32
- if (!left || !right)
33
- return null;
34
- const leftPts = collectPoints(shapeMap, left.props.pointIds, lane.props.invertLeft, pointOverrides);
35
- const rightPts = collectPoints(shapeMap, right.props.pointIds, lane.props.invertRight, pointOverrides);
36
- if (leftPts.length < 2 || rightPts.length < 2)
37
- return null;
38
- const samples = computeCenterlineWithWidth(leftPts, rightPts);
39
- if (samples.length < 2)
40
- return null;
41
- const odrSamples = samples.map((s) => ({
42
- x: pxToEnuX(s.x),
43
- y: pxToEnuY(s.y),
44
- width: pxToMeter(s.width),
45
- s: pxToMeter(s.s),
46
- }));
47
- const length = odrSamples[odrSamples.length - 1].s;
48
- return { laneId: lane.id, samples, odrSamples, length };
49
- }
50
36
  function collectPoints(shapeMap, pointIds, invert, pointOverrides) {
51
37
  const ids = invert ? [...pointIds].reverse() : pointIds;
52
38
  const pts = [];
@@ -62,6 +48,263 @@ function collectPoints(shapeMap, pointIds, invert, pointOverrides) {
62
48
  }
63
49
  return pts;
64
50
  }
51
+ /** Boundary polyline of a linestring in travel order, or null when unusable. */
52
+ function boundaryPointsOf(shapeMap, boundaryId, invert, pointOverrides) {
53
+ if (!boundaryId)
54
+ return null;
55
+ const ls = shapeMap.get(boundaryId);
56
+ if (!ls)
57
+ return null;
58
+ const pts = collectPoints(shapeMap, ls.props.pointIds, invert, pointOverrides);
59
+ return pts.length >= 2 ? pts : null;
60
+ }
61
+ /**
62
+ * Group lanes into road bundles by lateral adjacency.
63
+ *
64
+ * Lane B is the direct right neighbour of lane A when A's right boundary IS
65
+ * B's left boundary — the same linestring traversed in the same direction
66
+ * (`A.invertRight === B.invertLeft`); a direction mismatch means the
67
+ * neighbour travels the other way (e.g. the two sides of a two-way road) and
68
+ * belongs in its own bundle. The relation must be unique on both sides so
69
+ * pathological data (several lanes claiming one boundary side) degrades to
70
+ * separate bundles instead of guessing.
71
+ *
72
+ * Because adjacency requires sharing the whole linestring, every lane of a
73
+ * bundle spans the same longitudinal extent by construction.
74
+ */
75
+ function detectBundles(lanes) {
76
+ const byLeft = new Map();
77
+ const byRight = new Map();
78
+ for (const lane of lanes) {
79
+ const l = lane.props.leftBoundaryId;
80
+ const r = lane.props.rightBoundaryId;
81
+ if (l)
82
+ byLeft.set(l, [...(byLeft.get(l) ?? []), lane]);
83
+ if (r)
84
+ byRight.set(r, [...(byRight.get(r) ?? []), lane]);
85
+ }
86
+ const rightNeighbor = new Map();
87
+ const hasLeftNeighbor = new Set();
88
+ for (const lane of lanes) {
89
+ const rb = lane.props.rightBoundaryId;
90
+ if (!rb)
91
+ continue;
92
+ const candidates = (byLeft.get(rb) ?? []).filter(b => b.id !== lane.id && b.props.invertLeft === lane.props.invertRight);
93
+ if (candidates.length !== 1)
94
+ continue;
95
+ const b = candidates[0];
96
+ const owners = (byRight.get(rb) ?? []).filter(a => a.props.invertRight === b.props.invertLeft);
97
+ if (owners.length !== 1 || owners[0].id !== lane.id)
98
+ continue;
99
+ rightNeighbor.set(lane.id, b);
100
+ hasLeftNeighbor.add(b.id);
101
+ }
102
+ const bundles = [];
103
+ const visited = new Set();
104
+ const walk = (start) => {
105
+ const bundle = [];
106
+ let cur = start;
107
+ while (cur && !visited.has(cur.id)) {
108
+ visited.add(cur.id);
109
+ bundle.push(cur);
110
+ cur = rightNeighbor.get(cur.id);
111
+ }
112
+ if (bundle.length > 0)
113
+ bundles.push(bundle);
114
+ };
115
+ // Start from the leftmost lane of each chain ...
116
+ for (const lane of lanes) {
117
+ if (!visited.has(lane.id) && !hasLeftNeighbor.has(lane.id))
118
+ walk(lane);
119
+ }
120
+ // ... and break adjacency cycles (degenerate ring data) deterministically.
121
+ for (const lane of lanes) {
122
+ if (!visited.has(lane.id))
123
+ walk(lane);
124
+ }
125
+ return bundles;
126
+ }
127
+ /** Distance from `p` to the polyline `pts` (projection onto each segment). */
128
+ function distancePointToPolyline(p, pts) {
129
+ let best = Infinity;
130
+ for (let i = 0; i < pts.length - 1; i++) {
131
+ const a = pts[i];
132
+ const b = pts[i + 1];
133
+ const dx = b.x - a.x;
134
+ const dy = b.y - a.y;
135
+ const len2 = dx * dx + dy * dy;
136
+ let t = len2 > 1e-18 ? ((p.x - a.x) * dx + (p.y - a.y) * dy) / len2 : 0;
137
+ if (t < 0)
138
+ t = 0;
139
+ if (t > 1)
140
+ t = 1;
141
+ const d = Math.hypot(p.x - (a.x + t * dx), p.y - (a.y + t * dy));
142
+ if (d < best)
143
+ best = d;
144
+ }
145
+ return best;
146
+ }
147
+ /**
148
+ * Signed lateral offset (m, positive toward -t / the right of the travel
149
+ * direction) from each fitted reference pose to a boundary polyline, measured
150
+ * along the pose normal — the exact inverse of the importer's
151
+ * offset-along-normal boundary reconstruction. Among multiple normal/boundary
152
+ * intersections the one closest to the previous station's offset wins
153
+ * (continuity); stations whose normal misses the boundary entirely (e.g. the
154
+ * very ends, where boundary extents differ slightly) fall back to the
155
+ * closest-point distance.
156
+ */
157
+ /**
158
+ * Largest believable offset change between neighbouring width stations (m).
159
+ * An intersection jumping further than this is the normal ray hitting a far
160
+ * branch of the boundary (e.g. the opposite end of a ~180° ramp), not the
161
+ * adjacent lane edge, and is discarded for the closest-point fallback.
162
+ */
163
+ const OFFSET_JUMP_TOL_M = 5;
164
+ function normalOffsets(poses, bnd) {
165
+ const out = [];
166
+ let prev = null;
167
+ for (const pose of poses) {
168
+ // Right normal of heading h in ENU: (sin h, -cos h).
169
+ const nx = Math.sin(pose.hdg);
170
+ const ny = -Math.cos(pose.hdg);
171
+ const fallback = distancePointToPolyline({ x: pose.x, y: pose.y }, bnd);
172
+ const refVal = prev ?? fallback;
173
+ let best = null;
174
+ for (let i = 0; i < bnd.length - 1; i++) {
175
+ const dx = bnd[i + 1].x - bnd[i].x;
176
+ const dy = bnd[i + 1].y - bnd[i].y;
177
+ // Solve pose + t·n = bnd[i] + w·(bnd[i+1]-bnd[i]) for (t, w).
178
+ const det = dx * ny - dy * nx;
179
+ if (Math.abs(det) < 1e-12)
180
+ continue;
181
+ const rx = bnd[i].x - pose.x;
182
+ const ry = bnd[i].y - pose.y;
183
+ const w = (nx * ry - ny * rx) / det;
184
+ if (w < -1e-9 || w > 1 + 1e-9)
185
+ continue;
186
+ const t = (dx * ry - dy * rx) / det;
187
+ if (best === null || Math.abs(t - refVal) < Math.abs(best - refVal))
188
+ best = t;
189
+ }
190
+ if (best === null || Math.abs(best - refVal) > OFFSET_JUMP_TOL_M)
191
+ best = fallback;
192
+ prev = best;
193
+ out.push(best);
194
+ }
195
+ return out;
196
+ }
197
+ /**
198
+ * Build the bundle geometry: the fitted reference line (leftmost boundary)
199
+ * plus per-lane width samples.
200
+ *
201
+ * The reference polyline keeps the boundary's own vertices and is refined
202
+ * with uniform arc-length stations so the width grid is at least as dense as
203
+ * the densest boundary of the bundle. The polyline is then fitted into
204
+ * analytic plan-view primitives (line / arc / paramPoly3), and the width of
205
+ * lane i at station j is the gap between its inner and outer boundary
206
+ * measured along the fitted reference normal at that station, so the
207
+ * importer's offset-along-normal reconstruction reproduces the original
208
+ * boundaries with no longitudinal skew.
209
+ */
210
+ function buildBundleGeometry(shapeMap, bundleLanes, pointOverrides) {
211
+ const first = bundleLanes[0];
212
+ const boundaries = [];
213
+ const left = boundaryPointsOf(shapeMap, first.props.leftBoundaryId, first.props.invertLeft, pointOverrides);
214
+ if (!left)
215
+ return null;
216
+ boundaries.push(left);
217
+ for (const lane of bundleLanes) {
218
+ const right = boundaryPointsOf(shapeMap, lane.props.rightBoundaryId, lane.props.invertRight, pointOverrides);
219
+ if (!right)
220
+ return null;
221
+ boundaries.push(right);
222
+ }
223
+ // Boundaries in OpenDRIVE meters (ENU).
224
+ const bndOdr = boundaries.map(b => b.map(p => ({ x: pxToEnuX(p.x), y: pxToEnuY(p.y) })));
225
+ // The plan view is fitted to the reference boundary's own vertices only:
226
+ // they are true samples of the drawn curve, so the fitter's tangent
227
+ // estimates are sound there. Densifying the polyline before the fit would
228
+ // insert points along its chords, whose collinear runs masquerade as
229
+ // straight stretches and corrupt the tangent estimates (worst at the road
230
+ // ends, where the start/end heading defines the contact cross-section
231
+ // shared with the neighbouring roads).
232
+ const ref = bndOdr[0];
233
+ const fit = fitPlanView(ref);
234
+ if (fit.geometries.length === 0 || !(fit.length > 0))
235
+ return null;
236
+ // Width stations: the reference vertices (corners must survive into the
237
+ // width records) merged with a uniform grid as dense as the densest
238
+ // boundary (inner-boundary detail must survive too). Grid stations are
239
+ // placed by chord-length interpolation between the fitted stations of the
240
+ // surrounding reference vertices and posed on the fitted curve.
241
+ let n = 2;
242
+ for (const b of bndOdr)
243
+ n = Math.max(n, b.length);
244
+ const cum = [0];
245
+ for (let i = 1; i < ref.length; i++) {
246
+ cum.push(cum[i - 1] + Math.hypot(ref[i].x - ref[i - 1].x, ref[i].y - ref[i - 1].y));
247
+ }
248
+ const total = cum[cum.length - 1];
249
+ if (!(total > 0))
250
+ return null;
251
+ const params = cum.map(c => c / total);
252
+ for (let j = 0; j < n; j++)
253
+ params.push(j / (n - 1));
254
+ params.sort((a, b) => a - b);
255
+ const stationOf = (t) => {
256
+ const target = t * total;
257
+ let i = 1;
258
+ while (i < cum.length - 1 && cum[i] < target)
259
+ i++;
260
+ const c0 = cum[i - 1];
261
+ const c1 = cum[i];
262
+ const s0 = fit.samplePoses[i - 1].s;
263
+ const s1 = fit.samplePoses[i].s;
264
+ const f = c1 > c0 ? (target - c0) / (c1 - c0) : 0;
265
+ return s0 + (s1 - s0) * f;
266
+ };
267
+ const poseAtStation = (s) => {
268
+ const clamped = Math.min(Math.max(s, 0), fit.length);
269
+ let g = fit.geometries[0];
270
+ for (const geom of fit.geometries) {
271
+ if (geom.s <= clamped + 1e-12)
272
+ g = geom;
273
+ else
274
+ break;
275
+ }
276
+ const p = evalGeometry(g, Math.min(Math.max(clamped - g.s, 0), g.length));
277
+ return { s: clamped, x: p.x, y: p.y, hdg: p.hdg };
278
+ };
279
+ const samplePoses = [];
280
+ for (const t of params) {
281
+ const pose = poseAtStation(stationOf(t));
282
+ const last = samplePoses[samplePoses.length - 1];
283
+ if (!last || pose.s > last.s + 1e-9 || samplePoses.length === 0)
284
+ samplePoses.push(pose);
285
+ }
286
+ if (samplePoses.length < 2)
287
+ return null;
288
+ const offsets = bndOdr.map(b => normalOffsets(samplePoses, b));
289
+ // Contact stations measure each boundary's own endpoint (projected onto
290
+ // the contact normal) instead of the ray/polyline crossing: the endpoints
291
+ // are the welded corners shared with the neighbouring road, so both sides
292
+ // of a contact derive their border positions from the same drawn points
293
+ // and meet without a lateral step. (The ray crossing drifts by up to a few
294
+ // centimeters when a boundary meets the contact at a skew.)
295
+ const projectEndpoint = (pose, p, fallback) => {
296
+ const t = (p.x - pose.x) * Math.sin(pose.hdg) - (p.y - pose.y) * Math.cos(pose.hdg);
297
+ return Math.abs(t - fallback) <= 0.5 ? t : fallback;
298
+ };
299
+ const lastIdx = samplePoses.length - 1;
300
+ for (let b = 0; b < bndOdr.length; b++) {
301
+ const bnd = bndOdr[b];
302
+ offsets[b][0] = projectEndpoint(samplePoses[0], bnd[0], offsets[b][0]);
303
+ offsets[b][lastIdx] = projectEndpoint(samplePoses[lastIdx], bnd[bnd.length - 1], offsets[b][lastIdx]);
304
+ }
305
+ const laneWidths = bundleLanes.map((_, i) => samplePoses.map((_, j) => Math.max(0, offsets[i + 1][j] - offsets[i][j])));
306
+ return { planView: fit.geometries, samplePoses, laneWidths, length: fit.length };
307
+ }
65
308
  /**
66
309
  * Snap together boundary endpoints of connected lanes by clustering nearby
67
310
  * points and using the centroid as the canonical position.
@@ -92,7 +335,14 @@ function buildBoundaryAlignmentOverrides(shapeMap, lanes, epsilonPx = 30) {
92
335
  const pt = shapeMap.get(pid);
93
336
  if (!pt)
94
337
  continue;
95
- endpoints.push({ pointId: pid, x: pt.x, y: pt.y });
338
+ endpoints.push({
339
+ pointId: pid,
340
+ laneId: lane.id,
341
+ side,
342
+ boundary: sideKey === 'leftBoundaryId' ? 'left' : 'right',
343
+ x: pt.x,
344
+ y: pt.y,
345
+ });
96
346
  }
97
347
  };
98
348
  // Restrict to lanes that participate in a next/prev relationship.
@@ -104,22 +354,93 @@ function buildBoundaryAlignmentOverrides(shapeMap, lanes, epsilonPx = 30) {
104
354
  if (hasPrev)
105
355
  collectEndpoints(lane, 'start');
106
356
  }
107
- // Greedy clustering: group points within epsilon of each other.
357
+ // Forbidden point pairs: a lane's start-side and end-side endpoint Points
358
+ // must never land in the same cluster. A connecting lane shorter than
359
+ // epsilon would otherwise get its start and end merged into one cluster,
360
+ // collapsing its boundaries below the degenerate-road export guard and
361
+ // silently dropping the lane (and its next/prev chain). The constraint is
362
+ // tracked by point id — not by the owning lane entry — because boundary
363
+ // endpoints are often Point shapes shared with the neighbouring lanes, and
364
+ // a neighbour's entry could otherwise pull both of a short lane's end
365
+ // points into one cluster.
366
+ const sidePids = new Map();
367
+ for (const ep of endpoints) {
368
+ const entry = sidePids.get(ep.laneId) ?? { start: new Set(), end: new Set() };
369
+ entry[ep.side].add(ep.pointId);
370
+ sidePids.set(ep.laneId, entry);
371
+ }
372
+ const forbidden = new Map();
373
+ const forbid = (a, b) => {
374
+ forbidden.set(a, (forbidden.get(a) ?? new Set()).add(b));
375
+ forbidden.set(b, (forbidden.get(b) ?? new Set()).add(a));
376
+ };
377
+ for (const { start, end } of sidePids.values()) {
378
+ for (const s of start) {
379
+ for (const e of end) {
380
+ if (s !== e)
381
+ forbid(s, e);
382
+ }
383
+ }
384
+ }
385
+ // A lane's left-boundary endpoint must never merge with its right-boundary
386
+ // endpoint on the same side: lanes narrower than epsilon would be pinched
387
+ // to zero width at the contact (and the welded neighbours dragged along).
388
+ // Tracked by point id like above, so a genuine zero-width taper — where
389
+ // left and right already share one Point — is unaffected.
390
+ const boundaryPids = new Map();
391
+ for (const ep of endpoints) {
392
+ const key = `${ep.laneId}|${ep.side}`;
393
+ const entry = boundaryPids.get(key) ?? { left: new Set(), right: new Set() };
394
+ entry[ep.boundary].add(ep.pointId);
395
+ boundaryPids.set(key, entry);
396
+ }
397
+ for (const { left, right } of boundaryPids.values()) {
398
+ for (const l of left) {
399
+ for (const r of right) {
400
+ if (l !== r)
401
+ forbid(l, r);
402
+ }
403
+ }
404
+ }
405
+ // Greedy clustering: group points within epsilon of each other, with three
406
+ // refinements over plain first-fit grouping:
407
+ // 1. An endpoint never joins a cluster holding a point its point id is
408
+ // forbidden against (see above).
409
+ // 2. Among the eligible clusters the nearest one wins, so the far end of a
410
+ // short lane clusters with its true counterpart rather than with the
411
+ // first cluster found within epsilon.
412
+ // 3. An endpoint whose nearest in-range cluster is a forbidden one never
413
+ // hops to a farther eligible cluster: the forbidden cluster marks a
414
+ // neighbouring corner of the same contact (narrow lane / short lane),
415
+ // so anything beyond it belongs to a different corner entirely and
416
+ // merging would drag the contact sideways. It opens its own cluster.
108
417
  const clusters = [];
109
418
  const eps2 = epsilonPx * epsilonPx;
110
419
  for (const ep of endpoints) {
111
- let placed = false;
420
+ let best = null;
421
+ let bestD2 = Infinity;
422
+ let blockedD2 = Infinity;
423
+ const epForbidden = forbidden.get(ep.pointId);
112
424
  for (const cluster of clusters) {
113
425
  const c0 = cluster[0];
114
426
  const dx = ep.x - c0.x;
115
427
  const dy = ep.y - c0.y;
116
- if (dx * dx + dy * dy <= eps2) {
117
- cluster.push(ep);
118
- placed = true;
119
- break;
428
+ const d2 = dx * dx + dy * dy;
429
+ if (d2 > eps2)
430
+ continue;
431
+ if (epForbidden && cluster.some((m) => epForbidden.has(m.pointId))) {
432
+ if (d2 < blockedD2)
433
+ blockedD2 = d2;
434
+ continue;
120
435
  }
436
+ if (d2 >= bestD2)
437
+ continue;
438
+ best = cluster;
439
+ bestD2 = d2;
121
440
  }
122
- if (!placed)
441
+ if (best && bestD2 <= blockedD2)
442
+ best.push(ep);
443
+ else
123
444
  clusters.push([ep]);
124
445
  }
125
446
  // Use each cluster centroid as the snapped position.
@@ -145,89 +466,545 @@ function buildBoundaryAlignmentOverrides(shapeMap, lanes, epsilonPx = 30) {
145
466
  function emitPlanView(geom) {
146
467
  const lines = [];
147
468
  lines.push(` <planView>`);
148
- for (let i = 0; i < geom.odrSamples.length - 1; i++) {
149
- const a = geom.odrSamples[i];
150
- const b = geom.odrSamples[i + 1];
151
- const dx = b.x - a.x;
152
- const dy = b.y - a.y;
153
- const segLen = Math.hypot(dx, dy);
154
- if (segLen < 1e-9)
469
+ for (const g of geom.planView) {
470
+ if (g.length < 1e-9)
155
471
  continue;
156
- const hdg = Math.atan2(dy, dx);
157
- lines.push(` <geometry s="${fmt(a.s)}" x="${fmt(a.x)}" y="${fmt(a.y)}" hdg="${fmt(hdg)}" length="${fmt(segLen)}">`);
158
- lines.push(` <line/>`);
472
+ lines.push(` <geometry s="${fmt(g.s)}" x="${fmt(g.x)}" y="${fmt(g.y)}" hdg="${fmt(g.hdg)}" length="${fmt(g.length)}">`);
473
+ if (g.kind === 'arc') {
474
+ lines.push(` <arc curvature="${fmtPrecise(g.curvature)}"/>`);
475
+ }
476
+ else if (g.kind === 'paramPoly3') {
477
+ lines.push(` <paramPoly3 aU="${fmtPrecise(g.aU)}" bU="${fmtPrecise(g.bU)}" cU="${fmtPrecise(g.cU)}" dU="${fmtPrecise(g.dU)}" ` +
478
+ `aV="${fmtPrecise(g.aV)}" bV="${fmtPrecise(g.bV)}" cV="${fmtPrecise(g.cV)}" dV="${fmtPrecise(g.dV)}" pRange="arcLength"/>`);
479
+ }
480
+ else {
481
+ lines.push(` <line/>`);
482
+ }
159
483
  lines.push(` </geometry>`);
160
484
  }
161
485
  lines.push(` </planView>`);
162
486
  return lines.join('\n');
163
487
  }
164
- function emitLanes(geom, hasPrev, hasNext) {
488
+ /**
489
+ * Map a lanelet-style lane subtype to an OpenDRIVE lane type. The exact
490
+ * OpenDRIVE type wins when the lane carries `odr_type` (set by the OpenDRIVE
491
+ * importer), so imported maps round-trip their lane types.
492
+ */
493
+ const LANELET_SUBTYPE_TO_ODR_TYPE = {
494
+ road: 'driving',
495
+ highway: 'driving',
496
+ play_street: 'driving',
497
+ emergency_lane: 'shoulder',
498
+ bus_lane: 'bus',
499
+ bicycle_lane: 'biking',
500
+ walkway: 'sidewalk',
501
+ shared_walkway: 'sidewalk',
502
+ stairs: 'sidewalk',
503
+ crosswalk: 'walking',
504
+ exit: 'exit',
505
+ };
506
+ function odrLaneTypeFor(lane) {
507
+ const attrs = lane.props.attributes ?? {};
508
+ if (attrs.odr_type)
509
+ return attrs.odr_type;
510
+ return LANELET_SUBTYPE_TO_ODR_TYPE[attrs.subtype ?? ''] ?? 'driving';
511
+ }
512
+ /** Road mark type for a boundary linestring (dashed subtype -> broken). */
513
+ function roadMarkTypeFor(shapeMap, boundaryId) {
514
+ if (!boundaryId)
515
+ return 'solid';
516
+ const ls = shapeMap.get(boundaryId);
517
+ return ls?.props?.attributes?.subtype === 'dashed' ? 'broken' : 'solid';
518
+ }
519
+ function emitLanes(bundle, plan, shapeMap) {
520
+ const geom = bundle.geom;
165
521
  const lines = [];
166
522
  lines.push(` <lanes>`);
523
+ // The plan view follows the bundle's leftmost boundary, so lane 0 (center)
524
+ // lies on the left edge of lane -1 and no laneOffset is required. Lanes are
525
+ // emitted -1, -2, ... from the reference line outward (left→right in travel
526
+ // direction), each spanning its full drawn width.
167
527
  lines.push(` <laneSection s="0">`);
168
- // Left side: id=+1 (OpenDRIVE numbers lanes positively to the left of the reference line).
169
- lines.push(` <left>`);
170
- lines.push(` <lane id="1" type="driving" level="false">`);
171
- emitLaneLink(lines, hasPrev, hasNext, 1);
172
- emitHalfWidthEntries(geom, lines);
173
- lines.push(` <roadMark sOffset="0" type="solid" weight="standard" color="white" width="0.13"/>`);
174
- lines.push(` </lane>`);
175
- lines.push(` </left>`);
176
- // Center reference line.
177
528
  lines.push(` <center>`);
178
529
  lines.push(` <lane id="0" type="none" level="false">`);
179
530
  lines.push(` <link/>`);
180
- lines.push(` <roadMark sOffset="0" type="solid" weight="standard" color="standard" width="0.13"/>`);
531
+ const centerMark = roadMarkTypeFor(shapeMap, bundle.lanes[0].props.leftBoundaryId);
532
+ lines.push(` <roadMark sOffset="0" type="${centerMark}" weight="standard" color="white" width="0.13"/>`);
181
533
  lines.push(` </lane>`);
182
534
  lines.push(` </center>`);
183
- // Right side: id=-1.
184
535
  lines.push(` <right>`);
185
- lines.push(` <lane id="-1" type="driving" level="false">`);
186
- emitLaneLink(lines, hasPrev, hasNext, -1);
187
- emitHalfWidthEntries(geom, lines);
188
- lines.push(` <roadMark sOffset="0" type="solid" weight="standard" color="white" width="0.13"/>`);
189
- lines.push(` </lane>`);
536
+ bundle.lanes.forEach((lane, i) => {
537
+ const odrId = -(i + 1);
538
+ lines.push(` <lane id="${odrId}" type="${odrLaneTypeFor(lane)}" level="false">`);
539
+ emitLaneLink(lines, plan.lanePredecessor.get(lane.id), plan.laneSuccessor.get(lane.id));
540
+ emitWidthEntries(geom, i, lines);
541
+ const outerMark = roadMarkTypeFor(shapeMap, lane.props.rightBoundaryId);
542
+ lines.push(` <roadMark sOffset="0" type="${outerMark}" weight="standard" color="white" width="0.13"/>`);
543
+ lines.push(` </lane>`);
544
+ });
190
545
  lines.push(` </right>`);
191
546
  lines.push(` </laneSection>`);
192
547
  lines.push(` </lanes>`);
193
548
  return lines.join('\n');
194
549
  }
195
- function emitLaneLink(out, hasPrev, hasNext, laneId) {
196
- if (!hasPrev && !hasNext) {
550
+ function emitLaneLink(out, predId, succId) {
551
+ if (predId === undefined && succId === undefined) {
197
552
  out.push(` <link/>`);
198
553
  return;
199
554
  }
200
555
  out.push(` <link>`);
201
- if (hasPrev) {
202
- out.push(` <predecessor id="${laneId}"/>`);
556
+ if (predId !== undefined) {
557
+ out.push(` <predecessor id="${predId}"/>`);
203
558
  }
204
- if (hasNext) {
205
- out.push(` <successor id="${laneId}"/>`);
559
+ if (succId !== undefined) {
560
+ out.push(` <successor id="${succId}"/>`);
206
561
  }
207
562
  out.push(` </link>`);
208
563
  }
209
- function emitHalfWidthEntries(geom, out) {
210
- for (let i = 0; i < geom.odrSamples.length - 1; i++) {
211
- const s = geom.odrSamples[i].s;
212
- const halfA = geom.odrSamples[i].width / 2;
213
- const halfB = geom.odrSamples[i + 1].width / 2;
214
- const segLen = geom.odrSamples[i + 1].s - s;
215
- // Linear fit a + b*ds (c=d=0).
216
- const a = halfA;
217
- const b = segLen > 1e-9 ? (halfB - halfA) / segLen : 0;
218
- out.push(` <width sOffset="${fmt(s)}" a="${fmt(a)}" b="${fmt(b)}" c="0" d="0"/>`);
564
+ /**
565
+ * Maximum width error (m) tolerated when folding stations into one record.
566
+ * Width samples carry chordal noise of the same order (the boundary polyline
567
+ * is a chordal approximation of the original curve), so a 1 cm band mostly
568
+ * absorbs that noise while staying far below the 5 cm position tolerance.
569
+ */
570
+ const WIDTH_SIMPLIFY_TOL_M = 0.01;
571
+ /**
572
+ * Piecewise-linear full lane width records: a + b*ds (c=d=0). Station runs
573
+ * are simplified greedily: a record absorbs every following station whose
574
+ * widths stay within WIDTH_SIMPLIFY_TOL_M of the straight ramp between the
575
+ * record start and the run end, so constant-width lanes collapse to a single
576
+ * record and smoothly varying lanes to a few.
577
+ */
578
+ function emitWidthEntries(geom, laneIndex, out) {
579
+ const allWidths = geom.laneWidths[laneIndex];
580
+ const poses = geom.samplePoses;
581
+ // Strictly increasing stations (duplicates share one width sample).
582
+ const sArr = [];
583
+ const wArr = [];
584
+ for (let i = 0; i < poses.length; i++) {
585
+ if (sArr.length === 0 || poses[i].s > sArr[sArr.length - 1] + 1e-9) {
586
+ sArr.push(poses[i].s);
587
+ wArr.push(allWidths[i]);
588
+ }
589
+ }
590
+ const recs = [];
591
+ if (sArr.length < 2) {
592
+ recs.push({ s: 0, a: wArr[0] ?? 0, b: 0 });
593
+ }
594
+ let i0 = 0;
595
+ while (i0 < sArr.length - 1) {
596
+ let end = i0 + 1;
597
+ for (let j = i0 + 2; j < sArr.length; j++) {
598
+ const slope = (wArr[j] - wArr[i0]) / (sArr[j] - sArr[i0]);
599
+ let ok = true;
600
+ for (let k = i0 + 1; k < j; k++) {
601
+ if (Math.abs(wArr[i0] + slope * (sArr[k] - sArr[i0]) - wArr[k]) > WIDTH_SIMPLIFY_TOL_M) {
602
+ ok = false;
603
+ break;
604
+ }
605
+ }
606
+ if (!ok)
607
+ break;
608
+ end = j;
609
+ }
610
+ recs.push({ s: sArr[i0], a: wArr[i0], b: (wArr[end] - wArr[i0]) / (sArr[end] - sArr[i0]) });
611
+ i0 = end;
612
+ }
613
+ for (const r of recs) {
614
+ out.push(` <width sOffset="${fmt(r.s)}" a="${fmt(r.a)}" b="${fmtPrecise(r.b)}" c="0" d="0"/>`);
219
615
  }
220
616
  }
221
- function emitLink(lane, laneIdToRoadId) {
617
+ /** Length (m) of a synthesized junction connecting road. Kept below the
618
+ * importer's micro-section threshold so re-imports skip it and bridge the
619
+ * lane links across instead of materializing an extra sliver lane. Also kept
620
+ * below the 1 cm contact-point gap tolerance of ASAM quality checkers: the
621
+ * incoming lane end and the outgoing lane start coincide in the drawing, so
622
+ * the stub necessarily overlaps the outgoing road and its whole length shows
623
+ * up as a contact-point discontinuity to gap checks. */
624
+ const CONNECTING_ROAD_LENGTH_M = 0.005;
625
+ /**
626
+ * Contact widths below this (m) count as zero for lane linking: OpenDRIVE
627
+ * forbids predecessor/successor records on lanes that have zero width at the
628
+ * linked contact (zero-width / appearing-lane semantics). Welded taper lanes
629
+ * produce exact zeros; the epsilon also covers values that round to zero in
630
+ * the 6-decimal output.
631
+ */
632
+ const ZERO_WIDTH_LINK_EPS_M = 1e-3;
633
+ /** Wrap an angle to (-pi, pi]. */
634
+ function wrapAngleRad(a) {
635
+ while (a > Math.PI)
636
+ a -= 2 * Math.PI;
637
+ while (a < -Math.PI)
638
+ a += 2 * Math.PI;
639
+ return a;
640
+ }
641
+ /**
642
+ * Plan road links, lane links and synthesized <junction> elements.
643
+ *
644
+ * A road <link> can name only one predecessor and one successor, so a road
645
+ * pair (P → Q) is representable as a plain road link only when Q is P's only
646
+ * successor road AND P is Q's only predecessor road AND every lane edge
647
+ * between them is 1:1 (its source's only `next` and its target's only
648
+ * `prev`). Every other lane edge is routed through a synthesized junction
649
+ * with the standard structure: a short connecting road (junction-stamped,
650
+ * with a guaranteed road-level predecessor=incoming / successor=outgoing
651
+ * link) is synthesized at the contact point for each lane edge, and the
652
+ * junction's <connection incomingRoad connectingRoad contactPoint="start">
653
+ * carries the per-lane <laneLink>. The mainline roads stay junction="-1" and
654
+ * link to the junction by id. Edges that share a road collapse into the same
655
+ * junction (connected components), so a 2-in x 2-out diamond becomes one
656
+ * junction with four connections.
657
+ *
658
+ * Right-of-way lane pairs (`yieldLaneIds`) whose two lanes both feed
659
+ * connecting roads of the same junction are emitted as standard
660
+ * <priority high low> records between those connecting roads.
661
+ */
662
+ function planConnectivity(exportBundles, roadIdOf, odrIdOf, firstJunctionId, connectingSourceFor, connectingTargetFor, contactWidth, externalLanes = new Map()) {
663
+ const validNext = new Map();
664
+ const validPrev = new Map();
665
+ for (const bundle of exportBundles) {
666
+ for (const lane of bundle.lanes) {
667
+ validNext.set(lane.id, (lane.props.next ?? []).filter(id => roadIdOf.has(id)));
668
+ validPrev.set(lane.id, (lane.props.prev ?? []).filter(id => roadIdOf.has(id)));
669
+ }
670
+ }
671
+ // Carry-through: lanes of verbatim (unedited) roads participate as link
672
+ // endpoints — their roads are never re-emitted here, but regenerated roads
673
+ // must still link to / from them. Edges between two external lanes are
674
+ // covered by the verbatim XML and are skipped below.
675
+ for (const [id, lane] of externalLanes) {
676
+ validNext.set(id, (lane.props.next ?? []).filter(t => roadIdOf.has(t)));
677
+ validPrev.set(id, (lane.props.prev ?? []).filter(t => roadIdOf.has(t)));
678
+ }
679
+ // Lanes with (near) zero width at a linked contact must not carry standard
680
+ // link records there (zero-width / appearing-lane rules), so those edges
681
+ // are diverted into the hiddenLaneLinks userData stash. The stash lives on
682
+ // the `from` road when it is re-emitted in this export, else on the `to`
683
+ // road (one of the two always is: external-external edges stay verbatim).
684
+ const hiddenLaneEdges = [];
685
+ for (const [laneId, nexts] of validNext) {
686
+ if (nexts.length === 0)
687
+ continue;
688
+ const kept = [];
689
+ for (const to of nexts) {
690
+ if (externalLanes.has(laneId) && externalLanes.has(to)) {
691
+ kept.push(to);
692
+ continue;
693
+ }
694
+ const wFrom = contactWidth(laneId, 'end');
695
+ const wTo = contactWidth(to, 'start');
696
+ if ((wFrom !== null && wFrom < ZERO_WIDTH_LINK_EPS_M) ||
697
+ (wTo !== null && wTo < ZERO_WIDTH_LINK_EPS_M)) {
698
+ hiddenLaneEdges.push({ from: laneId, to, home: externalLanes.has(laneId) ? to : laneId });
699
+ const prevs = validPrev.get(to);
700
+ if (prevs)
701
+ validPrev.set(to, prevs.filter(p => p !== laneId));
702
+ continue;
703
+ }
704
+ kept.push(to);
705
+ }
706
+ if (kept.length !== nexts.length)
707
+ validNext.set(laneId, kept);
708
+ }
709
+ const succRoads = new Map();
710
+ const predRoads = new Map();
711
+ const edgesByPair = new Map();
712
+ for (const [laneId, nexts] of validNext) {
713
+ const fromRoad = roadIdOf.get(laneId);
714
+ for (const to of nexts) {
715
+ if (externalLanes.has(laneId) && externalLanes.has(to))
716
+ continue;
717
+ const toRoad = roadIdOf.get(to);
718
+ succRoads.set(fromRoad, (succRoads.get(fromRoad) ?? new Set()).add(toRoad));
719
+ predRoads.set(toRoad, (predRoads.get(toRoad) ?? new Set()).add(fromRoad));
720
+ const key = `${fromRoad}->${toRoad}`;
721
+ edgesByPair.set(key, [...(edgesByPair.get(key) ?? []), { from: laneId, to }]);
722
+ }
723
+ }
724
+ const plan = {
725
+ roadPredecessor: new Map(),
726
+ roadSuccessor: new Map(),
727
+ lanePredecessor: new Map(),
728
+ laneSuccessor: new Map(),
729
+ junctions: [],
730
+ connectingRoads: [],
731
+ handledYieldPairs: new Set(),
732
+ hiddenLaneEdges,
733
+ };
734
+ const junctionPairs = [];
735
+ for (const [key, laneEdges] of edgesByPair) {
736
+ const [fromRoad, toRoad] = key.split('->').map(Number);
737
+ const uniquePair = succRoads.get(fromRoad).size === 1 && predRoads.get(toRoad).size === 1;
738
+ const lanesOneToOne = laneEdges.every(e => (validNext.get(e.from) ?? []).length === 1 && (validPrev.get(e.to) ?? []).length === 1);
739
+ if (uniquePair && lanesOneToOne) {
740
+ plan.roadSuccessor.set(fromRoad, { kind: 'road', id: toRoad });
741
+ plan.roadPredecessor.set(toRoad, { kind: 'road', id: fromRoad });
742
+ for (const e of laneEdges) {
743
+ plan.laneSuccessor.set(e.from, odrIdOf.get(e.to));
744
+ plan.lanePredecessor.set(e.to, odrIdOf.get(e.from));
745
+ }
746
+ }
747
+ else {
748
+ junctionPairs.push({ incoming: fromRoad, outgoing: toRoad, laneEdges });
749
+ }
750
+ }
751
+ // Union-find over road ids: junction-routed pairs sharing a road merge into
752
+ // one junction.
753
+ const parent = new Map();
754
+ const find = (x) => {
755
+ let root = x;
756
+ while (true) {
757
+ const p = parent.get(root);
758
+ if (p === undefined || p === root)
759
+ break;
760
+ root = p;
761
+ }
762
+ let cur = x;
763
+ while (cur !== root) {
764
+ const p = parent.get(cur);
765
+ parent.set(cur, root);
766
+ cur = p;
767
+ }
768
+ return root;
769
+ };
770
+ const union = (a, b) => {
771
+ if (!parent.has(a))
772
+ parent.set(a, a);
773
+ if (!parent.has(b))
774
+ parent.set(b, b);
775
+ const ra = find(a);
776
+ const rb = find(b);
777
+ if (ra !== rb)
778
+ parent.set(rb, ra);
779
+ };
780
+ for (const pair of junctionPairs)
781
+ union(pair.incoming, pair.outgoing);
782
+ // Pass 1: one junction per connected component (ids first, so junction ids
783
+ // and connecting road ids stay sequential and collision-free).
784
+ const junctionByRoot = new Map();
785
+ let nextId = firstJunctionId;
786
+ for (const pair of junctionPairs) {
787
+ const root = find(pair.incoming);
788
+ if (!junctionByRoot.has(root)) {
789
+ const junction = { id: nextId++, connections: [], priorities: [] };
790
+ junctionByRoot.set(root, junction);
791
+ plan.junctions.push(junction);
792
+ }
793
+ }
794
+ // Pass 2: synthesize one short connecting road per junction-routed lane
795
+ // edge and register it as a <connection> of its junction.
796
+ const connectingByLane = new Map();
797
+ for (const pair of junctionPairs) {
798
+ const junction = junctionByRoot.get(find(pair.incoming));
799
+ for (const e of pair.laneEdges.slice().sort((a, b) => odrIdOf.get(b.from) - odrIdOf.get(a.from) || odrIdOf.get(b.to) - odrIdOf.get(a.to))) {
800
+ const source = connectingSourceFor(e.from);
801
+ if (!source)
802
+ continue;
803
+ const spec = {
804
+ roadId: nextId++,
805
+ junctionId: junction.id,
806
+ incomingRoadId: pair.incoming,
807
+ outgoingRoadId: pair.outgoing,
808
+ fromOdrLaneId: odrIdOf.get(e.from),
809
+ toOdrLaneId: odrIdOf.get(e.to),
810
+ source,
811
+ target: connectingTargetFor(e.to),
812
+ };
813
+ plan.connectingRoads.push(spec);
814
+ junction.connections.push({
815
+ incoming: pair.incoming,
816
+ connecting: spec.roadId,
817
+ laneLinks: [{ from: spec.fromOdrLaneId, to: -1 }],
818
+ });
819
+ const list = connectingByLane.get(e.from) ?? [];
820
+ list.push(spec);
821
+ connectingByLane.set(e.from, list);
822
+ }
823
+ plan.roadSuccessor.set(pair.incoming, { kind: 'junction', id: junction.id });
824
+ plan.roadPredecessor.set(pair.outgoing, { kind: 'junction', id: junction.id });
825
+ }
826
+ // Right-of-way: a lane pair (X has priority, Y yields) whose maneuvers both
827
+ // run through connecting roads of one junction becomes <priority high low>
828
+ // records between those connecting roads.
829
+ const junctionById = new Map(plan.junctions.map(j => [j.id, j]));
830
+ for (const bundle of exportBundles) {
831
+ for (const lane of bundle.lanes) {
832
+ const highSpecs = connectingByLane.get(lane.id);
833
+ if (!highSpecs?.length)
834
+ continue;
835
+ for (const yieldShapeId of lane.props.yieldLaneIds ?? []) {
836
+ const lowSpecs = connectingByLane.get(yieldShapeId);
837
+ if (!lowSpecs?.length)
838
+ continue;
839
+ let expressed = false;
840
+ for (const hi of highSpecs) {
841
+ for (const lo of lowSpecs) {
842
+ if (hi.junctionId !== lo.junctionId)
843
+ continue;
844
+ const junction = junctionById.get(hi.junctionId);
845
+ if (!junction.priorities.some(p => p.high === hi.roadId && p.low === lo.roadId)) {
846
+ junction.priorities.push({ high: hi.roadId, low: lo.roadId });
847
+ }
848
+ expressed = true;
849
+ }
850
+ }
851
+ if (expressed)
852
+ plan.handledYieldPairs.add(`${lane.id}|${yieldShapeId}`);
853
+ }
854
+ }
855
+ }
856
+ return plan;
857
+ }
858
+ /**
859
+ * Emit a synthesized junction connecting road: a single short segment
860
+ * starting at the incoming lane's inner-boundary endpoint, heading along the
861
+ * incoming road's end direction, carrying one right lane as wide as the
862
+ * source lane. When the outgoing lane starts with a different heading or
863
+ * width (drawn branch points may kink), the stub blends onto them — an <arc>
864
+ * sweeping the heading difference and a linear width ramp — so the borders
865
+ * meet both neighbours without a lateral step. The road always links
866
+ * predecessor=incoming(road, end) and successor=outgoing(road, start), so
867
+ * standard consumers can traverse incoming -> connecting -> outgoing without
868
+ * dead ends.
869
+ */
870
+ function emitConnectingRoad(spec) {
871
+ const { x, y, hdg, width } = spec.source;
872
+ const dHdg = spec.target ? wrapAngleRad(spec.target.hdg - hdg) : 0;
873
+ // Target border point in the source frame: when the outgoing lane's
874
+ // emitted start sits measurably ahead of the source corner (drawn branch
875
+ // points stagger by centimeters), a cubic Hermite interpolates both end
876
+ // poses exactly; otherwise a minimum-length arc (or line) blends the
877
+ // heading in place.
878
+ let geometry = '';
879
+ let len = CONNECTING_ROAD_LENGTH_M;
880
+ if (spec.target) {
881
+ const cosH = Math.cos(hdg);
882
+ const sinH = Math.sin(hdg);
883
+ const ex = spec.target.x - x;
884
+ const ey = spec.target.y - y;
885
+ const u1 = ex * cosH + ey * sinH;
886
+ const v1 = -ex * sinH + ey * cosH;
887
+ const dist = Math.hypot(ex, ey);
888
+ // A target at or behind the source corner (the outgoing road's emitted
889
+ // start can sit a few millimeters behind the drawn weld) is unreachable
890
+ // by a forward curve; an in-place blend as short as representable keeps
891
+ // the leftover contact offset at the stagger itself.
892
+ if (u1 < CONNECTING_ROAD_LENGTH_M)
893
+ len = 0.001;
894
+ if (dist >= CONNECTING_ROAD_LENGTH_M && u1 >= 0.7 * dist && Math.abs(dHdg) <= 1.45) {
895
+ // Hermite with parameter domain [0, L]: u(0)=0,u'(0)=1,v(0)=0,v'(0)=0,
896
+ // u(L)=u1, u'(L)=cosθ, v(L)=v1, v'(L)=sinθ (same construction as the
897
+ // plan-view fitter, emitted as paramPoly3 pRange="arcLength").
898
+ let L = Math.max(dist, u1);
899
+ let cU = 0;
900
+ let dU = 0;
901
+ let cV = 0;
902
+ let dV = 0;
903
+ const cosT = Math.cos(dHdg);
904
+ const sinT = Math.sin(dHdg);
905
+ const solve = (dom) => {
906
+ const A = u1 - dom;
907
+ const B = cosT - 1;
908
+ cU = (3 * A - B * dom) / (dom * dom);
909
+ dU = (B * dom - 2 * A) / (dom * dom * dom);
910
+ cV = (3 * v1 - sinT * dom) / (dom * dom);
911
+ dV = (sinT * dom - 2 * v1) / (dom * dom * dom);
912
+ };
913
+ const arcLength = (dom) => {
914
+ const n = 32;
915
+ let acc = 0;
916
+ let px = 0;
917
+ let py = 0;
918
+ for (let k = 1; k <= n; k++) {
919
+ const p = (dom * k) / n;
920
+ const lu = p * (1 + p * (cU + p * dU));
921
+ const lv = p * p * (cV + p * dV);
922
+ acc += Math.hypot(lu - px, lv - py);
923
+ px = lu;
924
+ py = lv;
925
+ }
926
+ return acc;
927
+ };
928
+ for (let iter = 0; iter < 3; iter++) {
929
+ solve(L);
930
+ const actual = arcLength(L);
931
+ if (!(actual > 1e-6))
932
+ break;
933
+ if (Math.abs(actual - L) < 1e-6)
934
+ break;
935
+ L = actual;
936
+ }
937
+ solve(L);
938
+ // Stay below the importer's micro-section threshold (0.3 m) so the
939
+ // stub keeps being bridged on re-import instead of materializing as a
940
+ // sliver lane; larger staggers fall back to the in-place blend.
941
+ if (L > 1e-6 && L <= 0.25) {
942
+ len = L;
943
+ geometry = ` <paramPoly3 aU="0" bU="1" cU="${fmtPrecise(cU)}" dU="${fmtPrecise(dU)}" aV="0" bV="0" cV="${fmtPrecise(cV)}" dV="${fmtPrecise(dV)}" pRange="arcLength"/>`;
944
+ }
945
+ }
946
+ }
947
+ if (!geometry) {
948
+ geometry =
949
+ Math.abs(dHdg) > 1e-4
950
+ ? ` <arc curvature="${fmtPrecise(dHdg / len)}"/>`
951
+ : ` <line/>`;
952
+ }
953
+ const widthSlope = spec.target !== null && Math.abs(spec.target.width - width) > 1e-6
954
+ ? (spec.target.width - width) / len
955
+ : 0;
222
956
  const lines = [];
957
+ lines.push(` <road name="connecting" length="${fmt(len)}" id="${spec.roadId}" junction="${spec.junctionId}">`);
223
958
  lines.push(` <link>`);
224
- if (lane.props.prev?.[0] && laneIdToRoadId.has(lane.props.prev[0])) {
225
- const rid = laneIdToRoadId.get(lane.props.prev[0]);
226
- lines.push(` <predecessor elementType="road" elementId="${rid}" contactPoint="end"/>`);
959
+ lines.push(` <predecessor elementType="road" elementId="${spec.incomingRoadId}" contactPoint="end"/>`);
960
+ lines.push(` <successor elementType="road" elementId="${spec.outgoingRoadId}" contactPoint="start"/>`);
961
+ lines.push(` </link>`);
962
+ lines.push(` <planView>`);
963
+ lines.push(` <geometry s="0" x="${fmt(x)}" y="${fmt(y)}" hdg="${fmt(hdg)}" length="${fmt(len)}">`);
964
+ lines.push(geometry);
965
+ lines.push(` </geometry>`);
966
+ lines.push(` </planView>`);
967
+ lines.push(` <elevationProfile/>`);
968
+ lines.push(` <lateralProfile/>`);
969
+ lines.push(` <lanes>`);
970
+ lines.push(` <laneSection s="0">`);
971
+ lines.push(` <center>`);
972
+ lines.push(` <lane id="0" type="none" level="false">`);
973
+ lines.push(` <link/>`);
974
+ lines.push(` <roadMark sOffset="0" type="none" weight="standard" color="white" width="0.13"/>`);
975
+ lines.push(` </lane>`);
976
+ lines.push(` </center>`);
977
+ lines.push(` <right>`);
978
+ lines.push(` <lane id="-1" type="${spec.source.laneType}" level="false">`);
979
+ lines.push(` <link>`);
980
+ lines.push(` <predecessor id="${spec.fromOdrLaneId}"/>`);
981
+ lines.push(` <successor id="${spec.toOdrLaneId}"/>`);
982
+ lines.push(` </link>`);
983
+ lines.push(` <width sOffset="0" a="${fmt(width)}" b="${widthSlope === 0 ? '0' : fmtPrecise(widthSlope)}" c="0" d="0"/>`);
984
+ lines.push(` <roadMark sOffset="0" type="none" weight="standard" color="white" width="0.13"/>`);
985
+ lines.push(` </lane>`);
986
+ lines.push(` </right>`);
987
+ lines.push(` </laneSection>`);
988
+ lines.push(` </lanes>`);
989
+ lines.push(` <objects/>`);
990
+ lines.push(` <signals/>`);
991
+ lines.push(` </road>`);
992
+ return lines.join('\n');
993
+ }
994
+ function emitLink(roadId, plan) {
995
+ const lines = [];
996
+ lines.push(` <link>`);
997
+ const pred = plan.roadPredecessor.get(roadId);
998
+ if (pred) {
999
+ lines.push(pred.kind === 'junction'
1000
+ ? ` <predecessor elementType="junction" elementId="${pred.id}"/>`
1001
+ : ` <predecessor elementType="road" elementId="${pred.id}" contactPoint="end"/>`);
227
1002
  }
228
- if (lane.props.next?.[0] && laneIdToRoadId.has(lane.props.next[0])) {
229
- const rid = laneIdToRoadId.get(lane.props.next[0]);
230
- lines.push(` <successor elementType="road" elementId="${rid}" contactPoint="start"/>`);
1003
+ const succ = plan.roadSuccessor.get(roadId);
1004
+ if (succ) {
1005
+ lines.push(succ.kind === 'junction'
1006
+ ? ` <successor elementType="junction" elementId="${succ.id}"/>`
1007
+ : ` <successor elementType="road" elementId="${succ.id}" contactPoint="start"/>`);
231
1008
  }
232
1009
  lines.push(` </link>`);
233
1010
  return lines.join('\n');
@@ -238,7 +1015,9 @@ function projectToRoad(geom, xG, yG) {
238
1015
  let bestDist = Infinity;
239
1016
  let bestHdg = 0;
240
1017
  let bestClamped = false;
241
- const samples = geom.odrSamples;
1018
+ // The fitted sample poses lie on the analytic reference line, so chord
1019
+ // projection between them yields stations directly in the emitted s domain.
1020
+ const samples = geom.samplePoses;
242
1021
  for (let i = 0; i < samples.length - 1; i++) {
243
1022
  const a = samples[i];
244
1023
  const b = samples[i + 1];
@@ -266,7 +1045,7 @@ function projectToRoad(geom, xG, yG) {
266
1045
  const dist = Math.hypot(xG - projX, yG - projY);
267
1046
  if (dist < bestDist) {
268
1047
  bestDist = dist;
269
- bestS = a.s + tNorm;
1048
+ bestS = a.s + (tNorm / segLen) * (b.s - a.s);
270
1049
  const nx = -uy;
271
1050
  const ny = ux;
272
1051
  bestT = px * nx + py * ny;
@@ -276,27 +1055,81 @@ function projectToRoad(geom, xG, yG) {
276
1055
  }
277
1056
  return { s: bestS, t: bestT, hdg: bestHdg, distance: bestDist, clampedAtEnd: bestClamped };
278
1057
  }
279
- function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, laneIdToRoadId, maxAttachDistanceMeter = 50) {
1058
+ /** Contiguous <validity> ranges from a set of ODR lane ids. */
1059
+ function laneIdRanges(ids) {
1060
+ const sorted = [...new Set(ids)].sort((a, b) => a - b);
1061
+ const ranges = [];
1062
+ if (sorted.length === 0)
1063
+ return ranges;
1064
+ let start = sorted[0];
1065
+ let prev = sorted[0];
1066
+ for (let i = 1; i < sorted.length; i++) {
1067
+ const v = sorted[i];
1068
+ if (v === prev + 1) {
1069
+ prev = v;
1070
+ continue;
1071
+ }
1072
+ ranges.push({ fromLane: start, toLane: prev });
1073
+ start = v;
1074
+ prev = v;
1075
+ }
1076
+ ranges.push({ fromLane: start, toLane: prev });
1077
+ return ranges;
1078
+ }
1079
+ function attachShapesToRoads(shapeMap, trafficLights, crosswalks, polygons, roads, laneIdToRoadId, laneIdToOdrLaneId, maxAttachDistanceMeter = 50, signalIdStart = 1) {
280
1080
  const roadSignals = new Map();
281
1081
  const roadObjects = new Map();
282
- let signalIdCounter = 1;
1082
+ const roadSignalRefs = new Map();
1083
+ const signalIdByShape = new Map();
1084
+ let signalIdCounter = signalIdStart;
283
1085
  let objectIdCounter = 1;
284
- const roads = [];
285
- laneToGeom.forEach((geom, laneId) => {
286
- const roadId = laneIdToRoadId.get(laneId);
287
- if (roadId !== undefined)
288
- roads.push({ laneId, geom, roadId });
289
- });
1086
+ const geomByRoadId = new Map();
1087
+ for (const r of roads)
1088
+ geomByRoadId.set(r.roadId, r.geom);
1089
+ /** Affected lanes grouped per road: road id -> ODR lane ids. */
1090
+ const affectedLanesByRoad = (laneShapeIds) => {
1091
+ const byRoad = new Map();
1092
+ for (const laneShapeId of laneShapeIds ?? []) {
1093
+ const rid = laneIdToRoadId.get(laneShapeId);
1094
+ const oid = laneIdToOdrLaneId.get(laneShapeId);
1095
+ if (rid === undefined || oid === undefined)
1096
+ continue;
1097
+ const list = byRoad.get(rid) ?? [];
1098
+ if (!list.includes(oid))
1099
+ list.push(oid);
1100
+ byRoad.set(rid, list);
1101
+ }
1102
+ return byRoad;
1103
+ };
290
1104
  for (const tl of trafficLights) {
291
1105
  const xG = pxToEnuX(tl.x);
292
1106
  const yG = pxToEnuY(tl.y);
1107
+ // Regulatory layer: a signal that names affected lanes attaches to the
1108
+ // nearest of those lanes' roads and carries <validity> records for the
1109
+ // affected lane range. Distance gating does not apply — the assignment is
1110
+ // explicit.
1111
+ const affectedByRoad = affectedLanesByRoad(tl.props.affectedLaneIds);
293
1112
  let best = null;
294
- for (const r of roads) {
295
- const proj = projectToRoad(r.geom, xG, yG);
296
- if (!best || proj.distance < best.proj.distance)
297
- best = { roadId: r.roadId, proj };
1113
+ if (affectedByRoad.size > 0) {
1114
+ for (const r of roads) {
1115
+ if (!affectedByRoad.has(r.roadId))
1116
+ continue;
1117
+ const proj = projectToRoad(r.geom, xG, yG);
1118
+ if (!best || proj.distance < best.proj.distance)
1119
+ best = { roadId: r.roadId, proj };
1120
+ }
298
1121
  }
299
- if (!best || best.proj.distance > maxAttachDistanceMeter)
1122
+ else {
1123
+ // Fall back to the nearest road within the attachment distance.
1124
+ for (const r of roads) {
1125
+ const proj = projectToRoad(r.geom, xG, yG);
1126
+ if (!best || proj.distance < best.proj.distance)
1127
+ best = { roadId: r.roadId, proj };
1128
+ }
1129
+ if (best && best.proj.distance > maxAttachDistanceMeter)
1130
+ best = null;
1131
+ }
1132
+ if (!best)
300
1133
  continue;
301
1134
  const style = tl.props.style ?? '';
302
1135
  const isPed = style.startsWith('pedestrian') || style.includes('ped');
@@ -305,7 +1138,7 @@ function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, la
305
1138
  const heightM = pxToMeter(tl.props.h);
306
1139
  const widthM = pxToMeter(tl.props.w);
307
1140
  const list = roadSignals.get(best.roadId) ?? [];
308
- list.push({
1141
+ const entry = {
309
1142
  id: signalIdCounter++,
310
1143
  s: best.proj.s,
311
1144
  t: best.proj.t,
@@ -317,8 +1150,69 @@ function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, la
317
1150
  subtype: '-1',
318
1151
  dynamic: 'yes',
319
1152
  orientation: best.proj.t >= 0 ? '+' : '-',
320
- });
1153
+ };
1154
+ if (affectedByRoad.size > 0) {
1155
+ entry.validity = laneIdRanges(affectedByRoad.get(best.roadId));
1156
+ }
1157
+ list.push(entry);
321
1158
  roadSignals.set(best.roadId, list);
1159
+ signalIdByShape.set(tl.id, entry.id);
1160
+ // A signal controlling lanes in several road bundles cannot carry a
1161
+ // single <validity> (it cannot cross roads); the remaining affected roads
1162
+ // get a standard <signalReference> pointing back at the signal with their
1163
+ // own lane ranges, so the validity links survive a round trip.
1164
+ if (affectedByRoad.size > 1) {
1165
+ for (const r of roads) {
1166
+ if (r.roadId === best.roadId || !affectedByRoad.has(r.roadId))
1167
+ continue;
1168
+ const proj = projectToRoad(r.geom, xG, yG);
1169
+ const refs = roadSignalRefs.get(r.roadId) ?? [];
1170
+ refs.push({
1171
+ id: entry.id,
1172
+ s: proj.s,
1173
+ t: proj.t,
1174
+ orientation: proj.t >= 0 ? '+' : '-',
1175
+ validity: laneIdRanges(affectedByRoad.get(r.roadId)),
1176
+ });
1177
+ roadSignalRefs.set(r.roadId, refs);
1178
+ }
1179
+ }
1180
+ // Stop line: emitted on the signal's road as a conventional
1181
+ // <object name="StopLine"> at the projected station of the line's midpoint.
1182
+ if (tl.props.stopLineId) {
1183
+ const stopLs = shapeMap.get(tl.props.stopLineId);
1184
+ const pts = stopLs
1185
+ ? collectPoints(shapeMap, stopLs.props.pointIds, false, new Map())
1186
+ : [];
1187
+ if (pts.length >= 2) {
1188
+ // Carry the full polyline (ENU meters) on the signal so an importer
1189
+ // can rebuild the stop-line linestring and re-link it to the signal.
1190
+ entry.stopLinePoints = pts.map((p) => ({ x: pxToEnuX(p.x), y: pxToEnuY(p.y) }));
1191
+ const a = pts[0];
1192
+ const b = pts[pts.length - 1];
1193
+ const midX = pxToEnuX((a.x + b.x) / 2);
1194
+ const midY = pxToEnuY((a.y + b.y) / 2);
1195
+ const geom = geomByRoadId.get(best.roadId);
1196
+ const proj = projectToRoad(geom, midX, midY);
1197
+ const objList = roadObjects.get(best.roadId) ?? [];
1198
+ objList.push({
1199
+ id: objectIdCounter++,
1200
+ s: proj.s,
1201
+ t: proj.t,
1202
+ zOffset: 0,
1203
+ // Stop lines lie across the road (like crosswalks): local hdg = π/2,
1204
+ // length spanning the painted line, conventional 0.3 m paint width.
1205
+ hdg: Math.PI / 2,
1206
+ length: pxToMeter(Math.hypot(b.x - a.x, b.y - a.y)),
1207
+ width: 0.3,
1208
+ height: 0,
1209
+ name: 'StopLine',
1210
+ type: 'none',
1211
+ orientation: 'none',
1212
+ });
1213
+ roadObjects.set(best.roadId, objList);
1214
+ }
1215
+ }
322
1216
  }
323
1217
  for (const cw of crosswalks) {
324
1218
  const rotDeg = cw.rotation || 0;
@@ -331,13 +1225,30 @@ function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, la
331
1225
  const cyGlobal = cw.y + cyLocal;
332
1226
  const xG = pxToEnuX(cxGlobal);
333
1227
  const yG = pxToEnuY(cyGlobal);
1228
+ // Regulatory layer: a crosswalk that names affected lanes attaches to the
1229
+ // nearest of those lanes' roads (no distance gate — the assignment is
1230
+ // explicit), mirroring the signal behavior above.
1231
+ const affectedByRoad = affectedLanesByRoad(cw.props.affectedLaneIds);
334
1232
  let best = null;
335
- for (const r of roads) {
336
- const proj = projectToRoad(r.geom, xG, yG);
337
- if (!best || proj.distance < best.proj.distance)
338
- best = { roadId: r.roadId, proj };
1233
+ if (affectedByRoad.size > 0) {
1234
+ for (const r of roads) {
1235
+ if (!affectedByRoad.has(r.roadId))
1236
+ continue;
1237
+ const proj = projectToRoad(r.geom, xG, yG);
1238
+ if (!best || proj.distance < best.proj.distance)
1239
+ best = { roadId: r.roadId, proj };
1240
+ }
339
1241
  }
340
- if (!best || best.proj.distance > maxAttachDistanceMeter)
1242
+ else {
1243
+ for (const r of roads) {
1244
+ const proj = projectToRoad(r.geom, xG, yG);
1245
+ if (!best || proj.distance < best.proj.distance)
1246
+ best = { roadId: r.roadId, proj };
1247
+ }
1248
+ if (best && best.proj.distance > maxAttachDistanceMeter)
1249
+ best = null;
1250
+ }
1251
+ if (!best)
341
1252
  continue;
342
1253
  const dxLocal = cw.props.endX - cw.props.startX;
343
1254
  const dyLocal = cw.props.endY - cw.props.startY;
@@ -359,6 +1270,29 @@ function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, la
359
1270
  // π/2 regardless of the user-drawn axis direction.
360
1271
  void relativeHdg;
361
1272
  const crosswalkHdg = Math.PI / 2;
1273
+ // Regulatory links (affected lanes + stop line polyline) ride along as
1274
+ // <userData> — OpenDRIVE's standard extension mechanism — so they survive
1275
+ // an .xodr round trip. Coordinates are ENU meters.
1276
+ const userData = [];
1277
+ if (affectedByRoad.size > 0) {
1278
+ const affectedLanes = [];
1279
+ for (const [rid, ids] of [...affectedByRoad.entries()].sort((a, b) => a[0] - b[0])) {
1280
+ for (const oid of [...ids].sort((a, b) => a - b)) {
1281
+ affectedLanes.push([String(rid), String(oid)]);
1282
+ }
1283
+ }
1284
+ const links = {
1285
+ affectedLanes,
1286
+ };
1287
+ if (cw.props.stopLineId) {
1288
+ const stopLs = shapeMap.get(cw.props.stopLineId);
1289
+ const pts = stopLs ? collectPoints(shapeMap, stopLs.props.pointIds, false, new Map()) : [];
1290
+ if (pts.length >= 2) {
1291
+ links.stopLine = pts.map((p) => [roundMm(pxToEnuX(p.x)), roundMm(pxToEnuY(p.y))]);
1292
+ }
1293
+ }
1294
+ userData.push({ code: 'crosswalkLinks', value: JSON.stringify(links) });
1295
+ }
362
1296
  list.push({
363
1297
  id: objectIdCounter++,
364
1298
  s: best.proj.s,
@@ -371,6 +1305,7 @@ function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, la
371
1305
  name: 'crosswalk',
372
1306
  type: 'crosswalk',
373
1307
  orientation: 'none',
1308
+ userData: userData.length ? userData : undefined,
374
1309
  });
375
1310
  roadObjects.set(best.roadId, list);
376
1311
  }
@@ -395,12 +1330,13 @@ function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, la
395
1330
  for (const r of roads) {
396
1331
  const proj = projectToRoad(r.geom, xG, yG);
397
1332
  if (proj.clampedAtEnd) {
398
- if (!fallback || proj.distance < fallback.proj.distance)
399
- fallback = { roadId: r.roadId, proj };
1333
+ if (!fallback || proj.distance < fallback.proj.distance) {
1334
+ fallback = { roadId: r.roadId, geom: r.geom, proj };
1335
+ }
400
1336
  continue;
401
1337
  }
402
1338
  if (!best || proj.distance < best.proj.distance)
403
- best = { roadId: r.roadId, proj };
1339
+ best = { roadId: r.roadId, geom: r.geom, proj };
404
1340
  }
405
1341
  if (!best)
406
1342
  best = fallback;
@@ -408,7 +1344,7 @@ function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, la
408
1344
  continue;
409
1345
  const cosH = Math.cos(best.proj.hdg);
410
1346
  const sinH = Math.sin(best.proj.hdg);
411
- const samples = laneToGeom.get([...laneToGeom.keys()].find((k) => laneIdToRoadId.get(k) === best.roadId)).odrSamples;
1347
+ const samples = best.geom.samplePoses;
412
1348
  const anchorPoint = best.proj.clampedAtEnd && best.proj.s >= samples[samples.length - 1].s - 1e-6
413
1349
  ? samples[samples.length - 1]
414
1350
  : samples[0];
@@ -445,19 +1381,44 @@ function attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, la
445
1381
  });
446
1382
  roadObjects.set(best.roadId, list);
447
1383
  }
448
- return { roadSignals, roadObjects };
1384
+ return { roadSignals, roadObjects, roadSignalRefs, signalIdByShape };
449
1385
  }
450
- function emitSignals(signals) {
451
- if (!signals.length)
1386
+ function emitSignals(signals, references) {
1387
+ if (!signals.length && !references.length)
452
1388
  return ` <signals/>`;
453
1389
  const lines = [];
454
1390
  lines.push(` <signals>`);
455
1391
  for (const s of signals) {
456
- lines.push(` <signal id="${s.id}" s="${fmt(s.s)}" t="${fmt(s.t)}" zOffset="${fmt(s.zOffset)}" name="${escapeXml(s.name)}" dynamic="${s.dynamic}" orientation="${s.orientation}" type="${s.type}" subtype="${s.subtype}" country="OpenDRIVE" value="0" height="${fmt(s.height)}" width="${fmt(s.width)}"/>`);
1392
+ const attrs = `id="${s.id}" s="${fmt(s.s)}" t="${fmt(s.t)}" zOffset="${fmt(s.zOffset)}" name="${escapeXml(s.name)}" dynamic="${s.dynamic}" orientation="${s.orientation}" type="${s.type}" subtype="${s.subtype}" country="OpenDRIVE" value="0" height="${fmt(s.height)}" width="${fmt(s.width)}"`;
1393
+ if (s.validity?.length || s.stopLinePoints) {
1394
+ lines.push(` <signal ${attrs}>`);
1395
+ for (const v of s.validity ?? []) {
1396
+ lines.push(` <validity fromLane="${v.fromLane}" toLane="${v.toLane}"/>`);
1397
+ }
1398
+ if (s.stopLinePoints) {
1399
+ const json = JSON.stringify(s.stopLinePoints.map((p) => [roundMm(p.x), roundMm(p.y)]));
1400
+ lines.push(` <userData code="stopLine" value="${escapeXml(json)}"/>`);
1401
+ }
1402
+ lines.push(` </signal>`);
1403
+ }
1404
+ else {
1405
+ lines.push(` <signal ${attrs}/>`);
1406
+ }
1407
+ }
1408
+ for (const ref of references) {
1409
+ lines.push(` <signalReference s="${fmt(ref.s)}" t="${fmt(ref.t)}" id="${ref.id}" orientation="${ref.orientation}">`);
1410
+ for (const v of ref.validity) {
1411
+ lines.push(` <validity fromLane="${v.fromLane}" toLane="${v.toLane}"/>`);
1412
+ }
1413
+ lines.push(` </signalReference>`);
457
1414
  }
458
1415
  lines.push(` </signals>`);
459
1416
  return lines.join('\n');
460
1417
  }
1418
+ /** Round to millimeter precision for compact embedded JSON. */
1419
+ function roundMm(v) {
1420
+ return Math.round(v * 1000) / 1000;
1421
+ }
461
1422
  function emitObjects(objects) {
462
1423
  if (!objects.length)
463
1424
  return ` <objects/>`;
@@ -478,38 +1439,435 @@ function emitObjects(objects) {
478
1439
  lines.push(` </object>`);
479
1440
  }
480
1441
  else {
481
- lines.push(` <object id="${o.id}" s="${fmt(o.s)}" t="${fmt(o.t)}" zOffset="${fmt(o.zOffset)}" hdg="${fmt(o.hdg)}" name="${escapeXml(o.name)}" type="${o.type}" orientation="${o.orientation}" length="${fmt(o.length)}" width="${fmt(o.width)}" height="${fmt(o.height)}"/>`);
1442
+ const attrs = `id="${o.id}" s="${fmt(o.s)}" t="${fmt(o.t)}" zOffset="${fmt(o.zOffset)}" hdg="${fmt(o.hdg)}" name="${escapeXml(o.name)}" type="${o.type}" orientation="${o.orientation}" length="${fmt(o.length)}" width="${fmt(o.width)}" height="${fmt(o.height)}"`;
1443
+ if (o.userData?.length) {
1444
+ lines.push(` <object ${attrs}>`);
1445
+ for (const ud of o.userData) {
1446
+ lines.push(` <userData code="${escapeXml(ud.code)}" value="${escapeXml(ud.value)}"/>`);
1447
+ }
1448
+ lines.push(` </object>`);
1449
+ }
1450
+ else {
1451
+ lines.push(` <object ${attrs}/>`);
1452
+ }
482
1453
  }
483
1454
  }
484
1455
  lines.push(` </objects>`);
485
1456
  return lines.join('\n');
486
1457
  }
487
- function emitRoad(lane, geom, roadId, laneIdToRoadId, signals, objects) {
488
- const speed = lane.props.attributes?.speed_limit;
489
- const name = escapeXml(lane.props.attributes?.subtype || 'road');
490
- const hasPrev = !!(lane.props.prev?.[0] && laneIdToRoadId.has(lane.props.prev[0]));
491
- const hasNext = !!(lane.props.next?.[0] && laneIdToRoadId.has(lane.props.next[0]));
1458
+ /**
1459
+ * Lane attributes that have no OpenDRIVE representation (speed_limit only
1460
+ * partially maps, one_way / turn_direction / location and custom tags not at
1461
+ * all) are stashed as JSON in <userData code="laneAttributes"> OpenDRIVE's
1462
+ * standard extension mechanism — keyed by the lane's ODR id so per-lane
1463
+ * attributes stay separate in multi-lane roads, and restored by the importer.
1464
+ * `odr_*` meta attributes are excluded: they are regenerated on import.
1465
+ */
1466
+ function emitLaneAttributesUserData(bundleLanes) {
1467
+ const byLane = {};
1468
+ bundleLanes.forEach((lane, i) => {
1469
+ const stash = {};
1470
+ for (const [k, v] of Object.entries(lane.props.attributes ?? {})) {
1471
+ if (k === 'type' || k.startsWith('odr_'))
1472
+ continue;
1473
+ if (v === undefined || v === null || v === '')
1474
+ continue;
1475
+ stash[k] = String(v);
1476
+ }
1477
+ if (Object.keys(stash).length > 0)
1478
+ byLane[String(-(i + 1))] = stash;
1479
+ });
1480
+ if (Object.keys(byLane).length === 0)
1481
+ return null;
1482
+ return ` <userData code="laneAttributes" value="${escapeXml(JSON.stringify(byLane))}"/>`;
1483
+ }
1484
+ /**
1485
+ * Right-of-way links (`yieldLaneIds`) between two lanes that both feed a
1486
+ * connecting road of the same junction are expressed as standard junction
1487
+ * <priority> records (see planConnectivity); every remaining link is stashed
1488
+ * in <userData code="yieldLanes"> as { ownLaneId: [[roadId, laneId], ...] }
1489
+ * and restored by the importer.
1490
+ */
1491
+ function emitYieldLanesUserData(bundleLanes, laneIdToRoadId, laneIdToOdrLaneId, handledYieldPairs) {
1492
+ const byLane = {};
1493
+ bundleLanes.forEach((lane, i) => {
1494
+ const targets = [];
1495
+ for (const yieldShapeId of lane.props.yieldLaneIds ?? []) {
1496
+ if (handledYieldPairs.has(`${lane.id}|${yieldShapeId}`))
1497
+ continue;
1498
+ const rid = laneIdToRoadId.get(yieldShapeId);
1499
+ const oid = laneIdToOdrLaneId.get(yieldShapeId);
1500
+ if (rid === undefined || oid === undefined)
1501
+ continue;
1502
+ if (!targets.some(t => t[0] === String(rid) && t[1] === String(oid))) {
1503
+ targets.push([String(rid), String(oid)]);
1504
+ }
1505
+ }
1506
+ if (targets.length > 0) {
1507
+ targets.sort((a, b) => Number(a[0]) - Number(b[0]) || Number(a[1]) - Number(b[1]));
1508
+ byLane[String(-(i + 1))] = targets;
1509
+ }
1510
+ });
1511
+ if (Object.keys(byLane).length === 0)
1512
+ return null;
1513
+ return ` <userData code="yieldLanes" value="${escapeXml(JSON.stringify(byLane))}"/>`;
1514
+ }
1515
+ /**
1516
+ * Zero-width-contact lane edges homed on this road (see
1517
+ * ConnectivityPlan.hiddenLaneEdges), stashed as
1518
+ * <userData code="hiddenLaneLinks"> records of
1519
+ * { fr, fl, tr, tl } = from road id / from ODR lane id / to road id /
1520
+ * to ODR lane id (from end -> to start in travel direction), and restored
1521
+ * into next/prev by the importer.
1522
+ */
1523
+ function emitHiddenLinksUserData(bundleLanes, plan, laneIdToRoadId, laneIdToOdrLaneId) {
1524
+ const inBundle = new Set(bundleLanes.map(l => l.id));
1525
+ const recs = [];
1526
+ for (const e of plan.hiddenLaneEdges) {
1527
+ if (!inBundle.has(e.home))
1528
+ continue;
1529
+ const fr = laneIdToRoadId.get(e.from);
1530
+ const fl = laneIdToOdrLaneId.get(e.from);
1531
+ const tr = laneIdToRoadId.get(e.to);
1532
+ const tl = laneIdToOdrLaneId.get(e.to);
1533
+ if (fr === undefined || fl === undefined || tr === undefined || tl === undefined)
1534
+ continue;
1535
+ recs.push({ fr, fl, tr, tl });
1536
+ }
1537
+ if (recs.length === 0)
1538
+ return null;
1539
+ recs.sort((a, b) => a.fr - b.fr || a.fl - b.fl || a.tr - b.tr || a.tl - b.tl);
1540
+ return ` <userData code="hiddenLaneLinks" value="${escapeXml(JSON.stringify(recs))}"/>`;
1541
+ }
1542
+ /**
1543
+ * Road length attribute. The plan-view geometries are emitted with rounded
1544
+ * (6-decimal) s/length values whose cumulative extent can exceed the exact
1545
+ * road length by ~1e-6, which strict consumers flag as "s too large". Use the
1546
+ * emitted extent plus a tiny pad so the length always covers the geometry.
1547
+ */
1548
+ function emittedRoadLength(geom) {
1549
+ let extent = geom.length;
1550
+ for (const g of geom.planView) {
1551
+ const end = parseFloat(fmt(g.s)) + parseFloat(fmt(g.length));
1552
+ if (end > extent)
1553
+ extent = end;
1554
+ }
1555
+ return extent + 1e-4;
1556
+ }
1557
+ function emitRoad(bundle, roadId, plan, signals, signalRefs, objects, shapeMap, laneIdToRoadId, laneIdToOdrLaneId) {
1558
+ const first = bundle.lanes[0];
1559
+ const speed = bundle.lanes.find(l => l.props.attributes?.speed_limit)?.props.attributes?.speed_limit;
1560
+ const name = escapeXml(first.props.attributes?.subtype || 'road');
492
1561
  const lines = [];
493
- lines.push(` <road name="${name}" length="${fmt(geom.length)}" id="${roadId}" junction="-1">`);
494
- lines.push(emitLink(lane, laneIdToRoadId));
1562
+ // Mainline (bundle) roads never belong to a junction; junction membership
1563
+ // is carried by the synthesized connecting roads (emitConnectingRoad).
1564
+ lines.push(` <road name="${name}" length="${fmt(emittedRoadLength(bundle.geom))}" id="${roadId}" junction="-1">`);
1565
+ lines.push(emitLink(roadId, plan));
495
1566
  if (speed) {
496
1567
  lines.push(` <type s="0" type="town">`);
497
1568
  lines.push(` <speed max="${escapeXml(speed)}" unit="km/h"/>`);
498
1569
  lines.push(` </type>`);
499
1570
  }
500
- lines.push(emitPlanView(geom));
1571
+ lines.push(emitPlanView(bundle.geom));
501
1572
  lines.push(` <elevationProfile/>`);
502
1573
  lines.push(` <lateralProfile/>`);
503
- lines.push(emitLanes(geom, hasPrev, hasNext));
1574
+ lines.push(emitLanes(bundle, plan, shapeMap));
504
1575
  lines.push(emitObjects(objects));
505
- lines.push(emitSignals(signals));
1576
+ lines.push(emitSignals(signals, signalRefs));
1577
+ const userData = emitLaneAttributesUserData(bundle.lanes);
1578
+ if (userData)
1579
+ lines.push(userData);
1580
+ const yieldUserData = emitYieldLanesUserData(bundle.lanes, laneIdToRoadId, laneIdToOdrLaneId, plan.handledYieldPairs);
1581
+ if (yieldUserData)
1582
+ lines.push(yieldUserData);
1583
+ const hiddenLinksUserData = emitHiddenLinksUserData(bundle.lanes, plan, laneIdToRoadId, laneIdToOdrLaneId);
1584
+ if (hiddenLinksUserData)
1585
+ lines.push(hiddenLinksUserData);
506
1586
  lines.push(` </road>`);
507
1587
  return lines.join('\n');
508
1588
  }
1589
+ /** Empty point-override map (raw stored coordinates). */
1590
+ const NO_OVERRIDES = new Map();
1591
+ /**
1592
+ * Decide which original roads can be re-emitted verbatim.
1593
+ *
1594
+ * A recorded road is clean when every lane shape it produced still exists and
1595
+ * the state hash recomputed from the live shapes equals the import-time hash
1596
+ * (geometry, attributes, connectivity, right-of-way, and the regulatory
1597
+ * shapes touching the road — see odrCarryThrough.ts).
1598
+ *
1599
+ * Dirtiness then propagates until stable:
1600
+ * - A junction is dirty when any member road (connecting roads, incoming /
1601
+ * outgoing roads, roads linking to the junction) is dirty or unrecorded.
1602
+ * A dirty junction regenerates together with its CONNECTING
1603
+ * (junction-stamped) roads, whose connection table it replaces; clean
1604
+ * incoming / outgoing roads stay verbatim and their junction link
1605
+ * elementIds are re-pointed at the regenerated junction on emission.
1606
+ * - Regulatory shapes are atomic: a traffic light / crosswalk touching a
1607
+ * dirty road dirties every road it touches, so its signal + references are
1608
+ * either all verbatim or all regenerated.
1609
+ *
1610
+ * Roads referencing unrecorded elements (e.g. a selective import) are never
1611
+ * carried verbatim, so verbatim output cannot dangle into missing roads.
1612
+ */
1613
+ function planCarryThrough(sidecar, shapeMap, trafficLights, crosswalks) {
1614
+ const records = sidecar?.roadRecords;
1615
+ if (!sidecar || !records || Object.keys(records).length === 0)
1616
+ return null;
1617
+ const doc = extractOdrDocument(sidecar.rawXml);
1618
+ if (!doc)
1619
+ return null;
1620
+ const docRoadById = new Map(doc.roads.map(r => [r.id, r]));
1621
+ const docJunctionById = new Map(doc.junctions.map(j => [j.id, j]));
1622
+ // laneShapeId -> recorded road id (over every record).
1623
+ const laneRoadOf = new Map();
1624
+ for (const [rid, rec] of Object.entries(records)) {
1625
+ for (const lid of rec.laneShapeIds)
1626
+ laneRoadOf.set(lid, rid);
1627
+ }
1628
+ const stopLinePts = (lsId) => {
1629
+ if (!lsId)
1630
+ return null;
1631
+ const ls = shapeMap.get(lsId);
1632
+ if (!ls)
1633
+ return null;
1634
+ const pts = collectPoints(shapeMap, ls.props.pointIds, false, NO_OVERRIDES);
1635
+ return pts.length >= 2 ? pts : null;
1636
+ };
1637
+ // Regulatory shapes: state + the set of recorded roads each one touches
1638
+ // (mirrors the importer's record builder).
1639
+ const regStatesByRoad = new Map();
1640
+ const regShapes = [];
1641
+ const addRegState = (state, affected, own) => {
1642
+ const touching = new Set();
1643
+ if (own && records[own])
1644
+ touching.add(own);
1645
+ for (const lid of affected) {
1646
+ const rid = laneRoadOf.get(lid);
1647
+ if (rid)
1648
+ touching.add(rid);
1649
+ }
1650
+ for (const rid of touching) {
1651
+ const list = regStatesByRoad.get(rid) ?? [];
1652
+ list.push(state);
1653
+ regStatesByRoad.set(rid, list);
1654
+ }
1655
+ regShapes.push({ shapeId: state.shapeId, touching });
1656
+ };
1657
+ for (const tl of trafficLights) {
1658
+ addRegState({
1659
+ kind: 'traffic_light',
1660
+ shapeId: tl.id,
1661
+ numbers: [tl.x, tl.y, tl.props.w, tl.props.h, tl.rotation || 0],
1662
+ attributes: tl.props.attributes ?? {},
1663
+ affectedLaneIds: tl.props.affectedLaneIds ?? [],
1664
+ stopLinePts: stopLinePts(tl.props.stopLineId),
1665
+ controllerId: tl.props.controllerId ?? '',
1666
+ }, tl.props.affectedLaneIds ?? [], tl.props.attributes?.odr_road_id);
1667
+ }
1668
+ for (const cw of crosswalks) {
1669
+ addRegState({
1670
+ kind: 'crosswalk',
1671
+ shapeId: cw.id,
1672
+ numbers: [
1673
+ cw.x,
1674
+ cw.y,
1675
+ cw.props.startX,
1676
+ cw.props.startY,
1677
+ cw.props.endX,
1678
+ cw.props.endY,
1679
+ cw.props.crosswalkWidth,
1680
+ cw.rotation || 0,
1681
+ ],
1682
+ attributes: cw.props.attributes ?? {},
1683
+ affectedLaneIds: cw.props.affectedLaneIds ?? [],
1684
+ stopLinePts: stopLinePts(cw.props.stopLineId),
1685
+ controllerId: '',
1686
+ }, cw.props.affectedLaneIds ?? [], cw.props.attributes?.odr_road_id);
1687
+ }
1688
+ // Export-side lane states (null when any recorded lane shape is missing).
1689
+ const exportLaneStates = (rec) => {
1690
+ const states = [];
1691
+ for (const lid of rec.laneShapeIds) {
1692
+ const shape = shapeMap.get(lid);
1693
+ if (!shape || shape.type !== 'lane')
1694
+ return null;
1695
+ const lane = shape;
1696
+ states.push({
1697
+ leftPts: boundaryPointsOf(shapeMap, lane.props.leftBoundaryId, lane.props.invertLeft, NO_OVERRIDES),
1698
+ rightPts: boundaryPointsOf(shapeMap, lane.props.rightBoundaryId, lane.props.invertRight, NO_OVERRIDES),
1699
+ attributes: lane.props.attributes ?? {},
1700
+ next: lane.props.next ?? [],
1701
+ prev: lane.props.prev ?? [],
1702
+ yieldLaneIds: lane.props.yieldLaneIds ?? [],
1703
+ });
1704
+ }
1705
+ return states;
1706
+ };
1707
+ // Seed dirtiness: hash mismatch, missing shapes, or references that leave
1708
+ // the recorded set.
1709
+ const dirty = new Set();
1710
+ for (const [rid, rec] of Object.entries(records)) {
1711
+ const docRoad = docRoadById.get(rid);
1712
+ if (!docRoad) {
1713
+ dirty.add(rid);
1714
+ continue;
1715
+ }
1716
+ if (docRoad.linkRoadRefs.some(ref => !records[ref]) ||
1717
+ docRoad.linkJunctionRefs.some(ref => !docJunctionById.has(ref))) {
1718
+ dirty.add(rid);
1719
+ continue;
1720
+ }
1721
+ const laneStates = exportLaneStates(rec);
1722
+ if (!laneStates || hashRoadState(laneStates, regStatesByRoad.get(rid) ?? []) !== rec.stateHash) {
1723
+ dirty.add(rid);
1724
+ }
1725
+ }
1726
+ // Junction membership: connection roads, junction-stamped roads (plus
1727
+ // their link targets — the maneuver's incoming/outgoing roads), and roads
1728
+ // whose link references the junction.
1729
+ const members = new Map();
1730
+ const junctionStamped = new Map();
1731
+ for (const j of doc.junctions) {
1732
+ members.set(j.id, new Set(j.memberRoadIds));
1733
+ junctionStamped.set(j.id, new Set());
1734
+ }
1735
+ for (const r of doc.roads) {
1736
+ if (r.junction !== '-1') {
1737
+ const set = members.get(r.junction);
1738
+ if (set) {
1739
+ set.add(r.id);
1740
+ for (const ref of r.linkRoadRefs)
1741
+ set.add(ref);
1742
+ junctionStamped.get(r.junction).add(r.id);
1743
+ }
1744
+ }
1745
+ for (const jref of r.linkJunctionRefs)
1746
+ members.get(jref)?.add(r.id);
1747
+ }
1748
+ // Propagate to a fixpoint. A dirty junction drags only its connecting
1749
+ // (junction-stamped) roads into regeneration — clean incoming / outgoing
1750
+ // roads keep their verbatim text (with the junction link id rewritten) —
1751
+ // so a single edited road regenerates its own junctions, not the whole
1752
+ // junction graph. Regulatory shapes are atomic across the roads they touch.
1753
+ const dirtyJunctionIds = new Set();
1754
+ let changed = true;
1755
+ while (changed) {
1756
+ changed = false;
1757
+ for (const [jid, memberSet] of members) {
1758
+ let bad = false;
1759
+ for (const m of memberSet) {
1760
+ if (!records[m] || dirty.has(m)) {
1761
+ bad = true;
1762
+ break;
1763
+ }
1764
+ }
1765
+ if (!bad)
1766
+ continue;
1767
+ if (!dirtyJunctionIds.has(jid)) {
1768
+ dirtyJunctionIds.add(jid);
1769
+ changed = true;
1770
+ }
1771
+ for (const m of junctionStamped.get(jid) ?? []) {
1772
+ if (records[m] && !dirty.has(m)) {
1773
+ dirty.add(m);
1774
+ changed = true;
1775
+ }
1776
+ }
1777
+ }
1778
+ for (const reg of regShapes) {
1779
+ let bad = false;
1780
+ for (const rid of reg.touching) {
1781
+ if (dirty.has(rid)) {
1782
+ bad = true;
1783
+ break;
1784
+ }
1785
+ }
1786
+ if (!bad)
1787
+ continue;
1788
+ for (const rid of reg.touching) {
1789
+ if (!dirty.has(rid)) {
1790
+ dirty.add(rid);
1791
+ changed = true;
1792
+ }
1793
+ }
1794
+ }
1795
+ }
1796
+ const cleanRoadIds = new Set();
1797
+ for (const rid of Object.keys(records)) {
1798
+ if (!dirty.has(rid) && docRoadById.has(rid))
1799
+ cleanRoadIds.add(rid);
1800
+ }
1801
+ const verbatimLaneIds = new Set();
1802
+ for (const rid of cleanRoadIds) {
1803
+ for (const lid of records[rid].laneShapeIds)
1804
+ verbatimLaneIds.add(lid);
1805
+ }
1806
+ const consumedShapeIds = new Set();
1807
+ for (const reg of regShapes) {
1808
+ if (reg.touching.size === 0)
1809
+ continue;
1810
+ let allClean = true;
1811
+ for (const rid of reg.touching) {
1812
+ if (!cleanRoadIds.has(rid)) {
1813
+ allClean = false;
1814
+ break;
1815
+ }
1816
+ }
1817
+ if (allClean)
1818
+ consumedShapeIds.add(reg.shapeId);
1819
+ }
1820
+ const verbatimRoads = [];
1821
+ for (const r of doc.roads) {
1822
+ if (cleanRoadIds.has(r.id))
1823
+ verbatimRoads.push(r);
1824
+ }
1825
+ const verbatimJunctionTexts = [];
1826
+ for (const j of doc.junctions) {
1827
+ if (!dirtyJunctionIds.has(j.id))
1828
+ verbatimJunctionTexts.push(j.text);
1829
+ }
1830
+ // A controller stays verbatim when every signal it controls is defined in
1831
+ // a verbatim road.
1832
+ const signalRoadOf = new Map();
1833
+ for (const r of doc.roads) {
1834
+ for (const sid of r.signalIds)
1835
+ signalRoadOf.set(sid, r.id);
1836
+ }
1837
+ const verbatimControllerTexts = [];
1838
+ for (const c of doc.controllers) {
1839
+ const ok = c.signalIds.length > 0 &&
1840
+ c.signalIds.every(sid => {
1841
+ const rid = signalRoadOf.get(sid);
1842
+ return rid !== undefined && cleanRoadIds.has(rid);
1843
+ });
1844
+ if (ok)
1845
+ verbatimControllerTexts.push(c.text);
1846
+ }
1847
+ return {
1848
+ doc,
1849
+ records,
1850
+ cleanRoadIds,
1851
+ dirtyRecordedIds: dirty,
1852
+ verbatimLaneIds,
1853
+ consumedShapeIds,
1854
+ headerText: doc.headerText,
1855
+ verbatimRoads,
1856
+ dirtyJunctionIds,
1857
+ verbatimJunctionTexts,
1858
+ verbatimControllerTexts,
1859
+ idBase: Math.max(doc.maxNumericElementId, 0) + 1,
1860
+ signalIdBase: Math.max(doc.maxNumericSignalId, 0) + 1,
1861
+ controllerIdBase: Math.max(doc.maxNumericControllerId, 0) + 1,
1862
+ };
1863
+ }
509
1864
  /**
510
1865
  * Build an OpenDRIVE 1.8 XML document from a snapshot.
1866
+ *
1867
+ * With `options.sidecar` (captured by the OpenDRIVE importer), unedited
1868
+ * roads are re-emitted verbatim from the original XML; see planCarryThrough.
511
1869
  */
512
- export function exportToOpenDrive(snapshot) {
1870
+ export function exportToOpenDrive(snapshot, options = {}) {
513
1871
  const shapes = snapshot.shapes;
514
1872
  const shapeMap = buildShapeMap(shapes);
515
1873
  const lanes = [];
@@ -535,41 +1893,333 @@ export function exportToOpenDrive(snapshot) {
535
1893
  polygons.push({ shape: poly, vertices });
536
1894
  }
537
1895
  }
538
- // Assign sequential road ids.
539
- const laneIdToRoadId = new Map();
540
- lanes.forEach((lane, i) => laneIdToRoadId.set(lane.id, i + 1));
1896
+ // Carry-through: with an importer sidecar, unedited original roads are
1897
+ // re-emitted verbatim and excluded from regeneration.
1898
+ const carry = planCarryThrough(options.sidecar, shapeMap, trafficLights, crosswalks);
1899
+ const regenLanes = carry ? lanes.filter(l => !carry.verbatimLaneIds.has(l.id)) : lanes;
1900
+ const regenTrafficLights = carry
1901
+ ? trafficLights.filter(t => !carry.consumedShapeIds.has(t.id))
1902
+ : trafficLights;
1903
+ const regenCrosswalks = carry
1904
+ ? crosswalks.filter(c => !carry.consumedShapeIds.has(c.id))
1905
+ : crosswalks;
541
1906
  const dateStr = new Date().toISOString();
542
1907
  const bbox = computeEnuBoundingBox(shapeMap);
543
1908
  const geoRefProj = originToProjString(snapshot.origin);
544
1909
  const lines = [];
545
1910
  lines.push(`<?xml version="1.0" encoding="UTF-8"?>`);
546
1911
  lines.push(`<OpenDRIVE>`);
547
- // OpenDRIVE 1.8 expects <geoReference> inside <header>. We always emit one —
548
- // tmerc-at-origin when snapshot.origin is set, WGS84 longlat as a fallback —
549
- // so downstream tools (esmini, RoadRunner, asam-qc-opendrive) see a defined
550
- // coordinate reference system rather than nothing. The N/S/E/W attributes
551
- // are populated from the actual point cloud so the header bbox reflects the
552
- // map extent in ENU metres.
553
- lines.push(` <header revMajor="1" revMinor="8" name="drawtonomy" version="1.0" date="${dateStr}" ` +
554
- `north="${fmt(bbox.north)}" south="${fmt(bbox.south)}" east="${fmt(bbox.east)}" west="${fmt(bbox.west)}" vendor="drawtonomy">`);
555
- lines.push(` <geoReference><![CDATA[${escapeCdata(geoRefProj)}]]></geoReference>`);
556
- lines.push(` </header>`);
1912
+ if (carry?.headerText) {
1913
+ // Carry-through keeps the original header (geoReference, bbox, vendor)
1914
+ // so an unedited round trip preserves the source coordinate frame.
1915
+ lines.push(carry.headerText);
1916
+ }
1917
+ else {
1918
+ // OpenDRIVE 1.8 expects <geoReference> inside <header>. We always emit one
1919
+ // tmerc-at-origin when snapshot.origin is set, WGS84 longlat as a fallback —
1920
+ // so downstream tools (esmini, RoadRunner, asam-qc-opendrive) see a defined
1921
+ // coordinate reference system rather than nothing. The N/S/E/W attributes
1922
+ // are populated from the actual point cloud so the header bbox reflects the
1923
+ // map extent in ENU metres.
1924
+ lines.push(` <header revMajor="1" revMinor="8" name="drawtonomy" version="1.0" date="${dateStr}" ` +
1925
+ `north="${fmt(bbox.north)}" south="${fmt(bbox.south)}" east="${fmt(bbox.east)}" west="${fmt(bbox.west)}" vendor="drawtonomy">`);
1926
+ lines.push(` <geoReference><![CDATA[${escapeCdata(geoRefProj)}]]></geoReference>`);
1927
+ lines.push(` </header>`);
1928
+ }
557
1929
  const pointOverrides = buildBoundaryAlignmentOverrides(shapeMap, lanes);
558
- const laneToGeom = new Map();
559
- for (const lane of lanes) {
560
- const geom = buildRoadGeometry(shapeMap, lane, pointOverrides);
561
- if (geom)
562
- laneToGeom.set(lane.id, geom);
1930
+ // Group laterally adjacent lanes into road bundles and build their
1931
+ // geometry. Degenerate bundles (zero-length reference lines) are dropped;
1932
+ // a multi-lane bundle whose geometry cannot be built (broken boundary
1933
+ // references) degrades to per-lane bundles so one bad lane does not drop
1934
+ // its neighbours.
1935
+ const exportBundles = [];
1936
+ for (const bundleLanes of detectBundles(regenLanes)) {
1937
+ const geom = buildBundleGeometry(shapeMap, bundleLanes, pointOverrides);
1938
+ if (geom && geom.length >= 0.01) {
1939
+ exportBundles.push({ lanes: bundleLanes, geom });
1940
+ }
1941
+ else if (bundleLanes.length > 1) {
1942
+ for (const lane of bundleLanes) {
1943
+ const g = buildBundleGeometry(shapeMap, [lane], pointOverrides);
1944
+ if (g && g.length >= 0.01)
1945
+ exportBundles.push({ lanes: [lane], geom: g });
1946
+ }
1947
+ }
563
1948
  }
564
- const { roadSignals, roadObjects } = attachShapesToRoads(trafficLights, crosswalks, polygons, laneToGeom, laneIdToRoadId);
565
- for (const lane of lanes) {
566
- const geom = laneToGeom.get(lane.id);
567
- if (!geom)
1949
+ // Stable road id assignment: bundles ordered by their first lane's position
1950
+ // in the snapshot. Lane ids count -1, -2, ... left→right within a bundle.
1951
+ // Carry-through: a regenerated bundle covering exactly the lane set of a
1952
+ // dirty original road keeps that road's id, so links inside verbatim
1953
+ // neighbours stay valid without rewriting; other bundles take fresh ids
1954
+ // above every original id.
1955
+ const laneOrder = new Map();
1956
+ lanes.forEach((lane, i) => laneOrder.set(lane.id, i));
1957
+ exportBundles.sort((a, b) => Math.min(...a.lanes.map(l => laneOrder.get(l.id))) - Math.min(...b.lanes.map(l => laneOrder.get(l.id))));
1958
+ const reuseKey = (ids) => [...ids].sort().join('\n');
1959
+ const reusableRoadIds = new Map();
1960
+ if (carry) {
1961
+ for (const rid of carry.dirtyRecordedIds) {
1962
+ const rec = carry.records[rid];
1963
+ if (!rec || rec.laneShapeIds.length === 0 || !/^\d+$/.test(rid))
1964
+ continue;
1965
+ reusableRoadIds.set(reuseKey(rec.laneShapeIds), parseInt(rid, 10));
1966
+ }
1967
+ }
1968
+ const laneIdToRoadId = new Map();
1969
+ const laneIdToOdrLaneId = new Map();
1970
+ const roadIdByBundle = new Map();
1971
+ let nextRoadId = carry ? carry.idBase : 1;
1972
+ for (const bundle of exportBundles) {
1973
+ const key = reuseKey(bundle.lanes.map(l => l.id));
1974
+ const reused = reusableRoadIds.get(key);
1975
+ if (reused !== undefined)
1976
+ reusableRoadIds.delete(key);
1977
+ const roadId = reused ?? nextRoadId++;
1978
+ roadIdByBundle.set(bundle, roadId);
1979
+ bundle.lanes.forEach((lane, i) => {
1980
+ laneIdToRoadId.set(lane.id, roadId);
1981
+ laneIdToOdrLaneId.set(lane.id, -(i + 1));
1982
+ });
1983
+ }
1984
+ // Carry-through: lanes of verbatim roads join the connectivity id maps as
1985
+ // external endpoints, so regenerated roads link to / from them.
1986
+ const externalLanes = new Map();
1987
+ if (carry) {
1988
+ for (const rid of carry.cleanRoadIds) {
1989
+ if (!/^\d+$/.test(rid))
1990
+ continue;
1991
+ for (const lid of carry.records[rid].laneShapeIds) {
1992
+ const shape = shapeMap.get(lid);
1993
+ if (!shape)
1994
+ continue;
1995
+ const odrLaneId = parseInt(shape.props.attributes?.odr_lane_id ?? '', 10);
1996
+ if (!Number.isFinite(odrLaneId))
1997
+ continue;
1998
+ laneIdToRoadId.set(lid, parseInt(rid, 10));
1999
+ laneIdToOdrLaneId.set(lid, odrLaneId);
2000
+ externalLanes.set(lid, shape);
2001
+ }
2002
+ }
2003
+ }
2004
+ // Geometry seed for synthesized connecting roads: bundle lanes read it off
2005
+ // their fitted bundle geometry; external (verbatim) lanes off their drawn
2006
+ // boundary endpoints.
2007
+ const laneLocation = new Map();
2008
+ for (const bundle of exportBundles) {
2009
+ bundle.lanes.forEach((lane, index) => laneLocation.set(lane.id, { bundle, index }));
2010
+ }
2011
+ const connectingSourceFor = (laneShapeId) => {
2012
+ const loc = laneLocation.get(laneShapeId);
2013
+ if (loc) {
2014
+ const geom = loc.bundle.geom;
2015
+ const lastGeom = geom.planView[geom.planView.length - 1];
2016
+ const endPose = evalGeometry(lastGeom, lastGeom.length);
2017
+ // Lane boundaries sit toward -t (right of the reference direction): the
2018
+ // inner boundary of lane -(i+1) is offset by the widths of lanes 0..i-1.
2019
+ const lastIdx = geom.samplePoses.length - 1;
2020
+ let offset = 0;
2021
+ for (let m = 0; m < loc.index; m++)
2022
+ offset += geom.laneWidths[m][lastIdx];
2023
+ return {
2024
+ x: endPose.x + Math.sin(endPose.hdg) * offset,
2025
+ y: endPose.y - Math.cos(endPose.hdg) * offset,
2026
+ hdg: endPose.hdg,
2027
+ width: geom.laneWidths[loc.index][lastIdx],
2028
+ laneType: odrLaneTypeFor(loc.bundle.lanes[loc.index]),
2029
+ };
2030
+ }
2031
+ const lane = externalLanes.get(laneShapeId);
2032
+ if (!lane)
2033
+ return null;
2034
+ const left = boundaryPointsOf(shapeMap, lane.props.leftBoundaryId, lane.props.invertLeft, NO_OVERRIDES);
2035
+ const right = boundaryPointsOf(shapeMap, lane.props.rightBoundaryId, lane.props.invertRight, NO_OVERRIDES);
2036
+ if (!left || !right)
2037
+ return null;
2038
+ const ex = pxToEnuX(left[left.length - 1].x);
2039
+ const ey = pxToEnuY(left[left.length - 1].y);
2040
+ const px = pxToEnuX(left[left.length - 2].x);
2041
+ const py = pxToEnuY(left[left.length - 2].y);
2042
+ const rx = pxToEnuX(right[right.length - 1].x);
2043
+ const ry = pxToEnuY(right[right.length - 1].y);
2044
+ return {
2045
+ x: ex,
2046
+ y: ey,
2047
+ hdg: Math.atan2(ey - py, ex - px),
2048
+ width: Math.hypot(rx - ex, ry - ey),
2049
+ laneType: odrLaneTypeFor(lane),
2050
+ };
2051
+ };
2052
+ // Travel heading / width of a connecting road's target lane at its start,
2053
+ // for blending the stub onto the outgoing road (see ConnectingTarget).
2054
+ const connectingTargetFor = (laneShapeId) => {
2055
+ const loc = laneLocation.get(laneShapeId);
2056
+ if (loc) {
2057
+ const geom = loc.bundle.geom;
2058
+ if (geom.samplePoses.length === 0)
2059
+ return null;
2060
+ const pose = geom.samplePoses[0];
2061
+ // Lane boundaries sit toward -t (right of the reference direction).
2062
+ let offset = 0;
2063
+ for (let m = 0; m < loc.index; m++)
2064
+ offset += geom.laneWidths[m][0];
2065
+ return {
2066
+ x: pose.x + Math.sin(pose.hdg) * offset,
2067
+ y: pose.y - Math.cos(pose.hdg) * offset,
2068
+ hdg: pose.hdg,
2069
+ width: geom.laneWidths[loc.index][0],
2070
+ };
2071
+ }
2072
+ const lane = externalLanes.get(laneShapeId);
2073
+ if (!lane)
2074
+ return null;
2075
+ const left = boundaryPointsOf(shapeMap, lane.props.leftBoundaryId, lane.props.invertLeft, NO_OVERRIDES);
2076
+ const right = boundaryPointsOf(shapeMap, lane.props.rightBoundaryId, lane.props.invertRight, NO_OVERRIDES);
2077
+ if (!left || !right || left.length < 2 || right.length < 1)
2078
+ return null;
2079
+ const ax = pxToEnuX(left[0].x);
2080
+ const ay = pxToEnuY(left[0].y);
2081
+ const bx = pxToEnuX(left[1].x);
2082
+ const by = pxToEnuY(left[1].y);
2083
+ const rx = pxToEnuX(right[0].x);
2084
+ const ry = pxToEnuY(right[0].y);
2085
+ return {
2086
+ x: ax,
2087
+ y: ay,
2088
+ hdg: Math.atan2(by - ay, bx - ax),
2089
+ width: Math.hypot(rx - ax, ry - ay),
2090
+ };
2091
+ };
2092
+ // Full lane width at a linked contact, for the zero-width link rules:
2093
+ // bundle lanes read their fitted width samples, external (verbatim) lanes
2094
+ // measure their drawn boundary endpoints.
2095
+ const contactWidth = (laneShapeId, contact) => {
2096
+ const loc = laneLocation.get(laneShapeId);
2097
+ if (loc) {
2098
+ const widths = loc.bundle.geom.laneWidths[loc.index];
2099
+ if (!widths || widths.length === 0)
2100
+ return null;
2101
+ return contact === 'start' ? widths[0] : widths[widths.length - 1];
2102
+ }
2103
+ const lane = externalLanes.get(laneShapeId);
2104
+ if (!lane)
2105
+ return null;
2106
+ const left = boundaryPointsOf(shapeMap, lane.props.leftBoundaryId, lane.props.invertLeft, NO_OVERRIDES);
2107
+ const right = boundaryPointsOf(shapeMap, lane.props.rightBoundaryId, lane.props.invertRight, NO_OVERRIDES);
2108
+ if (!left || !right || left.length === 0 || right.length === 0)
2109
+ return null;
2110
+ const li = contact === 'start' ? left[0] : left[left.length - 1];
2111
+ const ri = contact === 'start' ? right[0] : right[right.length - 1];
2112
+ return pxToMeter(Math.hypot(ri.x - li.x, ri.y - li.y));
2113
+ };
2114
+ const plan = planConnectivity(exportBundles, laneIdToRoadId, laneIdToOdrLaneId, nextRoadId, connectingSourceFor, connectingTargetFor, contactWidth, externalLanes);
2115
+ const roads = exportBundles.map(b => ({ roadId: roadIdByBundle.get(b), geom: b.geom }));
2116
+ const { roadSignals, roadObjects, roadSignalRefs, signalIdByShape } = attachShapesToRoads(shapeMap, regenTrafficLights, regenCrosswalks, polygons, roads, laneIdToRoadId, laneIdToOdrLaneId, undefined, carry?.signalIdBase);
2117
+ // Verbatim road blocks first (original document order). Two minimal
2118
+ // rewrites keep their links valid; nothing else is touched:
2119
+ // - road links to a dirty road whose lanes regenerated into exactly one
2120
+ // bundle under a different id are re-pointed at that bundle;
2121
+ // - junction links to a dirty (regenerated) junction are re-pointed at the
2122
+ // synthesized junction this road participates in (junction-routed pairs
2123
+ // sharing a road always merge, so the target is unique per road).
2124
+ if (carry) {
2125
+ const rewriteMap = new Map();
2126
+ const bundleRoadOfLane = new Map();
2127
+ for (const bundle of exportBundles) {
2128
+ const rid = roadIdByBundle.get(bundle);
2129
+ for (const l of bundle.lanes)
2130
+ bundleRoadOfLane.set(l.id, rid);
2131
+ }
2132
+ for (const rid of carry.dirtyRecordedIds) {
2133
+ const rec = carry.records[rid];
2134
+ if (!rec)
2135
+ continue;
2136
+ const newIds = new Set();
2137
+ for (const lid of rec.laneShapeIds) {
2138
+ const nid = bundleRoadOfLane.get(lid);
2139
+ if (nid !== undefined)
2140
+ newIds.add(nid);
2141
+ }
2142
+ if (newIds.size === 1) {
2143
+ const nid = String([...newIds][0]);
2144
+ if (nid !== rid)
2145
+ rewriteMap.set(rid, nid);
2146
+ }
2147
+ }
2148
+ const newJunctionOfRoad = new Map();
2149
+ for (const spec of plan.connectingRoads) {
2150
+ newJunctionOfRoad.set(spec.incomingRoadId, spec.junctionId);
2151
+ newJunctionOfRoad.set(spec.outgoingRoadId, spec.junctionId);
2152
+ }
2153
+ for (const r of carry.verbatimRoads) {
2154
+ let junctionMap;
2155
+ for (const jref of r.linkJunctionRefs) {
2156
+ if (!carry.dirtyJunctionIds.has(jref))
2157
+ continue;
2158
+ const exportedId = /^\d+$/.test(r.id) ? parseInt(r.id, 10) : NaN;
2159
+ const replacement = newJunctionOfRoad.get(exportedId);
2160
+ if (replacement !== undefined) {
2161
+ junctionMap = junctionMap ?? new Map();
2162
+ junctionMap.set(jref, String(replacement));
2163
+ }
2164
+ }
2165
+ lines.push(rewriteRoadLinkTargets(r.text, rewriteMap, junctionMap ?? new Map()));
2166
+ }
2167
+ }
2168
+ for (const bundle of exportBundles) {
2169
+ const roadId = roadIdByBundle.get(bundle);
2170
+ lines.push(emitRoad(bundle, roadId, plan, roadSignals.get(roadId) ?? [], roadSignalRefs.get(roadId) ?? [], roadObjects.get(roadId) ?? [], shapeMap, laneIdToRoadId, laneIdToOdrLaneId));
2171
+ }
2172
+ // Synthesized junction connecting roads (standard incoming -> connecting ->
2173
+ // outgoing structure; see planConnectivity).
2174
+ for (const spec of plan.connectingRoads) {
2175
+ lines.push(emitConnectingRoad(spec));
2176
+ }
2177
+ // Verbatim controllers (every controlled signal lives in a verbatim road).
2178
+ if (carry) {
2179
+ for (const text of carry.verbatimControllerTexts)
2180
+ lines.push(text);
2181
+ }
2182
+ // Signal groups: traffic lights sharing a controllerId (one intersection)
2183
+ // become a <controller> listing their emitted signals as <control> records.
2184
+ const controllerGroups = new Map();
2185
+ for (const tl of regenTrafficLights) {
2186
+ const groupId = tl.props.controllerId;
2187
+ if (!groupId)
2188
+ continue;
2189
+ const signalId = signalIdByShape.get(tl.id);
2190
+ if (signalId === undefined)
568
2191
  continue;
569
- const roadId = laneIdToRoadId.get(lane.id);
570
- const signals = roadSignals.get(roadId) ?? [];
571
- const objects = roadObjects.get(roadId) ?? [];
572
- lines.push(emitRoad(lane, geom, roadId, laneIdToRoadId, signals, objects));
2192
+ const group = controllerGroups.get(groupId) ?? [];
2193
+ group.push(signalId);
2194
+ controllerGroups.set(groupId, group);
2195
+ }
2196
+ let controllerIdCounter = carry ? carry.controllerIdBase : 1;
2197
+ for (const [groupId, signalIds] of controllerGroups) {
2198
+ lines.push(` <controller id="${controllerIdCounter++}" name="${escapeXml(groupId)}" sequence="0">`);
2199
+ for (const signalId of signalIds) {
2200
+ lines.push(` <control signalId="${signalId}" type="0"/>`);
2201
+ }
2202
+ lines.push(` </controller>`);
2203
+ }
2204
+ // Verbatim junctions (all member roads verbatim).
2205
+ if (carry) {
2206
+ for (const text of carry.verbatimJunctionTexts)
2207
+ lines.push(text);
2208
+ }
2209
+ // Synthesized junctions for branch / merge connectivity (see planConnectivity).
2210
+ for (const junction of plan.junctions) {
2211
+ lines.push(` <junction id="${junction.id}" name="junction${junction.id}">`);
2212
+ junction.connections.forEach((conn, idx) => {
2213
+ lines.push(` <connection id="${idx}" incomingRoad="${conn.incoming}" connectingRoad="${conn.connecting}" contactPoint="start">`);
2214
+ for (const ll of conn.laneLinks) {
2215
+ lines.push(` <laneLink from="${ll.from}" to="${ll.to}"/>`);
2216
+ }
2217
+ lines.push(` </connection>`);
2218
+ });
2219
+ for (const pr of junction.priorities) {
2220
+ lines.push(` <priority high="${pr.high}" low="${pr.low}"/>`);
2221
+ }
2222
+ lines.push(` </junction>`);
573
2223
  }
574
2224
  lines.push(`</OpenDRIVE>`);
575
2225
  return lines.join('\n');