diff --git a/packages/drawtonomy-sdk/__tests__/exporter/odrElevation.test.ts b/packages/drawtonomy-sdk/__tests__/exporter/odrElevation.test.ts new file mode 100644 index 0000000..2ff0cd7 --- /dev/null +++ b/packages/drawtonomy-sdk/__tests__/exporter/odrElevation.test.ts @@ -0,0 +1,234 @@ +import { describe, it, expect } from 'vitest' +import { parseOpenDriveXml } from '../../src/exporter/opendriveParser' +import { odrToShapes } from '../../src/exporter/odrToShapes' +import { exportToOpenDrive } from '../../src/exporter/opendrive' +import { evalElevation, sampleReferenceLine } from '../../src/exporter/odrGeometry' +import { fitElevationProfile, evalElevationRecords } from '../../src/exporter/odrElevationFit' +import type { DrawtonomySnapshot } from '../../src/types' + +/** A straight road climbing from 12 m to ~15 m over 100 m, in two segments. */ +const SLOPED_ROAD = ` + +
+ + + + + + + + + + + + + + + + + + + + + +` + +/** Same road with no at all. */ +const FLAT_ROAD = SLOPED_ROAD.replace(/[\s\S]*?<\/elevationProfile>/, '') + +/** Build a snapshot from an odrToShapes result, carrying point heights. */ +function snapshotOf(xml: string): DrawtonomySnapshot { + const imported = odrToShapes(parseOpenDriveXml(xml)) + const shapes: unknown[] = [] + for (const p of imported.points) { + shapes.push({ + id: p.id, + type: 'point', + x: p.x, + y: p.y, + rotation: 0, + zIndex: 0, + props: { color: 'black', visible: true, osmId: p.osmId, ...(p.z !== undefined ? { z: p.z } : {}) }, + }) + } + for (const ls of imported.linestrings) { + shapes.push({ + id: ls.id, + type: 'linestring', + x: ls.x, + y: ls.y, + rotation: 0, + zIndex: 0, + props: { pointIds: ls.pointIds, color: 'black', strokeWidth: 2, attributes: ls.attributes, osmId: ls.osmId }, + }) + } + for (const lane of imported.lanes) { + shapes.push({ + id: lane.id, + type: 'lane', + x: lane.x, + y: lane.y, + rotation: 0, + zIndex: 0, + props: { + leftBoundaryId: lane.leftBoundaryId, + rightBoundaryId: lane.rightBoundaryId, + invertLeft: lane.invertLeft, + invertRight: lane.invertRight, + color: 'default', + size: 'm', + attributes: lane.attributes, + next: lane.next, + prev: lane.prev, + osmId: lane.osmId, + }, + }) + } + return { + version: '1.1', + timestamp: new Date().toISOString(), + shapes: shapes as DrawtonomySnapshot['shapes'], + } +} + +describe('elevation parsing', () => { + it('retains records and evaluates them piecewise', () => { + const map = parseOpenDriveXml(SLOPED_ROAD) + const road = map.roads[0] + expect(road.elevations).toHaveLength(2) + expect(road.hasElevation).toBe(true) + // First segment at s = 0 is the record's own `a`. + expect(evalElevation(road.elevations, 0)).toBeCloseTo(12.0, 9) + // At s = 10 (still in segment 1): 12 + 0.02*10 + 1e-4*100 - 5e-7*1000 + expect(evalElevation(road.elevations, 10)).toBeCloseTo(12 + 0.2 + 0.01 - 0.0005, 9) + // At s = 50 the second record takes over. + expect(evalElevation(road.elevations, 50)).toBeCloseTo(13.2, 9) + // Before the first record the profile is 0 (no extrapolation). + expect(evalElevation(road.elevations, -5)).toBe(0) + }) + + it('treats an all-zero profile as no elevation', () => { + const xml = SLOPED_ROAD.replace( + /[\s\S]*?<\/elevationProfile>/, + '' + ) + const road = parseOpenDriveXml(xml).roads[0] + expect(road.elevations).toHaveLength(1) + expect(road.hasElevation).toBe(false) + }) + + it('every sample carries the reference-line height', () => { + const road = parseOpenDriveXml(SLOPED_ROAD).roads[0] + const samples = sampleReferenceLine(road) + for (const s of samples) { + expect(s.z).toBeCloseTo(evalElevation(road.elevations, s.s), 9) + } + }) + + // Regression pin: elevation records must not perturb the 2D station set. + // CARLA exports carry dense all-zero records; when those were inserted as + // stations they shifted the plan-view fit of short junction roads enough + // to open contact-point gaps (ASAM QC lane_smoothness failure on Town01). + it('elevation records leave the 2D station set unchanged', () => { + // Breakpoints deliberately off the 5 m base-station grid — an on-grid + // breakpoint dedupes against an existing station and hides the bug. + const offGrid = SLOPED_ROAD.replace( + /[\s\S]*?<\/elevationProfile>/, + '' + + '' + + '' + + '' + + '' + ) + const withElev = parseOpenDriveXml(offGrid).roads[0] + const noElev = parseOpenDriveXml( + offGrid.replace(/[\s\S]*?<\/elevationProfile>/, '') + ).roads[0] + const a = sampleReferenceLine(withElev) + const b = sampleReferenceLine(noElev) + expect(a.map(p => p.s)).toEqual(b.map(p => p.s)) + expect(a.map(p => [p.x, p.y, p.hdg])).toEqual(b.map(p => [p.x, p.y, p.hdg])) + }) +}) + +describe('elevation on imported points', () => { + it('stamps the reference-line height on every boundary point', () => { + const road = parseOpenDriveXml(SLOPED_ROAD).roads[0] + const imported = odrToShapes(parseOpenDriveXml(SLOPED_ROAD)) + expect(imported.points.length).toBeGreaterThan(0) + const zs = imported.points.map(p => p.z) + expect(zs.every(z => typeof z === 'number')).toBe(true) + const min = Math.min(...(zs as number[])) + const max = Math.max(...(zs as number[])) + // The profile spans roughly 12 m .. 15 m. + expect(min).toBeGreaterThan(11.5) + expect(max).toBeLessThan(16) + expect(max - min).toBeGreaterThan(1) + // Height comes only from the profile, so the extremes match its ends. + expect(min).toBeCloseTo(evalElevation(road.elevations, 0), 6) + }) + + it('leaves points height-free when the road has no elevation profile', () => { + const imported = odrToShapes(parseOpenDriveXml(FLAT_ROAD)) + expect(imported.points.length).toBeGreaterThan(0) + expect(imported.points.every(p => p.z === undefined)).toBe(true) + }) +}) + +describe('fitElevationProfile', () => { + it('emits nothing for empty / all-flat samples', () => { + expect(fitElevationProfile([])).toEqual([]) + expect(fitElevationProfile([{ s: 0, z: 0 }, { s: 10, z: 0 }])).toEqual([]) + }) + + it('fits a constant grade with a single record', () => { + const samples = Array.from({ length: 11 }, (_, i) => ({ s: i * 10, z: 5 + 0.02 * i * 10 })) + const records = fitElevationProfile(samples) + expect(records).toHaveLength(1) + expect(records[0].s).toBe(0) + expect(records[0].a).toBeCloseTo(5, 6) + expect(records[0].b).toBeCloseTo(0.02, 6) + }) + + it('reproduces every sample within tolerance for a curved profile', () => { + const samples = Array.from({ length: 41 }, (_, i) => { + const s = i * 2.5 + return { s, z: 10 + 3 * Math.sin(s / 30) } + }) + const records = fitElevationProfile(samples) + expect(records.length).toBeGreaterThan(0) + expect(records[0].s).toBe(0) + for (const smp of samples) { + expect(Math.abs(evalElevationRecords(records, smp.s) - smp.z)).toBeLessThanOrEqual(0.05) + } + }) + + it('starts the profile at s = 0 even when samples start later', () => { + const records = fitElevationProfile([{ s: 4, z: 7 }, { s: 20, z: 8 }]) + expect(records[0].s).toBe(0) + }) +}) + +describe('elevation round-trip (import -> export)', () => { + it('re-emits a height profile that matches the source within 5 cm', () => { + const source = parseOpenDriveXml(SLOPED_ROAD).roads[0] + const xml = exportToOpenDrive(snapshotOf(SLOPED_ROAD)) + expect(xml).toContain('') + const out = parseOpenDriveXml(xml).roads[0] + expect(out.elevations.length).toBeGreaterThan(0) + expect(out.hasElevation).toBe(true) + // Compare along the road: the exported reference line is the leftmost + // boundary, so stations shift slightly; sample by fraction of length. + for (let f = 0; f <= 1.0001; f += 0.05) { + const srcZ = evalElevation(source.elevations, f * source.length) + const outZ = evalElevation(out.elevations, f * out.length) + expect(Math.abs(outZ - srcZ)).toBeLessThanOrEqual(0.05) + } + }) + + it('keeps emitting an empty profile for roads with no height', () => { + const xml = exportToOpenDrive(snapshotOf(FLAT_ROAD)) + expect(xml).toContain('') + expect(xml).not.toContain(' from per-point heights. +// +// The editor stores a height (m) on each boundary point. When a road is +// regenerated (its shapes were edited, so it cannot be re-emitted verbatim), +// the height samples along the reference line are refitted into the piecewise +// cubic form OpenDRIVE requires: +// +// z(s) = a + b*ds + c*ds^2 + d*ds^3, ds = s - record.s +// +// The fit is segment-wise and C0-continuous by construction: each segment's +// `a` is the height at its start station, and its cubic is solved so the +// segment ends exactly on the next height. Segments are only split where the +// data demands it, so a straight grade emits a single record. +// +// No external dependencies. + +/** One (station, height) sample along a road's reference line. */ +export interface ElevationSample { + /** Station along the reference line (m), ascending. */ + s: number + /** Height above the map datum (m). */ + z: number +} + +/** One `` record: `z(ds) = a + b*ds + c*ds^2 + d*ds^3`. */ +export interface ElevationRecord { + s: number + a: number + b: number + c: number + d: number +} + +export interface FitElevationOptions { + /** + * Maximum allowed height error at any input sample (m). Matches the + * plan-view fitter's default chord tolerance. + */ + maxErrorMeters?: number + /** Heights whose magnitude is below this count as "no elevation" (m). */ + flatEpsilonMeters?: number +} + +const DEFAULT_MAX_ERROR = 0.05 +const DEFAULT_FLAT_EPS = 1e-6 +const S_EPS = 1e-9 + +/** + * Evaluate a fitted profile at station `s` (same rule as the parser: the last + * record with `record.s <= s` applies; before the first record the height is + * 0). + */ +export function evalElevationRecords(records: readonly ElevationRecord[], s: number): number { + if (records.length === 0) return 0 + let lo = 0 + let hi = records.length - 1 + if (s < records[0].s) return 0 + while (lo < hi) { + const mid = (lo + hi + 1) >> 1 + if (records[mid].s <= s) lo = mid + else hi = mid - 1 + } + const rec = records[lo] + const ds = s - rec.s + return rec.a + rec.b * ds + rec.c * ds * ds + rec.d * ds * ds * ds +} + +/** + * Cubic Hermite record spanning [s0, s1] with the given end heights and end + * slopes, expressed in OpenDRIVE's `a + b*ds + c*ds^2 + d*ds^3` form. + */ +function hermiteRecord(s0: number, s1: number, z0: number, z1: number, m0: number, m1: number): ElevationRecord { + const h = s1 - s0 + if (!(h > S_EPS)) return { s: s0, a: z0, b: 0, c: 0, d: 0 } + // p(t) with t = ds/h, then rescale into ds. + // p = z0 + (m0*h) t + (3(z1-z0) - 2 m0 h - m1 h) t^2 + (-2(z1-z0) + m0 h + m1 h) t^3 + const dz = z1 - z0 + const c2 = 3 * dz - 2 * m0 * h - m1 * h + const c3 = -2 * dz + m0 * h + m1 * h + return { s: s0, a: z0, b: m0, c: c2 / (h * h), d: c3 / (h * h * h) } +} + +/** Finite-difference slopes at each sample (monotone-safe enough for roads). */ +function estimateSlopes(samples: readonly ElevationSample[]): number[] { + const n = samples.length + const m = new Array(n).fill(0) + if (n < 2) return m + for (let i = 0; i < n; i++) { + const prev = samples[Math.max(0, i - 1)] + const next = samples[Math.min(n - 1, i + 1)] + const ds = next.s - prev.s + m[i] = ds > S_EPS ? (next.z - prev.z) / ds : 0 + } + return m +} + +/** + * Fit a piecewise cubic elevation profile through `samples`. + * + * Returns an empty array when the samples carry no usable height (all zero / + * fewer than two samples), which the caller emits as `` — + * the existing "no elevation" convention. + * + * The returned records always start at s = 0 so the profile covers the whole + * road, and every input sample is reproduced within `maxErrorMeters`. + */ +export function fitElevationProfile( + samples: readonly ElevationSample[], + options: FitElevationOptions = {} +): ElevationRecord[] { + const maxError = options.maxErrorMeters ?? DEFAULT_MAX_ERROR + const flatEps = options.flatEpsilonMeters ?? DEFAULT_FLAT_EPS + + // Deduplicate / sort by station; drop non-finite samples. + const clean: ElevationSample[] = [] + for (const smp of [...samples].sort((p, q) => p.s - q.s)) { + if (!Number.isFinite(smp.s) || !Number.isFinite(smp.z)) continue + const last = clean[clean.length - 1] + if (last && smp.s - last.s <= S_EPS) { + // Same station twice: keep the later height (endpoints welded by the + // boundary aligner can repeat a station). + last.z = smp.z + continue + } + clean.push({ s: smp.s, z: smp.z }) + } + if (clean.length === 0) return [] + if (clean.every(smp => Math.abs(smp.z) <= flatEps)) return [] + + // A single usable sample means a constant height over the whole road. + if (clean.length === 1) return [{ s: 0, a: clean[0].z, b: 0, c: 0, d: 0 }] + + // Extend to s = 0 so the profile is defined from the road start. + if (clean[0].s > S_EPS) clean.unshift({ s: 0, z: clean[0].z }) + + const slopes = estimateSlopes(clean) + + // Greedy segment growth: extend a record as far as a single cubic through + // (start, end) with the estimated end slopes stays within tolerance at every + // intermediate sample. Emits one record for a constant grade. + const records: ElevationRecord[] = [] + let i = 0 + while (i < clean.length - 1) { + let best: { rec: ElevationRecord; end: number } | null = null + for (let j = i + 1; j < clean.length; j++) { + const rec = hermiteRecord(clean[i].s, clean[j].s, clean[i].z, clean[j].z, slopes[i], slopes[j]) + let ok = true + for (let k = i + 1; k < j; k++) { + const ds = clean[k].s - rec.s + const z = rec.a + rec.b * ds + rec.c * ds * ds + rec.d * ds * ds * ds + if (Math.abs(z - clean[k].z) > maxError) { + ok = false + break + } + } + if (!ok) break + best = { rec, end: j } + } + if (!best) { + // Cannot even span one interval (degenerate station spacing): emit a + // constant record and move on rather than dropping the height. + records.push({ s: clean[i].s, a: clean[i].z, b: 0, c: 0, d: 0 }) + i++ + continue + } + records.push(best.rec) + i = best.end + } + + return records +} diff --git a/packages/drawtonomy-sdk/src/exporter/odrGeometry.ts b/packages/drawtonomy-sdk/src/exporter/odrGeometry.ts index 47d214d..8a699dc 100644 --- a/packages/drawtonomy-sdk/src/exporter/odrGeometry.ts +++ b/packages/drawtonomy-sdk/src/exporter/odrGeometry.ts @@ -283,6 +283,37 @@ export interface ReferenceSample { x: number y: number hdg: number + /** + * Reference-line height at this station (m), evaluated from the road's + * ``. 0 when the road carries no profile — the convention + * throughout the SDK is that an all-zero height means "no elevation", which + * round-trips to an empty ``. + */ + z: number +} + +/** + * Evaluate an OpenDRIVE `` at station `s`. + * + * The applicable record is the last one with `record.s <= s`; before the first + * record (or with no records at all) the height is 0. Each record is a cubic + * in `ds = s - record.s`. + * + * `records` must be sorted by `s` (the parser sorts them). + */ +export function evalElevation(records: readonly { s: number; a: number; b: number; c: number; d: number }[], s: number): number { + if (records.length === 0) return 0 + let lo = 0 + let hi = records.length - 1 + if (s < records[0].s) return 0 + while (lo < hi) { + const mid = (lo + hi + 1) >> 1 + if (records[mid].s <= s) lo = mid + else hi = mid - 1 + } + const rec = records[lo] + const ds = s - rec.s + return rec.a + rec.b * ds + rec.c * ds * ds + rec.d * ds * ds * ds } const STATION_EPS = 1e-9 @@ -376,8 +407,17 @@ export function sampleReferenceLine( } stations.push(roadLength) + // Height is evaluated at the 2D station set as-is. Elevation record + // boundaries are deliberately NOT inserted as stations: extra stations + // perturb the plan-view fit downstream (degenerate all-zero records in + // CARLA exports rotated short junction roads enough to fail the ASAM QC + // contact-point gap check), while the z interpolation error from skipping + // a breakpoint is bounded by the <= 5 m station spacing and realistic + // vertical-curve curvature — centimetres, the same order as the export + // refit tolerance. + const elevations = road.elevations ?? [] return stations.map(s => { const pose = evalAt(s) - return { s, x: pose.x, y: pose.y, hdg: pose.hdg } + return { s, x: pose.x, y: pose.y, hdg: pose.hdg, z: evalElevation(elevations, s) } }) } diff --git a/packages/drawtonomy-sdk/src/exporter/odrToShapes.ts b/packages/drawtonomy-sdk/src/exporter/odrToShapes.ts index 1ef9b11..aa41a4d 100644 --- a/packages/drawtonomy-sdk/src/exporter/odrToShapes.ts +++ b/packages/drawtonomy-sdk/src/exporter/odrToShapes.ts @@ -95,6 +95,12 @@ export interface OdrToShapesOptions { interface EnuPoint { x: number y: number + /** + * Reference-line height at this point (m). Lane boundaries are offset only + * laterally (no superelevation support yet), so every point across a road + * cross-section shares the station's reference height. + */ + z?: number } /** Lanes narrower than this (m) carry no usable area and are skipped. */ @@ -1059,7 +1065,9 @@ export function odrToShapes(map: OdrMap, options: OdrToShapesOptions = {}): OdrI // Lane reference polyline: reference line shifted by the laneOffset. const centerPts: EnuPoint[] = stations.map((st, j) => { const off = laneOffsetAt(road, st.s) - return { x: st.x + normals[j].x * off, y: st.y + normals[j].y * off } + // z is the reference-line height: lateral offsets do not change it + // (superelevation / lateralProfile is still dropped — see warnings). + return { x: st.x + normals[j].x * off, y: st.y + normals[j].y * off, z: st.z } }) // Accumulate boundary polylines from the center outward. Index 0 is the @@ -1071,7 +1079,7 @@ export function odrToShapes(map: OdrMap, options: OdrToShapesOptions = {}): OdrI for (const lane of lanes) { const next = prev.map((p, j) => { const w = laneWidthAt(lane, stations[j].s - sec.s) - return { x: p.x + sign * normals[j].x * w, y: p.y + sign * normals[j].y * w } + return { x: p.x + sign * normals[j].x * w, y: p.y + sign * normals[j].y * w, z: p.z } }) boundaries.push(next) prev = next @@ -1100,6 +1108,9 @@ export function odrToShapes(map: OdrMap, options: OdrToShapesOptions = {}): OdrI } const pointId = idAllocator.next('point') const data: ImportedPoint = { id: pointId, x, y, osmId: '' } + // Keep the third dimension on the point so 2D editing preserves it. + // Omit an exact 0 so "no elevation" roads produce no z at all. + if (p.z !== undefined && p.z !== 0) data.z = p.z result.points.push(data) pointIds.push(pointId) }) @@ -1694,7 +1705,9 @@ export function odrToShapes(map: OdrMap, options: OdrToShapesOptions = {}): OdrI // ---- Aggregated warnings ---- if (elevationRoads > 0) { - warnings.push(`Elevation profiles on ${elevationRoads} road(s) were flattened to 2D.`) + warnings.push( + `Elevation profiles on ${elevationRoads} road(s) were kept as per-point heights (the canvas view stays 2D).` + ) } if (superelevationRoads > 0) { warnings.push(`Superelevation/lateral profiles on ${superelevationRoads} road(s) were ignored (2D import).`) diff --git a/packages/drawtonomy-sdk/src/exporter/opendrive.ts b/packages/drawtonomy-sdk/src/exporter/opendrive.ts index 27c819a..ce646ad 100644 --- a/packages/drawtonomy-sdk/src/exporter/opendrive.ts +++ b/packages/drawtonomy-sdk/src/exporter/opendrive.ts @@ -36,6 +36,7 @@ import type { import { sampleAtParam, type Point2D } from './laneCenterline' import { evalGeometry } from './odrGeometry' import { fitPlanView, type FittedSamplePose } from './odrGeometryFit' +import { fitElevationProfile, type ElevationSample } from './odrElevationFit' import type { OdrGeometry } from './opendriveParser' import { originToProjString } from './projection' import { escapeXml, fmt, fmtPrecise, pxToEnuX, pxToEnuY, pxToMeter } from './units' @@ -75,6 +76,12 @@ interface BundleGeometry { laneWidths: number[][] /** Total fitted reference-line arc length (m). */ length: number + /** + * Reference-line height samples (m) at the fitted stations of the reference + * boundary's own vertices. Empty when the drawn points carry no height, in + * which case the road emits `` as before. + */ + elevationSamples: ElevationSample[] } /** A road bundle: laterally adjacent lanes emitted as one . */ @@ -96,28 +103,41 @@ function collectPoints( pointIds: string[], invert: boolean, pointOverrides: Map -): Point2D[] { +): BoundaryPoint[] { const ids = invert ? [...pointIds].reverse() : pointIds - const pts: Point2D[] = [] + const pts: BoundaryPoint[] = [] for (const id of ids) { + // A point override replaces the planar position only; the height rides on + // the stored point shape (overrides come from planar snapping). + const p = shapeMap.get(id) as unknown as PointShape | undefined + const z = p?.props?.z const ov = pointOverrides.get(id) if (ov) { - pts.push({ x: ov.x, y: ov.y }) + pts.push(z === undefined ? { x: ov.x, y: ov.y } : { x: ov.x, y: ov.y, z }) continue } - const p = shapeMap.get(id) as unknown as PointShape | undefined - if (p) pts.push({ x: p.x, y: p.y }) + if (p) pts.push(z === undefined ? { x: p.x, y: p.y } : { x: p.x, y: p.y, z }) } return pts } +/** + * A boundary vertex in canvas pixels, carrying the optional world height (m) + * stored on the point shape. `z` is in meters even though `x` / `y` are + * pixels: it is never subject to the pixel/meter scale because no planar + * transform touches it. + */ +interface BoundaryPoint extends Point2D { + z?: number +} + /** Boundary polyline of a linestring in travel order, or null when unusable. */ function boundaryPointsOf( shapeMap: Map, boundaryId: string | null, invert: boolean, pointOverrides: Map -): Point2D[] | null { +): BoundaryPoint[] | null { if (!boundaryId) return null const ls = shapeMap.get(boundaryId) as unknown as LinestringShape | undefined if (!ls) return null @@ -275,7 +295,7 @@ function buildBundleGeometry( pointOverrides: Map ): BundleGeometry | null { const first = bundleLanes[0] - const boundaries: Point2D[][] = [] + const boundaries: BoundaryPoint[][] = [] const left = boundaryPointsOf(shapeMap, first.props.leftBoundaryId, first.props.invertLeft, pointOverrides) if (!left) return null boundaries.push(left) @@ -365,7 +385,27 @@ function buildBundleGeometry( samplePoses.map((_, j) => Math.max(0, offsets[i + 1][j] - offsets[i][j])) ) - return { planView: fit.geometries, samplePoses, laneWidths, length: fit.length } + // Elevation samples: the reference boundary's own vertices already have a + // fitted station (fit.samplePoses is index-aligned with `ref`), so the + // height rides along without resampling. Boundaries other than the + // reference share the station's height (no superelevation support yet), so + // taking the reference boundary alone is exact for imported roads. + const elevationSamples: ElevationSample[] = [] + for (let i = 0; i < ref.length && i < fit.samplePoses.length; i++) { + const z = boundaries[0][i]?.z + if (z === undefined) continue + elevationSamples.push({ s: fit.samplePoses[i].s, z }) + } + + return { + planView: fit.geometries, + samplePoses, + laneWidths, + length: fit.length, + // All-or-nothing: a partially annotated boundary would fabricate a datum + // of 0 for the un-annotated stretch and invent a cliff. + elevationSamples: elevationSamples.length === ref.length ? elevationSamples : [], + } } /** @@ -555,6 +595,27 @@ function emitPlanView(geom: BundleGeometry): string { return lines.join('\n') } +/** + * Emit `` from the road's per-point heights. + * + * Roads whose points carry no height (all drawn content, and imported roads + * from flat maps) keep emitting the empty `` — the + * long-standing "no elevation" convention that consumers already handle. + */ +function emitElevationProfile(geom: BundleGeometry): string { + const records = fitElevationProfile(geom.elevationSamples) + if (records.length === 0) return ` ` + const lines: string[] = [` `] + for (const r of records) { + lines.push( + ` ` + ) + } + lines.push(` `) + return lines.join('\n') +} + /** * Map a lanelet-style lane subtype to an OpenDRIVE lane type. The exact * OpenDRIVE type wins when the lane carries `odr_type` (set by the OpenDRIVE @@ -1922,7 +1983,7 @@ function emitRoad( lines.push(` `) } lines.push(emitPlanView(bundle.geom)) - lines.push(` `) + lines.push(emitElevationProfile(bundle.geom)) lines.push(` `) lines.push(emitLanes(bundle, plan, shapeMap)) lines.push(emitObjects(objects)) diff --git a/packages/drawtonomy-sdk/src/exporter/opendriveParser.ts b/packages/drawtonomy-sdk/src/exporter/opendriveParser.ts index d25079a..fb5fa5e 100644 --- a/packages/drawtonomy-sdk/src/exporter/opendriveParser.ts +++ b/packages/drawtonomy-sdk/src/exporter/opendriveParser.ts @@ -72,6 +72,14 @@ export interface OdrLaneOffset extends OdrCubic { s: number } +/** + * `` — road reference-line height, piecewise cubic in + * `ds = s - record.s`: `z(ds) = a + b*ds + c*ds^2 + d*ds^3`. + */ +export interface OdrElevation extends OdrCubic { + s: number +} + /** — lane width polynomial, sOffset relative to the lane section start. */ export interface OdrWidth extends OdrCubic { sOffset: number @@ -256,6 +264,12 @@ export interface OdrRoad { objects: OdrObject[] /** records attached to the road (code -> value). */ userData: Record + /** + * `` records in ascending `s` order (empty when absent). + * Retained so importers can evaluate a height for each sampled station + * instead of dropping the third dimension. + */ + elevations: OdrElevation[] /** True when an with elevation records is present (flattened on import). */ hasElevation: boolean /** True when a with superelevation/shape records is present (flattened on import). */ @@ -672,10 +686,18 @@ function parseRoad(el: XmlNode): OdrRoad { laneSections.sort((a, b) => a.s - b.s) const elevationEl = child(el, 'elevationProfile') - const hasElevation = !!elevationEl && children(elevationEl, 'elevation').some(e => { - // A single flat elevation record (a=b=c=d=0) carries no height information. - return numAttr(e, 'a', 0) !== 0 || numAttr(e, 'b', 0) !== 0 || numAttr(e, 'c', 0) !== 0 || numAttr(e, 'd', 0) !== 0 - }) + const elevations: OdrElevation[] = elevationEl + ? children(elevationEl, 'elevation').map(e => ({ + s: numAttr(e, 's', 0), + a: numAttr(e, 'a', 0), + b: numAttr(e, 'b', 0), + c: numAttr(e, 'c', 0), + d: numAttr(e, 'd', 0), + })) + : [] + elevations.sort((a, b) => a.s - b.s) + // A profile whose records are all flat zeros carries no height information. + const hasElevation = elevations.some(e => e.a !== 0 || e.b !== 0 || e.c !== 0 || e.d !== 0) const lateralEl = child(el, 'lateralProfile') const hasSuperelevation = !!lateralEl && (children(lateralEl, 'superelevation').length > 0 || children(lateralEl, 'shape').length > 0) @@ -770,6 +792,7 @@ function parseRoad(el: XmlNode): OdrRoad { signalReferences, objects, userData: parseUserData(el), + elevations, hasElevation, hasSuperelevation, } diff --git a/packages/drawtonomy-sdk/src/exporter/osmToShapes.ts b/packages/drawtonomy-sdk/src/exporter/osmToShapes.ts index 7d7ecf4..6c03cb7 100644 --- a/packages/drawtonomy-sdk/src/exporter/osmToShapes.ts +++ b/packages/drawtonomy-sdk/src/exporter/osmToShapes.ts @@ -38,6 +38,16 @@ export interface ImportedPoint { x: number y: number osmId: string + /** + * Height above the map datum (m), in world units — NOT canvas pixels like + * `x` / `y`. Present only when the source format carries a third dimension + * (currently the OpenDRIVE importer, from ``). Absent or 0 + * means "no elevation", which round-trips to an empty ``. + * + * The value travels with the point, so ordinary 2D editing (dragging, + * arrow-key nudges, rotation bake) preserves it for free. + */ + z?: number } export interface ImportedLinestring { diff --git a/packages/drawtonomy-sdk/src/types.ts b/packages/drawtonomy-sdk/src/types.ts index 6bf7261..bcd6d90 100644 --- a/packages/drawtonomy-sdk/src/types.ts +++ b/packages/drawtonomy-sdk/src/types.ts @@ -18,6 +18,16 @@ export interface PointProps { color: string visible: boolean osmId: string + /** + * Height above the map datum (m), in world units — NOT canvas pixels like + * the shape's `x` / `y`. Set by importers that read a third dimension + * (currently OpenDRIVE ``); absent or 0 means "no + * elevation", which round-trips to an empty ``. + * + * Because the height rides on the point, plain 2D editing (dragging, + * arrow-key nudges, rotation bake) preserves it without extra plumbing. + */ + z?: number } export interface LinestringProps {