@drawtonomy/sdk 0.9.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.
- package/dist/exporter/index.d.ts +2 -2
- package/dist/exporter/index.d.ts.map +1 -1
- package/dist/exporter/index.js.map +1 -1
- package/dist/exporter/odrCarryThrough.d.ts +89 -0
- package/dist/exporter/odrCarryThrough.d.ts.map +1 -0
- package/dist/exporter/odrCarryThrough.js +186 -0
- package/dist/exporter/odrCarryThrough.js.map +1 -0
- package/dist/exporter/odrGeometryFit.d.ts +37 -0
- package/dist/exporter/odrGeometryFit.d.ts.map +1 -0
- package/dist/exporter/odrGeometryFit.js +476 -0
- package/dist/exporter/odrGeometryFit.js.map +1 -0
- package/dist/exporter/odrToShapes.d.ts +8 -0
- package/dist/exporter/odrToShapes.d.ts.map +1 -1
- package/dist/exporter/odrToShapes.js +361 -35
- package/dist/exporter/odrToShapes.js.map +1 -1
- package/dist/exporter/opendrive.d.ts +14 -1
- package/dist/exporter/opendrive.d.ts.map +1 -1
- package/dist/exporter/opendrive.js +1521 -296
- package/dist/exporter/opendrive.js.map +1 -1
- package/dist/exporter/opendriveParser.d.ts +8 -0
- package/dist/exporter/opendriveParser.d.ts.map +1 -1
- package/dist/exporter/opendriveParser.js +5 -2
- package/dist/exporter/opendriveParser.js.map +1 -1
- package/dist/exporter/units.d.ts +7 -0
- package/dist/exporter/units.d.ts.map +1 -1
- package/dist/exporter/units.js +11 -0
- package/dist/exporter/units.js.map +1 -1
- package/package.json +1 -1
|
@@ -2,18 +2,30 @@
|
|
|
2
2
|
// No external library dependencies.
|
|
3
3
|
//
|
|
4
4
|
// Design:
|
|
5
|
-
// -
|
|
6
|
-
//
|
|
7
|
-
//
|
|
8
|
-
// -
|
|
9
|
-
//
|
|
10
|
-
//
|
|
11
|
-
//
|
|
12
|
-
//
|
|
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
|
|
13
23
|
// - Coordinate frame: canvas (x right, y down) → ENU (x right, y up); y is flipped
|
|
14
|
-
import {
|
|
24
|
+
import { evalGeometry } from './odrGeometry';
|
|
25
|
+
import { fitPlanView } from './odrGeometryFit';
|
|
15
26
|
import { originToProjString } from './projection';
|
|
16
|
-
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';
|
|
17
29
|
/** O(1) shape lookup by id. */
|
|
18
30
|
function buildShapeMap(shapes) {
|
|
19
31
|
const map = new Map();
|
|
@@ -21,36 +33,6 @@ function buildShapeMap(shapes) {
|
|
|
21
33
|
map.set(s.id, s);
|
|
22
34
|
return map;
|
|
23
35
|
}
|
|
24
|
-
/**
|
|
25
|
-
* Build the centerline + width samples for a lane from its two boundaries.
|
|
26
|
-
* If pointOverrides is provided, those point ids are read from the override
|
|
27
|
-
* map instead of from the shape map (used for boundary alignment snapping).
|
|
28
|
-
*/
|
|
29
|
-
function buildRoadGeometry(shapeMap, lane, pointOverrides) {
|
|
30
|
-
const leftId = lane.props.leftBoundaryId;
|
|
31
|
-
const rightId = lane.props.rightBoundaryId;
|
|
32
|
-
if (!leftId || !rightId)
|
|
33
|
-
return null;
|
|
34
|
-
const left = shapeMap.get(leftId);
|
|
35
|
-
const right = shapeMap.get(rightId);
|
|
36
|
-
if (!left || !right)
|
|
37
|
-
return null;
|
|
38
|
-
const leftPts = collectPoints(shapeMap, left.props.pointIds, lane.props.invertLeft, pointOverrides);
|
|
39
|
-
const rightPts = collectPoints(shapeMap, right.props.pointIds, lane.props.invertRight, pointOverrides);
|
|
40
|
-
if (leftPts.length < 2 || rightPts.length < 2)
|
|
41
|
-
return null;
|
|
42
|
-
const samples = computeCenterlineWithWidth(leftPts, rightPts);
|
|
43
|
-
if (samples.length < 2)
|
|
44
|
-
return null;
|
|
45
|
-
const odrSamples = samples.map((s) => ({
|
|
46
|
-
x: pxToEnuX(s.x),
|
|
47
|
-
y: pxToEnuY(s.y),
|
|
48
|
-
width: pxToMeter(s.width),
|
|
49
|
-
s: pxToMeter(s.s),
|
|
50
|
-
}));
|
|
51
|
-
const length = odrSamples[odrSamples.length - 1].s;
|
|
52
|
-
return { laneId: lane.id, samples, odrSamples, length };
|
|
53
|
-
}
|
|
54
36
|
function collectPoints(shapeMap, pointIds, invert, pointOverrides) {
|
|
55
37
|
const ids = invert ? [...pointIds].reverse() : pointIds;
|
|
56
38
|
const pts = [];
|
|
@@ -66,6 +48,263 @@ function collectPoints(shapeMap, pointIds, invert, pointOverrides) {
|
|
|
66
48
|
}
|
|
67
49
|
return pts;
|
|
68
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
|
+
}
|
|
69
308
|
/**
|
|
70
309
|
* Snap together boundary endpoints of connected lanes by clustering nearby
|
|
71
310
|
* points and using the centroid as the canonical position.
|
|
@@ -96,7 +335,14 @@ function buildBoundaryAlignmentOverrides(shapeMap, lanes, epsilonPx = 30) {
|
|
|
96
335
|
const pt = shapeMap.get(pid);
|
|
97
336
|
if (!pt)
|
|
98
337
|
continue;
|
|
99
|
-
endpoints.push({
|
|
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
|
+
});
|
|
100
346
|
}
|
|
101
347
|
};
|
|
102
348
|
// Restrict to lanes that participate in a next/prev relationship.
|
|
@@ -108,35 +354,91 @@ function buildBoundaryAlignmentOverrides(shapeMap, lanes, epsilonPx = 30) {
|
|
|
108
354
|
if (hasPrev)
|
|
109
355
|
collectEndpoints(lane, 'start');
|
|
110
356
|
}
|
|
111
|
-
//
|
|
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
|
|
112
406
|
// refinements over plain first-fit grouping:
|
|
113
|
-
// 1. An endpoint never joins a cluster
|
|
114
|
-
//
|
|
115
|
-
// otherwise get its start and end merged into one cluster, collapsing
|
|
116
|
-
// its centerline below the degenerate-road export guard and silently
|
|
117
|
-
// dropping the lane (and its next/prev chain).
|
|
407
|
+
// 1. An endpoint never joins a cluster holding a point its point id is
|
|
408
|
+
// forbidden against (see above).
|
|
118
409
|
// 2. Among the eligible clusters the nearest one wins, so the far end of a
|
|
119
410
|
// short lane clusters with its true counterpart rather than with the
|
|
120
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.
|
|
121
417
|
const clusters = [];
|
|
122
418
|
const eps2 = epsilonPx * epsilonPx;
|
|
123
419
|
for (const ep of endpoints) {
|
|
124
420
|
let best = null;
|
|
125
421
|
let bestD2 = Infinity;
|
|
422
|
+
let blockedD2 = Infinity;
|
|
423
|
+
const epForbidden = forbidden.get(ep.pointId);
|
|
126
424
|
for (const cluster of clusters) {
|
|
127
425
|
const c0 = cluster[0];
|
|
128
426
|
const dx = ep.x - c0.x;
|
|
129
427
|
const dy = ep.y - c0.y;
|
|
130
428
|
const d2 = dx * dx + dy * dy;
|
|
131
|
-
if (d2 > eps2
|
|
429
|
+
if (d2 > eps2)
|
|
430
|
+
continue;
|
|
431
|
+
if (epForbidden && cluster.some((m) => epForbidden.has(m.pointId))) {
|
|
432
|
+
if (d2 < blockedD2)
|
|
433
|
+
blockedD2 = d2;
|
|
132
434
|
continue;
|
|
133
|
-
|
|
134
|
-
if (
|
|
435
|
+
}
|
|
436
|
+
if (d2 >= bestD2)
|
|
135
437
|
continue;
|
|
136
438
|
best = cluster;
|
|
137
439
|
bestD2 = d2;
|
|
138
440
|
}
|
|
139
|
-
if (best)
|
|
441
|
+
if (best && bestD2 <= blockedD2)
|
|
140
442
|
best.push(ep);
|
|
141
443
|
else
|
|
142
444
|
clusters.push([ep]);
|
|
@@ -164,17 +466,20 @@ function buildBoundaryAlignmentOverrides(shapeMap, lanes, epsilonPx = 30) {
|
|
|
164
466
|
function emitPlanView(geom) {
|
|
165
467
|
const lines = [];
|
|
166
468
|
lines.push(` <planView>`);
|
|
167
|
-
for (
|
|
168
|
-
|
|
169
|
-
const b = geom.odrSamples[i + 1];
|
|
170
|
-
const dx = b.x - a.x;
|
|
171
|
-
const dy = b.y - a.y;
|
|
172
|
-
const segLen = Math.hypot(dx, dy);
|
|
173
|
-
if (segLen < 1e-9)
|
|
469
|
+
for (const g of geom.planView) {
|
|
470
|
+
if (g.length < 1e-9)
|
|
174
471
|
continue;
|
|
175
|
-
|
|
176
|
-
|
|
177
|
-
|
|
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
|
+
}
|
|
178
483
|
lines.push(` </geometry>`);
|
|
179
484
|
}
|
|
180
485
|
lines.push(` </planView>`);
|
|
@@ -211,93 +516,240 @@ function roadMarkTypeFor(shapeMap, boundaryId) {
|
|
|
211
516
|
const ls = shapeMap.get(boundaryId);
|
|
212
517
|
return ls?.props?.attributes?.subtype === 'dashed' ? 'broken' : 'solid';
|
|
213
518
|
}
|
|
214
|
-
function emitLanes(
|
|
519
|
+
function emitLanes(bundle, plan, shapeMap) {
|
|
520
|
+
const geom = bundle.geom;
|
|
215
521
|
const lines = [];
|
|
216
522
|
lines.push(` <lanes>`);
|
|
217
|
-
// The plan view follows the
|
|
218
|
-
//
|
|
219
|
-
//
|
|
220
|
-
//
|
|
221
|
-
emitLaneOffsetEntries(geom, lines);
|
|
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.
|
|
222
527
|
lines.push(` <laneSection s="0">`);
|
|
223
|
-
// Center reference line (= left boundary after the laneOffset shift).
|
|
224
528
|
lines.push(` <center>`);
|
|
225
529
|
lines.push(` <lane id="0" type="none" level="false">`);
|
|
226
530
|
lines.push(` <link/>`);
|
|
531
|
+
const centerMark = roadMarkTypeFor(shapeMap, bundle.lanes[0].props.leftBoundaryId);
|
|
227
532
|
lines.push(` <roadMark sOffset="0" type="${centerMark}" weight="standard" color="white" width="0.13"/>`);
|
|
228
533
|
lines.push(` </lane>`);
|
|
229
534
|
lines.push(` </center>`);
|
|
230
535
|
lines.push(` <right>`);
|
|
231
|
-
|
|
232
|
-
|
|
233
|
-
|
|
234
|
-
|
|
235
|
-
|
|
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
|
+
});
|
|
236
545
|
lines.push(` </right>`);
|
|
237
546
|
lines.push(` </laneSection>`);
|
|
238
547
|
lines.push(` </lanes>`);
|
|
239
548
|
return lines.join('\n');
|
|
240
549
|
}
|
|
241
|
-
function emitLaneLink(out,
|
|
242
|
-
if (
|
|
550
|
+
function emitLaneLink(out, predId, succId) {
|
|
551
|
+
if (predId === undefined && succId === undefined) {
|
|
243
552
|
out.push(` <link/>`);
|
|
244
553
|
return;
|
|
245
554
|
}
|
|
246
555
|
out.push(` <link>`);
|
|
247
|
-
if (
|
|
248
|
-
out.push(` <predecessor id="${
|
|
556
|
+
if (predId !== undefined) {
|
|
557
|
+
out.push(` <predecessor id="${predId}"/>`);
|
|
249
558
|
}
|
|
250
|
-
if (
|
|
251
|
-
out.push(` <successor id="${
|
|
559
|
+
if (succId !== undefined) {
|
|
560
|
+
out.push(` <successor id="${succId}"/>`);
|
|
252
561
|
}
|
|
253
562
|
out.push(` </link>`);
|
|
254
563
|
}
|
|
255
|
-
/**
|
|
256
|
-
|
|
257
|
-
|
|
258
|
-
|
|
259
|
-
|
|
260
|
-
|
|
261
|
-
|
|
262
|
-
|
|
263
|
-
|
|
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
|
+
}
|
|
264
589
|
}
|
|
265
|
-
|
|
266
|
-
|
|
267
|
-
|
|
268
|
-
for (let i = 0; i < geom.odrSamples.length - 1; i++) {
|
|
269
|
-
const s = geom.odrSamples[i].s;
|
|
270
|
-
const wA = geom.odrSamples[i].width;
|
|
271
|
-
const wB = geom.odrSamples[i + 1].width;
|
|
272
|
-
const segLen = geom.odrSamples[i + 1].s - s;
|
|
273
|
-
const b = segLen > 1e-9 ? (wB - wA) / segLen : 0;
|
|
274
|
-
out.push(` <width sOffset="${fmt(s)}" a="${fmt(wA)}" b="${fmt(b)}" c="0" d="0"/>`);
|
|
590
|
+
const recs = [];
|
|
591
|
+
if (sArr.length < 2) {
|
|
592
|
+
recs.push({ s: 0, a: wArr[0] ?? 0, b: 0 });
|
|
275
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"/>`);
|
|
615
|
+
}
|
|
616
|
+
}
|
|
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;
|
|
276
640
|
}
|
|
277
641
|
/**
|
|
278
|
-
* Plan synthesized <junction> elements
|
|
642
|
+
* Plan road links, lane links and synthesized <junction> elements.
|
|
279
643
|
*
|
|
280
|
-
* A road <link> can name only one predecessor and one successor, so
|
|
281
|
-
* is representable as plain road
|
|
282
|
-
*
|
|
283
|
-
*
|
|
284
|
-
*
|
|
285
|
-
*
|
|
286
|
-
*
|
|
287
|
-
*
|
|
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
|
|
288
655
|
* junction (connected components), so a 2-in x 2-out diamond becomes one
|
|
289
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.
|
|
290
661
|
*/
|
|
291
|
-
function
|
|
662
|
+
function planConnectivity(exportBundles, roadIdOf, odrIdOf, firstJunctionId, connectingSourceFor, connectingTargetFor, contactWidth, externalLanes = new Map()) {
|
|
292
663
|
const validNext = new Map();
|
|
293
664
|
const validPrev = new Map();
|
|
294
|
-
for (const
|
|
295
|
-
|
|
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)
|
|
296
687
|
continue;
|
|
297
|
-
|
|
298
|
-
|
|
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
|
+
}
|
|
299
750
|
}
|
|
300
|
-
// Union-find over
|
|
751
|
+
// Union-find over road ids: junction-routed pairs sharing a road merge into
|
|
752
|
+
// one junction.
|
|
301
753
|
const parent = new Map();
|
|
302
754
|
const find = (x) => {
|
|
303
755
|
let root = x;
|
|
@@ -325,66 +777,234 @@ function planJunctions(lanes, laneIdToRoadId, firstJunctionId) {
|
|
|
325
777
|
if (ra !== rb)
|
|
326
778
|
parent.set(rb, ra);
|
|
327
779
|
};
|
|
328
|
-
const
|
|
329
|
-
|
|
330
|
-
|
|
331
|
-
|
|
332
|
-
if (nexts.length === 1 && prevsOfTarget.length === 1)
|
|
333
|
-
continue; // plain road link
|
|
334
|
-
junctionEdges.push({ from: laneId, to });
|
|
335
|
-
union(laneId, to);
|
|
336
|
-
}
|
|
337
|
-
}
|
|
338
|
-
const plan = {
|
|
339
|
-
nextJunction: new Map(),
|
|
340
|
-
prevJunction: new Map(),
|
|
341
|
-
roadJunction: new Map(),
|
|
342
|
-
junctions: [],
|
|
343
|
-
};
|
|
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).
|
|
344
784
|
const junctionByRoot = new Map();
|
|
345
785
|
let nextId = firstJunctionId;
|
|
346
|
-
for (const
|
|
347
|
-
const root = find(
|
|
348
|
-
|
|
349
|
-
|
|
350
|
-
junction = { id: nextId++, connections: [] };
|
|
786
|
+
for (const pair of junctionPairs) {
|
|
787
|
+
const root = find(pair.incoming);
|
|
788
|
+
if (!junctionByRoot.has(root)) {
|
|
789
|
+
const junction = { id: nextId++, connections: [], priorities: [] };
|
|
351
790
|
junctionByRoot.set(root, junction);
|
|
352
791
|
plan.junctions.push(junction);
|
|
353
792
|
}
|
|
354
|
-
|
|
355
|
-
|
|
356
|
-
|
|
357
|
-
|
|
358
|
-
|
|
359
|
-
|
|
360
|
-
|
|
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
|
+
}
|
|
361
855
|
}
|
|
362
856
|
return plan;
|
|
363
857
|
}
|
|
364
|
-
|
|
365
|
-
|
|
366
|
-
|
|
367
|
-
|
|
368
|
-
|
|
369
|
-
|
|
370
|
-
|
|
371
|
-
|
|
372
|
-
|
|
373
|
-
|
|
374
|
-
|
|
375
|
-
|
|
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
|
+
}
|
|
376
945
|
}
|
|
377
946
|
}
|
|
378
|
-
|
|
379
|
-
|
|
380
|
-
|
|
947
|
+
if (!geometry) {
|
|
948
|
+
geometry =
|
|
949
|
+
Math.abs(dHdg) > 1e-4
|
|
950
|
+
? ` <arc curvature="${fmtPrecise(dHdg / len)}"/>`
|
|
951
|
+
: ` <line/>`;
|
|
381
952
|
}
|
|
382
|
-
|
|
383
|
-
|
|
384
|
-
|
|
385
|
-
|
|
386
|
-
|
|
387
|
-
|
|
953
|
+
const widthSlope = spec.target !== null && Math.abs(spec.target.width - width) > 1e-6
|
|
954
|
+
? (spec.target.width - width) / len
|
|
955
|
+
: 0;
|
|
956
|
+
const lines = [];
|
|
957
|
+
lines.push(` <road name="connecting" length="${fmt(len)}" id="${spec.roadId}" junction="${spec.junctionId}">`);
|
|
958
|
+
lines.push(` <link>`);
|
|
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"/>`);
|
|
1002
|
+
}
|
|
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"/>`);
|
|
388
1008
|
}
|
|
389
1009
|
lines.push(` </link>`);
|
|
390
1010
|
return lines.join('\n');
|
|
@@ -395,7 +1015,9 @@ function projectToRoad(geom, xG, yG) {
|
|
|
395
1015
|
let bestDist = Infinity;
|
|
396
1016
|
let bestHdg = 0;
|
|
397
1017
|
let bestClamped = false;
|
|
398
|
-
|
|
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;
|
|
399
1021
|
for (let i = 0; i < samples.length - 1; i++) {
|
|
400
1022
|
const a = samples[i];
|
|
401
1023
|
const b = samples[i + 1];
|
|
@@ -423,7 +1045,7 @@ function projectToRoad(geom, xG, yG) {
|
|
|
423
1045
|
const dist = Math.hypot(xG - projX, yG - projY);
|
|
424
1046
|
if (dist < bestDist) {
|
|
425
1047
|
bestDist = dist;
|
|
426
|
-
bestS = a.s + tNorm;
|
|
1048
|
+
bestS = a.s + (tNorm / segLen) * (b.s - a.s);
|
|
427
1049
|
const nx = -uy;
|
|
428
1050
|
const ny = ux;
|
|
429
1051
|
bestT = px * nx + py * ny;
|
|
@@ -433,39 +1055,64 @@ function projectToRoad(geom, xG, yG) {
|
|
|
433
1055
|
}
|
|
434
1056
|
return { s: bestS, t: bestT, hdg: bestHdg, distance: bestDist, clampedAtEnd: bestClamped };
|
|
435
1057
|
}
|
|
436
|
-
|
|
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) {
|
|
437
1080
|
const roadSignals = new Map();
|
|
438
1081
|
const roadObjects = new Map();
|
|
439
1082
|
const roadSignalRefs = new Map();
|
|
440
1083
|
const signalIdByShape = new Map();
|
|
441
|
-
let signalIdCounter =
|
|
1084
|
+
let signalIdCounter = signalIdStart;
|
|
442
1085
|
let objectIdCounter = 1;
|
|
443
|
-
const roads = [];
|
|
444
|
-
laneToGeom.forEach((geom, laneId) => {
|
|
445
|
-
const roadId = laneIdToRoadId.get(laneId);
|
|
446
|
-
if (roadId !== undefined)
|
|
447
|
-
roads.push({ laneId, geom, roadId });
|
|
448
|
-
});
|
|
449
1086
|
const geomByRoadId = new Map();
|
|
450
1087
|
for (const r of roads)
|
|
451
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
|
+
};
|
|
452
1104
|
for (const tl of trafficLights) {
|
|
453
1105
|
const xG = pxToEnuX(tl.x);
|
|
454
1106
|
const yG = pxToEnuY(tl.y);
|
|
455
1107
|
// Regulatory layer: a signal that names affected lanes attaches to the
|
|
456
|
-
// nearest of those lanes' roads and carries
|
|
457
|
-
//
|
|
458
|
-
//
|
|
459
|
-
const
|
|
460
|
-
for (const laneShapeId of tl.props.affectedLaneIds ?? []) {
|
|
461
|
-
const rid = laneIdToRoadId.get(laneShapeId);
|
|
462
|
-
if (rid !== undefined)
|
|
463
|
-
affectedRoadIds.add(rid);
|
|
464
|
-
}
|
|
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);
|
|
465
1112
|
let best = null;
|
|
466
|
-
if (
|
|
1113
|
+
if (affectedByRoad.size > 0) {
|
|
467
1114
|
for (const r of roads) {
|
|
468
|
-
if (!
|
|
1115
|
+
if (!affectedByRoad.has(r.roadId))
|
|
469
1116
|
continue;
|
|
470
1117
|
const proj = projectToRoad(r.geom, xG, yG);
|
|
471
1118
|
if (!best || proj.distance < best.proj.distance)
|
|
@@ -504,20 +1151,19 @@ function attachShapesToRoads(shapeMap, trafficLights, crosswalks, polygons, lane
|
|
|
504
1151
|
dynamic: 'yes',
|
|
505
1152
|
orientation: best.proj.t >= 0 ? '+' : '-',
|
|
506
1153
|
};
|
|
507
|
-
if (
|
|
508
|
-
|
|
509
|
-
entry.validity = { fromLane: -1, toLane: -1 };
|
|
1154
|
+
if (affectedByRoad.size > 0) {
|
|
1155
|
+
entry.validity = laneIdRanges(affectedByRoad.get(best.roadId));
|
|
510
1156
|
}
|
|
511
1157
|
list.push(entry);
|
|
512
1158
|
roadSignals.set(best.roadId, list);
|
|
513
1159
|
signalIdByShape.set(tl.id, entry.id);
|
|
514
|
-
// A signal controlling
|
|
515
|
-
//
|
|
516
|
-
// <signalReference> pointing back at the signal
|
|
517
|
-
//
|
|
518
|
-
if (
|
|
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) {
|
|
519
1165
|
for (const r of roads) {
|
|
520
|
-
if (r.roadId === best.roadId || !
|
|
1166
|
+
if (r.roadId === best.roadId || !affectedByRoad.has(r.roadId))
|
|
521
1167
|
continue;
|
|
522
1168
|
const proj = projectToRoad(r.geom, xG, yG);
|
|
523
1169
|
const refs = roadSignalRefs.get(r.roadId) ?? [];
|
|
@@ -526,6 +1172,7 @@ function attachShapesToRoads(shapeMap, trafficLights, crosswalks, polygons, lane
|
|
|
526
1172
|
s: proj.s,
|
|
527
1173
|
t: proj.t,
|
|
528
1174
|
orientation: proj.t >= 0 ? '+' : '-',
|
|
1175
|
+
validity: laneIdRanges(affectedByRoad.get(r.roadId)),
|
|
529
1176
|
});
|
|
530
1177
|
roadSignalRefs.set(r.roadId, refs);
|
|
531
1178
|
}
|
|
@@ -581,16 +1228,11 @@ function attachShapesToRoads(shapeMap, trafficLights, crosswalks, polygons, lane
|
|
|
581
1228
|
// Regulatory layer: a crosswalk that names affected lanes attaches to the
|
|
582
1229
|
// nearest of those lanes' roads (no distance gate — the assignment is
|
|
583
1230
|
// explicit), mirroring the signal behavior above.
|
|
584
|
-
const
|
|
585
|
-
for (const laneShapeId of cw.props.affectedLaneIds ?? []) {
|
|
586
|
-
const rid = laneIdToRoadId.get(laneShapeId);
|
|
587
|
-
if (rid !== undefined)
|
|
588
|
-
affectedRoadIds.add(rid);
|
|
589
|
-
}
|
|
1231
|
+
const affectedByRoad = affectedLanesByRoad(cw.props.affectedLaneIds);
|
|
590
1232
|
let best = null;
|
|
591
|
-
if (
|
|
1233
|
+
if (affectedByRoad.size > 0) {
|
|
592
1234
|
for (const r of roads) {
|
|
593
|
-
if (!
|
|
1235
|
+
if (!affectedByRoad.has(r.roadId))
|
|
594
1236
|
continue;
|
|
595
1237
|
const proj = projectToRoad(r.geom, xG, yG);
|
|
596
1238
|
if (!best || proj.distance < best.proj.distance)
|
|
@@ -628,13 +1270,19 @@ function attachShapesToRoads(shapeMap, trafficLights, crosswalks, polygons, lane
|
|
|
628
1270
|
// π/2 regardless of the user-drawn axis direction.
|
|
629
1271
|
void relativeHdg;
|
|
630
1272
|
const crosswalkHdg = Math.PI / 2;
|
|
631
|
-
// Regulatory links (affected
|
|
1273
|
+
// Regulatory links (affected lanes + stop line polyline) ride along as
|
|
632
1274
|
// <userData> — OpenDRIVE's standard extension mechanism — so they survive
|
|
633
1275
|
// an .xodr round trip. Coordinates are ENU meters.
|
|
634
1276
|
const userData = [];
|
|
635
|
-
if (
|
|
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
|
+
}
|
|
636
1284
|
const links = {
|
|
637
|
-
|
|
1285
|
+
affectedLanes,
|
|
638
1286
|
};
|
|
639
1287
|
if (cw.props.stopLineId) {
|
|
640
1288
|
const stopLs = shapeMap.get(cw.props.stopLineId);
|
|
@@ -682,12 +1330,13 @@ function attachShapesToRoads(shapeMap, trafficLights, crosswalks, polygons, lane
|
|
|
682
1330
|
for (const r of roads) {
|
|
683
1331
|
const proj = projectToRoad(r.geom, xG, yG);
|
|
684
1332
|
if (proj.clampedAtEnd) {
|
|
685
|
-
if (!fallback || proj.distance < fallback.proj.distance)
|
|
686
|
-
fallback = { roadId: r.roadId, proj };
|
|
1333
|
+
if (!fallback || proj.distance < fallback.proj.distance) {
|
|
1334
|
+
fallback = { roadId: r.roadId, geom: r.geom, proj };
|
|
1335
|
+
}
|
|
687
1336
|
continue;
|
|
688
1337
|
}
|
|
689
1338
|
if (!best || proj.distance < best.proj.distance)
|
|
690
|
-
best = { roadId: r.roadId, proj };
|
|
1339
|
+
best = { roadId: r.roadId, geom: r.geom, proj };
|
|
691
1340
|
}
|
|
692
1341
|
if (!best)
|
|
693
1342
|
best = fallback;
|
|
@@ -695,7 +1344,7 @@ function attachShapesToRoads(shapeMap, trafficLights, crosswalks, polygons, lane
|
|
|
695
1344
|
continue;
|
|
696
1345
|
const cosH = Math.cos(best.proj.hdg);
|
|
697
1346
|
const sinH = Math.sin(best.proj.hdg);
|
|
698
|
-
const samples =
|
|
1347
|
+
const samples = best.geom.samplePoses;
|
|
699
1348
|
const anchorPoint = best.proj.clampedAtEnd && best.proj.s >= samples[samples.length - 1].s - 1e-6
|
|
700
1349
|
? samples[samples.length - 1]
|
|
701
1350
|
: samples[0];
|
|
@@ -741,10 +1390,10 @@ function emitSignals(signals, references) {
|
|
|
741
1390
|
lines.push(` <signals>`);
|
|
742
1391
|
for (const s of signals) {
|
|
743
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)}"`;
|
|
744
|
-
if (s.validity || s.stopLinePoints) {
|
|
1393
|
+
if (s.validity?.length || s.stopLinePoints) {
|
|
745
1394
|
lines.push(` <signal ${attrs}>`);
|
|
746
|
-
|
|
747
|
-
lines.push(` <validity fromLane="${
|
|
1395
|
+
for (const v of s.validity ?? []) {
|
|
1396
|
+
lines.push(` <validity fromLane="${v.fromLane}" toLane="${v.toLane}"/>`);
|
|
748
1397
|
}
|
|
749
1398
|
if (s.stopLinePoints) {
|
|
750
1399
|
const json = JSON.stringify(s.stopLinePoints.map((p) => [roundMm(p.x), roundMm(p.y)]));
|
|
@@ -758,7 +1407,9 @@ function emitSignals(signals, references) {
|
|
|
758
1407
|
}
|
|
759
1408
|
for (const ref of references) {
|
|
760
1409
|
lines.push(` <signalReference s="${fmt(ref.s)}" t="${fmt(ref.t)}" id="${ref.id}" orientation="${ref.orientation}">`);
|
|
761
|
-
|
|
1410
|
+
for (const v of ref.validity) {
|
|
1411
|
+
lines.push(` <validity fromLane="${v.fromLane}" toLane="${v.toLane}"/>`);
|
|
1412
|
+
}
|
|
762
1413
|
lines.push(` </signalReference>`);
|
|
763
1414
|
}
|
|
764
1415
|
lines.push(` </signals>`);
|
|
@@ -807,40 +1458,86 @@ function emitObjects(objects) {
|
|
|
807
1458
|
/**
|
|
808
1459
|
* Lane attributes that have no OpenDRIVE representation (speed_limit only
|
|
809
1460
|
* partially maps, one_way / turn_direction / location and custom tags not at
|
|
810
|
-
* all) are stashed
|
|
811
|
-
*
|
|
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.
|
|
812
1464
|
* `odr_*` meta attributes are excluded: they are regenerated on import.
|
|
813
1465
|
*/
|
|
814
|
-
function emitLaneAttributesUserData(
|
|
815
|
-
const
|
|
816
|
-
|
|
817
|
-
|
|
818
|
-
|
|
819
|
-
|
|
820
|
-
|
|
821
|
-
|
|
822
|
-
|
|
823
|
-
|
|
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)
|
|
824
1481
|
return null;
|
|
825
|
-
return ` <userData code="laneAttributes" value="${escapeXml(JSON.stringify(
|
|
1482
|
+
return ` <userData code="laneAttributes" value="${escapeXml(JSON.stringify(byLane))}"/>`;
|
|
826
1483
|
}
|
|
827
1484
|
/**
|
|
828
|
-
* Right-of-way links (`yieldLaneIds`)
|
|
829
|
-
*
|
|
830
|
-
*
|
|
831
|
-
* <userData code="
|
|
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.
|
|
832
1490
|
*/
|
|
833
|
-
function
|
|
834
|
-
const
|
|
835
|
-
|
|
836
|
-
const
|
|
837
|
-
|
|
838
|
-
|
|
839
|
-
|
|
840
|
-
|
|
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)
|
|
841
1512
|
return null;
|
|
842
|
-
|
|
843
|
-
|
|
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))}"/>`;
|
|
844
1541
|
}
|
|
845
1542
|
/**
|
|
846
1543
|
* Road length attribute. The plan-view geometries are emitted with rounded
|
|
@@ -850,51 +1547,327 @@ function emitYieldRoadsUserData(lane, laneIdToRoadId) {
|
|
|
850
1547
|
*/
|
|
851
1548
|
function emittedRoadLength(geom) {
|
|
852
1549
|
let extent = geom.length;
|
|
853
|
-
for (
|
|
854
|
-
const
|
|
855
|
-
const b = geom.odrSamples[i + 1];
|
|
856
|
-
const segLen = Math.hypot(b.x - a.x, b.y - a.y);
|
|
857
|
-
if (segLen < 1e-9)
|
|
858
|
-
continue;
|
|
859
|
-
const end = parseFloat(fmt(a.s)) + parseFloat(fmt(segLen));
|
|
1550
|
+
for (const g of geom.planView) {
|
|
1551
|
+
const end = parseFloat(fmt(g.s)) + parseFloat(fmt(g.length));
|
|
860
1552
|
if (end > extent)
|
|
861
1553
|
extent = end;
|
|
862
1554
|
}
|
|
863
1555
|
return extent + 1e-4;
|
|
864
1556
|
}
|
|
865
|
-
function emitRoad(
|
|
866
|
-
const
|
|
867
|
-
const
|
|
868
|
-
const
|
|
869
|
-
const hasNext = (lane.props.next ?? []).some((id) => laneIdToRoadId.has(id));
|
|
870
|
-
const junctionAttr = junctionPlan.roadJunction.get(lane.id) ?? -1;
|
|
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');
|
|
871
1561
|
const lines = [];
|
|
872
|
-
|
|
873
|
-
|
|
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));
|
|
874
1566
|
if (speed) {
|
|
875
1567
|
lines.push(` <type s="0" type="town">`);
|
|
876
1568
|
lines.push(` <speed max="${escapeXml(speed)}" unit="km/h"/>`);
|
|
877
1569
|
lines.push(` </type>`);
|
|
878
1570
|
}
|
|
879
|
-
lines.push(emitPlanView(geom));
|
|
1571
|
+
lines.push(emitPlanView(bundle.geom));
|
|
880
1572
|
lines.push(` <elevationProfile/>`);
|
|
881
1573
|
lines.push(` <lateralProfile/>`);
|
|
882
|
-
lines.push(emitLanes(
|
|
1574
|
+
lines.push(emitLanes(bundle, plan, shapeMap));
|
|
883
1575
|
lines.push(emitObjects(objects));
|
|
884
1576
|
lines.push(emitSignals(signals, signalRefs));
|
|
885
|
-
const userData = emitLaneAttributesUserData(
|
|
1577
|
+
const userData = emitLaneAttributesUserData(bundle.lanes);
|
|
886
1578
|
if (userData)
|
|
887
1579
|
lines.push(userData);
|
|
888
|
-
const yieldUserData =
|
|
1580
|
+
const yieldUserData = emitYieldLanesUserData(bundle.lanes, laneIdToRoadId, laneIdToOdrLaneId, plan.handledYieldPairs);
|
|
889
1581
|
if (yieldUserData)
|
|
890
1582
|
lines.push(yieldUserData);
|
|
1583
|
+
const hiddenLinksUserData = emitHiddenLinksUserData(bundle.lanes, plan, laneIdToRoadId, laneIdToOdrLaneId);
|
|
1584
|
+
if (hiddenLinksUserData)
|
|
1585
|
+
lines.push(hiddenLinksUserData);
|
|
891
1586
|
lines.push(` </road>`);
|
|
892
1587
|
return lines.join('\n');
|
|
893
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
|
+
}
|
|
894
1864
|
/**
|
|
895
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.
|
|
896
1869
|
*/
|
|
897
|
-
export function exportToOpenDrive(snapshot) {
|
|
1870
|
+
export function exportToOpenDrive(snapshot, options = {}) {
|
|
898
1871
|
const shapes = snapshot.shapes;
|
|
899
1872
|
const shapeMap = buildShapeMap(shapes);
|
|
900
1873
|
const lanes = [];
|
|
@@ -920,54 +1893,296 @@ export function exportToOpenDrive(snapshot) {
|
|
|
920
1893
|
polygons.push({ shape: poly, vertices });
|
|
921
1894
|
}
|
|
922
1895
|
}
|
|
923
|
-
//
|
|
924
|
-
//
|
|
925
|
-
const
|
|
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;
|
|
926
1906
|
const dateStr = new Date().toISOString();
|
|
927
1907
|
const bbox = computeEnuBoundingBox(shapeMap);
|
|
928
1908
|
const geoRefProj = originToProjString(snapshot.origin);
|
|
929
1909
|
const lines = [];
|
|
930
1910
|
lines.push(`<?xml version="1.0" encoding="UTF-8"?>`);
|
|
931
1911
|
lines.push(`<OpenDRIVE>`);
|
|
932
|
-
|
|
933
|
-
|
|
934
|
-
|
|
935
|
-
|
|
936
|
-
|
|
937
|
-
|
|
938
|
-
|
|
939
|
-
|
|
940
|
-
|
|
941
|
-
|
|
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
|
+
}
|
|
942
1929
|
const pointOverrides = buildBoundaryAlignmentOverrides(shapeMap, lanes);
|
|
943
|
-
|
|
944
|
-
|
|
945
|
-
|
|
946
|
-
|
|
947
|
-
|
|
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
|
+
}
|
|
948
1947
|
}
|
|
949
1948
|
}
|
|
950
|
-
|
|
951
|
-
|
|
952
|
-
|
|
953
|
-
|
|
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
|
+
}
|
|
954
1967
|
}
|
|
955
|
-
const
|
|
956
|
-
const
|
|
957
|
-
|
|
958
|
-
|
|
959
|
-
|
|
960
|
-
|
|
961
|
-
const
|
|
962
|
-
|
|
963
|
-
|
|
964
|
-
const
|
|
965
|
-
|
|
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);
|
|
966
2181
|
}
|
|
967
2182
|
// Signal groups: traffic lights sharing a controllerId (one intersection)
|
|
968
2183
|
// become a <controller> listing their emitted signals as <control> records.
|
|
969
2184
|
const controllerGroups = new Map();
|
|
970
|
-
for (const tl of
|
|
2185
|
+
for (const tl of regenTrafficLights) {
|
|
971
2186
|
const groupId = tl.props.controllerId;
|
|
972
2187
|
if (!groupId)
|
|
973
2188
|
continue;
|
|
@@ -978,7 +2193,7 @@ export function exportToOpenDrive(snapshot) {
|
|
|
978
2193
|
group.push(signalId);
|
|
979
2194
|
controllerGroups.set(groupId, group);
|
|
980
2195
|
}
|
|
981
|
-
let controllerIdCounter = 1;
|
|
2196
|
+
let controllerIdCounter = carry ? carry.controllerIdBase : 1;
|
|
982
2197
|
for (const [groupId, signalIds] of controllerGroups) {
|
|
983
2198
|
lines.push(` <controller id="${controllerIdCounter++}" name="${escapeXml(groupId)}" sequence="0">`);
|
|
984
2199
|
for (const signalId of signalIds) {
|
|
@@ -986,14 +2201,24 @@ export function exportToOpenDrive(snapshot) {
|
|
|
986
2201
|
}
|
|
987
2202
|
lines.push(` </controller>`);
|
|
988
2203
|
}
|
|
989
|
-
//
|
|
990
|
-
|
|
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) {
|
|
991
2211
|
lines.push(` <junction id="${junction.id}" name="junction${junction.id}">`);
|
|
992
2212
|
junction.connections.forEach((conn, idx) => {
|
|
993
2213
|
lines.push(` <connection id="${idx}" incomingRoad="${conn.incoming}" connectingRoad="${conn.connecting}" contactPoint="start">`);
|
|
994
|
-
|
|
2214
|
+
for (const ll of conn.laneLinks) {
|
|
2215
|
+
lines.push(` <laneLink from="${ll.from}" to="${ll.to}"/>`);
|
|
2216
|
+
}
|
|
995
2217
|
lines.push(` </connection>`);
|
|
996
2218
|
});
|
|
2219
|
+
for (const pr of junction.priorities) {
|
|
2220
|
+
lines.push(` <priority high="${pr.high}" low="${pr.low}"/>`);
|
|
2221
|
+
}
|
|
997
2222
|
lines.push(` </junction>`);
|
|
998
2223
|
}
|
|
999
2224
|
lines.push(`</OpenDRIVE>`);
|