@drawtonomy/sdk 0.8.0 → 0.9.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 +4 -1
- package/dist/exporter/index.d.ts.map +1 -1
- package/dist/exporter/index.js +3 -0
- package/dist/exporter/index.js.map +1 -1
- package/dist/exporter/lanelet2.d.ts.map +1 -1
- package/dist/exporter/lanelet2.js +434 -2
- package/dist/exporter/lanelet2.js.map +1 -1
- package/dist/exporter/odrGeometry.d.ts +64 -0
- package/dist/exporter/odrGeometry.d.ts.map +1 -0
- package/dist/exporter/odrGeometry.js +324 -0
- package/dist/exporter/odrGeometry.js.map +1 -0
- package/dist/exporter/odrToShapes.d.ts +45 -0
- package/dist/exporter/odrToShapes.d.ts.map +1 -0
- package/dist/exporter/odrToShapes.js +1370 -0
- package/dist/exporter/odrToShapes.js.map +1 -0
- package/dist/exporter/opendrive.d.ts.map +1 -1
- package/dist/exporter/opendrive.js +493 -68
- package/dist/exporter/opendrive.js.map +1 -1
- package/dist/exporter/opendriveParser.d.ts +186 -0
- package/dist/exporter/opendriveParser.d.ts.map +1 -0
- package/dist/exporter/opendriveParser.js +426 -0
- package/dist/exporter/opendriveParser.js.map +1 -0
- package/dist/exporter/osmToShapes.d.ts +52 -3
- package/dist/exporter/osmToShapes.d.ts.map +1 -1
- package/dist/exporter/osmToShapes.js +208 -5
- package/dist/exporter/osmToShapes.js.map +1 -1
- package/dist/exporter/projection.d.ts +1 -1
- package/dist/exporter/projection.d.ts.map +1 -1
- package/dist/exporter/projection.js +2 -3
- package/dist/exporter/projection.js.map +1 -1
- package/dist/types.d.ts +27 -1
- package/dist/types.d.ts.map +1 -1
- package/package.json +1 -1
|
@@ -0,0 +1,1370 @@
|
|
|
1
|
+
// Convert a parsed OpenDRIVE model (`OdrMap`) into the shape primitives that
|
|
2
|
+
// the drawtonomy editor consumes (points, linestrings, lanes, traffic lights).
|
|
3
|
+
//
|
|
4
|
+
// Mirrors the Lanelet2 OSM import path (`osmToShapes`): the output is the
|
|
5
|
+
// same intermediate `ImportedShapes` structure, extended with a sidecar (the
|
|
6
|
+
// original XML plus the derived geographic origin) and a list of warnings for
|
|
7
|
+
// features that were parsed but flattened or ignored.
|
|
8
|
+
//
|
|
9
|
+
// Geometry pipeline per road:
|
|
10
|
+
// 1. Adaptively sample the reference line (line/arc/spiral/paramPoly3).
|
|
11
|
+
// 2. At each station, shift by the laneOffset polynomial along the normal
|
|
12
|
+
// to obtain the lane reference ("center") polyline.
|
|
13
|
+
// 3. Accumulate lane widths outward (left lanes +t, right lanes -t) to get
|
|
14
|
+
// every lane boundary polyline in meters.
|
|
15
|
+
// 4. Convert meters -> pixels and ENU (y up) -> canvas (y down), the exact
|
|
16
|
+
// inverse of the OpenDRIVE exporter's conversion, so that
|
|
17
|
+
// import -> export is near-identity.
|
|
18
|
+
//
|
|
19
|
+
// Boundary sharing: adjacent lanes within one road/section share a single
|
|
20
|
+
// boundary linestring (lane -1's outer boundary IS lane -2's inner boundary),
|
|
21
|
+
// and the center boundary is shared between lane +1 and lane -1.
|
|
22
|
+
//
|
|
23
|
+
// Direction semantics: in OpenDRIVE, left lanes (positive ids) run opposite
|
|
24
|
+
// to the reference line. Boundary polylines are stored in reference-line
|
|
25
|
+
// order, so left lanes set invertLeft/invertRight = true — the editor then
|
|
26
|
+
// reads the boundaries reversed, consistent with how `osmToShapes` encodes
|
|
27
|
+
// direction via boundary inversion.
|
|
28
|
+
import { evalPoly3, sampleReferenceLine } from './odrGeometry';
|
|
29
|
+
import { sampleAtParam } from './laneCenterline';
|
|
30
|
+
import { createShapeIdAllocator, } from './osmToShapes';
|
|
31
|
+
import { PIXELS_PER_METER } from './units';
|
|
32
|
+
/** Lanes narrower than this (m) carry no usable area and are skipped. */
|
|
33
|
+
const WIDTH_EPS = 1e-3;
|
|
34
|
+
const S_EPS = 1e-6;
|
|
35
|
+
/**
|
|
36
|
+
* Lane sections shorter than this (m) are skipped as transition slivers.
|
|
37
|
+
* Generated maps (e.g. CARLA towns) often encode lane-count transitions as
|
|
38
|
+
* chains of centimeter-scale lane sections; materializing those as lanes
|
|
39
|
+
* produces degenerate slivers that no exporter can represent. A lane section
|
|
40
|
+
* below vehicle scale carries no usable lane area, so connectivity is
|
|
41
|
+
* bridged across skipped sections via their lane-level links instead.
|
|
42
|
+
*/
|
|
43
|
+
const MIN_SECTION_LEN_M = 0.3;
|
|
44
|
+
/**
|
|
45
|
+
* Derive a WGS84 origin from an OpenDRIVE <geoReference> PROJ string.
|
|
46
|
+
*
|
|
47
|
+
* Supports `+proj=tmerc +lat_0=.. +lon_0=..` (exact: the projection origin is
|
|
48
|
+
* the local (0, 0)) and `+proj=utm +zone=..` (approximate: the zone's central
|
|
49
|
+
* meridian on the equator; UTM's false easting/northing is not compensated).
|
|
50
|
+
* Returns null when the origin cannot be derived.
|
|
51
|
+
*/
|
|
52
|
+
export function parseGeoReferenceOrigin(geoReference) {
|
|
53
|
+
if (!geoReference)
|
|
54
|
+
return null;
|
|
55
|
+
if (/\+proj=tmerc\b/.test(geoReference)) {
|
|
56
|
+
const latMatch = geoReference.match(/\+lat_0=(-?[\d.]+(?:[eE][-+]?\d+)?)/);
|
|
57
|
+
const lonMatch = geoReference.match(/\+lon_0=(-?[\d.]+(?:[eE][-+]?\d+)?)/);
|
|
58
|
+
if (latMatch && lonMatch) {
|
|
59
|
+
const lat = parseFloat(latMatch[1]);
|
|
60
|
+
const lon = parseFloat(lonMatch[1]);
|
|
61
|
+
if (Number.isFinite(lat) && Number.isFinite(lon)) {
|
|
62
|
+
return { lat, lon, approximate: false };
|
|
63
|
+
}
|
|
64
|
+
}
|
|
65
|
+
return null;
|
|
66
|
+
}
|
|
67
|
+
if (/\+proj=utm\b/.test(geoReference)) {
|
|
68
|
+
const zoneMatch = geoReference.match(/\+zone=(\d+)/);
|
|
69
|
+
if (zoneMatch) {
|
|
70
|
+
const zone = parseInt(zoneMatch[1], 10);
|
|
71
|
+
if (zone >= 1 && zone <= 60) {
|
|
72
|
+
return { lat: 0, lon: zone * 6 - 183, approximate: true };
|
|
73
|
+
}
|
|
74
|
+
}
|
|
75
|
+
return null;
|
|
76
|
+
}
|
|
77
|
+
return null;
|
|
78
|
+
}
|
|
79
|
+
/** ENU meters -> canvas pixels (inverse of the exporter's pxToEnuX). */
|
|
80
|
+
function enuToCanvasX(m) {
|
|
81
|
+
return m * PIXELS_PER_METER;
|
|
82
|
+
}
|
|
83
|
+
/** ENU meters -> canvas pixels with the y-axis flip (inverse of pxToEnuY). */
|
|
84
|
+
function enuToCanvasY(m) {
|
|
85
|
+
return -m * PIXELS_PER_METER;
|
|
86
|
+
}
|
|
87
|
+
function laneOffsetAt(road, s) {
|
|
88
|
+
let active = null;
|
|
89
|
+
for (const rec of road.laneOffsets) {
|
|
90
|
+
if (rec.s <= s + S_EPS)
|
|
91
|
+
active = rec;
|
|
92
|
+
else
|
|
93
|
+
break;
|
|
94
|
+
}
|
|
95
|
+
return active ? evalPoly3(active, s - active.s) : 0;
|
|
96
|
+
}
|
|
97
|
+
/** Lane width at `ds` meters past the lane-section start. */
|
|
98
|
+
function laneWidthAt(lane, ds) {
|
|
99
|
+
let active = null;
|
|
100
|
+
for (const rec of lane.widths) {
|
|
101
|
+
if (rec.sOffset <= ds + S_EPS)
|
|
102
|
+
active = rec;
|
|
103
|
+
else
|
|
104
|
+
break;
|
|
105
|
+
}
|
|
106
|
+
if (!active)
|
|
107
|
+
return 0;
|
|
108
|
+
const w = evalPoly3(active, ds - active.sOffset);
|
|
109
|
+
return w > 0 ? w : 0;
|
|
110
|
+
}
|
|
111
|
+
function roadMarkToSubtype(rm) {
|
|
112
|
+
if (!rm)
|
|
113
|
+
return 'solid';
|
|
114
|
+
if (rm.type.includes('broken'))
|
|
115
|
+
return 'dashed';
|
|
116
|
+
return 'solid';
|
|
117
|
+
}
|
|
118
|
+
/**
|
|
119
|
+
* OpenDRIVE lane type -> lanelet-style lane subtype. Lane shapes follow the
|
|
120
|
+
* lanelet vocabulary (`attributes.type = 'lanelet'`, kind in `subtype`) so
|
|
121
|
+
* that the Lanelet2 exporter emits valid `type=lanelet` relations; the exact
|
|
122
|
+
* OpenDRIVE type is preserved separately in `odr_type` for re-export.
|
|
123
|
+
*/
|
|
124
|
+
const ODR_TYPE_TO_LANELET_SUBTYPE = {
|
|
125
|
+
driving: 'road',
|
|
126
|
+
sidewalk: 'walkway',
|
|
127
|
+
walking: 'walkway',
|
|
128
|
+
biking: 'bicycle_lane',
|
|
129
|
+
exit: 'exit',
|
|
130
|
+
entry: 'road',
|
|
131
|
+
onRamp: 'road',
|
|
132
|
+
offRamp: 'road',
|
|
133
|
+
bus: 'bus_lane',
|
|
134
|
+
taxi: 'bus_lane',
|
|
135
|
+
crosswalk: 'crosswalk',
|
|
136
|
+
};
|
|
137
|
+
function laneletSubtypeFor(odrType) {
|
|
138
|
+
return ODR_TYPE_TO_LANELET_SUBTYPE[odrType] ?? odrType;
|
|
139
|
+
}
|
|
140
|
+
function shouldKeepLane(lane, maxWidth) {
|
|
141
|
+
if (lane.type === 'none')
|
|
142
|
+
return false;
|
|
143
|
+
return maxWidth > WIDTH_EPS;
|
|
144
|
+
}
|
|
145
|
+
function emptyBounds() {
|
|
146
|
+
return {
|
|
147
|
+
minX: Infinity,
|
|
148
|
+
maxX: -Infinity,
|
|
149
|
+
minY: Infinity,
|
|
150
|
+
maxY: -Infinity,
|
|
151
|
+
centerX: 0,
|
|
152
|
+
centerY: 0,
|
|
153
|
+
width: 0,
|
|
154
|
+
height: 0,
|
|
155
|
+
};
|
|
156
|
+
}
|
|
157
|
+
/**
|
|
158
|
+
* Classify a junction lane's turning direction from the accumulated signed
|
|
159
|
+
* heading change along its reference samples (left lanes travel against the
|
|
160
|
+
* reference direction, flipping the sign). Consumers such as Autoware require
|
|
161
|
+
* a turn_direction tag on every lanelet inside an intersection.
|
|
162
|
+
*/
|
|
163
|
+
function turnDirectionFor(stations, isLeftLane) {
|
|
164
|
+
let total = 0;
|
|
165
|
+
for (let i = 1; i < stations.length; i++) {
|
|
166
|
+
let d = stations[i].hdg - stations[i - 1].hdg;
|
|
167
|
+
while (d > Math.PI)
|
|
168
|
+
d -= 2 * Math.PI;
|
|
169
|
+
while (d < -Math.PI)
|
|
170
|
+
d += 2 * Math.PI;
|
|
171
|
+
total += d;
|
|
172
|
+
}
|
|
173
|
+
if (isLeftLane)
|
|
174
|
+
total = -total;
|
|
175
|
+
const TURN_THRESHOLD = (20 * Math.PI) / 180;
|
|
176
|
+
if (total > TURN_THRESHOLD)
|
|
177
|
+
return 'left';
|
|
178
|
+
if (total < -TURN_THRESHOLD)
|
|
179
|
+
return 'right';
|
|
180
|
+
return 'straight';
|
|
181
|
+
}
|
|
182
|
+
/** OpenDRIVE signal types converted to traffic light shapes (vehicle / pedestrian). */
|
|
183
|
+
const TRAFFIC_LIGHT_SIGNAL_TYPES = new Set(['1000001', '1000002']);
|
|
184
|
+
/** Interpolated pose on the sampled reference line at station `s` (clamped). */
|
|
185
|
+
function poseAt(samples, s) {
|
|
186
|
+
const first = samples[0];
|
|
187
|
+
if (s <= first.s)
|
|
188
|
+
return first;
|
|
189
|
+
const last = samples[samples.length - 1];
|
|
190
|
+
if (s >= last.s)
|
|
191
|
+
return last;
|
|
192
|
+
for (let i = 0; i < samples.length - 1; i++) {
|
|
193
|
+
const a = samples[i];
|
|
194
|
+
const b = samples[i + 1];
|
|
195
|
+
if (s > b.s)
|
|
196
|
+
continue;
|
|
197
|
+
const span = b.s - a.s;
|
|
198
|
+
const f = span > S_EPS ? (s - a.s) / span : 0;
|
|
199
|
+
return { x: a.x + (b.x - a.x) * f, y: a.y + (b.y - a.y) * f, hdg: a.hdg };
|
|
200
|
+
}
|
|
201
|
+
return last;
|
|
202
|
+
}
|
|
203
|
+
/**
|
|
204
|
+
* Convert a parsed OpenDRIVE map into editor-ready point/linestring/lane
|
|
205
|
+
* records plus a sidecar for round-trip export.
|
|
206
|
+
*
|
|
207
|
+
* Pass `selectedRoadIds` to restrict the conversion to a subset of roads
|
|
208
|
+
* (selective import); leave it `undefined` to import every road.
|
|
209
|
+
*/
|
|
210
|
+
export function odrToShapes(map, options = {}) {
|
|
211
|
+
const idAllocator = options.idAllocator ?? createShapeIdAllocator();
|
|
212
|
+
const warnings = [];
|
|
213
|
+
const origin = parseGeoReferenceOrigin(map.header.geoReference);
|
|
214
|
+
if (origin?.approximate) {
|
|
215
|
+
warnings.push('geoReference uses a UTM projection; the derived origin is the zone central meridian on the equator (false easting/northing not compensated).');
|
|
216
|
+
}
|
|
217
|
+
const result = {
|
|
218
|
+
points: [],
|
|
219
|
+
linestrings: [],
|
|
220
|
+
lanes: [],
|
|
221
|
+
trafficLights: [],
|
|
222
|
+
crosswalks: [],
|
|
223
|
+
bounds: emptyBounds(),
|
|
224
|
+
sidecar: {
|
|
225
|
+
rawXml: map.rawXml,
|
|
226
|
+
originLat: origin ? origin.lat : null,
|
|
227
|
+
originLon: origin ? origin.lon : null,
|
|
228
|
+
},
|
|
229
|
+
warnings,
|
|
230
|
+
};
|
|
231
|
+
if (origin) {
|
|
232
|
+
result.originLatLon = { lat: origin.lat, lon: origin.lon };
|
|
233
|
+
}
|
|
234
|
+
let roads = map.roads;
|
|
235
|
+
if (options.selectedRoadIds) {
|
|
236
|
+
const selected = new Set(options.selectedRoadIds);
|
|
237
|
+
roads = roads.filter(r => selected.has(r.id));
|
|
238
|
+
}
|
|
239
|
+
// ---- Statistics for aggregated warnings ----
|
|
240
|
+
let elevationRoads = 0;
|
|
241
|
+
let superelevationRoads = 0;
|
|
242
|
+
let poly3Roads = 0;
|
|
243
|
+
let signalCount = 0;
|
|
244
|
+
let convertedSignalCount = 0;
|
|
245
|
+
let objectCount = 0;
|
|
246
|
+
let convertedObjectCount = 0;
|
|
247
|
+
let microSectionRoads = 0;
|
|
248
|
+
// ---- Shape materialization ----
|
|
249
|
+
const laneRegistry = new Map();
|
|
250
|
+
const lanesByRoad = new Map();
|
|
251
|
+
const laneShapeById = new Map();
|
|
252
|
+
const sectionCount = new Map();
|
|
253
|
+
/** Section indices skipped per road (micro sections / unsampleable). */
|
|
254
|
+
const skippedSections = new Map();
|
|
255
|
+
const roadById = new Map();
|
|
256
|
+
/** Roads (with their reference samples) whose signals are converted after all lanes exist. */
|
|
257
|
+
const signalRoads = [];
|
|
258
|
+
/** Lane attributes restored from <userData code="laneAttributes"> per road. */
|
|
259
|
+
const restoredAttrCache = new Map();
|
|
260
|
+
const restoredLaneAttributes = (road) => {
|
|
261
|
+
const cached = restoredAttrCache.get(road.id);
|
|
262
|
+
if (cached !== undefined)
|
|
263
|
+
return cached;
|
|
264
|
+
let parsed = null;
|
|
265
|
+
const raw = road.userData['laneAttributes'];
|
|
266
|
+
if (raw) {
|
|
267
|
+
try {
|
|
268
|
+
const obj = JSON.parse(raw);
|
|
269
|
+
if (obj && typeof obj === 'object' && !Array.isArray(obj)) {
|
|
270
|
+
parsed = {};
|
|
271
|
+
for (const [k, v] of Object.entries(obj)) {
|
|
272
|
+
// `type` is fixed to 'lanelet' and odr_* meta is regenerated.
|
|
273
|
+
if (typeof v !== 'string' || k === 'type' || k.startsWith('odr_'))
|
|
274
|
+
continue;
|
|
275
|
+
parsed[k] = v;
|
|
276
|
+
}
|
|
277
|
+
if (Object.keys(parsed).length === 0)
|
|
278
|
+
parsed = null;
|
|
279
|
+
}
|
|
280
|
+
}
|
|
281
|
+
catch {
|
|
282
|
+
// Malformed userData JSON is ignored (third-party files).
|
|
283
|
+
}
|
|
284
|
+
}
|
|
285
|
+
restoredAttrCache.set(road.id, parsed);
|
|
286
|
+
return parsed;
|
|
287
|
+
};
|
|
288
|
+
const registryKey = (roadId, sectionIdx, odrLaneId) => `${roadId}|${sectionIdx}|${odrLaneId}`;
|
|
289
|
+
for (const road of roads) {
|
|
290
|
+
roadById.set(road.id, road);
|
|
291
|
+
sectionCount.set(road.id, road.laneSections.length);
|
|
292
|
+
if (road.hasElevation)
|
|
293
|
+
elevationRoads++;
|
|
294
|
+
if (road.hasSuperelevation)
|
|
295
|
+
superelevationRoads++;
|
|
296
|
+
if (road.planView.some(g => g.kind === 'poly3'))
|
|
297
|
+
poly3Roads++;
|
|
298
|
+
signalCount += road.signals.length;
|
|
299
|
+
objectCount += road.objects.length;
|
|
300
|
+
if (road.planView.length === 0 || road.laneSections.length === 0) {
|
|
301
|
+
warnings.push(`Road ${road.id}: no plan view geometry or lane sections; skipped.`);
|
|
302
|
+
continue;
|
|
303
|
+
}
|
|
304
|
+
// Stations that must be present: laneSection starts, laneOffset
|
|
305
|
+
// breakpoints, and lane width record breakpoints.
|
|
306
|
+
const extraStations = [];
|
|
307
|
+
for (const sec of road.laneSections) {
|
|
308
|
+
extraStations.push(sec.s);
|
|
309
|
+
for (const lane of [...sec.left, ...sec.right]) {
|
|
310
|
+
for (const w of lane.widths)
|
|
311
|
+
extraStations.push(sec.s + w.sOffset);
|
|
312
|
+
}
|
|
313
|
+
}
|
|
314
|
+
for (const lo of road.laneOffsets)
|
|
315
|
+
extraStations.push(lo.s);
|
|
316
|
+
const samples = sampleReferenceLine(road, {
|
|
317
|
+
maxChordErrorMeters: options.maxChordErrorMeters,
|
|
318
|
+
maxStepMeters: options.maxStepMeters,
|
|
319
|
+
extraStations,
|
|
320
|
+
});
|
|
321
|
+
if (samples.length < 2) {
|
|
322
|
+
warnings.push(`Road ${road.id}: reference line could not be sampled; skipped.`);
|
|
323
|
+
continue;
|
|
324
|
+
}
|
|
325
|
+
const skipped = new Set();
|
|
326
|
+
let microSections = 0;
|
|
327
|
+
for (let secIdx = 0; secIdx < road.laneSections.length; secIdx++) {
|
|
328
|
+
const sec = road.laneSections[secIdx];
|
|
329
|
+
const secEnd = secIdx + 1 < road.laneSections.length ? road.laneSections[secIdx + 1].s : road.length;
|
|
330
|
+
// Micro sections (a few centimeters or less — common in generated maps
|
|
331
|
+
// as lane-count transition slivers) carry no usable lane area; they are
|
|
332
|
+
// skipped and lane-level connectivity is bridged across them below.
|
|
333
|
+
if (secEnd - sec.s < MIN_SECTION_LEN_M) {
|
|
334
|
+
skipped.add(secIdx);
|
|
335
|
+
microSections++;
|
|
336
|
+
continue;
|
|
337
|
+
}
|
|
338
|
+
const stations = samples.filter(st => st.s >= sec.s - S_EPS && st.s <= secEnd + S_EPS);
|
|
339
|
+
if (stations.length < 2) {
|
|
340
|
+
skipped.add(secIdx);
|
|
341
|
+
continue;
|
|
342
|
+
}
|
|
343
|
+
materializeSection(road, sec, secIdx, stations);
|
|
344
|
+
}
|
|
345
|
+
skippedSections.set(road.id, skipped);
|
|
346
|
+
if (microSections > 0)
|
|
347
|
+
microSectionRoads++;
|
|
348
|
+
// Signals are materialized after every road's lanes exist, because a
|
|
349
|
+
// <signalReference> may point at lanes of a road processed later.
|
|
350
|
+
signalRoads.push({ road, samples });
|
|
351
|
+
}
|
|
352
|
+
/**
|
|
353
|
+
* Resolve lane references that point into skipped (micro) sections to the
|
|
354
|
+
* nearest materialized section, following lane-level links in `dir`
|
|
355
|
+
* (+1 = toward larger section indices, -1 = toward smaller). Returns the
|
|
356
|
+
* (sectionIdx, laneId) pairs in the first materialized section reached.
|
|
357
|
+
*/
|
|
358
|
+
function resolveThroughSkipped(road, secIdx, laneIds, dir) {
|
|
359
|
+
const out = [];
|
|
360
|
+
const skipped = skippedSections.get(road.id);
|
|
361
|
+
const visit = (si, ids, depth) => {
|
|
362
|
+
if (si < 0 || si >= road.laneSections.length || ids.length === 0 || depth > road.laneSections.length) {
|
|
363
|
+
return;
|
|
364
|
+
}
|
|
365
|
+
if (!skipped?.has(si)) {
|
|
366
|
+
for (const id of ids)
|
|
367
|
+
out.push({ secIdx: si, laneId: id });
|
|
368
|
+
return;
|
|
369
|
+
}
|
|
370
|
+
const sec = road.laneSections[si];
|
|
371
|
+
const nextIds = [];
|
|
372
|
+
for (const id of ids) {
|
|
373
|
+
const lane = [...sec.left, ...sec.right].find(l => l.id === id);
|
|
374
|
+
if (!lane)
|
|
375
|
+
continue;
|
|
376
|
+
for (const linked of dir === 1 ? lane.successorIds : lane.predecessorIds) {
|
|
377
|
+
if (!nextIds.includes(linked))
|
|
378
|
+
nextIds.push(linked);
|
|
379
|
+
}
|
|
380
|
+
}
|
|
381
|
+
visit(si + dir, nextIds, depth + 1);
|
|
382
|
+
};
|
|
383
|
+
visit(secIdx, laneIds, 0);
|
|
384
|
+
return out;
|
|
385
|
+
}
|
|
386
|
+
/**
|
|
387
|
+
* Resolve the lane shapes a <validity> lane range applies to on `road` at
|
|
388
|
+
* station `s`, falling back to every driving lane of the road when the
|
|
389
|
+
* validity list is empty. Resolved shape ids are appended to `into`.
|
|
390
|
+
*/
|
|
391
|
+
function resolveAffectedLanes(road, s, validity, into) {
|
|
392
|
+
let secIdx = 0;
|
|
393
|
+
for (let i = 0; i < road.laneSections.length; i++) {
|
|
394
|
+
if (road.laneSections[i].s <= s + S_EPS)
|
|
395
|
+
secIdx = i;
|
|
396
|
+
else
|
|
397
|
+
break;
|
|
398
|
+
}
|
|
399
|
+
if (validity.length > 0) {
|
|
400
|
+
for (const v of validity) {
|
|
401
|
+
const lo = Math.min(v.fromLane, v.toLane);
|
|
402
|
+
const hi = Math.max(v.fromLane, v.toLane);
|
|
403
|
+
for (let odrLaneId = lo; odrLaneId <= hi; odrLaneId++) {
|
|
404
|
+
if (odrLaneId === 0)
|
|
405
|
+
continue;
|
|
406
|
+
const reg = laneRegistry.get(registryKey(road.id, secIdx, odrLaneId));
|
|
407
|
+
if (reg && !into.includes(reg.shapeId))
|
|
408
|
+
into.push(reg.shapeId);
|
|
409
|
+
}
|
|
410
|
+
}
|
|
411
|
+
}
|
|
412
|
+
else {
|
|
413
|
+
for (const reg of lanesByRoad.get(road.id) ?? []) {
|
|
414
|
+
if (reg.laneType === 'driving' && !into.includes(reg.shapeId)) {
|
|
415
|
+
into.push(reg.shapeId);
|
|
416
|
+
}
|
|
417
|
+
}
|
|
418
|
+
}
|
|
419
|
+
}
|
|
420
|
+
/**
|
|
421
|
+
* Rebuild a stop-line linestring from a signal's
|
|
422
|
+
* <userData code="stopLine" value="[[x,y],...]"> record (ENU meters).
|
|
423
|
+
* Returns the linestring shape id, or null when the record is absent or
|
|
424
|
+
* malformed.
|
|
425
|
+
*/
|
|
426
|
+
function materializeStopLine(stopLineJson) {
|
|
427
|
+
if (!stopLineJson)
|
|
428
|
+
return null;
|
|
429
|
+
let coords;
|
|
430
|
+
try {
|
|
431
|
+
coords = JSON.parse(stopLineJson);
|
|
432
|
+
}
|
|
433
|
+
catch {
|
|
434
|
+
return null;
|
|
435
|
+
}
|
|
436
|
+
if (!Array.isArray(coords) || coords.length < 2)
|
|
437
|
+
return null;
|
|
438
|
+
const pts = [];
|
|
439
|
+
for (const entry of coords) {
|
|
440
|
+
if (!Array.isArray(entry) || entry.length < 2)
|
|
441
|
+
return null;
|
|
442
|
+
const [x, y] = entry;
|
|
443
|
+
if (typeof x !== 'number' || typeof y !== 'number')
|
|
444
|
+
return null;
|
|
445
|
+
pts.push({ x, y });
|
|
446
|
+
}
|
|
447
|
+
const pointIds = [];
|
|
448
|
+
let firstX = 0;
|
|
449
|
+
let firstY = 0;
|
|
450
|
+
pts.forEach((p, i) => {
|
|
451
|
+
const x = enuToCanvasX(p.x);
|
|
452
|
+
const y = enuToCanvasY(p.y);
|
|
453
|
+
if (i === 0) {
|
|
454
|
+
firstX = x;
|
|
455
|
+
firstY = y;
|
|
456
|
+
}
|
|
457
|
+
const pointId = idAllocator.next('point');
|
|
458
|
+
result.points.push({ id: pointId, x, y, osmId: '' });
|
|
459
|
+
pointIds.push(pointId);
|
|
460
|
+
});
|
|
461
|
+
const ls = {
|
|
462
|
+
id: idAllocator.next('linestring'),
|
|
463
|
+
x: firstX,
|
|
464
|
+
y: firstY,
|
|
465
|
+
pointIds,
|
|
466
|
+
osmId: '',
|
|
467
|
+
attributes: { type: 'stop_line', subtype: 'solid', width: '0.2' },
|
|
468
|
+
};
|
|
469
|
+
result.linestrings.push(ls);
|
|
470
|
+
return ls.id;
|
|
471
|
+
}
|
|
472
|
+
// <signalReference> records grouped by the referenced signal id, so a
|
|
473
|
+
// signal controlling several roads recovers its full validity set.
|
|
474
|
+
const referencesBySignalId = new Map();
|
|
475
|
+
for (const road of roads) {
|
|
476
|
+
for (const ref of road.signalReferences) {
|
|
477
|
+
if (!ref.id)
|
|
478
|
+
continue;
|
|
479
|
+
const list = referencesBySignalId.get(ref.id) ?? [];
|
|
480
|
+
list.push({ road, s: ref.s, validity: ref.validity });
|
|
481
|
+
referencesBySignalId.set(ref.id, list);
|
|
482
|
+
}
|
|
483
|
+
}
|
|
484
|
+
/**
|
|
485
|
+
* Convert traffic light signals (type 1000001/1000002) into traffic light
|
|
486
|
+
* shapes. The position is the (s, t) station evaluated on the reference
|
|
487
|
+
* line; `affectedLaneIds` resolves the <validity> lane range against the
|
|
488
|
+
* lane section containing s (falling back to every driving lane of the
|
|
489
|
+
* road), merged with the lanes of any road that re-applies the signal via
|
|
490
|
+
* <signalReference>. A <userData code="stopLine"> record is rebuilt into a
|
|
491
|
+
* stop-line linestring and linked through `stopLineId`.
|
|
492
|
+
*/
|
|
493
|
+
function materializeSignals(road, samples) {
|
|
494
|
+
for (const sig of road.signals) {
|
|
495
|
+
if (!TRAFFIC_LIGHT_SIGNAL_TYPES.has(sig.type))
|
|
496
|
+
continue;
|
|
497
|
+
const pose = poseAt(samples, sig.s);
|
|
498
|
+
// Unit normal toward +t (left of the reference direction in ENU).
|
|
499
|
+
const ex = pose.x - Math.sin(pose.hdg) * sig.t;
|
|
500
|
+
const ey = pose.y + Math.cos(pose.hdg) * sig.t;
|
|
501
|
+
const affected = [];
|
|
502
|
+
resolveAffectedLanes(road, sig.s, sig.validity, affected);
|
|
503
|
+
for (const ref of referencesBySignalId.get(sig.id) ?? []) {
|
|
504
|
+
if (ref.road.id === road.id)
|
|
505
|
+
continue;
|
|
506
|
+
resolveAffectedLanes(ref.road, ref.s, ref.validity, affected);
|
|
507
|
+
}
|
|
508
|
+
const data = {
|
|
509
|
+
id: idAllocator.next('traffic_light'),
|
|
510
|
+
x: enuToCanvasX(ex),
|
|
511
|
+
y: enuToCanvasY(ey),
|
|
512
|
+
// Default editor proportions (30x60 px) when the signal carries no size.
|
|
513
|
+
w: sig.width > 0 ? sig.width * PIXELS_PER_METER : 30,
|
|
514
|
+
h: sig.height > 0 ? sig.height * PIXELS_PER_METER : 60,
|
|
515
|
+
osmId: '',
|
|
516
|
+
affectedLaneIds: affected,
|
|
517
|
+
stopLineId: materializeStopLine(sig.userData['stopLine']),
|
|
518
|
+
attributes: {
|
|
519
|
+
type: 'traffic_light',
|
|
520
|
+
odr_signal_id: sig.id,
|
|
521
|
+
odr_road_id: road.id,
|
|
522
|
+
odr_signal_type: sig.type,
|
|
523
|
+
odr_signal_subtype: sig.subtype,
|
|
524
|
+
},
|
|
525
|
+
};
|
|
526
|
+
result.trafficLights.push(data);
|
|
527
|
+
convertedSignalCount++;
|
|
528
|
+
}
|
|
529
|
+
}
|
|
530
|
+
for (const { road, samples } of signalRoads) {
|
|
531
|
+
materializeSignals(road, samples);
|
|
532
|
+
}
|
|
533
|
+
/** Driving-lane shape ids of a road (regulatory link resolution). */
|
|
534
|
+
const drivingLanesOf = (roadId) => {
|
|
535
|
+
const out = [];
|
|
536
|
+
for (const reg of lanesByRoad.get(roadId) ?? []) {
|
|
537
|
+
if (reg.laneType === 'driving' && !out.includes(reg.shapeId))
|
|
538
|
+
out.push(reg.shapeId);
|
|
539
|
+
}
|
|
540
|
+
return out;
|
|
541
|
+
};
|
|
542
|
+
/**
|
|
543
|
+
* Convert crosswalk objects into crosswalk shapes. The band center is the
|
|
544
|
+
* (s, t) station on the reference line; the walking axis follows the
|
|
545
|
+
* object's heading (relative to the road direction), spanning `length`
|
|
546
|
+
* with band width `width`. Regulatory links stashed by the exporter in
|
|
547
|
+
* <userData code="crosswalkLinks"> (affected roads + stop line polyline)
|
|
548
|
+
* are resolved back to lane shape ids / a stop-line linestring.
|
|
549
|
+
*/
|
|
550
|
+
function materializeCrosswalks(road, samples) {
|
|
551
|
+
for (const obj of road.objects) {
|
|
552
|
+
if (obj.type !== 'crosswalk')
|
|
553
|
+
continue;
|
|
554
|
+
if (!(obj.length > 0) || !(obj.width > 0))
|
|
555
|
+
continue;
|
|
556
|
+
const pose = poseAt(samples, obj.s);
|
|
557
|
+
// Unit normal toward +t (left of the reference direction in ENU).
|
|
558
|
+
const cx = pose.x - Math.sin(pose.hdg) * obj.t;
|
|
559
|
+
const cy = pose.y + Math.cos(pose.hdg) * obj.t;
|
|
560
|
+
const axisHdg = pose.hdg + obj.hdg;
|
|
561
|
+
const hx = (Math.cos(axisHdg) * obj.length) / 2;
|
|
562
|
+
const hy = (Math.sin(axisHdg) * obj.length) / 2;
|
|
563
|
+
const startX = enuToCanvasX(cx - hx);
|
|
564
|
+
const startY = enuToCanvasY(cy - hy);
|
|
565
|
+
const endX = enuToCanvasX(cx + hx);
|
|
566
|
+
const endY = enuToCanvasY(cy + hy);
|
|
567
|
+
const shapeX = (startX + endX) / 2;
|
|
568
|
+
const shapeY = (startY + endY) / 2;
|
|
569
|
+
const affected = [];
|
|
570
|
+
let stopLineId = null;
|
|
571
|
+
const rawLinks = obj.userData['crosswalkLinks'];
|
|
572
|
+
if (rawLinks) {
|
|
573
|
+
try {
|
|
574
|
+
const links = JSON.parse(rawLinks);
|
|
575
|
+
if (Array.isArray(links.affectedRoads)) {
|
|
576
|
+
for (const rid of links.affectedRoads) {
|
|
577
|
+
if (typeof rid !== 'string')
|
|
578
|
+
continue;
|
|
579
|
+
for (const shapeId of drivingLanesOf(rid)) {
|
|
580
|
+
if (!affected.includes(shapeId))
|
|
581
|
+
affected.push(shapeId);
|
|
582
|
+
}
|
|
583
|
+
}
|
|
584
|
+
}
|
|
585
|
+
if (Array.isArray(links.stopLine)) {
|
|
586
|
+
stopLineId = materializeStopLine(JSON.stringify(links.stopLine));
|
|
587
|
+
}
|
|
588
|
+
}
|
|
589
|
+
catch {
|
|
590
|
+
// Malformed userData JSON is ignored (third-party files).
|
|
591
|
+
}
|
|
592
|
+
}
|
|
593
|
+
const data = {
|
|
594
|
+
id: idAllocator.next('crosswalk'),
|
|
595
|
+
x: shapeX,
|
|
596
|
+
y: shapeY,
|
|
597
|
+
startX: startX - shapeX,
|
|
598
|
+
startY: startY - shapeY,
|
|
599
|
+
endX: endX - shapeX,
|
|
600
|
+
endY: endY - shapeY,
|
|
601
|
+
crosswalkWidth: obj.width * PIXELS_PER_METER,
|
|
602
|
+
osmId: '',
|
|
603
|
+
affectedLaneIds: affected,
|
|
604
|
+
stopLineId,
|
|
605
|
+
attributes: {
|
|
606
|
+
type: 'crosswalk',
|
|
607
|
+
odr_road_id: road.id,
|
|
608
|
+
odr_object_id: obj.id,
|
|
609
|
+
},
|
|
610
|
+
};
|
|
611
|
+
result.crosswalks.push(data);
|
|
612
|
+
convertedObjectCount++;
|
|
613
|
+
}
|
|
614
|
+
}
|
|
615
|
+
for (const { road, samples } of signalRoads) {
|
|
616
|
+
materializeCrosswalks(road, samples);
|
|
617
|
+
}
|
|
618
|
+
// Restore right-of-way links stashed by the exporter in
|
|
619
|
+
// <userData code="yieldRoads">: every driving lane of the carrying road
|
|
620
|
+
// yields priority over the driving lanes of the listed roads.
|
|
621
|
+
for (const road of roads) {
|
|
622
|
+
const rawYield = road.userData['yieldRoads'];
|
|
623
|
+
if (!rawYield)
|
|
624
|
+
continue;
|
|
625
|
+
let yieldRoadIds;
|
|
626
|
+
try {
|
|
627
|
+
yieldRoadIds = JSON.parse(rawYield);
|
|
628
|
+
}
|
|
629
|
+
catch {
|
|
630
|
+
continue;
|
|
631
|
+
}
|
|
632
|
+
if (!Array.isArray(yieldRoadIds))
|
|
633
|
+
continue;
|
|
634
|
+
const yieldLaneIds = [];
|
|
635
|
+
for (const rid of yieldRoadIds) {
|
|
636
|
+
if (typeof rid !== 'string')
|
|
637
|
+
continue;
|
|
638
|
+
for (const shapeId of drivingLanesOf(rid)) {
|
|
639
|
+
if (!yieldLaneIds.includes(shapeId))
|
|
640
|
+
yieldLaneIds.push(shapeId);
|
|
641
|
+
}
|
|
642
|
+
}
|
|
643
|
+
if (yieldLaneIds.length === 0)
|
|
644
|
+
continue;
|
|
645
|
+
for (const shapeId of drivingLanesOf(road.id)) {
|
|
646
|
+
const lane = laneShapeById.get(shapeId);
|
|
647
|
+
if (lane)
|
|
648
|
+
lane.yieldLaneIds = [...yieldLaneIds];
|
|
649
|
+
}
|
|
650
|
+
}
|
|
651
|
+
function materializeSection(road, sec, secIdx, stations) {
|
|
652
|
+
// Unit normals pointing toward +t (left of the reference direction in ENU).
|
|
653
|
+
const normals = stations.map(st => ({ x: -Math.sin(st.hdg), y: Math.cos(st.hdg) }));
|
|
654
|
+
// Lane reference polyline: reference line shifted by the laneOffset.
|
|
655
|
+
const centerPts = stations.map((st, j) => {
|
|
656
|
+
const off = laneOffsetAt(road, st.s);
|
|
657
|
+
return { x: st.x + normals[j].x * off, y: st.y + normals[j].y * off };
|
|
658
|
+
});
|
|
659
|
+
// Accumulate boundary polylines from the center outward. Index 0 is the
|
|
660
|
+
// center; index i is the outer boundary of the i-th lane (inner-to-outer
|
|
661
|
+
// order). Widths of skipped lanes still shift the outer boundaries.
|
|
662
|
+
const accumulate = (lanes, sign) => {
|
|
663
|
+
const boundaries = [centerPts];
|
|
664
|
+
let prev = centerPts;
|
|
665
|
+
for (const lane of lanes) {
|
|
666
|
+
const next = prev.map((p, j) => {
|
|
667
|
+
const w = laneWidthAt(lane, stations[j].s - sec.s);
|
|
668
|
+
return { x: p.x + sign * normals[j].x * w, y: p.y + sign * normals[j].y * w };
|
|
669
|
+
});
|
|
670
|
+
boundaries.push(next);
|
|
671
|
+
prev = next;
|
|
672
|
+
}
|
|
673
|
+
return boundaries;
|
|
674
|
+
};
|
|
675
|
+
const leftBoundaries = accumulate(sec.left, 1);
|
|
676
|
+
const rightBoundaries = accumulate(sec.right, -1);
|
|
677
|
+
// Lazily materialize boundary linestrings so adjacent lanes share them.
|
|
678
|
+
// The center boundary (index 0) is shared across both sides.
|
|
679
|
+
const lsCache = new Map();
|
|
680
|
+
const getLinestring = (side, index, pts, rm) => {
|
|
681
|
+
const key = index === 0 ? 'C' : `${side}${index}`;
|
|
682
|
+
const cached = lsCache.get(key);
|
|
683
|
+
if (cached)
|
|
684
|
+
return cached;
|
|
685
|
+
const pointIds = [];
|
|
686
|
+
let firstX = 0;
|
|
687
|
+
let firstY = 0;
|
|
688
|
+
pts.forEach((p, j) => {
|
|
689
|
+
const x = enuToCanvasX(p.x);
|
|
690
|
+
const y = enuToCanvasY(p.y);
|
|
691
|
+
if (j === 0) {
|
|
692
|
+
firstX = x;
|
|
693
|
+
firstY = y;
|
|
694
|
+
}
|
|
695
|
+
const pointId = idAllocator.next('point');
|
|
696
|
+
const data = { id: pointId, x, y, osmId: '' };
|
|
697
|
+
result.points.push(data);
|
|
698
|
+
pointIds.push(pointId);
|
|
699
|
+
});
|
|
700
|
+
const data = {
|
|
701
|
+
id: idAllocator.next('linestring'),
|
|
702
|
+
x: firstX,
|
|
703
|
+
y: firstY,
|
|
704
|
+
pointIds,
|
|
705
|
+
osmId: '',
|
|
706
|
+
attributes: {
|
|
707
|
+
type: 'line_thin',
|
|
708
|
+
subtype: roadMarkToSubtype(rm),
|
|
709
|
+
width: '0.2',
|
|
710
|
+
},
|
|
711
|
+
};
|
|
712
|
+
result.linestrings.push(data);
|
|
713
|
+
lsCache.set(key, data);
|
|
714
|
+
return data;
|
|
715
|
+
};
|
|
716
|
+
const centerRoadMark = sec.center.find(l => l.id === 0)?.roadMarks[0];
|
|
717
|
+
const boundaryRoadMark = (lanes, index) => index === 0 ? centerRoadMark : lanes[index - 1]?.roadMarks[0];
|
|
718
|
+
const materializeSide = (lanes, boundaries, side) => {
|
|
719
|
+
for (let i = 0; i < lanes.length; i++) {
|
|
720
|
+
const lane = lanes[i];
|
|
721
|
+
let maxWidth = 0;
|
|
722
|
+
for (const st of stations) {
|
|
723
|
+
const w = laneWidthAt(lane, st.s - sec.s);
|
|
724
|
+
if (w > maxWidth)
|
|
725
|
+
maxWidth = w;
|
|
726
|
+
}
|
|
727
|
+
if (!shouldKeepLane(lane, maxWidth))
|
|
728
|
+
continue;
|
|
729
|
+
const innerLs = getLinestring(side, i, boundaries[i], boundaryRoadMark(lanes, i));
|
|
730
|
+
const outerLs = getLinestring(side, i + 1, boundaries[i + 1], boundaryRoadMark(lanes, i + 1));
|
|
731
|
+
// Left lanes (positive ids) travel opposite to the reference line, so
|
|
732
|
+
// their boundaries (stored in reference-line order) are read reversed.
|
|
733
|
+
// For both sides the inner boundary is the lane's left edge in travel
|
|
734
|
+
// direction (verified against screen coordinates with y pointing down).
|
|
735
|
+
const isLeftLane = lane.id > 0;
|
|
736
|
+
const laneShapeId = idAllocator.next('lane');
|
|
737
|
+
const attributes = {
|
|
738
|
+
type: 'lanelet',
|
|
739
|
+
subtype: laneletSubtypeFor(lane.type),
|
|
740
|
+
one_way: 'yes',
|
|
741
|
+
// Lanelet tags stashed by the exporter in <userData
|
|
742
|
+
// code="laneAttributes"> (speed_limit, turn_direction, location,
|
|
743
|
+
// one_way=no, exact subtype, custom tags) override the defaults.
|
|
744
|
+
...restoredLaneAttributes(road),
|
|
745
|
+
odr_type: lane.type,
|
|
746
|
+
odr_road_id: road.id,
|
|
747
|
+
odr_lane_id: String(lane.id),
|
|
748
|
+
odr_section_s: String(sec.s),
|
|
749
|
+
};
|
|
750
|
+
attributes.type = 'lanelet';
|
|
751
|
+
if (road.junction !== '-1') {
|
|
752
|
+
attributes.odr_junction_id = road.junction;
|
|
753
|
+
if (!attributes.turn_direction) {
|
|
754
|
+
attributes.turn_direction = turnDirectionFor(stations, isLeftLane);
|
|
755
|
+
}
|
|
756
|
+
}
|
|
757
|
+
const data = {
|
|
758
|
+
id: laneShapeId,
|
|
759
|
+
x: innerLs.x,
|
|
760
|
+
y: innerLs.y,
|
|
761
|
+
leftBoundaryId: innerLs.id,
|
|
762
|
+
rightBoundaryId: outerLs.id,
|
|
763
|
+
invertLeft: isLeftLane,
|
|
764
|
+
invertRight: isLeftLane,
|
|
765
|
+
osmId: '',
|
|
766
|
+
attributes,
|
|
767
|
+
next: [],
|
|
768
|
+
prev: [],
|
|
769
|
+
};
|
|
770
|
+
result.lanes.push(data);
|
|
771
|
+
laneShapeById.set(laneShapeId, data);
|
|
772
|
+
const registered = {
|
|
773
|
+
shapeId: laneShapeId,
|
|
774
|
+
roadId: road.id,
|
|
775
|
+
sectionIdx: secIdx,
|
|
776
|
+
odrLaneId: lane.id,
|
|
777
|
+
laneType: lane.type,
|
|
778
|
+
};
|
|
779
|
+
laneRegistry.set(registryKey(road.id, secIdx, lane.id), registered);
|
|
780
|
+
const roadLanes = lanesByRoad.get(road.id) ?? [];
|
|
781
|
+
roadLanes.push(registered);
|
|
782
|
+
lanesByRoad.set(road.id, roadLanes);
|
|
783
|
+
}
|
|
784
|
+
};
|
|
785
|
+
materializeSide(sec.left, leftBoundaries, 'L');
|
|
786
|
+
materializeSide(sec.right, rightBoundaries, 'R');
|
|
787
|
+
}
|
|
788
|
+
// ---- Connectivity ----
|
|
789
|
+
const connect = (fromShapeId, toShapeId) => {
|
|
790
|
+
const from = laneShapeById.get(fromShapeId);
|
|
791
|
+
const to = laneShapeById.get(toShapeId);
|
|
792
|
+
if (!from || !to)
|
|
793
|
+
return;
|
|
794
|
+
if (!from.next.includes(toShapeId))
|
|
795
|
+
from.next.push(toShapeId);
|
|
796
|
+
if (!to.prev.includes(fromShapeId))
|
|
797
|
+
to.prev.push(fromShapeId);
|
|
798
|
+
};
|
|
799
|
+
/**
|
|
800
|
+
* Registered lanes at a road's start (first section) or end (last
|
|
801
|
+
* section). When the outermost section is a skipped micro section, the
|
|
802
|
+
* lane reference is resolved through it along the lane-level links to the
|
|
803
|
+
* first materialized section.
|
|
804
|
+
*/
|
|
805
|
+
const lanesAt = (roadId, contact, odrLaneId) => {
|
|
806
|
+
const road = roadById.get(roadId);
|
|
807
|
+
const count = sectionCount.get(roadId) ?? 0;
|
|
808
|
+
if (!road || count === 0)
|
|
809
|
+
return [];
|
|
810
|
+
const secIdx = contact === 'start' ? 0 : count - 1;
|
|
811
|
+
const direct = laneRegistry.get(registryKey(roadId, secIdx, odrLaneId));
|
|
812
|
+
if (direct)
|
|
813
|
+
return [direct];
|
|
814
|
+
const out = [];
|
|
815
|
+
for (const t of resolveThroughSkipped(road, secIdx, [odrLaneId], contact === 'start' ? 1 : -1)) {
|
|
816
|
+
const reg = laneRegistry.get(registryKey(roadId, t.secIdx, t.laneId));
|
|
817
|
+
if (reg && !out.includes(reg))
|
|
818
|
+
out.push(reg);
|
|
819
|
+
}
|
|
820
|
+
return out;
|
|
821
|
+
};
|
|
822
|
+
/**
|
|
823
|
+
* Link two lanes meeting at a shared contact, respecting travel direction:
|
|
824
|
+
* right lanes (id < 0) travel toward the road's end, left lanes (id > 0)
|
|
825
|
+
* toward its start. A connection is `a -> b` when a's travel exits at the
|
|
826
|
+
* contact and b's travel enters there (and vice versa); when both exit or
|
|
827
|
+
* both enter, the directions are inconsistent and the pair is skipped.
|
|
828
|
+
*/
|
|
829
|
+
const linkLanes = (a, aOdrId, cpA, b, bOdrId, cpB) => {
|
|
830
|
+
const aExits = (aOdrId < 0) === (cpA === 'end');
|
|
831
|
+
const bEnters = (bOdrId < 0) === (cpB === 'start');
|
|
832
|
+
if (aExits && bEnters)
|
|
833
|
+
connect(a.shapeId, b.shapeId);
|
|
834
|
+
else if (!aExits && !bEnters)
|
|
835
|
+
connect(b.shapeId, a.shapeId);
|
|
836
|
+
};
|
|
837
|
+
// Chain consecutive lane sections within each road via lane-level links.
|
|
838
|
+
// References into skipped micro sections are resolved through them to the
|
|
839
|
+
// nearest materialized section.
|
|
840
|
+
for (const road of roads) {
|
|
841
|
+
for (let secIdx = 0; secIdx < road.laneSections.length; secIdx++) {
|
|
842
|
+
const sec = road.laneSections[secIdx];
|
|
843
|
+
for (const lane of [...sec.left, ...sec.right]) {
|
|
844
|
+
const cur = laneRegistry.get(registryKey(road.id, secIdx, lane.id));
|
|
845
|
+
if (!cur)
|
|
846
|
+
continue;
|
|
847
|
+
if (secIdx + 1 < road.laneSections.length) {
|
|
848
|
+
for (const succId of lane.successorIds) {
|
|
849
|
+
for (const t of resolveThroughSkipped(road, secIdx + 1, [succId], 1)) {
|
|
850
|
+
const nxt = laneRegistry.get(registryKey(road.id, t.secIdx, t.laneId));
|
|
851
|
+
if (!nxt)
|
|
852
|
+
continue;
|
|
853
|
+
if (lane.id < 0 && t.laneId < 0)
|
|
854
|
+
connect(cur.shapeId, nxt.shapeId);
|
|
855
|
+
else if (lane.id > 0 && t.laneId > 0)
|
|
856
|
+
connect(nxt.shapeId, cur.shapeId);
|
|
857
|
+
}
|
|
858
|
+
}
|
|
859
|
+
}
|
|
860
|
+
if (secIdx > 0) {
|
|
861
|
+
for (const predId of lane.predecessorIds) {
|
|
862
|
+
for (const t of resolveThroughSkipped(road, secIdx - 1, [predId], -1)) {
|
|
863
|
+
const prv = laneRegistry.get(registryKey(road.id, t.secIdx, t.laneId));
|
|
864
|
+
if (!prv)
|
|
865
|
+
continue;
|
|
866
|
+
if (lane.id < 0 && t.laneId < 0)
|
|
867
|
+
connect(prv.shapeId, cur.shapeId);
|
|
868
|
+
else if (lane.id > 0 && t.laneId > 0)
|
|
869
|
+
connect(cur.shapeId, prv.shapeId);
|
|
870
|
+
}
|
|
871
|
+
}
|
|
872
|
+
}
|
|
873
|
+
}
|
|
874
|
+
}
|
|
875
|
+
}
|
|
876
|
+
// Road-level links (road <-> road). Junction links are resolved separately
|
|
877
|
+
// through the junction connection table.
|
|
878
|
+
const processRoadLink = (roadA, link, cpA) => {
|
|
879
|
+
if (!link || link.elementType !== 'road')
|
|
880
|
+
return;
|
|
881
|
+
const roadB = roadById.get(link.elementId);
|
|
882
|
+
if (!roadB)
|
|
883
|
+
return;
|
|
884
|
+
const cpB = link.contactPoint ?? (cpA === 'end' ? 'start' : 'end');
|
|
885
|
+
const sec = roadA.laneSections[cpA === 'start' ? 0 : roadA.laneSections.length - 1];
|
|
886
|
+
if (!sec)
|
|
887
|
+
return;
|
|
888
|
+
for (const lane of [...sec.left, ...sec.right]) {
|
|
889
|
+
const targetIds = cpA === 'end' ? lane.successorIds : lane.predecessorIds;
|
|
890
|
+
for (const a of lanesAt(roadA.id, cpA, lane.id)) {
|
|
891
|
+
for (const toId of targetIds) {
|
|
892
|
+
for (const b of lanesAt(roadB.id, cpB, toId)) {
|
|
893
|
+
linkLanes(a, lane.id, cpA, b, toId, cpB);
|
|
894
|
+
}
|
|
895
|
+
}
|
|
896
|
+
}
|
|
897
|
+
}
|
|
898
|
+
};
|
|
899
|
+
for (const road of roads) {
|
|
900
|
+
processRoadLink(road, road.predecessor, 'start');
|
|
901
|
+
processRoadLink(road, road.successor, 'end');
|
|
902
|
+
}
|
|
903
|
+
// Junction connections: incomingRoad -> connectingRoad laneLinks.
|
|
904
|
+
for (const junction of map.junctions) {
|
|
905
|
+
for (const conn of junction.connections) {
|
|
906
|
+
const roadA = roadById.get(conn.incomingRoad);
|
|
907
|
+
const roadC = roadById.get(conn.connectingRoad);
|
|
908
|
+
if (!roadA || !roadC)
|
|
909
|
+
continue;
|
|
910
|
+
// Which end of the incoming road faces this junction?
|
|
911
|
+
const contacts = [];
|
|
912
|
+
if (roadA.successor?.elementType === 'junction' && roadA.successor.elementId === junction.id) {
|
|
913
|
+
contacts.push('end');
|
|
914
|
+
}
|
|
915
|
+
if (roadA.predecessor?.elementType === 'junction' && roadA.predecessor.elementId === junction.id) {
|
|
916
|
+
contacts.push('start');
|
|
917
|
+
}
|
|
918
|
+
if (contacts.length === 0)
|
|
919
|
+
contacts.push('end'); // Tolerant default.
|
|
920
|
+
for (const cpA of contacts) {
|
|
921
|
+
for (const ll of conn.laneLinks) {
|
|
922
|
+
for (const a of lanesAt(roadA.id, cpA, ll.from)) {
|
|
923
|
+
for (const b of lanesAt(roadC.id, conn.contactPoint, ll.to)) {
|
|
924
|
+
linkLanes(a, ll.from, cpA, b, ll.to, conn.contactPoint);
|
|
925
|
+
}
|
|
926
|
+
}
|
|
927
|
+
}
|
|
928
|
+
}
|
|
929
|
+
}
|
|
930
|
+
}
|
|
931
|
+
// ---- Round-trip fidelity post-processing ----
|
|
932
|
+
// 1. Boundaries that are geometrically one line (each exported road carries
|
|
933
|
+
// its own copy of a boundary shared with its neighbour) collapse into a
|
|
934
|
+
// single linestring so left/right adjacency is expressed by sharing.
|
|
935
|
+
// 2. Boundary endpoints of connected lanes are welded into shared Point
|
|
936
|
+
// shapes so a Lanelet2 export emits shared nodes (Autoware routing and
|
|
937
|
+
// shared-node connection detection both depend on this).
|
|
938
|
+
dedupeSharedBoundaries(result);
|
|
939
|
+
weldConnectedLaneContacts(result);
|
|
940
|
+
removeOrphanPoints(result);
|
|
941
|
+
// ---- Aggregated warnings ----
|
|
942
|
+
if (elevationRoads > 0) {
|
|
943
|
+
warnings.push(`Elevation profiles on ${elevationRoads} road(s) were flattened to 2D.`);
|
|
944
|
+
}
|
|
945
|
+
if (superelevationRoads > 0) {
|
|
946
|
+
warnings.push(`Superelevation/lateral profiles on ${superelevationRoads} road(s) were ignored (2D import).`);
|
|
947
|
+
}
|
|
948
|
+
if (poly3Roads > 0) {
|
|
949
|
+
warnings.push(`Deprecated <poly3> geometry on ${poly3Roads} road(s) was approximated (local abscissa taken as arc length).`);
|
|
950
|
+
}
|
|
951
|
+
const unconvertedSignals = signalCount - convertedSignalCount;
|
|
952
|
+
const unconvertedObjects = objectCount - convertedObjectCount;
|
|
953
|
+
if (unconvertedSignals > 0 || unconvertedObjects > 0) {
|
|
954
|
+
warnings.push(`${unconvertedSignals} signal(s) and ${unconvertedObjects} object(s) were parsed but not converted to shapes.`);
|
|
955
|
+
}
|
|
956
|
+
if (microSectionRoads > 0) {
|
|
957
|
+
warnings.push(`Micro lane sections (< ${MIN_SECTION_LEN_M} m) on ${microSectionRoads} road(s) were skipped; lane connectivity was bridged across them.`);
|
|
958
|
+
}
|
|
959
|
+
// ---- Bounds ----
|
|
960
|
+
for (const point of result.points) {
|
|
961
|
+
if (point.x < result.bounds.minX)
|
|
962
|
+
result.bounds.minX = point.x;
|
|
963
|
+
if (point.x > result.bounds.maxX)
|
|
964
|
+
result.bounds.maxX = point.x;
|
|
965
|
+
if (point.y < result.bounds.minY)
|
|
966
|
+
result.bounds.minY = point.y;
|
|
967
|
+
if (point.y > result.bounds.maxY)
|
|
968
|
+
result.bounds.maxY = point.y;
|
|
969
|
+
}
|
|
970
|
+
if (result.points.length > 0) {
|
|
971
|
+
result.bounds.width = result.bounds.maxX - result.bounds.minX;
|
|
972
|
+
result.bounds.height = result.bounds.maxY - result.bounds.minY;
|
|
973
|
+
result.bounds.centerX = result.bounds.minX + result.bounds.width / 2;
|
|
974
|
+
result.bounds.centerY = result.bounds.minY + result.bounds.height / 2;
|
|
975
|
+
}
|
|
976
|
+
return result;
|
|
977
|
+
}
|
|
978
|
+
// ---------------------------------------------------------------------------
|
|
979
|
+
// Round-trip fidelity post-processing
|
|
980
|
+
// ---------------------------------------------------------------------------
|
|
981
|
+
/**
|
|
982
|
+
* Max pointwise deviation (m) for two boundaries to count as one line, in
|
|
983
|
+
* the interior of the polyline. Two genuinely distinct parallel boundaries
|
|
984
|
+
* are at least a lane width apart in their interior, so this can stay tight.
|
|
985
|
+
*/
|
|
986
|
+
const BOUNDARY_DEDUPE_INTERIOR_TOL_M = 0.3;
|
|
987
|
+
/**
|
|
988
|
+
* Max pointwise deviation (m) near the polyline ends. Contact-point welding
|
|
989
|
+
* (see weldConnectedLaneContacts) moves junction corners by up to a couple
|
|
990
|
+
* of meters, and the per-road reconstruction of a shared boundary diverges
|
|
991
|
+
* around such a kink, so the comparison is more permissive there. A false
|
|
992
|
+
* match would require two boundaries that pinch below this at both ends AND
|
|
993
|
+
* run within the interior tolerance in between — i.e. a degenerate sliver.
|
|
994
|
+
*/
|
|
995
|
+
const BOUNDARY_DEDUPE_END_TOL_M = 1.5;
|
|
996
|
+
/**
|
|
997
|
+
* Max contact gap (m) tolerated when welding the boundary endpoints of two
|
|
998
|
+
* lanes joined by a next/prev edge. At junction entries/exits the corner on
|
|
999
|
+
* the outer side of a turning connecting lane legitimately sits up to about
|
|
1000
|
+
* `laneWidth * 2 * sin(turnAngle / 2)` away from the incoming lane's corner,
|
|
1001
|
+
* so this is generous; the weld is only ever applied across declared edges,
|
|
1002
|
+
* never discovered by proximity.
|
|
1003
|
+
*/
|
|
1004
|
+
const CONTACT_WELD_MAX_GAP_M = 10;
|
|
1005
|
+
/**
|
|
1006
|
+
* Merge boundary linestrings that trace the same geometry (all resampled
|
|
1007
|
+
* points within tolerance), so adjacent lanes reference one shared
|
|
1008
|
+
* linestring. Reversed duplicates merge too, flipping the lane's invert flag.
|
|
1009
|
+
*
|
|
1010
|
+
* The OpenDRIVE exporter emits one road per lane with its own reference
|
|
1011
|
+
* line, so a boundary shared between two adjacent lanes comes back as two
|
|
1012
|
+
* near-identical linestrings; this pass restores the sharing (and with it
|
|
1013
|
+
* the left/right adjacency information).
|
|
1014
|
+
*/
|
|
1015
|
+
function dedupeSharedBoundaries(result) {
|
|
1016
|
+
const pointById = new Map(result.points.map(p => [p.id, p]));
|
|
1017
|
+
const boundaryIds = new Set();
|
|
1018
|
+
for (const lane of result.lanes) {
|
|
1019
|
+
boundaryIds.add(lane.leftBoundaryId);
|
|
1020
|
+
boundaryIds.add(lane.rightBoundaryId);
|
|
1021
|
+
}
|
|
1022
|
+
const entries = [];
|
|
1023
|
+
for (const ls of result.linestrings) {
|
|
1024
|
+
if (!boundaryIds.has(ls.id))
|
|
1025
|
+
continue;
|
|
1026
|
+
const pts = [];
|
|
1027
|
+
for (const pid of ls.pointIds) {
|
|
1028
|
+
const p = pointById.get(pid);
|
|
1029
|
+
if (p)
|
|
1030
|
+
pts.push({ x: p.x, y: p.y });
|
|
1031
|
+
}
|
|
1032
|
+
if (pts.length >= 2)
|
|
1033
|
+
entries.push({ ls, pts });
|
|
1034
|
+
}
|
|
1035
|
+
const candidates = [];
|
|
1036
|
+
for (let i = 0; i < entries.length; i++) {
|
|
1037
|
+
for (let j = i + 1; j < entries.length; j++) {
|
|
1038
|
+
const m = boundaryMatchScore(entries[i].pts, entries[j].pts);
|
|
1039
|
+
if (m === null)
|
|
1040
|
+
continue;
|
|
1041
|
+
candidates.push({
|
|
1042
|
+
a: entries[i].ls.id,
|
|
1043
|
+
b: entries[j].ls.id,
|
|
1044
|
+
reversed: m.reversed,
|
|
1045
|
+
score: m.score,
|
|
1046
|
+
});
|
|
1047
|
+
}
|
|
1048
|
+
}
|
|
1049
|
+
if (candidates.length === 0)
|
|
1050
|
+
return;
|
|
1051
|
+
// Best (lowest-deviation) matches merge first, so the true duplicate of a
|
|
1052
|
+
// boundary wins over a nearby copy across a very narrow lane; the
|
|
1053
|
+
// left!=right constraint below then blocks the false match.
|
|
1054
|
+
candidates.sort((x, y) => x.score - y.score);
|
|
1055
|
+
// 2. Union-find with orientation parity (0 = same order as parent, 1 =
|
|
1056
|
+
// reversed), constrained so no lane ends up with left === right.
|
|
1057
|
+
const parent = new Map();
|
|
1058
|
+
const parity = new Map();
|
|
1059
|
+
const findWithParity = (x) => {
|
|
1060
|
+
let root = x;
|
|
1061
|
+
let p = 0;
|
|
1062
|
+
while (true) {
|
|
1063
|
+
const up = parent.get(root);
|
|
1064
|
+
if (up === undefined || up === root)
|
|
1065
|
+
break;
|
|
1066
|
+
p ^= parity.get(root) ?? 0;
|
|
1067
|
+
root = up;
|
|
1068
|
+
}
|
|
1069
|
+
// Path compression (re-walk, pointing every node at the root).
|
|
1070
|
+
let cur = x;
|
|
1071
|
+
let curP = p;
|
|
1072
|
+
while (cur !== root) {
|
|
1073
|
+
const up = parent.get(cur);
|
|
1074
|
+
const upP = parity.get(cur) ?? 0;
|
|
1075
|
+
parent.set(cur, root);
|
|
1076
|
+
parity.set(cur, curP);
|
|
1077
|
+
cur = up;
|
|
1078
|
+
curP ^= upP;
|
|
1079
|
+
}
|
|
1080
|
+
return { root, parity: p };
|
|
1081
|
+
};
|
|
1082
|
+
const pairKey = (x, y) => (x < y ? `${x}|${y}` : `${y}|${x}`);
|
|
1083
|
+
/** Current root pairs (left|right) of every lane; merges may not collapse one. */
|
|
1084
|
+
const lanePairs = new Set();
|
|
1085
|
+
const lanesByRoot = new Map();
|
|
1086
|
+
for (const lane of result.lanes) {
|
|
1087
|
+
lanePairs.add(pairKey(lane.leftBoundaryId, lane.rightBoundaryId));
|
|
1088
|
+
for (const b of [lane.leftBoundaryId, lane.rightBoundaryId]) {
|
|
1089
|
+
const list = lanesByRoot.get(b) ?? [];
|
|
1090
|
+
list.push(lane);
|
|
1091
|
+
lanesByRoot.set(b, list);
|
|
1092
|
+
}
|
|
1093
|
+
}
|
|
1094
|
+
let merges = 0;
|
|
1095
|
+
for (const c of candidates) {
|
|
1096
|
+
const fa = findWithParity(c.a);
|
|
1097
|
+
const fb = findWithParity(c.b);
|
|
1098
|
+
if (fa.root === fb.root)
|
|
1099
|
+
continue;
|
|
1100
|
+
if (lanePairs.has(pairKey(fa.root, fb.root)))
|
|
1101
|
+
continue; // would collapse a lane
|
|
1102
|
+
// Attach b's tree under a's root, composing orientation parities.
|
|
1103
|
+
parent.set(fb.root, fa.root);
|
|
1104
|
+
parity.set(fb.root, fa.parity ^ (c.reversed ? 1 : 0) ^ fb.parity);
|
|
1105
|
+
merges++;
|
|
1106
|
+
// Re-key the root pairs of the lanes that referenced b's old root.
|
|
1107
|
+
const moved = lanesByRoot.get(fb.root) ?? [];
|
|
1108
|
+
const target = lanesByRoot.get(fa.root) ?? [];
|
|
1109
|
+
for (const lane of moved) {
|
|
1110
|
+
target.push(lane);
|
|
1111
|
+
lanePairs.add(pairKey(findWithParity(lane.leftBoundaryId).root, findWithParity(lane.rightBoundaryId).root));
|
|
1112
|
+
}
|
|
1113
|
+
lanesByRoot.set(fa.root, target);
|
|
1114
|
+
lanesByRoot.delete(fb.root);
|
|
1115
|
+
}
|
|
1116
|
+
if (merges === 0)
|
|
1117
|
+
return;
|
|
1118
|
+
// 3. Apply: every boundary resolves to its root linestring; a reversed
|
|
1119
|
+
// parity flips the lane's invert flag for that boundary.
|
|
1120
|
+
const replaced = new Map();
|
|
1121
|
+
for (const entry of entries) {
|
|
1122
|
+
const id = entry.ls.id;
|
|
1123
|
+
const f = findWithParity(id);
|
|
1124
|
+
if (f.root !== id)
|
|
1125
|
+
replaced.set(id, { keepId: f.root, reversed: f.parity === 1 });
|
|
1126
|
+
}
|
|
1127
|
+
for (const lane of result.lanes) {
|
|
1128
|
+
const left = replaced.get(lane.leftBoundaryId);
|
|
1129
|
+
if (left) {
|
|
1130
|
+
lane.leftBoundaryId = left.keepId;
|
|
1131
|
+
if (left.reversed)
|
|
1132
|
+
lane.invertLeft = !lane.invertLeft;
|
|
1133
|
+
}
|
|
1134
|
+
const right = replaced.get(lane.rightBoundaryId);
|
|
1135
|
+
if (right) {
|
|
1136
|
+
lane.rightBoundaryId = right.keepId;
|
|
1137
|
+
if (right.reversed)
|
|
1138
|
+
lane.invertRight = !lane.invertRight;
|
|
1139
|
+
}
|
|
1140
|
+
}
|
|
1141
|
+
result.linestrings = result.linestrings.filter(ls => !replaced.has(ls.id));
|
|
1142
|
+
}
|
|
1143
|
+
/**
|
|
1144
|
+
* Position-dependent dedupe tolerance (px): the interior tolerance over the
|
|
1145
|
+
* middle half of the polyline, tapering up to the end tolerance at t=0 / t=1.
|
|
1146
|
+
*/
|
|
1147
|
+
function dedupeTolAt(t) {
|
|
1148
|
+
const interior = BOUNDARY_DEDUPE_INTERIOR_TOL_M * PIXELS_PER_METER;
|
|
1149
|
+
const end = BOUNDARY_DEDUPE_END_TOL_M * PIXELS_PER_METER;
|
|
1150
|
+
// 0 for t in [0.25, 0.75], rising linearly to 1 at t = 0 / t = 1.
|
|
1151
|
+
const edge = Math.max(0, Math.abs(t - 0.5) * 2 - 0.5) / 0.5;
|
|
1152
|
+
return interior + (end - interior) * edge;
|
|
1153
|
+
}
|
|
1154
|
+
/**
|
|
1155
|
+
* Score how well two boundary polylines trace the same line. Each polyline
|
|
1156
|
+
* is resampled by normalized arc length and the distance from every sample
|
|
1157
|
+
* to the OTHER polyline (nearest point on any segment) is taken, normalized
|
|
1158
|
+
* by the graded tolerance; the worst ratio over both directions is the score
|
|
1159
|
+
* (<= 1 means a match). Nearest-point distance is used instead of comparing
|
|
1160
|
+
* param-matched samples because differing vertex distributions of the same
|
|
1161
|
+
* curve cause longitudinal slip that is not a geometric deviation.
|
|
1162
|
+
* The relative orientation is decided by the endpoint pairing, which also
|
|
1163
|
+
* acts as a cheap pre-filter. Returns null for no match.
|
|
1164
|
+
*/
|
|
1165
|
+
function boundaryMatchScore(a, b) {
|
|
1166
|
+
const d = (p, q) => Math.hypot(p.x - q.x, p.y - q.y);
|
|
1167
|
+
const endTolPx = BOUNDARY_DEDUPE_END_TOL_M * PIXELS_PER_METER;
|
|
1168
|
+
const a0 = a[0];
|
|
1169
|
+
const a1 = a[a.length - 1];
|
|
1170
|
+
const b0 = b[0];
|
|
1171
|
+
const b1 = b[b.length - 1];
|
|
1172
|
+
const forwardEnds = Math.max(d(a0, b0), d(a1, b1));
|
|
1173
|
+
const reversedEnds = Math.max(d(a0, b1), d(a1, b0));
|
|
1174
|
+
if (Math.min(forwardEnds, reversedEnds) > endTolPx)
|
|
1175
|
+
return null;
|
|
1176
|
+
const score = Math.max(polylineDeviationScore(a, b), polylineDeviationScore(b, a));
|
|
1177
|
+
if (score > 1)
|
|
1178
|
+
return null;
|
|
1179
|
+
return { reversed: reversedEnds < forwardEnds, score };
|
|
1180
|
+
}
|
|
1181
|
+
/**
|
|
1182
|
+
* Worst nearest-point distance from arc-length resampled points of `a` to
|
|
1183
|
+
* the polyline `b`, normalized by the graded tolerance (1 = at tolerance).
|
|
1184
|
+
*/
|
|
1185
|
+
function polylineDeviationScore(a, b) {
|
|
1186
|
+
const n = Math.max(a.length, 8);
|
|
1187
|
+
let worst = 0;
|
|
1188
|
+
for (let i = 0; i < n; i++) {
|
|
1189
|
+
const t = i / (n - 1);
|
|
1190
|
+
const pa = sampleAtParam(a, t);
|
|
1191
|
+
const score = distanceToPolyline(pa, b) / dedupeTolAt(t);
|
|
1192
|
+
if (score > worst) {
|
|
1193
|
+
worst = score;
|
|
1194
|
+
if (worst > 1)
|
|
1195
|
+
return worst;
|
|
1196
|
+
}
|
|
1197
|
+
}
|
|
1198
|
+
return worst;
|
|
1199
|
+
}
|
|
1200
|
+
/** Distance from a point to the nearest segment of a polyline. */
|
|
1201
|
+
function distanceToPolyline(q, poly) {
|
|
1202
|
+
let best = Infinity;
|
|
1203
|
+
for (let i = 0; i < poly.length - 1; i++) {
|
|
1204
|
+
const ax = poly[i].x;
|
|
1205
|
+
const ay = poly[i].y;
|
|
1206
|
+
const dx = poly[i + 1].x - ax;
|
|
1207
|
+
const dy = poly[i + 1].y - ay;
|
|
1208
|
+
const len2 = dx * dx + dy * dy;
|
|
1209
|
+
let t = len2 > 0 ? ((q.x - ax) * dx + (q.y - ay) * dy) / len2 : 0;
|
|
1210
|
+
if (t < 0)
|
|
1211
|
+
t = 0;
|
|
1212
|
+
else if (t > 1)
|
|
1213
|
+
t = 1;
|
|
1214
|
+
const px = ax + dx * t;
|
|
1215
|
+
const py = ay + dy * t;
|
|
1216
|
+
const dist = Math.hypot(q.x - px, q.y - py);
|
|
1217
|
+
if (dist < best)
|
|
1218
|
+
best = dist;
|
|
1219
|
+
}
|
|
1220
|
+
return best;
|
|
1221
|
+
}
|
|
1222
|
+
/**
|
|
1223
|
+
* Weld the boundary endpoint Points of lanes joined by a next/prev edge into
|
|
1224
|
+
* shared Point shapes (union-find; the cluster centroid is the welded
|
|
1225
|
+
* position). Lane-section chains and road links meet exactly; junction
|
|
1226
|
+
* connections can differ on the outer corner of a turn, which is exactly the
|
|
1227
|
+
* corner a Lanelet2-style map shares between consecutive lanelets.
|
|
1228
|
+
*
|
|
1229
|
+
* Welds are derived ONLY from declared edges, and each weld joins a lane's
|
|
1230
|
+
* end to its successor's start; a lane's own start and end are never merged,
|
|
1231
|
+
* so short connecting lanes survive unchanged.
|
|
1232
|
+
*/
|
|
1233
|
+
function weldConnectedLaneContacts(result) {
|
|
1234
|
+
const maxGapPx = CONTACT_WELD_MAX_GAP_M * PIXELS_PER_METER;
|
|
1235
|
+
const pointById = new Map(result.points.map(p => [p.id, p]));
|
|
1236
|
+
const lsById = new Map(result.linestrings.map(l => [l.id, l]));
|
|
1237
|
+
const laneById = new Map(result.lanes.map(l => [l.id, l]));
|
|
1238
|
+
const parent = new Map();
|
|
1239
|
+
const find = (x) => {
|
|
1240
|
+
let root = x;
|
|
1241
|
+
while (true) {
|
|
1242
|
+
const p = parent.get(root);
|
|
1243
|
+
if (p === undefined || p === root)
|
|
1244
|
+
break;
|
|
1245
|
+
root = p;
|
|
1246
|
+
}
|
|
1247
|
+
let cur = x;
|
|
1248
|
+
while (cur !== root) {
|
|
1249
|
+
const next = parent.get(cur);
|
|
1250
|
+
parent.set(cur, root);
|
|
1251
|
+
cur = next;
|
|
1252
|
+
}
|
|
1253
|
+
return root;
|
|
1254
|
+
};
|
|
1255
|
+
const union = (a, b) => {
|
|
1256
|
+
if (!parent.has(a))
|
|
1257
|
+
parent.set(a, a);
|
|
1258
|
+
if (!parent.has(b))
|
|
1259
|
+
parent.set(b, b);
|
|
1260
|
+
const ra = find(a);
|
|
1261
|
+
const rb = find(b);
|
|
1262
|
+
if (ra !== rb)
|
|
1263
|
+
parent.set(rb, ra);
|
|
1264
|
+
};
|
|
1265
|
+
/** Boundary endpoint Point id at the lane's travel start/end. */
|
|
1266
|
+
const corner = (lane, boundary, side) => {
|
|
1267
|
+
const ls = lsById.get(boundary === 'left' ? lane.leftBoundaryId : lane.rightBoundaryId);
|
|
1268
|
+
if (!ls || ls.pointIds.length === 0)
|
|
1269
|
+
return null;
|
|
1270
|
+
const invert = boundary === 'left' ? lane.invertLeft : lane.invertRight;
|
|
1271
|
+
const ids = ls.pointIds;
|
|
1272
|
+
const atStoredStart = side === 'start' ? !invert : invert;
|
|
1273
|
+
return atStoredStart ? ids[0] : ids[ids.length - 1];
|
|
1274
|
+
};
|
|
1275
|
+
const gap = (aId, bId) => {
|
|
1276
|
+
const a = pointById.get(aId);
|
|
1277
|
+
const b = pointById.get(bId);
|
|
1278
|
+
if (!a || !b)
|
|
1279
|
+
return Infinity;
|
|
1280
|
+
return Math.hypot(a.x - b.x, a.y - b.y);
|
|
1281
|
+
};
|
|
1282
|
+
let welds = 0;
|
|
1283
|
+
for (const lane of result.lanes) {
|
|
1284
|
+
for (const nextId of lane.next) {
|
|
1285
|
+
const next = laneById.get(nextId);
|
|
1286
|
+
if (!next)
|
|
1287
|
+
continue;
|
|
1288
|
+
const aL = corner(lane, 'left', 'end');
|
|
1289
|
+
const aR = corner(lane, 'right', 'end');
|
|
1290
|
+
const bL = corner(next, 'left', 'start');
|
|
1291
|
+
const bR = corner(next, 'right', 'start');
|
|
1292
|
+
if (!aL || !aR || !bL || !bR)
|
|
1293
|
+
continue;
|
|
1294
|
+
if (gap(aL, bL) > maxGapPx || gap(aR, bR) > maxGapPx)
|
|
1295
|
+
continue;
|
|
1296
|
+
union(aL, bL);
|
|
1297
|
+
union(aR, bR);
|
|
1298
|
+
welds++;
|
|
1299
|
+
}
|
|
1300
|
+
}
|
|
1301
|
+
if (welds === 0)
|
|
1302
|
+
return;
|
|
1303
|
+
// Cluster centroid becomes the welded position (representative value).
|
|
1304
|
+
const clusters = new Map();
|
|
1305
|
+
for (const id of parent.keys()) {
|
|
1306
|
+
const root = find(id);
|
|
1307
|
+
const list = clusters.get(root) ?? [];
|
|
1308
|
+
list.push(id);
|
|
1309
|
+
clusters.set(root, list);
|
|
1310
|
+
}
|
|
1311
|
+
for (const [root, members] of clusters) {
|
|
1312
|
+
if (members.length < 2)
|
|
1313
|
+
continue;
|
|
1314
|
+
let sx = 0;
|
|
1315
|
+
let sy = 0;
|
|
1316
|
+
let n = 0;
|
|
1317
|
+
for (const m of members) {
|
|
1318
|
+
const p = pointById.get(m);
|
|
1319
|
+
if (!p)
|
|
1320
|
+
continue;
|
|
1321
|
+
sx += p.x;
|
|
1322
|
+
sy += p.y;
|
|
1323
|
+
n++;
|
|
1324
|
+
}
|
|
1325
|
+
const rp = pointById.get(root);
|
|
1326
|
+
if (!rp || n === 0)
|
|
1327
|
+
continue;
|
|
1328
|
+
rp.x = sx / n;
|
|
1329
|
+
rp.y = sy / n;
|
|
1330
|
+
}
|
|
1331
|
+
// Rewrite linestring point references to the cluster roots and refresh the
|
|
1332
|
+
// anchor coordinates (linestrings anchor on their first point).
|
|
1333
|
+
for (const ls of result.linestrings) {
|
|
1334
|
+
let changed = false;
|
|
1335
|
+
const ids = ls.pointIds.map(id => {
|
|
1336
|
+
if (!parent.has(id))
|
|
1337
|
+
return id;
|
|
1338
|
+
const root = find(id);
|
|
1339
|
+
if (root !== id)
|
|
1340
|
+
changed = true;
|
|
1341
|
+
return root;
|
|
1342
|
+
});
|
|
1343
|
+
if (changed)
|
|
1344
|
+
ls.pointIds = ids;
|
|
1345
|
+
const first = pointById.get(ls.pointIds[0]);
|
|
1346
|
+
if (first) {
|
|
1347
|
+
ls.x = first.x;
|
|
1348
|
+
ls.y = first.y;
|
|
1349
|
+
}
|
|
1350
|
+
}
|
|
1351
|
+
for (const lane of result.lanes) {
|
|
1352
|
+
const ls = lsById.get(lane.leftBoundaryId);
|
|
1353
|
+
if (ls) {
|
|
1354
|
+
lane.x = ls.x;
|
|
1355
|
+
lane.y = ls.y;
|
|
1356
|
+
}
|
|
1357
|
+
}
|
|
1358
|
+
}
|
|
1359
|
+
/** Drop Point records no longer referenced by any linestring. */
|
|
1360
|
+
function removeOrphanPoints(result) {
|
|
1361
|
+
const used = new Set();
|
|
1362
|
+
for (const ls of result.linestrings) {
|
|
1363
|
+
for (const pid of ls.pointIds)
|
|
1364
|
+
used.add(pid);
|
|
1365
|
+
}
|
|
1366
|
+
if (used.size === result.points.length)
|
|
1367
|
+
return;
|
|
1368
|
+
result.points = result.points.filter(p => used.has(p.id));
|
|
1369
|
+
}
|
|
1370
|
+
//# sourceMappingURL=odrToShapes.js.map
|