use std::cell::Cell;
use std::collections::{HashMap, HashSet};
use crate::coords::{Point, Vector};
use crate::crg::Stretch;
use crate::object::orient;
use crate::road::{
active, along, height_at, is_valid, side_borders, side_heights, CrossSection, Crossfall, Cubic,
GeomRec, GeomShape, Ground, HeightDef, LaneBorders, LaneExtent, LaneGeom, Lateral, Road,
RoadId, RoadSection, ShapeProfile, Strip, TrafficRule, Validity,
};
use crate::{
Border, ControllerId, Corner, Coverage, CrgAlong, CrgMode, CrgObject, CrgPose, CrgPurpose,
CrgSurface, DefaultRegulations, Direction, Extent, GeoOffset, GeoReference, Header, Lane,
LaneAdvisory, LaneId, LaneType, Layer, Marking, Material, Object, ObjectId, ObjectType,
ParkingSpace, Polyline, Priority, RoadMarkId, RoadMarkWeight, RoadNeighbor, RoadNetwork,
RoadRegulation, Section, Shape, Side, SignalId, SignalRegulation, SkeletonVertex, Structure,
StructureId, StructureKind, UserData,
};
mod additional;
mod curve_local;
mod gaps;
mod ignored;
mod include;
mod introduced;
mod junction_areas;
mod links;
mod properties;
mod railway;
mod records;
mod regulations;
mod road_marks;
mod signals;
mod single_side;
mod versions;
mod virtual_junctions;
mod warning;
pub use additional::{Additional, AdditionalData, DataError, DataQuality, RawData};
pub use include::MAX_INCLUDED_BYTES;
use links::{LaneMeta, RoadInfo, Topology};
use road_marks::MarkDef;
pub use warning::{IncludeProblem, RoadEnd, RoadSkipReason, Warning};
const SAMPLE_STEP: f64 = 2.0;
const SAMPLE_TURN: f64 = 0.05;
const SAMPLE_MIN_STEP: f64 = 0.25;
const SAMPLE_GRADE: f64 = 0.25;
const SAMPLE_PROBE: f64 = 1.0;
const SWEEP_MAX_STEP: f64 = 10.0;
const SWEEP_MIN_STEP: f64 = 0.05;
const SWEEP_TOLERANCE: f32 = 0.01;
const MAX_REPEAT_INSTANCES: f64 = 100_000.0;
const MAX_OUTLINE_CORNERS: usize = 1_000_000;
const MAX_OBJECTS: usize = 100_000;
const MAX_OBJECT_PIECES: usize = 2_000_000;
#[derive(Clone, Copy, Default)]
struct Cost {
objects: usize,
pieces: usize,
}
impl std::ops::Add for Cost {
type Output = Cost;
fn add(self, o: Cost) -> Cost {
Cost {
objects: self.objects.saturating_add(o.objects),
pieces: self.pieces.saturating_add(o.pieces),
}
}
}
impl std::iter::Sum for Cost {
fn sum<I: Iterator<Item = Cost>>(iter: I) -> Cost {
iter.fold(Cost::default(), |a, b| a + b)
}
}
const PAINT_RAISE: f64 = 0.005;
#[derive(Debug, thiserror::Error)]
#[non_exhaustive]
pub enum ImportError {
#[error("invalid OpenDRIVE XML: {0}")]
Xml(#[from] roxmltree::Error),
#[error("malformed OpenDRIVE: {0}")]
Malformed(String),
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct LaneProvenance {
pub lane: LaneId,
pub road_id: String,
pub section: usize,
#[cfg_attr(feature = "serde", serde(default))]
pub layer: Layer,
pub od_id: i32,
pub heights: Vec<LaneHeight>,
#[cfg_attr(feature = "serde", serde(default))]
pub advisory: LaneAdvisory,
#[cfg_attr(feature = "serde", serde(default))]
pub dynamic_lane_type: bool,
#[cfg_attr(feature = "serde", serde(default))]
pub dynamic_lane_direction: bool,
#[cfg_attr(feature = "serde", serde(default))]
pub road_works: bool,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct LaneHeight {
pub inner: f32,
pub outer: f32,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct ObjectProvenance {
pub object: ObjectId,
pub road_id: String,
#[cfg_attr(feature = "serde", serde(default))]
pub junction_id: String,
pub od_id: String,
pub s: f64,
pub t: f64,
pub orientation: Orientation,
pub valid_length: Option<f64>,
pub referenced_from: Option<String>,
#[cfg_attr(feature = "serde", serde(default))]
pub material_road_mark_colors: Vec<String>,
#[cfg_attr(feature = "serde", serde(default))]
pub marking_weights: Vec<RoadMarkWeight>,
#[cfg_attr(feature = "serde", serde(default))]
pub invalidated: bool,
#[cfg_attr(feature = "serde", serde(default))]
pub temporary: bool,
#[cfg_attr(feature = "serde", serde(default))]
pub fill_type: String,
#[cfg_attr(feature = "serde", serde(default))]
pub outline_lane_type: Option<LaneType>,
#[cfg_attr(feature = "serde", serde(default))]
pub skeleton_ids: Vec<SkeletonIds>,
}
#[derive(Debug, Clone, Default, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct SkeletonIds {
pub polyline: String,
pub vertices: Vec<String>,
}
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum Orientation {
Positive,
Negative,
#[default]
Both,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct StructureProvenance {
pub structure: StructureId,
pub road_id: String,
pub od_id: String,
pub s: f64,
pub length: f64,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct SignalProvenance {
pub signal: SignalId,
pub road_id: String,
pub od_id: String,
pub s: f64,
pub t: f64,
pub orientation: Orientation,
pub references: Vec<SignalReferenceProvenance>,
#[cfg_attr(feature = "serde", serde(default))]
pub signs: Vec<SignProvenance>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct SignProvenance {
pub board: usize,
pub sign: usize,
pub id: String,
pub country_revision: String,
pub dynamic: bool,
pub invalidated: bool,
pub temporary: bool,
pub orientation: Orientation,
pub h_offset: f32,
pub pitch: f32,
pub roll: f32,
pub length: Option<f32>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct SignalReferenceProvenance {
pub road_id: String,
pub s: f64,
pub t: f64,
pub orientation: Orientation,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct ControllerProvenance {
pub controller: ControllerId,
pub od_id: String,
pub junctions: Vec<JunctionControllerProvenance>,
}
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct JunctionControllerProvenance {
pub junction_id: String,
pub kind: String,
pub sequence: Option<u32>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct RoadMarkProvenance {
pub road_mark: RoadMarkId,
pub road_id: String,
pub section: usize,
pub od_lane_id: i32,
pub s: f64,
pub length: f64,
#[cfg_attr(feature = "serde", serde(default))]
pub material: String,
#[cfg_attr(feature = "serde", serde(default))]
pub type_name: String,
}
#[derive(Debug, Clone, Default, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct Provenance {
pub lanes: Vec<LaneProvenance>,
#[cfg_attr(feature = "serde", serde(default))]
pub center_lanes: Vec<CenterLaneProvenance>,
pub objects: Vec<ObjectProvenance>,
pub structures: Vec<StructureProvenance>,
pub signals: Vec<SignalProvenance>,
pub controllers: Vec<ControllerProvenance>,
pub road_marks: Vec<RoadMarkProvenance>,
#[cfg_attr(feature = "serde", serde(default))]
pub priorities: Vec<PriorityProvenance>,
#[cfg_attr(feature = "serde", serde(default))]
pub cross_paths: Vec<CrossPathProvenance>,
#[cfg_attr(feature = "serde", serde(default))]
pub junctions: Vec<JunctionProvenance>,
#[cfg_attr(feature = "serde", serde(default))]
pub lane_link_overlaps: Vec<LaneLinkOverlap>,
#[cfg_attr(feature = "serde", serde(default))]
pub additional: Vec<AdditionalData>,
pub warnings: Vec<Warning>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct CenterLaneProvenance {
pub road_id: String,
pub section: usize,
#[cfg_attr(feature = "serde", serde(default))]
pub od_id: Option<i32>,
pub kind: LaneType,
pub level: bool,
#[cfg_attr(feature = "serde", serde(default))]
pub predecessor: Option<i32>,
#[cfg_attr(feature = "serde", serde(default))]
pub successor: Option<i32>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct LaneLinkOverlap {
pub junction_id: String,
pub connection_id: String,
pub incoming_road: String,
pub connecting_road: String,
pub from: i32,
pub to: i32,
pub overlap_zone: f64,
}
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct JunctionProvenance {
pub od_id: String,
pub name: String,
}
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct CrossPathProvenance {
pub junction_id: String,
pub od_id: String,
}
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub struct PriorityProvenance {
pub junction_id: String,
}
pub fn load_file(path: impl AsRef<std::path::Path>) -> Result<RoadNetwork, ImportError> {
load_file_with_provenance(path).map(|(net, _)| net)
}
pub fn load_str(xml: &str) -> Result<RoadNetwork, ImportError> {
load_str_with_provenance(xml).map(|(net, _)| net)
}
pub fn load_file_with_provenance(
path: impl AsRef<std::path::Path>,
) -> Result<(RoadNetwork, Provenance), ImportError> {
let xml = std::fs::read_to_string(path.as_ref())
.map_err(|e| ImportError::Malformed(format!("reading {:?}: {e}", path.as_ref())))?;
let origin = path.as_ref().file_name().map(|n| n.to_string_lossy());
load_with(&xml, origin.as_deref(), include::beside(path.as_ref()))
}
pub fn load_str_with_provenance(xml: &str) -> Result<(RoadNetwork, Provenance), ImportError> {
load_str_with(xml, |_| {
Err("a document read from a string has no folder to read it from".into())
})
}
pub fn load_str_with(
xml: &str,
read: impl FnMut(&str) -> Result<String, String>,
) -> Result<(RoadNetwork, Provenance), ImportError> {
load_with(xml, None, read)
}
fn load_with(
xml: &str,
origin: Option<&str>,
mut read: impl FnMut(&str) -> Result<String, String>,
) -> Result<(RoadNetwork, Provenance), ImportError> {
let mut warnings = Vec::new();
let expanded = include::expand(&sanitize(xml), origin, &mut read, &mut warnings);
bake(&expanded, warnings)
}
fn bake(xml: &str, mut warnings: Vec<Warning>) -> Result<(RoadNetwork, Provenance), ImportError> {
if deeper_than(xml, MAX_DEPTH) {
return Err(ImportError::Malformed(format!(
"elements nested more than {MAX_DEPTH} deep"
)));
}
let doc = roxmltree::Document::parse(xml)?;
let root = doc.root_element();
if let Some(unique) = without_repeated_junctions(xml, root, &mut warnings) {
return bake(&unique, warnings);
}
let mut lanes = Vec::new();
let mut objects = Objects::default();
let mut structures = Structures::default();
let mut surfaces = Vec::new();
let mut topo = Topology::default();
let index = object_index(root);
let grids = junction_grids(root);
let mut roads = Vec::new();
let mut length_left = MAX_BAKED_LENGTH;
for road in root.children().filter(|n| n.has_tag_name("road")) {
let length = attr_f64(road, "length")
.unwrap_or(0.0)
.clamp(0.0, MAX_LENGTH);
let cost = lane_length(road, length);
let parsed = match cost > length_left {
true => Err(RoadSkipReason::MapTooLarge),
false => parse_road(road, &grids, &mut lanes, &mut structures, &mut topo),
};
match parsed {
Ok(mut baked) => {
length_left -= cost;
for node in child(road, "surface")
.into_iter()
.flat_map(|n| n.children())
.filter(|n| n.has_tag_name("CRG"))
{
match road_crg(node, &baked.road, &mut length_left) {
Some(surface) => surfaces.push(surface),
None => warnings.push(crg_dropped(node, &baked.road.od_id, "")),
}
}
baked.road.id = RoadId(roads.len());
if let Err(rule) = traffic_rule(road) {
warnings.push(Warning::UnknownTrafficRule {
road_id: baked.road.od_id.clone(),
rule: rule.to_string(),
});
}
for (section, def) in baked.sections() {
warnings.extend(baked.lane_warnings(section, def));
}
warnings.extend(baked.warnings.iter().cloned());
warnings.extend(baked.shape_warnings());
warnings.extend(gaps::road_length(&baked.road));
warnings.extend(baked.speed_warnings());
warnings.extend(baked.access_warnings());
warnings.extend(baked.visibility_warnings());
warnings.extend(records::check(road));
roads.push((road, baked));
}
Err(reason) => warnings.push(Warning::RoadSkipped {
road_id: road.attribute("id").unwrap_or_default().to_string(),
reason,
}),
}
}
if lanes.is_empty() {
return Err(ImportError::Malformed("no lanes found".into()));
}
let (zones, zone_warnings) = junction_areas::transitions(root, &roads, &grids);
warnings.extend(zone_warnings);
for (_, baked) in roads.iter_mut() {
let zones = baked.road.junction.as_ref().and_then(|j| zones.get(j));
if let (Some(zones), Some(ground)) = (zones, baked.road.lateral.ground.as_mut()) {
ground.transitions = zones.clone();
resample(&baked.road, &mut lanes);
}
}
for (node, baked) in &roads {
if let Some(objects_node) = child(*node, "objects") {
place_objects(objects_node, &index, &baked.road, None, &mut objects);
}
}
for junction in root.children().filter(|n| n.has_tag_name("junction")) {
let Some(objects_node) = child(junction, "objects") else {
continue;
};
match junction_line(junction) {
Some(line) => {
let id = junction.attribute("id");
place_objects(objects_node, &index, &line, id, &mut objects);
}
None => warnings.push(Warning::JunctionObjectsDropped {
junction_id: junction.attribute("id").unwrap_or_default().to_string(),
}),
}
}
surfaces.extend(junction_crgs(root, &topo, &mut warnings));
let placed = std::mem::take(&mut objects.surfaces);
let object_surfaces = object_crgs(root, &topo, &surfaces, placed, &mut warnings);
surfaces.extend(object_surfaces);
let mut signals = signals::place(root, &roads, &lanes, &objects.provenance);
warnings.append(&mut signals.warnings);
let mut road_marks = road_marks::place(&mut roads);
warnings.append(&mut road_marks.warnings);
let header = header(root);
let properties = properties::place(
roads.iter().map(|(_, road)| road),
&lanes,
&header.default_regulations,
);
let (neighbors, dropped_neighbors) = road_neighbors(&roads);
warnings.extend(dropped_neighbors);
let (areas, area_warnings) = junction_areas::place(root, &roads);
warnings.extend(area_warnings);
let (cross_paths, cross_path_provenance, cross_path_warnings) =
junction_areas::cross_paths(root, &roads);
warnings.extend(cross_path_warnings);
let (groups, group_warnings) = junction_areas::groups(root);
warnings.extend(group_warnings);
let (crossings, crossing_warnings) = junction_areas::crossings(root, &roads, &lanes);
warnings.extend(crossing_warnings);
let (switches, switch_warnings) = railway::switches(&roads);
warnings.extend(switch_warnings);
let (stations, station_warnings) = railway::stations(root, &roads);
warnings.extend(station_warnings);
let (junctions, dropped) = links::junctions(root, &mut topo.roads);
topo.junctions = junctions;
warnings.extend(dropped);
let (priorities, priority_provenance, dropped_priorities) = priorities(root, &roads);
warnings.extend(dropped_priorities);
links::resolve(&mut lanes, &topo);
let (speed_limits, sign_warnings) =
regulations::signs_over(properties.speed_limits, &roads, &lanes, &signals);
warnings.extend(sign_warnings);
let (virtual_junctions, virtual_warnings) = virtual_junctions::place(root, &roads, &topo);
warnings.extend(virtual_warnings);
warnings.extend(gaps::link_gaps(&lanes, &topo.metas));
warnings.append(&mut objects.warnings);
warnings.extend(ignored::place(root));
warnings.extend(versions::place(root));
let provenance = Provenance {
center_lanes: roads
.iter()
.flat_map(|(_, road)| {
road.sections().filter_map(|(section, def)| {
let center = def.center_lane?;
Some(CenterLaneProvenance {
road_id: road.road.od_id.clone(),
section: section.index,
od_id: center.od_id,
kind: center.kind,
level: center.level,
predecessor: center.predecessor,
successor: center.successor,
})
})
})
.collect(),
lanes: topo
.metas
.iter()
.map(|m| LaneProvenance {
lane: m.id,
road_id: m.road.clone(),
section: m.section,
layer: m.layer,
od_id: m.od_id,
heights: m.heights.clone(),
advisory: m.flags.advisory,
dynamic_lane_type: m.flags.dynamic_type,
dynamic_lane_direction: m.flags.dynamic_direction,
road_works: m.flags.road_works,
})
.collect(),
objects: objects.provenance,
structures: structures.provenance,
signals: signals.provenance,
controllers: signals.controller_provenance,
road_marks: road_marks.provenance,
junctions: root
.children()
.filter(|n| n.has_tag_name("junction"))
.filter_map(|n| {
Some(JunctionProvenance {
od_id: n.attribute("id")?.to_string(),
name: n.attribute("name").unwrap_or_default().to_string(),
})
})
.collect(),
lane_link_overlaps: lane_link_overlaps(root),
priorities: priority_provenance,
cross_paths: cross_path_provenance,
additional: additional::collect(root),
warnings,
};
Ok((
RoadNetwork::new(lanes)
.with_objects(objects.baked)
.with_structures(structures.baked)
.with_signals(signals.baked)
.with_controllers(signals.controllers)
.with_vms_groups(signals.vms_groups)
.with_road_marks(road_marks.baked)
.with_crg_surfaces(surfaces)
.with_speed_limits(speed_limits)
.with_road_types(properties.road_types)
.with_road_countries(properties.road_countries)
.with_lane_rules(properties.lane_rules)
.with_lane_access(properties.lane_access)
.with_lane_materials(properties.lane_materials)
.with_lane_visibility(properties.lane_visibility)
.with_geo_reference(geo_reference(root))
.with_header(header)
.with_roads(roads.into_iter().map(|(_, baked)| baked.road).collect())
.with_priorities(priorities)
.with_road_neighbors(neighbors)
.with_junction_areas(areas)
.with_cross_paths(cross_paths)
.with_junction_groups(groups)
.with_crossings(crossings)
.with_virtual_junctions(virtual_junctions)
.with_railways(switches, stations),
provenance,
))
}
fn priorities(
root: roxmltree::Node,
roads: &[(roxmltree::Node, BakedRoad)],
) -> (Vec<Priority>, Vec<PriorityProvenance>, Vec<Warning>) {
let road = |id: Option<&str>| {
roads
.iter()
.find(|(_, r)| Some(r.road.od_id.as_str()) == id)
.map(|(_, r)| r.road.id)
};
let (mut out, mut provenance, mut warnings) = (Vec::new(), Vec::new(), Vec::new());
for junction in root.children().filter(|n| n.has_tag_name("junction")) {
let junction_id = junction.attribute("id").unwrap_or_default().to_string();
for p in junction.children().filter(|n| n.has_tag_name("priority")) {
let (high, low) = (p.attribute("high"), p.attribute("low"));
match (road(high), road(low)) {
(Some(high), Some(low)) => {
out.push(Priority { high, low });
provenance.push(PriorityProvenance {
junction_id: junction_id.clone(),
});
}
(found_high, _) => warnings.push(Warning::PriorityDropped {
junction_id: junction_id.clone(),
high: high.unwrap_or_default().to_string(),
low: low.unwrap_or_default().to_string(),
road_id: if found_high.is_some() { high } else { low }
.unwrap_or_default()
.to_string(),
}),
}
}
}
(out, provenance, warnings)
}
fn road_neighbors(roads: &[(roxmltree::Node, BakedRoad)]) -> (Vec<RoadNeighbor>, Vec<Warning>) {
let id_of = |od_id: &str| {
roads
.iter()
.find(|(_, r)| r.road.od_id == od_id)
.map(|(_, r)| r.road.id)
};
let (mut out, mut warnings) = (Vec::new(), Vec::new());
for (node, baked) in roads {
let neighbors = child(*node, "link")
.into_iter()
.flat_map(|l| l.children())
.filter(|n| n.has_tag_name("neighbor"));
for n in neighbors {
let text = |name| n.attribute(name).unwrap_or_default();
let side = match text("side") {
"left" => Some(Side::Left),
"right" => Some(Side::Right),
_ => None,
};
let same_direction = match text("direction") {
"same" => Some(true),
"opposite" => Some(false),
_ => None,
};
match (id_of(text("elementId")), side, same_direction) {
(Some(neighbor), Some(side), Some(same_direction)) => out.push(RoadNeighbor {
road: baked.road.id,
neighbor,
side,
same_direction,
}),
_ => warnings.push(Warning::NeighborDropped {
road_id: baked.road.od_id.clone(),
neighbor_id: text("elementId").to_string(),
side: text("side").to_string(),
direction: text("direction").to_string(),
}),
}
}
}
(out, warnings)
}
fn junction_crgs(
root: roxmltree::Node,
topo: &Topology,
warnings: &mut Vec<Warning>,
) -> Vec<CrgSurface> {
let mut surfaces = Vec::new();
for junction in root.children().filter(|n| n.has_tag_name("junction")) {
let id = junction.attribute("id").unwrap_or_default();
let lanes = junction_lanes(root, topo, id);
for node in child(junction, "surface")
.into_iter()
.flat_map(|n| n.children())
.filter(|n| n.has_tag_name("CRG"))
{
match junction_crg(node, lanes.clone()) {
Some(surface) => surfaces.push(surface),
None => warnings.push(crg_dropped(node, "", id)),
}
}
}
surfaces
}
fn crg_dropped(node: roxmltree::Node, road_id: &str, junction_id: &str) -> Warning {
Warning::CrgDropped {
road_id: road_id.to_string(),
junction_id: junction_id.to_string(),
file: node.attribute("file").unwrap_or_default().to_string(),
}
}
fn known_purpose(node: roxmltree::Node) -> bool {
matches!(
node.attribute("purpose"),
None | Some("elevation" | "friction")
)
}
fn junction_lanes(root: roxmltree::Node, topo: &Topology, id: &str) -> Vec<LaneId> {
let roads: Vec<&str> = root
.children()
.filter(|n| n.has_tag_name("road") && n.attribute("junction") == Some(id))
.filter_map(|n| n.attribute("id"))
.collect();
topo.metas
.iter()
.filter(|m| roads.contains(&m.road.as_str()))
.map(|m| m.id)
.collect()
}
fn object_crgs(
root: roxmltree::Node,
topo: &Topology,
surfaces: &[CrgSurface],
objects: Vec<ObjectCrg>,
warnings: &mut Vec<Warning>,
) -> Vec<CrgSurface> {
let replaces = |road: &CrgSurface, object: &ObjectCrg| {
let (from, to) = object.stretch;
let on_junction = !object.junction_id.is_empty();
let along = on_junction
|| road.road.as_ref().is_none_or(|r| {
let (first, last) = (r.stations.first(), r.stations.last());
first
.zip(last)
.is_some_and(|(a, b)| a.s <= to && from <= b.s)
});
road.purpose == CrgPurpose::Elevation
&& matches!(
road.mode,
CrgMode::Attached0(_) | CrgMode::Genuine { .. } | CrgMode::Global { .. }
)
&& along
&& road.lanes.iter().any(|l| object.surface.lanes.contains(l))
};
objects
.into_iter()
.map(|mut object| {
if !object.junction_id.is_empty() {
object.surface.lanes = junction_lanes(root, topo, &object.junction_id);
}
let hides = matches!(object.surface.mode, CrgMode::Object(o) if o.hide_road);
if hides && surfaces.iter().any(|road| replaces(road, &object)) {
warnings.push(Warning::CrgHideIgnored {
road_id: object.road_id,
junction_id: object.junction_id,
object_id: object.object_id,
});
}
object.surface
})
.collect()
}
const MAX_DEPTH: usize = 256;
fn deeper_than(xml: &str, limit: usize) -> bool {
let b = xml.as_bytes();
let find = |from: usize, pat: &[u8]| {
b[from.min(b.len())..]
.windows(pat.len())
.position(|w| w == pat)
.map_or(b.len(), |k| from + k + pat.len())
};
let (mut i, mut depth) = (0, 0usize);
while let Some(k) = b[i..].iter().position(|&c| c == b'<') {
let at = i + k + 1;
let rest = &b[at..];
i = if rest.starts_with(b"/") {
depth = depth.saturating_sub(1);
find(at, b">")
} else if rest.starts_with(b"!--") {
find(at, b"-->")
} else if rest.starts_with(b"![CDATA[") {
find(at, b"]]>")
} else if rest.starts_with(b"?") {
find(at, b"?>")
} else if rest.starts_with(b"!") {
let close = find(at, b">");
match b[at..close].contains(&b'[') {
true => find(at, b"]>"),
false => close,
}
} else {
let mut j = at;
let mut quote = None;
while j < b.len() {
match (quote, b[j]) {
(None, b'"' | b'\'') => quote = Some(b[j]),
(Some(q), c) if c == q => quote = None,
(None, b'>') => break,
_ => {}
}
j += 1;
}
if b.get(j.wrapping_sub(1)) != Some(&b'/') {
depth += 1;
if depth > limit {
return true;
}
}
j + 1
};
if i >= b.len() {
break;
}
}
false
}
fn without_repeated_junctions(
xml: &str,
root: roxmltree::Node,
warnings: &mut Vec<Warning>,
) -> Option<String> {
let mut seen = HashSet::new();
let repeated: Vec<_> = root
.children()
.filter(|n| n.has_tag_name("junction"))
.filter(|j| !seen.insert(j.attribute("id").unwrap_or_default()))
.collect();
if repeated.is_empty() {
return None;
}
let mut out = String::with_capacity(xml.len());
let mut copied = 0;
for junction in &repeated {
warnings.push(Warning::JunctionIdRepeated {
junction_id: junction.attribute("id").unwrap_or_default().to_string(),
});
out.push_str(&xml[copied..junction.range().start]);
copied = junction.range().end;
}
out.push_str(&xml[copied..]);
Some(out)
}
fn sanitize(xml: &str) -> std::borrow::Cow<'_, str> {
let xml = xml.trim_start_matches('\u{feff}');
if let Some(start) = xml.find("<?xml").filter(|&at| only_comments(&xml[..at])) {
if let Some(rel_end) = xml[start..].find("?>") {
let mut out = String::with_capacity(xml.len());
out.push_str(&xml[..start]);
out.push_str(&xml[start + rel_end + 2..]);
return std::borrow::Cow::Owned(out);
}
}
std::borrow::Cow::Borrowed(xml)
}
fn only_comments(mut text: &str) -> bool {
loop {
text = text.trim_start();
if text.is_empty() {
return true;
}
let Some(rest) = text.strip_prefix("<!--") else {
return false;
};
let Some(end) = rest.find("-->") else {
return false;
};
text = &rest[end + 3..];
}
}
#[derive(Clone)]
struct LaneDef {
id: i32,
kind: LaneType,
pred_links: Vec<links::LaneRef>,
succ_links: Vec<links::LaneRef>,
marks: Vec<MarkDef>,
speeds: Vec<properties::LaneSpeedDef>,
rules: Vec<(f64, String)>,
access: Vec<(f64, properties::AccessDef)>,
materials: Vec<(f64, Material)>,
visibility: Vec<(f64, properties::VisibilityDef)>,
direction: Result<LaneDirection, String>,
flags: LaneFlags,
}
#[derive(Clone, Copy, Default)]
pub(crate) struct LaneFlags {
advisory: LaneAdvisory,
dynamic_type: bool,
dynamic_direction: bool,
road_works: bool,
}
impl LaneFlags {
fn parse(lane: roxmltree::Node) -> Self {
let flag = |name| matches!(lane.attribute(name), Some("true" | "1"));
Self {
advisory: match lane.attribute("advisory") {
Some("inner") => LaneAdvisory::Inner,
Some("outer") => LaneAdvisory::Outer,
Some("both") => LaneAdvisory::Both,
_ => LaneAdvisory::None,
},
dynamic_type: flag("dynamicLaneType"),
dynamic_direction: flag("dynamicLaneDirection"),
road_works: flag("roadWorks"),
}
}
}
#[derive(Clone, Copy)]
enum LaneDirection {
Standard,
Reversed,
Both,
}
impl LaneDirection {
fn parse(lane: roxmltree::Node) -> Result<Self, String> {
match lane.attribute("direction") {
None if lane.attribute("type") == Some("bidirectional") => Ok(Self::Both),
None | Some("standard") => Ok(Self::Standard),
Some("reversed") => Ok(Self::Reversed),
Some("both") => Ok(Self::Both),
Some(other) => Err(other.to_string()),
}
}
fn apply(self, side: Direction) -> Direction {
match (self, side) {
(Self::Both, _) => Direction::Both,
(Self::Standard, side) => side,
(Self::Reversed, Direction::Forward) => Direction::Backward,
(Self::Reversed, _) => Direction::Forward,
}
}
}
fn crossfalls(lateral: roxmltree::Node) -> Vec<Crossfall> {
let mut out: Vec<Crossfall> = lateral
.children()
.filter(|n| n.has_tag_name("crossfall"))
.filter_map(|n| {
let coeff = |name| attr_f64(n, name).unwrap_or(0.0);
let (left, right) = match n.attribute("side") {
Some("left") => (true, false),
Some("right") => (false, true),
_ => (true, true),
};
Some(Crossfall {
angle: Cubic {
start: attr_f64(n, "s")?.max(0.0),
a: coeff("a"),
b: coeff("b"),
c: coeff("c"),
d: coeff("d"),
},
left,
right,
})
})
.collect();
out.sort_by(|a, b| a.angle.start.total_cmp(&b.angle.start));
out
}
fn shape_profiles(lateral: roxmltree::Node) -> Vec<ShapeProfile> {
let mut shapes: Vec<(f64, Cubic)> = lateral
.children()
.filter(|n| n.has_tag_name("shape"))
.filter_map(|n| {
let coeff = |name| attr_f64(n, name).unwrap_or(0.0);
Some((
attr_f64(n, "s")?,
Cubic {
start: attr_f64(n, "t")?,
a: coeff("a"),
b: coeff("b"),
c: coeff("c"),
d: coeff("d"),
},
))
})
.collect();
shapes.sort_by(|(s1, c1), (s2, c2)| s1.total_cmp(s2).then(c1.start.total_cmp(&c2.start)));
let mut profiles: Vec<ShapeProfile> = Vec::new();
for (s, shape) in shapes {
match profiles.last_mut() {
Some(p) if p.s == s => p.shapes.push(shape),
_ => profiles.push(ShapeProfile {
s,
shapes: vec![shape],
}),
}
}
profiles
}
fn cross_section(
node: roxmltree::Node,
road_id: &str,
warnings: &mut Vec<Warning>,
) -> CrossSection {
let records = |parent: Option<roxmltree::Node>| {
parent.map_or_else(Vec::new, |p| cubics_in(p, "coefficients", "s"))
};
let mut out = CrossSection {
t_offset: records(child(node, "tOffset")),
..CrossSection::default()
};
let strips = child(node, "surfaceStrips")
.into_iter()
.flat_map(|n| n.children())
.filter(|n| n.has_tag_name("strip"));
let dropped = |warnings: &mut Vec<Warning>, strip: String| {
warnings.push(Warning::StripDropped {
road_id: road_id.to_string(),
strip,
})
};
for strip in strips {
let id = attr_i32(strip, "id");
let slot = match id {
Some(1) => &mut out.left[0],
Some(2) => &mut out.left[1],
Some(-1) => &mut out.right[0],
Some(-2) => &mut out.right[1],
_ => {
dropped(
warnings,
strip.attribute("id").unwrap_or_default().to_string(),
);
continue;
}
};
if slot.is_some() {
dropped(warnings, id.unwrap_or_default().to_string());
continue;
}
let outer = id.is_some_and(|id| id.abs() == 2);
let relative = match strip.attribute("mode") {
Some("relative") if outer => true,
_ if !outer => false,
None | Some("independent") => false,
Some(other) => {
warnings.push(Warning::UnknownStripMode {
road_id: road_id.to_string(),
strip: id.unwrap_or_default(),
mode: other.to_string(),
});
false
}
};
*slot = Some(Strip {
width: records(child(strip, "width")),
terms: [
records(child(strip, "constant")),
records(child(strip, "linear")),
records(child(strip, "quadratic")),
records(child(strip, "cubic")),
],
relative,
});
}
for (side, id) in [(&mut out.left, 2), (&mut out.right, -2)] {
let beside = side[0]
.as_ref()
.is_some_and(|inner| !inner.width.is_empty());
if side[1].is_some() && !beside {
side[1] = None;
dropped(warnings, id.to_string());
}
}
out
}
fn parse_heights(lane: roxmltree::Node) -> Vec<HeightDef> {
let mut heights: Vec<HeightDef> = lane
.children()
.filter(|n| n.has_tag_name("height"))
.map(|h| HeightDef {
s_offset: attr_f64(h, "sOffset").unwrap_or(0.0).max(0.0),
inner: attr_f64(h, "inner").unwrap_or(0.0),
outer: attr_f64(h, "outer").unwrap_or(0.0),
})
.collect();
heights.sort_by(|a, b| a.s_offset.total_cmp(&b.s_offset));
heights
}
#[derive(Clone, Copy)]
struct CenterDef {
od_id: Option<i32>,
kind: LaneType,
level: bool,
predecessor: Option<i32>,
successor: Option<i32>,
}
struct SectionDef {
left: Vec<LaneDef>,
right: Vec<LaneDef>,
geoms: [Vec<LaneGeom>; 2],
center: Vec<MarkDef>,
center_lane: Option<CenterDef>,
dropped: Vec<i32>,
bad_ids: Vec<String>,
misplaced: Vec<i32>,
repeated: Vec<i32>,
mixed: Vec<i32>,
}
impl SectionDef {
fn lane(&self, od_id: i32) -> Option<&LaneDef> {
self.left.iter().chain(&self.right).find(|l| l.id == od_id)
}
fn parse(section: roxmltree::Node) -> Self {
let (mut left, mut right, mut dropped) = (Vec::new(), Vec::new(), Vec::new());
let (mut bad_ids, mut misplaced, mut repeated) = (Vec::new(), Vec::new(), Vec::new());
let mut seen = HashSet::new();
let (mut with_widths, mut with_borders) = (false, Vec::new());
for side in ["left", "right"] {
let Some(side_node) = child(section, side) else {
continue;
};
for lane in side_node.children().filter(|n| n.has_tag_name("lane")) {
let id = attr_i32(lane, "id");
let Some(id) = id.filter(|&id| id != 0) else {
bad_ids.push(lane.attribute("id").unwrap_or_default().to_string());
continue;
};
if (side == "left") != (id > 0) {
misplaced.push(id);
continue;
}
if !seen.insert(id) {
repeated.push(id);
continue;
}
let widths = lane_cubics(lane.children().filter(|n| n.has_tag_name("width")));
let borders = lane_cubics(lane.children().filter(|n| n.has_tag_name("border")));
with_widths |= !widths.is_empty();
if !borders.is_empty() {
with_borders.push(id);
}
let extent = match (widths.is_empty(), borders.is_empty()) {
(false, _) => LaneExtent::Width(widths),
(true, false) => LaneExtent::Border(borders),
(true, true) => {
dropped.push(id);
continue;
}
};
let (pred_links, succ_links) = links::lane_refs(lane);
let def = LaneDef {
id,
kind: lane_type(lane.attribute("type")),
pred_links,
succ_links,
marks: road_marks::parse(lane),
speeds: properties::lane_speeds(lane),
rules: properties::lane_rules(lane),
access: properties::lane_access(lane),
materials: properties::lane_materials(lane),
visibility: properties::lane_visibility(lane),
direction: LaneDirection::parse(lane),
flags: LaneFlags::parse(lane),
};
let geom = LaneGeom {
id,
extent,
heights: parse_heights(lane),
level: matches!(lane.attribute("level"), Some("true" | "1")),
};
let def = (def, geom);
if id > 0 {
left.push(def);
} else {
right.push(def);
}
}
}
let mixed = if with_widths {
with_borders
} else {
Vec::new()
};
left.sort_by_key(|(l, _)| l.id);
right.sort_by_key(|(l, _)| std::cmp::Reverse(l.id));
let (left, left_geoms) = left.into_iter().unzip();
let (right, right_geoms) = right.into_iter().unzip();
let center_node = child(section, "center")
.into_iter()
.flat_map(|c| c.children())
.find(|n| n.has_tag_name("lane"));
let center = center_node.map(road_marks::parse).unwrap_or_default();
let center_lane = center_node.map(|n| {
let (predecessor, successor) = links::lane_link(n);
CenterDef {
od_id: n.attribute("id").and_then(|v| v.trim().parse().ok()),
kind: lane_type(n.attribute("type")),
level: matches!(n.attribute("level"), Some("true" | "1")),
predecessor,
successor,
}
});
Self {
left,
right,
geoms: [left_geoms, right_geoms],
center,
center_lane,
dropped,
bad_ids,
misplaced,
repeated,
mixed,
}
}
}
fn lane_type(od_type: Option<&str>) -> LaneType {
let Some(od_type) = od_type else {
return LaneType::Unknown;
};
match od_type {
"none" => LaneType::None,
"driving" => LaneType::Driving,
"bidirectional" => LaneType::Bidirectional,
"bus" => LaneType::Bus,
"taxi" => LaneType::Taxi,
"HOV" => LaneType::Hov,
"entry" | "mwyEntry" => LaneType::Entry,
"exit" | "mwyExit" => LaneType::Exit,
"onRamp" => LaneType::OnRamp,
"offRamp" => LaneType::OffRamp,
"connectingRamp" => LaneType::ConnectingRamp,
"slipLane" => LaneType::SlipLane,
"parking" => LaneType::Parking,
"stop" => LaneType::Stop,
"restricted" => LaneType::Restricted,
"biking" => LaneType::Biking,
"sidewalk" | "walking" => LaneType::Sidewalk,
"shared" => LaneType::Shared,
"shoulder" => LaneType::Shoulder,
"border" => LaneType::Border,
"curb" => LaneType::Curb,
"median" => LaneType::Median,
"roadWorks" => LaneType::RoadWorks,
"tram" => LaneType::Tram,
"rail" => LaneType::Rail,
"special1" => LaneType::Special1,
"special2" => LaneType::Special2,
"special3" => LaneType::Special3,
_ => LaneType::Unknown,
}
}
fn attr_f64(node: roxmltree::Node, name: &str) -> Option<f64> {
node.attribute(name)
.and_then(|s| s.trim().parse::<f64>().ok())
.filter(|v| v.is_finite())
}
fn attr_i32(node: roxmltree::Node, name: &str) -> Option<i32> {
node.attribute(name)?.trim().parse().ok()
}
fn attr_f32(node: roxmltree::Node, name: &str) -> Option<f32> {
attr_f64(node, name)
.map(|v| v as f32)
.filter(|v| v.is_finite())
}
fn traffic_rule<'a>(road: roxmltree::Node<'a, '_>) -> Result<TrafficRule, &'a str> {
match road.attribute("rule") {
None | Some("RHT") => Ok(TrafficRule::RightHand),
Some("LHT") => Ok(TrafficRule::LeftHand),
Some(other) => Err(other),
}
}
fn material(node: roxmltree::Node) -> Material {
Material {
surface: node.attribute("surface").unwrap_or_default().to_string(),
friction: attr_f32(node, "friction").filter(|v| *v >= 0.0),
roughness: attr_f32(node, "roughness").filter(|v| *v >= 0.0),
}
}
fn lane_link_overlaps(root: roxmltree::Node) -> Vec<LaneLinkOverlap> {
let mut out = Vec::new();
for junction in root.children().filter(|n| n.has_tag_name("junction")) {
let Some(junction_id) = junction.attribute("id") else {
continue;
};
for connection in junction.children().filter(|n| n.has_tag_name("connection")) {
for link in connection.children().filter(|n| n.has_tag_name("laneLink")) {
let lane = |name| link.attribute(name).and_then(|v| v.trim().parse().ok());
let (Some(from), Some(to), Some(overlap_zone)) =
(lane("from"), lane("to"), attr_f64(link, "overlapZone"))
else {
continue;
};
let text = |name| connection.attribute(name).unwrap_or_default().to_string();
out.push(LaneLinkOverlap {
junction_id: junction_id.to_string(),
connection_id: text("id"),
incoming_road: text("incomingRoad"),
connecting_road: connection
.attribute("connectingRoad")
.or(connection.attribute("linkedRoad"))
.unwrap_or_default()
.to_string(),
from,
to,
overlap_zone,
});
}
}
}
out
}
fn header(root: roxmltree::Node) -> Header {
let Some(node) = root.children().find(|n| n.has_tag_name("header")) else {
return Header::default();
};
let text = |name| node.attribute(name).unwrap_or_default().to_string();
let revision = |name| node.attribute(name).and_then(|v| v.trim().parse().ok());
Header {
rev_major: revision("revMajor"),
rev_minor: revision("revMinor"),
name: text("name"),
version: text("version"),
date: text("date"),
vendor: text("vendor"),
north: attr_f64(node, "north"),
south: attr_f64(node, "south"),
east: attr_f64(node, "east"),
west: attr_f64(node, "west"),
default_regulations: child(node, "defaultRegulations")
.map(default_regulations)
.unwrap_or_default(),
license: additional::license(node),
}
}
fn default_regulations(node: roxmltree::Node) -> DefaultRegulations {
let rules = signals::semantics;
let text = |n: roxmltree::Node, names: &[&str]| {
let found = names.iter().find_map(|&name| n.attribute(name));
found.unwrap_or_default().to_string()
};
DefaultRegulations {
roads: (node.children())
.filter(|n| n.has_tag_name("roadRegulations"))
.map(|n| RoadRegulation {
road_type: properties::road_type(n.attribute("type")),
semantics: rules(n),
})
.collect(),
signals: (node.children())
.filter(|n| n.has_tag_name("signalRegulations"))
.map(|n| SignalRegulation {
kind: text(n, &["type"]),
subtype: text(n, &["subtype", "subType"]),
semantics: rules(n),
})
.collect(),
}
}
fn geo_reference(root: roxmltree::Node) -> GeoReference {
let Some(header) = root.children().find(|n| n.has_tag_name("header")) else {
return GeoReference::default();
};
let child = |tag| header.children().find(|n| n.has_tag_name(tag));
GeoReference {
proj: child("geoReference")
.and_then(|g| g.text())
.map(str::trim)
.filter(|p| !p.is_empty())
.map(String::from),
offset: child("offset").map(|o| {
let at = |name| attr_f64(o, name).unwrap_or(0.0);
GeoOffset {
x: at("x"),
y: at("y"),
z: at("z"),
hdg: at("hdg"),
}
}),
}
}
fn parse_road(
road: roxmltree::Node,
grids: &Grids,
out: &mut Vec<Lane>,
structures: &mut Structures,
topo: &mut Topology,
) -> Result<BakedRoad, RoadSkipReason> {
let road_id = road.attribute("id").unwrap_or_default().to_string();
let length = attr_f64(road, "length")
.filter(|l| *l > 0.0)
.ok_or(RoadSkipReason::NoLength)?;
if length > MAX_LENGTH {
return Err(RoadSkipReason::TooLong);
}
let rule = traffic_rule(road).unwrap_or(TrafficRule::RightHand);
let plan_view = child(road, "planView").ok_or(RoadSkipReason::NoPlanView)?;
let mut geoms: Vec<GeomRec> = Vec::new();
let mut dropped = Vec::new();
for g in plan_view.children().filter(|n| n.has_tag_name("geometry")) {
match geometry(g) {
Some(rec) => geoms.push(rec),
None => dropped.push(Warning::GeometryDropped {
road_id: road_id.clone(),
s: attr_f64(g, "s"),
}),
}
}
if geoms.is_empty() {
return Err(RoadSkipReason::NoGeometry);
}
geoms.sort_by(|a, b| a.s.total_cmp(&b.s));
let elevations = child(road, "elevationProfile")
.map(|n| cubics_in(n, "elevation", "s"))
.unwrap_or_default();
let superelevations = child(road, "lateralProfile")
.map(|n| cubics_in(n, "superelevation", "s"))
.unwrap_or_default();
let mut warnings = dropped;
let lateral = Lateral {
profiles: child(road, "lateralProfile")
.map(shape_profiles)
.unwrap_or_default(),
surface: child(road, "lateralProfile")
.and_then(|n| child(n, "crossSectionSurface"))
.map(|n| cross_section(n, &road_id, &mut warnings)),
ground: None,
crossfall: child(road, "lateralProfile")
.map(crossfalls)
.unwrap_or_default(),
};
if lateral.surface.is_some() && (!lateral.profiles.is_empty() || !superelevations.is_empty()) {
warnings.push(Warning::CrossSectionWithShape {
road_id: road_id.clone(),
});
}
let (permanent, temporary) = layer_nodes(road);
let lane_offsets = permanent
.map(|n| cubics_in(n, "laneOffset", "s"))
.unwrap_or_default();
let temporary_lane_offsets = temporary
.map(|n| cubics_in(n, "laneOffset", "s"))
.unwrap_or_default();
let junction = road
.attribute("junction")
.filter(|j| !j.is_empty() && *j != "-1")
.map(String::from);
let (mut lateral, mut elevations, mut superelevations) = (lateral, elevations, superelevations);
if let Some(grid) = junction.as_ref().and_then(|j| grids.get(j)) {
lateral = Lateral {
ground: Some(Box::new(Ground {
grid: grid.clone(),
plan: geoms.clone(),
elevations: std::mem::take(&mut elevations),
superelevations: std::mem::take(&mut superelevations),
transitions: Vec::new(),
})),
..Lateral::default()
};
}
let mut baked = BakedRoad {
road: Road {
id: RoadId(0),
junction,
od_id: road_id,
name: road.attribute("name").unwrap_or_default().to_string(),
length,
rule,
geoms,
elevations,
superelevations,
lateral,
lane_offsets,
temporary_lane_offsets,
sections: Vec::new(),
},
types: properties::road_types(road),
defs: Vec::new(),
warnings,
};
let (lanes_before, metas_before, next_before) = (out.len(), topo.metas.len(), topo.next_id);
let info_before = topo.roads.remove(&baked.road.od_id);
bake_lanes(road, &mut baked, out, topo);
if !out[lanes_before..].iter().all(finite) {
out.truncate(lanes_before);
topo.metas.truncate(metas_before);
topo.next_id = next_before;
topo.registry.retain(|_, id| id.0 < next_before);
match info_before {
Some(info) => topo.roads.insert(baked.road.od_id.clone(), info),
None => topo.roads.remove(&baked.road.od_id),
};
return Err(RoadSkipReason::OutOfRange);
}
if let Some(objects_node) = child(road, "objects") {
place_structures(
objects_node,
&baked.road,
out,
structures,
&mut baked.warnings,
);
}
Ok(baked)
}
fn resample(road: &Road, lanes: &mut [Lane]) {
for section in &road.sections {
for (side, sign) in [(§ion.left, 1.0), (§ion.right, -1.0)] {
let shapes = side_shapes(
road,
(section.layer, side),
sign,
section.start,
§ion.stations,
);
for (lane, shape) in side.iter().zip(shapes) {
let id = section.lanes.iter().find(|(od_id, _)| *od_id == lane.id);
let (Some(&(_, id)), Some(shape)) = (id, shape) else {
continue;
};
let baked = &mut lanes[id.0];
baked.center = shape.center;
baked.width = shape.width;
baked.widths = shape.widths;
baked.bank = shape.bank;
}
}
}
}
type Grids = HashMap<String, crate::ElevationGrid>;
fn junction_grids(root: roxmltree::Node) -> Grids {
root.children()
.filter(|n| n.has_tag_name("junction"))
.filter_map(|j| {
let grid = junction_areas::grid(j, child(j, "elevationGrid")?)?;
Some((j.attribute("id")?.to_string(), grid))
})
.collect()
}
fn geometry(g: roxmltree::Node) -> Option<GeomRec> {
let (Some(s), Some(x), Some(y), Some(hdg), Some(length)) = (
attr_f64(g, "s"),
attr_f64(g, "x"),
attr_f64(g, "y"),
attr_f64(g, "hdg"),
attr_f64(g, "length"),
) else {
return None;
};
if length <= 0.0 || length > MAX_LENGTH {
return None;
}
let shape = if let Some(arc) = child(g, "arc") {
let curvature = attr_f64(arc, "curvature")?;
GeomShape::Arc { curvature }
} else if let Some(sp) = child(g, "spiral") {
let (curv_start, curv_end) = (attr_f64(sp, "curvStart")?, attr_f64(sp, "curvEnd")?);
GeomShape::Spiral {
curv_start,
curv_end,
}
} else if let Some(pp) = child(g, "paramPoly3") {
let coeff = |n: &str| attr_f64(pp, n).unwrap_or(0.0);
let p_max = match pp.attribute("pRange").map(str::to_ascii_lowercase) {
Some(ref r) if r == "arclength" => length,
_ => 1.0,
};
GeomShape::ParamPoly3 {
u: [coeff("aU"), coeff("bU"), coeff("cU"), coeff("dU")],
v: [coeff("aV"), coeff("bV"), coeff("cV"), coeff("dV")],
p_max,
}
} else if let Some(p3) = child(g, "poly3") {
let coeff = |n: &str| attr_f64(p3, n).unwrap_or(0.0);
GeomShape::ParamPoly3 {
u: [0.0, 1.0, 0.0, 0.0],
v: [coeff("a"), coeff("b"), coeff("c"), coeff("d")],
p_max: length,
}
} else if child(g, "line").is_some() {
GeomShape::Line
} else {
return None;
};
Some(GeomRec::new(s, x, y, hdg, length, shape)).filter(GeomRec::is_finite)
}
fn finite(lane: &Lane) -> bool {
let center = &lane.center;
let points = center.points().iter().map(|p| p.to_array());
let tangents = center.tangents().iter().map(|t| t.to_array());
points.chain(tangents).flatten().all(f32::is_finite)
&& lane.width.is_finite()
&& lane.widths.iter().chain(&lane.bank).all(|v| v.is_finite())
}
pub const MAX_BAKED_LENGTH: f64 = 2_000_000.0;
fn lane_length(road: roxmltree::Node, length: f64) -> f64 {
let layers = road.children().filter(|n| n.has_tag_name("lanes"));
layers
.map(|lanes| {
let mut sections: Vec<(f64, usize)> = lanes
.children()
.filter(|n| n.has_tag_name("laneSection"))
.map(|section| {
let sides = ["left", "right"]
.into_iter()
.filter_map(|side| child(section, side));
let count = sides
.map(|side| side.children().filter(|n| n.has_tag_name("lane")).count())
.sum();
(
attr_f64(section, "s").unwrap_or(0.0).clamp(0.0, length),
count,
)
})
.collect();
sections.sort_by(|a, b| a.0.total_cmp(&b.0));
let ends = sections.iter().skip(1).map(|&(s, _)| s).chain([length]);
sections
.iter()
.zip(ends)
.map(|(&(s, count), end)| (end - s) * count as f64)
.sum::<f64>()
})
.sum()
}
pub const MAX_LENGTH: f64 = 100_000.0;
const BORDER_CROSSING: f64 = 1e-6;
const SHAPE_SHORT: f64 = 1e-3;
impl BakedRoad {
fn lane_warnings(&self, section: &RoadSection, def: &SectionDef) -> Vec<Warning> {
let road = &self.road;
let (road_id, index) = (&road.od_id, section.index);
let mut out: Vec<Warning> = def
.dropped
.iter()
.map(|&lane| Warning::LaneDropped {
road_id: road_id.clone(),
section: index,
lane,
})
.chain(def.mixed.iter().map(|&lane| Warning::WidthAndBorder {
road_id: road_id.clone(),
section: index,
lane,
}))
.chain(def.misplaced.iter().map(|&lane| Warning::LaneOnWrongSide {
road_id: road_id.clone(),
section: index,
lane,
}))
.chain(def.repeated.iter().map(|&lane| Warning::LaneIdRepeated {
road_id: road_id.clone(),
section: index,
lane,
}))
.collect();
out.splice(
0..0,
def.bad_ids.iter().map(|id| Warning::LaneIdUnreadable {
road_id: road_id.clone(),
section: index,
id: id.clone(),
}),
);
let border_lanes = || {
section
.left
.iter()
.chain(§ion.right)
.filter(|l| matches!(l.extent, LaneExtent::Border(_)))
};
if offset_within(road.offsets_of(section.layer), section.start, section.end) {
out.extend(border_lanes().map(|l| Warning::BorderWithLaneOffset {
road_id: road_id.clone(),
section: index,
lane: l.id,
}));
}
let mut crossed: Vec<(i32, f64)> = Vec::new();
if border_lanes().next().is_some() {
for &s in §ion.stations {
let base = road.base(s);
let s_lane = s - section.start;
for (side, sign) in [(§ion.left, 1.0), (§ion.right, -1.0)] {
for (lane, b) in side.iter().zip(side_borders(side, sign, base, s_lane)) {
let LaneExtent::Border(borders) = &lane.extent else {
continue;
};
let inside = active(borders, s_lane)
.is_some_and(|r| sign * (r.eval(s_lane) - b.inner) < -BORDER_CROSSING);
if inside && !crossed.iter().any(|&(id, _)| id == lane.id) {
crossed.push((lane.id, s));
}
}
}
}
}
out.extend(
crossed
.into_iter()
.map(|(lane, s)| Warning::BorderCrossesInnerLane {
road_id: road_id.clone(),
section: index,
lane,
s,
}),
);
for lane in def.left.iter().chain(&def.right) {
if let Err(direction) = &lane.direction {
out.push(Warning::UnknownLaneDirection {
road_id: road_id.clone(),
section: index,
lane: lane.id,
direction: direction.clone(),
});
}
}
for side in [§ion.left, §ion.right] {
let first_level = side.iter().position(|l| l.level).unwrap_or(side.len());
out.extend(side[first_level..].iter().filter(|l| !l.level).map(|l| {
Warning::LaneNotLevel {
road_id: road_id.clone(),
section: index,
lane: l.id,
}
}));
}
out
}
fn shape_warnings(&self) -> Vec<Warning> {
let road = &self.road;
road.lateral
.profiles
.iter()
.filter(|p| {
let Some(section) = road.section_at(p.s) else {
return false;
};
let edge = road
.across(section, p.s)
.fold(road.base(p.s), |edge, a| edge.min(a.inner_t).min(a.outer_t));
p.shapes[0].start > edge + SHAPE_SHORT
})
.map(|p| Warning::ShapeShortOfRoad {
road_id: road.od_id.clone(),
s: p.s,
})
.collect()
}
}
fn offset_within(lane_offsets: &[Cubic], start: f64, end: f64) -> bool {
lane_offsets.iter().enumerate().any(|(i, r)| {
let until = lane_offsets.get(i + 1).map_or(f64::INFINITY, |n| n.start);
r.start < end && until > start && [r.a, r.b, r.c, r.d].iter().any(|&c| c != 0.0)
})
}
const CRG_STEP: f64 = 0.25;
const CRG_COST: f64 = 8.0;
fn road_crg(node: roxmltree::Node, road: &Road, length_left: &mut f64) -> Option<CrgSurface> {
node.attribute("file")?;
if !known_purpose(node) {
return None;
}
let from = attr_f64(node, "sStart")?.max(0.0);
let to = attr_f64(node, "sEnd")?.min(road.length);
let cost = (to - from) * CRG_COST;
if to <= from || cost > *length_left {
return None;
}
let offset = |name: &str| attr_f64(node, name).unwrap_or(0.0);
let along = CrgAlong {
s_offset: offset("sOffset"),
t_offset: offset("tOffset"),
opposite: node.attribute("orientation") == Some("opposite"),
};
let mode = match node.attribute("mode")? {
"attached" => CrgMode::Attached(along),
"attached0" => CrgMode::Attached0(along),
"genuine" => {
let at = road.station(along.s_offset);
let t = along.t_offset * at.bank.cos();
let (sin, cos) = at.heading.sin_cos();
CrgMode::Genuine {
start: CrgPose {
x: at.x - t * sin,
y: at.y + t * cos,
heading: at.heading + offset("hOffset"),
},
}
}
"global" => global_crg(node),
_ => return None,
};
*length_left -= cost;
let steps = ((to - from) / CRG_STEP).ceil().max(1.0) as usize;
let stations = (0..=steps)
.map(|k| road.station(from + (to - from) * k as f64 / steps as f64))
.collect();
Some(crg_surface(
node,
mode,
road.lanes((from, to), &[]),
Some(Stretch { stations }),
))
}
fn junction_crg(node: roxmltree::Node, lanes: Vec<LaneId>) -> Option<CrgSurface> {
if node.attribute("mode") != Some("global") || !known_purpose(node) {
return None;
}
Some(crg_surface(node, global_crg(node), lanes, None))
}
fn global_crg(node: roxmltree::Node) -> CrgMode {
let offset = |name: &str| attr_f64(node, name).unwrap_or(0.0);
CrgMode::Global {
origin: CrgPose {
x: offset("xOffset"),
y: offset("yOffset"),
heading: offset("hOffset"),
},
}
}
fn crg_surface(
node: roxmltree::Node,
mode: CrgMode,
lanes: Vec<LaneId>,
road: Option<Stretch>,
) -> CrgSurface {
let purpose = match node.attribute("purpose") {
Some("friction") => CrgPurpose::Friction,
_ => CrgPurpose::Elevation,
};
let (z_offset, z_scale) = match purpose {
CrgPurpose::Elevation => (
attr_f64(node, "zOffset").unwrap_or(0.0),
attr_f64(node, "zScale").unwrap_or(1.0),
),
CrgPurpose::Friction => (0.0, 1.0),
};
CrgSurface {
file: node.attribute("file").unwrap_or_default().to_string(),
purpose,
mode,
z_offset,
z_scale,
lanes,
road,
}
}
struct BakedRoad {
road: Road,
types: Vec<properties::TypeDef>,
defs: Vec<SectionDef>,
warnings: Vec<Warning>,
}
impl BakedRoad {
fn sections(&self) -> impl Iterator<Item = (&RoadSection, &SectionDef)> {
self.road.sections.iter().zip(&self.defs)
}
}
fn bake_lanes(
road: roxmltree::Node,
baked: &mut BakedRoad,
out: &mut Vec<Lane>,
topo: &mut Topology,
) {
let (permanent, temporary) = layer_nodes(road);
let start = |section: &roxmltree::Node| attr_f64(*section, "s").unwrap_or(0.0).max(0.0);
let length = baked.road.length;
let mut next_index = 0;
let mut any = false;
for (layer, lanes_node) in [(Layer::Permanent, permanent), (Layer::Temporary, temporary)] {
let Some(lanes_node) = lanes_node else {
continue;
};
let mut sections: Vec<roxmltree::Node> = lanes_node
.children()
.filter(|n| n.has_tag_name("laneSection"))
.collect();
sections.sort_by(|a, b| start(a).total_cmp(&start(b)));
any |= !sections.is_empty();
let starts: Vec<f64> = sections.iter().map(start).collect();
let defs = single_side::full_sections(§ions, &starts);
for (i, (section, mut def)) in sections.iter().zip(defs).enumerate() {
let index = next_index + i;
let s_start = start(section);
let next = sections.get(i + 1).map_or(length, start);
let explicit = match layer {
Layer::Temporary => attr_f64(*section, "length").filter(|l| *l > 0.0),
Layer::Permanent => None,
};
let s_end = explicit
.map_or(next, |l| (s_start + l).min(next))
.min(length);
if s_end - s_start < 1e-3 {
if s_start > length + 1e-3 {
baked.warnings.push(Warning::LaneSectionDropped {
road_id: baked.road.od_id.clone(),
section: index,
s: s_start,
});
}
continue;
}
let first = topo.metas.len();
let [left, right] = std::mem::take(&mut def.geoms);
let mut geom = RoadSection {
index,
layer,
start: s_start,
end: s_end,
stations: Vec::new(),
left,
right,
lanes: Vec::new(),
marks: Vec::new(),
};
geom.stations = emit_section(&baked.road, &geom, &def, out, topo);
geom.lanes = topo.metas[first..]
.iter()
.map(|m| (m.od_id, m.id))
.collect();
baked.road.sections.push(geom);
baked.defs.push(def);
}
next_index += sections.len();
}
if !any {
return;
}
let (predecessor, successor) = links::road_link(road);
for (end, target) in [(RoadEnd::Start, &predecessor), (RoadEnd::End, &successor)] {
let guessed = target
.as_ref()
.is_some_and(|t| t.elem == links::ElemType::Road && !t.contact_given && !t.along);
if guessed {
baked.warnings.push(Warning::ContactPointMissing {
road_id: baked.road.od_id.clone(),
end,
});
}
}
topo.roads.insert(
baked.road.od_id.clone(),
RoadInfo {
sections: baked
.road
.sections
.iter()
.map(|s| links::SectionInfo {
index: s.index,
layer: s.layer,
start: s.start,
end: s.end,
})
.collect(),
length: baked.road.length,
predecessor,
successor,
},
);
}
fn layer_nodes<'a>(
road: roxmltree::Node<'a, 'a>,
) -> (
Option<roxmltree::Node<'a, 'a>>,
Option<roxmltree::Node<'a, 'a>>,
) {
let all: Vec<_> = road
.children()
.filter(|n| n.has_tag_name("lanes"))
.collect();
let temporary_of =
|n: &roxmltree::Node| Layer::from_attr(n.attribute("layer")) == Layer::Temporary;
let permanent = all
.iter()
.find(|n| !temporary_of(n))
.or(all.first())
.copied();
let temporary = all
.iter()
.find(|n| temporary_of(n) && Some(**n) != permanent)
.copied();
(permanent, temporary)
}
struct LaneShape {
center: Polyline,
width: f32,
widths: Vec<f32>,
bank: Vec<f32>,
}
fn side_shapes(
road: &Road,
(layer, side): (Layer, &[LaneGeom]),
sign: f64,
s_start: f64,
sample_s: &[f64],
) -> Vec<Option<LaneShape>> {
let road_bank: Vec<f64> = sample_s
.iter()
.map(|&s| active(&road.superelevations, s).map_or(0.0, |e| e.eval(s)))
.collect();
let level_bank = unless_flat(road_bank.iter().map(|&b| b as f32).collect());
let across = side_across(road, layer, side, sign, s_start, sample_s);
let heights = side_height_rows(
road,
(layer, side),
sign,
s_start,
sample_s,
&road_bank,
&across,
);
side.iter()
.zip(&across)
.zip(&heights)
.enumerate()
.map(|(k, ((lane, borders), heights))| {
let held = side[..=k].iter().any(|l| l.level);
let (points, headings) = sample_lane(road, borders, heights, sign, sample_s);
let (&start_tangent, &end_tangent) = (headings.first()?, headings.last()?);
let center = Polyline::try_new_with_tangents(points, start_tangent, end_tangent)?;
let tilted = !lane.heights.is_empty() || !road.lateral.is_empty() || held;
let (width, widths) = width_profile(borders, tilted.then_some(&heights[..]));
let bank = match tilted {
true => lane_bank(borders, heights, sign, &road_bank),
false => level_bank.clone(),
};
Some(LaneShape {
center,
width,
widths,
bank,
})
})
.collect()
}
fn emit_section(
road: &Road,
section: &RoadSection,
def: &SectionDef,
out: &mut Vec<Lane>,
topo: &mut Topology,
) -> Vec<f64> {
let (s_start, s_end) = (section.start, section.end);
let (road_id, section_idx) = (&road.od_id, section.index);
let bends = section_bends(
&road.geoms,
road.offsets_of(section.layer),
[§ion.left, §ion.right],
s_start,
s_end,
);
let knots: Vec<f64> = section
.left
.iter()
.chain(§ion.right)
.flat_map(|l| &l.heights)
.map(|h| s_start + h.s_offset)
.chain(road.lateral.knots())
.collect();
let sample_s = sample_positions(s_start, s_end, &bends, &knots);
for (defs, side) in [(&def.left, §ion.left), (&def.right, §ion.right)] {
let is_left = side.first().map(|l| l.id > 0).unwrap_or(false);
let sign = if is_left { 1.0 } else { -1.0 };
let side_direction = road.rule.direction(is_left);
let mut emitted: Vec<(LaneId, usize, LaneType, Direction)> = Vec::new();
let shapes = side_shapes(road, (section.layer, side), sign, s_start, &sample_s);
for ((def, lane), shape) in defs.iter().zip(side).zip(shapes) {
let kind = def.kind;
let direction = def
.direction
.as_ref()
.map_or(LaneDirection::Standard, |d| *d)
.apply(side_direction);
let Some(shape) = shape else {
continue;
};
let id = LaneId(topo.next_id);
topo.next_id += 1;
topo.registry
.insert((road_id.to_string(), section_idx, lane.id), id);
topo.metas.push(LaneMeta {
id,
road: road_id.to_string(),
section: section_idx,
od_id: lane.id,
direction,
layer: section.layer,
succ_links: def.succ_links.clone(),
pred_links: def.pred_links.clone(),
heights: lane_heights(lane, s_start, &sample_s),
flags: def.flags,
});
emitted.push((id, out.len(), kind, direction));
out.push(Lane {
id,
kind,
layer: section.layer,
direction,
center: shape.center,
width: shape.width,
widths: shape.widths,
bank: shape.bank,
successors: Vec::new(),
predecessors: Vec::new(),
neighbors: Vec::new(),
});
}
for k in 0..emitted.len() {
let (_, at, kind, direction) = emitted[k];
if !kind.is_drivable() {
continue;
}
let nbrs = [k.checked_sub(1), Some(k + 1)]
.into_iter()
.flatten()
.filter_map(|n| emitted.get(n))
.filter(|(_, _, kind, d)| kind.is_drivable() && *d == direction)
.map(|(id, _, _, _)| *id)
.collect();
out[at].neighbors = nbrs;
}
}
sample_s
}
fn lane_bank(
borders: &[LaneBorders],
heights: &[(f64, f64)],
sign: f64,
road_bank: &[f64],
) -> Vec<f32> {
unless_flat(
borders
.iter()
.zip(heights)
.zip(road_bank)
.map(|((b, &(inner, outer)), &phi)| {
let slope = (outer - inner).atan2(b.width);
(phi + sign * slope) as f32
})
.collect(),
)
}
fn unless_flat(bank: Vec<f32>) -> Vec<f32> {
if bank.iter().all(|b| b.abs() < 1e-9) {
Vec::new()
} else {
bank
}
}
fn lane_heights(lane: &LaneGeom, s_start: f64, sample_s: &[f64]) -> Vec<LaneHeight> {
if lane.heights.is_empty() {
return Vec::new();
}
let heights: Vec<LaneHeight> = sample_s
.iter()
.map(|&s| {
let (inner, outer) = height_at(lane, s - s_start);
LaneHeight {
inner: inner as f32,
outer: outer as f32,
}
})
.collect();
if heights.iter().all(|h| h.inner == 0.0 && h.outer == 0.0) {
Vec::new()
} else {
heights
}
}
fn sample_lane(
road: &Road,
borders: &[LaneBorders],
heights: &[(f64, f64)],
sign: f64,
sample_s: &[f64],
) -> (Vec<Point>, Vec<Vector>) {
sample_s
.iter()
.zip(borders)
.zip(heights)
.map(|((&s, b), &(inner, outer))| {
let t = b.inner + sign * b.width / 2.0;
let (point, hdg) = road.raised(s, t, (inner + outer) / 2.0);
let heading = Vector::new(hdg.cos() as f32, hdg.sin() as f32, 0.0);
(point, heading)
})
.unzip()
}
fn side_across(
road: &Road,
layer: Layer,
side: &[LaneGeom],
sign: f64,
s_start: f64,
sample_s: &[f64],
) -> Vec<Vec<LaneBorders>> {
let mut rows: Vec<Vec<LaneBorders>> = (0..side.len())
.map(|_| Vec::with_capacity(sample_s.len()))
.collect();
for &s in sample_s {
let base = road.base_of(layer, s);
for (row, b) in rows
.iter_mut()
.zip(side_borders(side, sign, base, s - s_start))
{
row.push(b);
}
}
rows
}
fn side_height_rows(
road: &Road,
(layer, side): (Layer, &[LaneGeom]),
sign: f64,
s_start: f64,
sample_s: &[f64],
road_bank: &[f64],
across: &[Vec<LaneBorders>],
) -> Vec<Vec<(f64, f64)>> {
let mut rows: Vec<Vec<(f64, f64)>> = (0..side.len())
.map(|_| Vec::with_capacity(sample_s.len()))
.collect();
for (i, (&s, &phi)) in sample_s.iter().zip(road_bank).enumerate() {
let borders = across.iter().map(|row| row[i]);
let heights = side_heights(
side,
sign,
&road.lateral,
(s, s - s_start),
road.base_of(layer, s),
phi,
borders,
);
for (row, h) in rows.iter_mut().zip(heights) {
row.push(h);
}
}
rows
}
fn width_profile(borders: &[LaneBorders], heights: Option<&[(f64, f64)]>) -> (f32, Vec<f32>) {
let widths: Vec<f32> = match heights {
None => borders.iter().map(|b| b.width as f32).collect(),
Some(heights) => borders
.iter()
.zip(heights)
.map(|(b, (inner, outer))| b.width.hypot(outer - inner) as f32)
.collect(),
};
let nominal = widths.iter().copied().fold(0.0_f32, f32::max);
let constant = widths.iter().all(|w| (w - nominal).abs() < 1e-6);
(nominal, if constant { Vec::new() } else { widths })
}
fn sample_positions(start: f64, end: f64, bends: &[f64], knots: &[f64]) -> Vec<f64> {
let n = bends.len().saturating_sub(1).max(1);
let h = (end - start) / n as f64;
let mut want: Vec<f64> = bends
.iter()
.map(|&b| (SAMPLE_TURN / b).clamp(SAMPLE_MIN_STEP, SAMPLE_STEP))
.collect();
for k in 1..want.len() {
want[k] = want[k].min(want[k - 1] + SAMPLE_GRADE * h);
}
for k in (1..want.len()).rev() {
want[k - 1] = want[k - 1].min(want[k] + SAMPLE_GRADE * h);
}
let last = want.len() - 1;
let probe = |s: f64| ((s - start) / h).clamp(0.0, last as f64);
let want_at = |s: f64| {
let p = probe(s);
let k = (p.floor() as usize).min(last.saturating_sub(1));
match want.get(k + 1) {
Some(&next) => want[k] + (next - want[k]) * (p - k as f64),
None => want[k],
}
};
let mut ss = Vec::new();
let mut s = start;
while s < end - 1e-6 {
ss.push(s);
let reach = want_at(s);
let inside = want[probe(s).ceil() as usize..=probe(s + reach).floor() as usize]
.iter()
.copied();
let step = inside.fold(reach.min(want_at(s + reach)), f64::min);
let knot = knots
.iter()
.map(|k| k - s)
.filter(|&d| d > 1e-6 && s + d < end - 1e-6)
.fold(f64::INFINITY, f64::min);
let next = s + if knot <= step {
knot
} else if knot < 2.0 * step {
knot / 2.0
} else {
step
};
if next <= s {
break;
}
s = next;
}
ss.push(end);
ss
}
fn section_bends(
geoms: &[GeomRec],
lane_offsets: &[Cubic],
sides: [&[LaneGeom]; 2],
start: f64,
end: f64,
) -> Vec<f64> {
let n = ((end - start) / SAMPLE_PROBE).ceil().max(1.0) as usize;
let h = (end - start) / n as f64;
let mut geom = 0;
let mut edges = |s: f64, out: &mut Vec<(f64, f64)>| {
while geom + 1 < geoms.len() && geoms[geom + 1].s <= s + 1e-9 {
geom += 1;
}
let (x, y, hdg) = geoms[geom].pose(s);
let (sin, cos) = hdg.sin_cos();
let base = active(lane_offsets, s).map_or(0.0, |o| o.eval(s));
out.clear();
out.push((x - base * sin, y + base * cos));
for (side, sign) in sides.into_iter().zip([1.0, -1.0]) {
for b in side_borders(side, sign, base, s - start) {
out.push((x - b.outer * sin, y + b.outer * cos));
}
}
};
let (mut prev, mut next) = (Vec::new(), Vec::new());
let mut chords: Vec<(f64, f64)> = Vec::new();
let mut bends = vec![0.0; n + 1];
edges(start, &mut prev);
for k in 1..=n {
edges(start + k as f64 * h, &mut next);
let mut sharpest = 0.0_f64;
for (e, (a, b)) in prev.iter().zip(&next).enumerate() {
let chord = (b.0 - a.0, b.1 - a.1);
if k > 1 {
let (u, v) = (chords[e], chord);
let turn = (u.0 * v.1 - u.1 * v.0).atan2(u.0 * v.0 + u.1 * v.1);
sharpest = sharpest.max(turn.abs() / h);
chords[e] = chord;
} else {
chords.push(chord);
}
}
if k > 1 {
bends[k - 1] = sharpest;
}
std::mem::swap(&mut prev, &mut next);
}
if n > 1 {
bends[0] = bends[1];
bends[n] = bends[n - 1];
}
(0..=n)
.map(|k| {
bends[k.saturating_sub(1)..=(k + 1).min(n)]
.iter()
.copied()
.fold(0.0, f64::max)
})
.collect()
}
fn cubics_in(parent: roxmltree::Node, item: &str, start_attr: &str) -> Vec<Cubic> {
let mut out: Vec<Cubic> = parent
.children()
.filter(|n| n.has_tag_name(item))
.filter_map(|n| {
Some(Cubic {
start: attr_f64(n, start_attr)?,
a: attr_f64(n, "a").unwrap_or(0.0),
b: attr_f64(n, "b").unwrap_or(0.0),
c: attr_f64(n, "c").unwrap_or(0.0),
d: attr_f64(n, "d").unwrap_or(0.0),
})
})
.collect();
out.sort_by(|a, b| a.start.total_cmp(&b.start));
out
}
fn lane_cubics<'a>(records: impl Iterator<Item = roxmltree::Node<'a, 'a>>) -> Vec<Cubic> {
let mut out: Vec<Cubic> = records
.filter_map(|n| {
Some(Cubic {
start: attr_f64(n, "sOffset").unwrap_or(0.0),
a: attr_f64(n, "a")?,
b: attr_f64(n, "b").unwrap_or(0.0),
c: attr_f64(n, "c").unwrap_or(0.0),
d: attr_f64(n, "d").unwrap_or(0.0),
})
})
.collect();
out.sort_by(|a, b| a.start.total_cmp(&b.start));
out
}
#[derive(Default)]
struct Objects {
baked: Vec<Object>,
provenance: Vec<ObjectProvenance>,
warnings: Vec<Warning>,
cost: Cost,
wasted: usize,
surfaces: Vec<ObjectCrg>,
}
impl Objects {
fn affords(&self, cost: Cost) -> bool {
let total = self.cost + cost;
total.objects <= MAX_OBJECTS
&& total.pieces <= MAX_OBJECT_PIECES
&& self.wasted <= MAX_OBJECT_PIECES
}
fn spend(&mut self, cost: Cost) -> bool {
let fits = self.affords(cost);
if fits {
self.cost = self.cost + cost;
}
fits
}
}
struct ObjectCrg {
surface: CrgSurface,
road_id: String,
junction_id: String,
object_id: String,
stretch: (f64, f64),
}
struct Station {
s: f64,
t: f64,
ground: Option<Point>,
z_offset: f64,
length: Option<f64>,
width: Option<f64>,
height: Option<f64>,
radius: Option<f64>,
}
impl Station {
fn sized(self) -> Self {
let above = |v: Option<f64>| v.filter(|v| *v > 0.0);
Self {
length: above(self.length),
width: above(self.width),
height: self.height.filter(|h| *h >= 0.0),
radius: above(self.radius),
..self
}
}
}
struct Frame {
origin: Point,
axes: [[f64; 3]; 3],
}
impl Frame {
fn at(
road: &Road,
st: &Station,
perp_to_road: bool,
(hdg, pitch, roll): (f64, f64, f64),
) -> Self {
let (on_road, heading) = road.surface(st.s, st.t);
let ground = st.ground.unwrap_or(on_road);
if perp_to_road {
let axes = road.axes(st.s);
let raise = axes[2].map(|c| (c * st.z_offset) as f32);
return Self::on_road(ground + Vector::from_array(raise), axes, (hdg, 0.0, 0.0));
}
let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
let origin = ground + Vector::Z * st.z_offset as f32;
Self::on_road(origin, level, (heading + hdg, pitch, roll))
}
fn on_road(origin: Point, road: [[f64; 3]; 3], (hdg, pitch, roll): (f64, f64, f64)) -> Self {
let turned = |local: [f64; 3]| {
let [u, v, z] = orient(hdg, pitch, roll, local);
[0, 1, 2].map(|i| road[0][i] * u + road[1][i] * v + road[2][i] * z)
};
Self {
origin,
axes: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]].map(turned),
}
}
fn point(&self, local: [f64; 3]) -> Point {
let [u, v, w] = self.axes;
let at = |i: usize| local[0] * u[i] + local[1] * v[i] + local[2] * w[i];
self.origin + Vector::new(at(0) as f32, at(1) as f32, at(2) as f32)
}
fn angles(&self) -> (f64, f64, f64) {
let [u, v, w] = self.axes;
let pitch = (-u[2]).clamp(-1.0, 1.0).asin();
if u[2].abs() > 1.0 - 1e-9 {
return (f64::atan2(-v[0], v[1]), pitch, 0.0);
}
(f64::atan2(u[1], u[0]), pitch, f64::atan2(v[2], w[2]))
}
}
#[derive(Default)]
struct Faults {
too_many: Cell<bool>,
pieces_left: Cell<usize>,
painted: Cell<usize>,
gap: Cell<f64>,
skeleton_dropped: Cell<bool>,
outline_dropped: Cell<bool>,
marking_dropped: Cell<bool>,
turned: Cell<bool>,
}
impl Faults {
fn affords(&self, out: &Objects, cost: Cost) -> bool {
cost.pieces <= self.pieces_left.get() && out.affords(cost)
}
fn spend(&self, out: &mut Objects, cost: Cost) -> bool {
let fits = cost.pieces <= self.pieces_left.get() && out.spend(cost);
if fits {
self.pieces_left.set(self.pieces_left.get() - cost.pieces);
}
fits
}
}
#[derive(Default)]
struct Structures {
baked: Vec<Structure>,
provenance: Vec<StructureProvenance>,
}
pub(super) fn coverage(
road: &Road,
lanes: &[Lane],
(from, to): (f64, f64),
validity: &[Validity],
) -> Vec<Coverage> {
let mut covered = Vec::new();
for sec in &road.sections {
let (from, to) = (from.max(sec.start), to.min(sec.end));
if to - from < 1e-6 {
continue;
}
for &(_, id) in sec
.lanes
.iter()
.filter(|(od_id, _)| is_valid(*od_id, sec.layer, validity))
{
let center = &lanes[id.0].center;
covered.push(Coverage {
lane: id,
from: along(center.points(), &sec.stations, from),
to: along(center.points(), &sec.stations, to),
});
}
}
covered
}
fn place_structures(
objects_node: roxmltree::Node,
road: &Road,
lanes: &[Lane],
out: &mut Structures,
warnings: &mut Vec<Warning>,
) {
for node in objects_node.children() {
let text = |name| node.attribute(name).unwrap_or_default().to_string();
let kind = if node.has_tag_name("tunnel") {
StructureKind::Tunnel {
kind: text("type"),
lighting: attr_f64(node, "lighting").map(|v| v as f32),
daylight: attr_f64(node, "daylight").map(|v| v as f32),
}
} else if node.has_tag_name("bridge") {
StructureKind::Bridge { kind: text("type") }
} else {
continue;
};
let (Some(s), Some(length)) = (attr_f64(node, "s"), attr_f64(node, "length")) else {
warnings.push(Warning::StructureDropped {
road_id: road.od_id.clone(),
structure_id: text("id"),
});
continue;
};
if length < 0.0 {
warnings.push(Warning::StructureDropped {
road_id: road.od_id.clone(),
structure_id: text("id"),
});
continue;
}
let covered = coverage(road, lanes, (s, s + length), &validity(node));
let id = StructureId(out.baked.len());
out.baked.push(Structure {
id,
kind,
name: text("name"),
lanes: covered,
});
out.provenance.push(StructureProvenance {
structure: id,
road_id: road.od_id.clone(),
od_id: text("id"),
s,
length,
});
}
}
fn junction_line(junction: roxmltree::Node) -> Option<Road> {
let mut geoms: Vec<GeomRec> = child(junction, "planView")?
.children()
.filter(|n| n.has_tag_name("geometry"))
.filter_map(geometry)
.collect();
if geoms.is_empty() {
return None;
}
geoms.sort_by(|a, b| a.s.total_cmp(&b.s));
Some(Road {
id: RoadId(usize::MAX),
od_id: String::new(),
name: String::new(),
junction: None,
length: geoms.iter().map(|g| g.s + g.length).fold(0.0, f64::max),
rule: TrafficRule::RightHand,
geoms,
elevations: Vec::new(),
superelevations: Vec::new(),
lateral: Lateral::default(),
lane_offsets: Vec::new(),
temporary_lane_offsets: Vec::new(),
sections: Vec::new(),
})
}
type ObjectIndex<'a> = HashMap<&'a str, (&'a str, roxmltree::Node<'a, 'a>)>;
fn object_index<'a>(root: roxmltree::Node<'a, 'a>) -> ObjectIndex<'a> {
let mut index = ObjectIndex::new();
for parent in root
.children()
.filter(|n| n.has_tag_name("road") || n.has_tag_name("junction"))
{
let road_id = match parent.has_tag_name("road") {
true => parent.attribute("id").unwrap_or_default(),
false => "",
};
let Some(objects) = child(parent, "objects") else {
continue;
};
for object in objects.children().filter(|n| n.has_tag_name("object")) {
if let Some(id) = object.attribute("id") {
index.entry(id).or_insert((road_id, object));
}
}
}
index
}
struct Placement<'a> {
s: f64,
t: f64,
z_offset: f64,
orientation: Orientation,
valid_length: Option<f64>,
shift: (f64, f64),
referenced_from: Option<&'a str>,
junction: Option<&'a str>,
validity: Vec<Validity>,
}
impl<'a> Placement<'a> {
fn own(object: roxmltree::Node) -> Option<Self> {
Some(Self {
s: attr_f64(object, "s")?,
t: attr_f64(object, "t")?,
z_offset: attr_f64(object, "zOffset").unwrap_or(0.0),
orientation: orientation(object),
valid_length: attr_f64(object, "validLength"),
shift: (0.0, 0.0),
referenced_from: None,
junction: None,
validity: validity(object),
})
}
fn reference(
reference: roxmltree::Node,
object: roxmltree::Node,
road: &'a str,
) -> Option<Self> {
let (s, t) = (attr_f64(reference, "s")?, attr_f64(reference, "t")?);
let from = |name| attr_f64(object, name).unwrap_or(0.0);
Some(Self {
s,
t,
z_offset: attr_f64(reference, "zOffset").unwrap_or(0.0),
orientation: orientation(reference),
valid_length: attr_f64(reference, "validLength"),
shift: (s - from("s"), t - from("t")),
referenced_from: Some(road),
junction: None,
validity: validity(reference),
})
}
}
fn validity(node: roxmltree::Node) -> Vec<Validity> {
let lane = attr_i32;
node.children()
.filter(|n| n.has_tag_name("validity"))
.filter_map(|v| {
Some((
lane(v, "fromLane")?,
lane(v, "toLane")?,
Layer::from_attr(v.attribute("layer")),
))
})
.collect()
}
fn orientation(node: roxmltree::Node) -> Orientation {
match node.attribute("orientation") {
Some("+") => Orientation::Positive,
Some("-") => Orientation::Negative,
_ => Orientation::Both,
}
}
fn place_objects<'a>(
objects_node: roxmltree::Node<'a, 'a>,
index: &ObjectIndex<'a>,
road: &Road,
junction: Option<&'a str>,
out: &mut Objects,
) {
for node in objects_node.children() {
let placed = if node.has_tag_name("object") {
Placement::own(node).map(|at| (node, at))
} else if node.has_tag_name("objectReference") {
node.attribute("id")
.and_then(|id| index.get(id))
.and_then(|&(road, object)| {
Placement::reference(node, object, road).map(|at| (object, at))
})
} else {
None
};
match placed {
Some((object, mut at)) => {
at.junction = junction;
place_object(object, &at, road, out);
}
None if node.has_tag_name("object") || node.has_tag_name("objectReference") => {
out.warnings.push(Warning::ObjectDropped {
road_id: road.od_id.clone(),
junction_id: junction.unwrap_or_default().to_string(),
object_id: node.attribute("id").unwrap_or_default().to_string(),
})
}
None => {}
}
}
}
fn place_object(node: roxmltree::Node, at: &Placement, road: &Road, out: &mut Objects) {
let faults = Faults {
pieces_left: Cell::new(MAX_OBJECT_PIECES.saturating_sub(out.cost.pieces)),
..Faults::default()
};
let too_many = &faults.too_many;
let base = Station {
s: at.s,
t: at.t,
ground: None,
z_offset: at.z_offset,
length: attr_f64(node, "length"),
width: attr_f64(node, "width"),
height: attr_f64(node, "height"),
radius: attr_f64(node, "radius"),
}
.sized();
let hdg = attr_f64(node, "hdg").unwrap_or(0.0);
let pitch = attr_f64(node, "pitch").unwrap_or(0.0);
let roll = attr_f64(node, "roll").unwrap_or(0.0);
let perp_to_road = matches!(node.attribute("perpToRoad"), Some("true" | "1"));
let frame = |st: &Station| Frame::at(road, st, perp_to_road, (hdg, pitch, roll));
let point = |st: &Station| {
let f = frame(st);
let (yaw, pitch, roll) = f.angles();
let solid = Shape::Solid {
position: f.origin,
heading: yaw as f32,
pitch: pitch as f32,
roll: roll as f32,
extent: extent(st),
};
let mut part = Part::new(solid, st, (st.s, st.s));
part.markings = side_markings(node, &f, extent(st), &faults);
part
};
let mut parts = Vec::new();
let repeat_nodes: Vec<_> = node
.children()
.filter(|n| n.has_tag_name("repeat"))
.collect();
let repeats: Vec<Repeat> = repeat_nodes
.iter()
.filter_map(|&n| Repeat::parse(n, at.shift, &base, road))
.collect();
if repeats.len() < repeat_nodes.len() {
out.warnings.push(Warning::RepeatDropped {
road_id: road.od_id.clone(),
junction_id: at.junction.unwrap_or_default().to_string(),
object_id: node.attribute("id").unwrap_or_default().to_string(),
});
}
for (t_end, reached) in repeats.iter().filter_map(|r| r.missed_end(&base)) {
out.warnings.push(Warning::RepeatEndMismatch {
road_id: road.od_id.clone(),
junction_id: at.junction.unwrap_or_default().to_string(),
object_id: node.attribute("id").unwrap_or_default().to_string(),
t_end,
reached,
});
}
let outlines = outline_nodes(node);
let owned = with_markings(node, &outlines);
let skeleton = child(node, "skeleton");
let shaped = !outlines.is_empty() || skeleton.is_some();
let outlined = |st: &Station| {
let origin = road.on_road(st.s).then(|| frame(st));
let shift = (at.shift.0 + st.s - base.s, at.shift.1 + st.t - base.t);
let mut parts = outline_parts(node, &owned, origin.as_ref(), st, shift, road, &faults);
let bones =
skeleton.and_then(|n| skeleton_part(n, origin.as_ref(), st, shift, road, &faults));
match bones {
Some(part) => parts.push(part),
None if parts.is_empty() && road.on_road(st.s) => parts.push(point(st)),
None => {}
}
parts
};
let marked: usize = owned
.iter()
.map(|(outline, marks)| marks.len() + marking_nodes(*outline).count())
.sum();
for repeat in &repeats {
let least = repeat.least_cost(road);
let least = Cost {
pieces: least
.pieces
.saturating_add(least.objects.saturating_mul(marked)),
..least
};
if !faults.affords(out, least) {
too_many.set(true);
} else if repeat.distance > 0.0 {
let Some(stations) = repeat.stations(&base, road) else {
too_many.set(true);
continue;
};
let saved = (out.cost, faults.pieces_left.get(), faults.painted.get());
let restore = |out: &mut Objects| {
let built = out.cost.pieces - saved.0.pieces + faults.painted.get() - saved.2;
out.wasted = out.wasted.saturating_add(built);
out.cost = saved.0;
faults.pieces_left.set(saved.1);
faults.painted.set(saved.2);
};
let copy_at = |st: &Station| match shaped {
true => outlined(st),
false => vec![point(st)],
};
let ends = [stations.first(), stations.last()];
let probes: Vec<(Cost, usize)> = ends
.into_iter()
.flatten()
.map(|st| {
let painted = faults.painted.get();
let copy = copy_at(st);
let mut cost: Cost = copy.iter().map(Part::cost).sum();
cost.pieces += faults.painted.get() - painted;
(cost, copy.iter().map(Part::corner_count).sum())
})
.collect();
let least = |f: fn(&(Cost, usize)) -> usize| probes.iter().map(f).min().unwrap_or(0);
let one = Cost {
objects: least(|p| p.0.objects),
pieces: least(|p| p.0.pieces),
};
let corners = least(|p| p.1);
let n = stations.len();
let guess = Cost {
objects: one.objects.saturating_mul(n),
pieces: one.pieces.saturating_mul(n),
};
let probed: usize = probes.iter().map(|p| p.0.pieces).sum();
let paint = faults.painted.get() - saved.2;
restore(out);
out.wasted = out.wasted.saturating_add(probed - paint);
if corners.saturating_mul(n) > MAX_OUTLINE_CORNERS || !faults.affords(out, guess) {
too_many.set(true);
continue;
}
let mut copies = Vec::new();
let mut corners = 0;
for st in stations {
let copy = copy_at(&st);
corners += copy.iter().map(Part::corner_count).sum::<usize>();
if corners > MAX_OUTLINE_CORNERS
|| !faults.spend(out, copy.iter().map(Part::cost).sum())
{
restore(out);
too_many.set(true);
copies.clear();
break;
}
copies.extend(copy);
}
parts.extend(copies);
} else {
let most = faults.pieces_left.get();
match sweep(repeat, &base, road, most) {
Ok(Some((shape, start, end))) => {
let part = Part::new(shape, &start, (start.s, end));
faults.spend(out, part.cost());
parts.push(part);
}
Ok(None) => {}
Err(TooMany) => too_many.set(true),
}
}
}
if !repeats.iter().any(|r| r.distance > 0.0) {
let (painted, left) = (faults.painted.get(), faults.pieces_left.get());
let mut own = Vec::new();
if shaped {
own.extend(outlined(&base));
} else if repeats.is_empty() && road.on_road(base.s) {
own.push(point(&base));
}
match faults.spend(out, own.iter().map(Part::cost).sum()) {
true => parts.extend(own),
false => {
let built = faults.painted.get() - painted;
out.wasted = out.wasted.saturating_add(built);
faults.painted.set(painted);
faults.pieces_left.set(left);
too_many.set(true);
}
}
}
let kind = object_type(node.attribute("type"));
let text = |name| node.attribute(name).unwrap_or_default().to_string();
let junction_id = at.junction.unwrap_or_default().to_string();
if let Some(crg) = child(node, "surface").and_then(|s| child(s, "CRG")) {
let surfaces: Vec<ObjectCrg> = parts
.iter()
.filter_map(|part| {
let (surface, stretch) = object_crg(crg, part, road)?;
Some(ObjectCrg {
surface,
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
object_id: text("id"),
stretch,
})
})
.collect();
if surfaces.is_empty() {
out.warnings.push(Warning::ObjectSurfaceDropped {
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
object_id: text("id"),
});
}
out.surfaces.extend(surfaces);
}
let flag = |name| matches!(node.attribute(name), Some("true" | "1"));
let parking_space = child(node, "parkingSpace").map(|p| {
let text = |name| p.attribute(name).unwrap_or_default().to_string();
ParkingSpace {
access: text("access"),
restrictions: text("restrictions"),
}
});
let materials: Vec<Material> = node
.children()
.filter(|n| n.has_tag_name("material"))
.map(material)
.collect();
let mark_colors: Vec<String> = node
.children()
.filter(|n| n.has_tag_name("material"))
.map(|m| m.attribute("roadMarkColor").unwrap_or_default().to_string())
.collect();
let user_data: Vec<UserData> = node
.children()
.filter(|n| n.has_tag_name("userData"))
.map(|u| {
let text = |name| u.attribute(name).unwrap_or_default().to_string();
UserData {
code: text("code"),
value: text("value"),
}
})
.collect();
out.cost.pieces = out.cost.pieces.saturating_add(faults.painted.get());
if too_many.get() {
out.warnings.push(Warning::TooManyCopies {
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
object_id: text("id"),
});
}
if faults.turned.get() {
out.warnings.push(Warning::CurveHeadingIgnored {
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
object_id: text("id"),
});
}
let unclaimed = marking_nodes(node).any(|m| {
let named = m.children().any(|n| n.has_tag_name("cornerReference"));
named && !owned.iter().any(|(_, marks)| marks.contains(&m))
});
if faults.marking_dropped.get() || unclaimed {
out.warnings.push(Warning::MarkingDropped {
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
object_id: text("id"),
});
}
if faults.outline_dropped.get() {
out.warnings.push(Warning::OutlineDropped {
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
object_id: text("id"),
});
}
if faults.skeleton_dropped.get() {
out.warnings.push(Warning::SkeletonDropped {
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
object_id: text("id"),
});
}
if faults.gap.get() > 0.0 {
out.warnings.push(Warning::OutlineGap {
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
object_id: text("id"),
gap: faults.gap.get(),
});
}
for part in parts {
let id = ObjectId(out.baked.len());
let (markings, marking_weights): (Vec<Marking>, Vec<RoadMarkWeight>) =
part.markings.into_iter().unzip();
out.baked.push(Object {
id,
kind,
subtype: text("subtype"),
name: text("name"),
dynamic: node.attribute("dynamic") == Some("yes"),
lanes: road.lanes(part.stretch, &at.validity),
markings,
borders: part.borders,
parking_space: parking_space.clone(),
materials: materials.clone(),
user_data: user_data.clone(),
shape: part.shape,
});
out.provenance.push(ObjectProvenance {
object: id,
road_id: road.od_id.clone(),
junction_id: junction_id.clone(),
od_id: text("id"),
s: part.s,
t: part.t,
orientation: at.orientation,
valid_length: at.valid_length,
referenced_from: at.referenced_from.map(str::to_string),
invalidated: flag("invalidated"),
temporary: flag("temporary"),
marking_weights,
material_road_mark_colors: mark_colors.clone(),
fill_type: part.fill_type,
outline_lane_type: part.outline_lane_type,
skeleton_ids: part.skeleton_ids,
});
}
}
fn object_crg(crg: roxmltree::Node, part: &Part, road: &Road) -> Option<(CrgSurface, (f64, f64))> {
let Shape::Solid {
position,
heading,
extent: Some(Extent::Box { length, width, .. }),
..
} = part.shape
else {
return None;
};
if length <= 0.0 || width <= 0.0 {
return None;
}
let (half_length, half_width) = (f64::from(length) / 2.0, f64::from(width) / 2.0);
let reach = 2.0 * half_length.hypot(half_width);
let stretch = (part.s - reach, part.s + reach);
let mode = CrgMode::Object(CrgObject {
origin: CrgPose {
x: f64::from(position.x),
y: f64::from(position.y),
heading: f64::from(heading),
},
half_length,
half_width,
hide_road: !matches!(crg.attribute("hideRoadSurfaceCRG"), Some("false" | "0")),
});
let mut surface = crg_surface(crg, mode, road.lanes(stretch, &[]), None);
surface.purpose = CrgPurpose::Elevation;
surface.z_offset = 0.0;
surface.z_scale = attr_f64(crg, "zScale").unwrap_or(1.0);
Some((surface, stretch))
}
fn outline_parts(
node: roxmltree::Node,
outlines: &[(roxmltree::Node, Vec<roxmltree::Node>)],
origin: Option<&Frame>,
st: &Station,
shift: (f64, f64),
road: &Road,
faults: &Faults,
) -> Vec<Part> {
let mut parts = Vec::new();
let mut holes = Vec::new();
for (outline_node, owned) in outlines {
let outline_node = *outline_node;
let Some(ring) = outline(outline_node, origin, st.s, shift, road) else {
faults.outline_dropped.set(true);
continue;
};
faults.gap.set(faults.gap.get().max(ring.gap));
faults.turned.set(faults.turned.get() || ring.turned);
if matches!(outline_node.attribute("outer"), Some("false" | "0")) {
holes.push((outline_node, owned, ring));
} else {
parts.push(area(node, outline_node, owned, &ring, st, faults));
}
}
let outer = parts.len();
for (outline_node, owned, ring) in holes {
let filled = outline_node.attribute("fillType").is_some()
|| outline_node.attribute("laneType").is_some();
let corners = &ring.corners;
let owner = parts[..outer].iter_mut().find(|p| {
matches!(&p.shape, Shape::Outline { corners: outer, closed: true, .. }
if corners.len() >= 3 && corners.iter().all(|c| encloses(outer, c.base)))
});
if owner.is_none() && !filled {
faults.outline_dropped.set(true);
}
if let Some(owner) = owner {
if !filled {
owner
.markings
.extend(markings(outline_node, owned, &ring, faults));
owner.borders.extend(borders(
node,
outline_node,
&ring,
faults,
owner.cost().pieces,
));
}
let painted = owner.fill_type == "paint";
if let Shape::Outline { holes, .. } = &mut owner.shape {
holes.push(if painted {
ring.paint()
} else {
ring.corners.clone()
});
}
}
if filled {
parts.push(area(node, outline_node, owned, &ring, st, faults));
}
}
parts
}
fn area(
node: roxmltree::Node,
outline_node: roxmltree::Node,
owned: &[roxmltree::Node],
ring: &Ring,
st: &Station,
faults: &Faults,
) -> Part {
let painted = ring.closed && outline_node.attribute("fillType") == Some("paint");
let mut part = Part::new(
Shape::Outline {
corners: if painted {
ring.paint()
} else {
ring.corners.clone()
},
closed: ring.closed,
holes: Vec::new(),
},
st,
ring.stretch,
);
part.markings = markings(outline_node, owned, ring, faults);
part.borders = borders(node, outline_node, ring, faults, part.cost().pieces);
part.fill_type = outline_node
.attribute("fillType")
.unwrap_or_default()
.to_string();
part.outline_lane_type = outline_node
.attribute("laneType")
.map(|t| lane_type(Some(t)));
part
}
struct Part {
shape: Shape,
s: f64,
t: f64,
stretch: (f64, f64),
markings: Vec<(Marking, RoadMarkWeight)>,
borders: Vec<Border>,
fill_type: String,
outline_lane_type: Option<LaneType>,
skeleton_ids: Vec<SkeletonIds>,
}
impl Part {
fn cost(&self) -> Cost {
let sections = match &self.shape {
Shape::Sweep { sections, .. } => sections.len(),
_ => 0,
};
let borders: usize = self.borders.iter().map(|b| b.pieces.len()).sum();
Cost {
objects: 1,
pieces: self.corner_count() + sections + borders,
}
}
fn corner_count(&self) -> usize {
match &self.shape {
Shape::Outline { corners, holes, .. } => {
corners.len() + holes.iter().map(Vec::len).sum::<usize>()
}
Shape::Skeleton { lines } => lines.iter().map(Vec::len).sum(),
_ => 0,
}
}
fn new(shape: Shape, at: &Station, stretch: (f64, f64)) -> Self {
Self {
shape,
s: at.s,
t: at.t,
stretch,
markings: Vec::new(),
borders: Vec::new(),
fill_type: String::new(),
outline_lane_type: None,
skeleton_ids: Vec::new(),
}
}
}
struct Repeat<'a> {
node: roxmltree::Node<'a, 'a>,
start: f64,
length: f64,
distance: f64,
shift_t: f64,
cubic: Option<[f64; 3]>,
line: Option<[Point; 2]>,
}
impl<'a> Repeat<'a> {
fn parse(
node: roxmltree::Node<'a, 'a>,
(shift_s, shift_t): (f64, f64),
base: &Station,
road: &Road,
) -> Option<Self> {
let terms = ["bT", "cT", "dT"].map(|name| attr_f64(node, name));
let mut repeat = Self {
node,
start: attr_f64(node, "s")? + shift_s,
length: attr_f64(node, "length")?,
distance: attr_f64(node, "distance")?,
shift_t,
cubic: terms
.iter()
.any(Option::is_some)
.then(|| terms.map(|c| c.unwrap_or(0.0))),
line: None,
};
if repeat.length < 0.0 || repeat.distance < 0.0 {
return None;
}
if matches!(
node.attribute("detachFromReferenceLine"),
Some("true" | "1")
) {
let end = |f: f64| {
road.surface(repeat.start + repeat.length * f, repeat.t(base, f))
.0
};
repeat.line = Some([end(0.0), end(1.0)]);
}
Some(repeat)
}
fn t(&self, base: &Station, f: f64) -> f64 {
let given = |name| attr_f64(self.node, name).map(|v| v + self.shift_t);
let start = given("tStart").unwrap_or(base.t);
match self.cubic {
Some([b, c, d]) => {
let ds = self.length * f;
start + ds * (b + ds * (c + ds * d))
}
None => start + (given("tEnd").unwrap_or(start) - start) * f,
}
}
fn missed_end(&self, base: &Station) -> Option<(f64, f64)> {
self.cubic?;
let t_end = attr_f64(self.node, "tEnd")? + self.shift_t;
let reached = self.t(base, 1.0);
((reached - t_end).abs() > 1e-3).then_some((t_end, reached))
}
fn at(&self, base: &Station, f: f64) -> Station {
let lerp = |name: &str, fallback: Option<f64>| {
let given = |end| attr_f64(self.node, &format!("{name}{end}"));
let start = given("Start").or(fallback)?;
let end = given("End").unwrap_or(start);
Some(start + (end - start) * f)
};
Station {
s: self.start + self.length * f,
t: self.t(base, f),
ground: self.line.map(|[a, b]| a.lerp(b, f as f32)),
z_offset: lerp("zOffset", Some(base.z_offset)).unwrap_or(base.z_offset),
length: lerp("length", base.length),
width: lerp("width", base.width),
height: lerp("height", base.height),
radius: lerp("radius", base.radius),
}
.sized()
}
fn least_cost(&self, road: &Road) -> Cost {
let count = |n: f64| n.clamp(0.0, usize::MAX as f64) as usize;
let (first, last) = self.steps(road);
let (from, to) = (
self.start.max(0.0),
(self.start + self.length).min(road.length),
);
match self.distance > 0.0 {
true => Cost {
objects: count(last - first + 1.0),
pieces: 0,
},
false => Cost {
objects: 1,
pieces: count((to - from) / SWEEP_MAX_STEP),
},
}
}
fn steps(&self, road: &Road) -> (f64, f64) {
const EPS: f64 = 1e-9;
let ahead = |s: f64| (s - self.start) / self.distance;
let last = (self.length / self.distance + EPS).floor();
let first = (ahead(0.0) - EPS).ceil().max(0.0);
(first, (ahead(road.length) + EPS).floor().min(last))
}
fn stations(&self, base: &Station, road: &Road) -> Option<Vec<Station>> {
let (first, last) = self.steps(road);
if last - first + 1.0 > MAX_REPEAT_INSTANCES {
return None;
}
let steps = first as usize..(last + 1.0).max(first) as usize;
let stations = steps
.map(|k| {
let along = k as f64 * self.distance;
let f = if self.length > 0.0 {
(along / self.length).min(1.0)
} else {
0.0
};
self.at(base, f)
})
.filter(|st| road.on_road(st.s))
.collect();
Some(stations)
}
}
fn sweep(
repeat: &Repeat,
base: &Station,
road: &Road,
most: usize,
) -> Result<Option<(Shape, Station, f64)>, TooMany> {
let start = repeat.start.max(0.0);
let end = (repeat.start + repeat.length).min(road.length);
if end - start < 1e-3 {
return Ok(None);
}
let first = repeat.at(base, (start - repeat.start) / repeat.length);
let round = first.radius.is_some();
let left = repeat
.line
.map(|[a, b]| Vector::new(a.y - b.y, b.x - a.x, 0.0).normalize_or_zero())
.filter(|left| *left != Vector::ZERO);
let section = |s: f64| {
let st = repeat.at(base, (s - repeat.start) / repeat.length);
let (half, height) = match st.radius {
Some(r) => (r, 2.0 * r as f32),
None => (
st.width.unwrap_or(0.0) / 2.0,
st.height.unwrap_or(0.0) as f32,
),
};
let corner = |side: f64| {
let mut base = match (st.ground, left) {
(Some(ground), Some(left)) => ground + left * (side * half) as f32,
_ => road.surface(s, st.t + side * half).0,
};
base.z += st.z_offset as f32;
Corner {
base,
top: base + Vector::Z * height,
}
};
Section {
left: corner(1.0),
right: corner(-1.0),
}
};
let sections = sweep_stations(start, end, most, |a, b, f| {
let (a, b, at) = (section(a), section(b), section(a + (b - a) * f));
let strays = |a: Corner, b: Corner, at: Corner| {
a.base.lerp(b.base, f as f32).distance_to(at.base) > SWEEP_TOLERANCE
};
strays(a.left, b.left, at.left) || strays(a.right, b.right, at.right)
})
.ok_or(TooMany)?
.into_iter()
.map(section)
.collect();
Ok(Some((Shape::Sweep { sections, round }, first, end)))
}
struct TooMany;
fn sweep_stations(
start: f64,
end: f64,
most: usize,
bends: impl Fn(f64, f64, f64) -> bool,
) -> Option<Vec<f64>> {
let seeds = ((end - start) / SWEEP_MAX_STEP).ceil().max(1.0);
if seeds >= most as f64 {
return None;
}
let seeds = seeds as usize;
let mut done = vec![start];
let mut todo: Vec<f64> = (1..=seeds)
.rev()
.map(|k| start + (end - start) * k as f64 / seeds as f64)
.collect();
while let Some(&b) = todo.last() {
let a = *done.last().expect("starts with start");
if done.len() + todo.len() > most {
return None;
}
if b - a > 2.0 * SWEEP_MIN_STEP && [0.25, 0.5, 0.75].iter().any(|&f| bends(a, b, f)) {
todo.push((a + b) / 2.0);
} else {
done.push(b);
todo.pop();
}
}
Some(done)
}
fn outline_nodes<'a>(object: roxmltree::Node<'a, 'a>) -> Vec<roxmltree::Node<'a, 'a>> {
child(object, "outlines")
.unwrap_or(object)
.children()
.filter(|n| n.has_tag_name("outline"))
.collect()
}
struct Ring<'a> {
corners: Vec<Corner>,
ids: Vec<Option<&'a str>>,
sampled: Vec<bool>,
ups: Vec<Vector>,
closed: bool,
stretch: (f64, f64),
gap: f64,
turned: bool,
}
impl Ring<'_> {
fn paint(&self) -> Vec<Corner> {
self.corners
.iter()
.zip(&self.ups)
.map(|(c, &up)| {
let base = c.base + up * PAINT_RAISE as f32;
Corner { base, top: base }
})
.collect()
}
fn along_curve(&self, from: usize, to: usize) -> Vec<usize> {
let n = self.corners.len();
let walk = |step: usize| {
let mut found = Vec::new();
let mut k = from;
loop {
let wraps = if step == 1 { k + 1 == n } else { k == 0 };
if wraps && !self.closed {
return None;
}
k = (k + step) % n;
if k == to {
return Some(found);
}
if !self.sampled[k] {
return None;
}
found.push(k);
}
};
walk(1).or_else(|| walk(n - 1)).unwrap_or_default()
}
fn path(&self) -> Vec<(Point, Vector)> {
self.corners
.iter()
.map(|c| c.base)
.zip(self.ups.iter().copied())
.collect()
}
}
fn outline<'a>(
node: roxmltree::Node<'a, 'a>,
frame: Option<&Frame>,
s: f64,
(shift_s, shift_t): (f64, f64),
road: &Road,
) -> Option<Ring<'a>> {
let mut stretch = (f64::INFINITY, f64::NEG_INFINITY);
let mut spans = |s: f64| stretch = (stretch.0.min(s), stretch.1.max(s));
let closed = !matches!(node.attribute("closed"), Some("false" | "0"));
let nodes: Vec<roxmltree::Node> = corner_nodes(node).collect();
let mut corners = Vec::new();
let mut ups = Vec::new();
let mut ids = Vec::new();
let mut sampled = Vec::new();
let mut gap: f64 = 0.0;
let mut turned = false;
let mut heading = None;
for (i, &c) in nodes.iter().enumerate() {
let height = attr_f64(c, "height").unwrap_or(0.0);
let previous = heading.take();
if c.has_tag_name("cornerRoad") {
let (s, t) = (attr_f64(c, "s")? + shift_s, attr_f64(c, "t")? + shift_t);
if !road.on_road(s) {
return None;
}
spans(s);
let (mut base, _) = road.surface(s, t);
base.z += attr_f64(c, "dz").unwrap_or(0.0) as f32;
corners.push(Corner {
base,
top: base + Vector::Z * height as f32,
});
ups.push(normal(road.axes(s)));
ids.push(c.attribute("id"));
sampled.push(false);
continue;
}
let frame = frame?;
spans(s);
let local: Vec<[f64; 4]> = if c.has_tag_name("curveLocal") {
let next = nodes.get(i + 1).or(nodes.first().filter(|_| closed));
let next = next.filter(|n| !n.has_tag_name("cornerRoad")).copied();
let curve = curve_local::sample(c, next, previous)?;
heading = Some(curve.end_heading);
gap = gap.max(curve.gap);
turned |= curve.turned;
curve.points
} else {
let z = attr_f64(c, "z").unwrap_or(0.0);
vec![[attr_f64(c, "u")?, attr_f64(c, "v")?, z, height]]
};
for (k, [u, v, z, height]) in local.into_iter().enumerate() {
corners.push(Corner {
base: frame.point([u, v, z]),
top: frame.point([u, v, z + height]),
});
ups.push(normal(frame.axes));
ids.push(c.attribute("id").filter(|_| k == 0));
sampled.push(k > 0);
}
}
if corners.len() < 2 {
return None;
}
Some(Ring {
corners,
ids,
sampled,
ups,
closed,
stretch,
gap,
turned,
})
}
fn skeleton_part(
node: roxmltree::Node,
frame: Option<&Frame>,
st: &Station,
(shift_s, shift_t): (f64, f64),
road: &Road,
faults: &Faults,
) -> Option<Part> {
let vertex = |v: roxmltree::Node| -> Option<(SkeletonVertex, f64)> {
let (position, s) = if v.has_tag_name("vertexRoad") {
let (s, t) = (attr_f64(v, "s")? + shift_s, attr_f64(v, "t")? + shift_t);
if !road.on_road(s) {
return None;
}
let (base, _) = road.surface(s, t);
(
base + Vector::Z * attr_f64(v, "dz").unwrap_or(0.0) as f32,
s,
)
} else {
let z = attr_f64(v, "z").unwrap_or(0.0);
let point = frame?.point([attr_f64(v, "u")?, attr_f64(v, "v")?, z]);
(point, st.s)
};
let vertex = SkeletonVertex {
position,
radius: attr_f64(v, "radius").unwrap_or(0.0).max(0.0) as f32,
ground: matches!(v.attribute("intersectionPoint"), Some("true" | "1")),
};
Some((vertex, s))
};
let text = |n: roxmltree::Node| n.attribute("id").unwrap_or_default().to_string();
let mut stretch = (f64::INFINITY, f64::NEG_INFINITY);
let mut lines = Vec::new();
let mut ids = Vec::new();
for polyline in node.children().filter(|n| n.has_tag_name("polyline")) {
let vertices: Vec<roxmltree::Node> = polyline
.children()
.filter(|n| n.has_tag_name("vertexRoad") || n.has_tag_name("vertexLocal"))
.collect();
let placed = vertices
.iter()
.map(|&v| vertex(v))
.collect::<Option<Vec<_>>>()
.filter(|line| line.len() >= 2);
let Some(line) = placed else {
faults.skeleton_dropped.set(true);
continue;
};
for &(_, s) in &line {
stretch = (stretch.0.min(s), stretch.1.max(s));
}
lines.push(line.into_iter().map(|(v, _)| v).collect());
ids.push(SkeletonIds {
polyline: text(polyline),
vertices: vertices.into_iter().map(text).collect(),
});
}
if lines.is_empty() {
return None;
}
let mut part = Part::new(Shape::Skeleton { lines }, st, stretch);
part.skeleton_ids = ids;
Some(part)
}
fn normal(axes: [[f64; 3]; 3]) -> Vector {
Vector::from_array(axes[2].map(|c| c as f32))
}
fn encloses(ring: &[Corner], point: Point) -> bool {
let n = ring.len();
(0..n).fold(false, |inside, i| {
let (a, b) = (ring[i].base, ring[(i + 1) % n].base);
let crosses = (a.y > point.y) != (b.y > point.y)
&& point.x < a.x + (point.y - a.y) * (b.x - a.x) / (b.y - a.y);
inside != crosses
})
}
fn corner_nodes<'a>(
outline: roxmltree::Node<'a, 'a>,
) -> impl Iterator<Item = roxmltree::Node<'a, 'a>> {
outline.children().filter(|n| {
n.has_tag_name("cornerRoad")
|| n.has_tag_name("cornerLocal")
|| n.has_tag_name("curveLocal")
})
}
fn marking_nodes<'a>(
node: roxmltree::Node<'a, 'a>,
) -> impl Iterator<Item = roxmltree::Node<'a, 'a>> {
child(node, "markings")
.into_iter()
.flat_map(|m| m.children())
.filter(|n| n.has_tag_name("marking"))
}
fn markings(
outline: roxmltree::Node,
owned: &[roxmltree::Node],
ring: &Ring,
faults: &Faults,
) -> Vec<(Marking, RoadMarkWeight)> {
marking_nodes(outline)
.chain(owned.iter().copied())
.filter_map(|m| match corner_path(m, ring) {
Some(path) => paint(m, &path, faults),
None => {
faults.marking_dropped.set(true);
None
}
})
.collect()
}
fn with_markings<'a>(
object: roxmltree::Node<'a, 'a>,
outlines: &[roxmltree::Node<'a, 'a>],
) -> Vec<(roxmltree::Node<'a, 'a>, Vec<roxmltree::Node<'a, 'a>>)> {
let ids: Vec<HashSet<&str>> = outlines
.iter()
.map(|o| o.children().filter_map(|c| c.attribute("id")).collect())
.collect();
let mut owned = vec![Vec::new(); outlines.len()];
for m in marking_nodes(object) {
let refs: Vec<Option<&str>> = m
.children()
.filter(|n| n.has_tag_name("cornerReference"))
.map(|r| r.attribute("id"))
.collect();
let names =
|set: &HashSet<&str>| refs.iter().all(|id| id.is_some_and(|id| set.contains(id)));
if let Some(k) = ids.iter().position(names) {
owned[k].push(m);
}
}
outlines.iter().copied().zip(owned).collect()
}
fn side_markings(
object: roxmltree::Node,
frame: &Frame,
extent: Option<Extent>,
faults: &Faults,
) -> Vec<(Marking, RoadMarkWeight)> {
let (u, v) = match extent {
Some(Extent::Box { length, width, .. }) => (length / 2.0, width / 2.0),
Some(Extent::Cylinder { radius, .. }) => (radius, radius),
None => return Vec::new(),
};
let (u, v) = (f64::from(u), f64::from(v));
let corner = |u, v| (frame.point([u, v, 0.0]), normal(frame.axes));
marking_nodes(object)
.filter(|m| !m.children().any(|n| n.has_tag_name("cornerReference")))
.filter_map(|m| {
let edge = match m.attribute("side") {
Some("front") => [corner(u, -v), corner(u, v)],
Some("left") => [corner(u, v), corner(-u, v)],
Some("rear") => [corner(-u, v), corner(-u, -v)],
Some("right") => [corner(-u, -v), corner(u, -v)],
_ => {
faults.marking_dropped.set(true);
return None;
}
};
paint(m, &edge, faults)
})
.collect()
}
fn paint(
m: roxmltree::Node,
path: &[(Point, Vector)],
faults: &Faults,
) -> Option<(Marking, RoadMarkWeight)> {
let Some(width) = attr_f64(m, "width").filter(|w| *w > 0.0) else {
faults.marking_dropped.set(true);
return None;
};
let raise = attr_f64(m, "zOffset").unwrap_or(PAINT_RAISE) as f32;
let path: Vec<(Point, Vector)> = path.iter().map(|&(p, up)| (p + up * raise, up)).collect();
let line = attr_f64(m, "lineLength").unwrap_or(0.0).max(0.0);
let space = attr_f64(m, "spaceLength").unwrap_or(0.0).max(0.0);
let text = |name| m.attribute(name).unwrap_or_default().to_string();
let weight = match m.attribute("weight") {
Some("bold") => RoadMarkWeight::Bold,
_ => RoadMarkWeight::Standard,
};
let marking = Marking {
side: text("side"),
color: text("color"),
width: width as f32,
line_length: line as f32,
space_length: space as f32,
pieces: strip(
&path,
attr_f64(m, "startOffset").unwrap_or(0.0),
attr_f64(m, "stopOffset").unwrap_or(0.0),
(line > 0.0 && space > 0.0).then_some((line, space)),
width / 2.0,
faults.pieces_left.get(),
)
.inspect(|pieces| {
faults
.pieces_left
.set(faults.pieces_left.get() - pieces.len());
faults.painted.set(faults.painted.get() + pieces.len());
})
.unwrap_or_else(|| {
faults.too_many.set(true);
Vec::new()
}),
};
Some((marking, weight))
}
fn borders(
object: roxmltree::Node,
outline: roxmltree::Node,
ring: &Ring,
faults: &Faults,
already: usize,
) -> Vec<Border> {
let Some(borders) = child(object, "borders") else {
return Vec::new();
};
let mut used = already;
borders
.children()
.filter(|n| n.has_tag_name("border"))
.filter(|b| {
b.attribute("outlineId")
.is_none_or(|id| outline.attribute("id") == Some(id))
})
.filter_map(|b| {
let width = attr_f64(b, "width").filter(|w| *w > 0.0)?;
let path = if matches!(b.attribute("useCompleteOutline"), Some("true" | "1")) {
let mut path = ring.path();
if ring.closed {
path.push(path[0]);
}
path
} else {
corner_path(b, ring)?
};
let most = faults.pieces_left.get().saturating_sub(used);
let pieces = strip(&path, 0.0, 0.0, None, width / 2.0, most).unwrap_or_else(|| {
faults.too_many.set(true);
Vec::new()
});
used += pieces.len();
Some(Border {
kind: b.attribute("type").unwrap_or_default().to_string(),
width: width as f32,
pieces,
})
})
.collect()
}
fn corner_path(node: roxmltree::Node, ring: &Ring) -> Option<Vec<(Point, Vector)>> {
let named = node
.children()
.filter(|n| n.has_tag_name("cornerReference"))
.map(|r| {
let id = r.attribute("id")?;
ring.ids.iter().position(|c| *c == Some(id))
})
.collect::<Option<Vec<usize>>>()?;
if named.len() < 2 {
return None;
}
let mut path = vec![named[0]];
for w in named.windows(2) {
path.extend(ring.along_curve(w[0], w[1]));
path.push(w[1]);
}
Some(
path.into_iter()
.map(|k| (ring.corners[k].base, ring.ups[k]))
.collect(),
)
}
fn strip(
path: &[(Point, Vector)],
start: f64,
stop: f64,
dashes: Option<(f64, f64)>,
half_width: f64,
most: usize,
) -> Option<Vec<[Point; 4]>> {
let mut along = vec![0.0];
for w in path.windows(2) {
along.push(along.last().unwrap() + f64::from(w[0].0.distance_to(w[1].0)));
}
let end = along.last().unwrap() - stop;
let painted: Vec<(f64, f64)> = match dashes {
None => vec![(start, end)],
Some((line, space)) => {
let period = line + space;
let before = if start < 0.0 && period > 0.0 {
(-start / period).floor()
} else {
0.0
};
let first = start + before * period;
if (end - first) / period > MAX_REPEAT_INSTANCES {
return None;
}
(0..)
.map(|k| first + k as f64 * period)
.take_while(|&a| a < end)
.map(|a| (a, (a + line).min(end)))
.collect()
}
};
let mut pieces = Vec::new();
for (i, w) in path.windows(2).enumerate() {
let ((a, up_a), (b, up_b)) = (w[0], w[1]);
let across = (up_a + up_b).cross(b - a).normalize_or_zero() * half_width as f32;
if across == Vector::ZERO {
continue;
}
let (from, to) = (along[i], along[i + 1]);
let first = painted.partition_point(|&(_, q)| q <= from);
for &(p, q) in painted[first..].iter().take_while(|&&(p, _)| p < to) {
let (p, q) = (p.max(from), q.min(to));
if q - p < 1e-6 {
continue;
}
let at = |x: f64| a.lerp(b, ((x - from) / (to - from)) as f32);
let (p, q) = (at(p), at(q));
if pieces.len() == most {
return None;
}
pieces.push([p - across, q - across, q + across, p + across]);
}
}
Some(pieces)
}
fn extent(st: &Station) -> Option<Extent> {
let height = st.height.unwrap_or(0.0) as f32;
if let Some(radius) = st.radius {
return Some(Extent::Cylinder {
radius: radius as f32,
height,
});
}
if st.length.is_none() && st.width.is_none() && st.height.is_none() {
return None;
}
Some(Extent::Box {
length: st.length.unwrap_or(0.0) as f32,
width: st.width.unwrap_or(0.0) as f32,
height,
})
}
fn object_type(od_type: Option<&str>) -> ObjectType {
match od_type.unwrap_or_default() {
"none" => ObjectType::None,
"obstacle" => ObjectType::Obstacle,
"pole" => ObjectType::Pole,
"tree" => ObjectType::Tree,
"vegetation" => ObjectType::Vegetation,
"barrier" => ObjectType::Barrier,
"building" => ObjectType::Building,
"parkingSpace" => ObjectType::ParkingSpace,
"patch" => ObjectType::Patch,
"railing" => ObjectType::Railing,
"trafficIsland" => ObjectType::TrafficIsland,
"crosswalk" => ObjectType::Crosswalk,
"streetLamp" => ObjectType::StreetLamp,
"gantry" => ObjectType::Gantry,
"soundBarrier" => ObjectType::SoundBarrier,
"roadMark" => ObjectType::RoadMark,
"roadSurface" => ObjectType::RoadSurface,
_ => ObjectType::Unknown,
}
}
fn child<'a>(node: roxmltree::Node<'a, 'a>, tag: &str) -> Option<roxmltree::Node<'a, 'a>> {
node.children().find(|n| n.has_tag_name(tag))
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn nesting_is_measured_by_tags_alone() {
let deep = |n| format!("{}{}", "<a>".repeat(n), "</a>".repeat(n));
assert!(!deeper_than(&deep(3), 3));
assert!(deeper_than(&deep(4), 3));
let flat =
r#"<r><a/><a x="1>2"/><!-- <a><a><a><a> --><![CDATA[<a><a><a><a>]]><?p <a><a>?></r>"#;
assert!(!deeper_than(flat, 1));
let doctype = r#"<!DOCTYPE r [<!ENTITY e "x">]><r><a><b/></a></r>"#;
assert!(!deeper_than(doctype, 2));
assert!(deeper_than(doctype, 1));
}
use crate::coords::Vector;
const STRAIGHT: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="s" length="40.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="40.0"><line/></geometry>
</planView>
<elevationProfile>
<elevation s="0.0" a="0.0" b="0.04" c="0.0" d="0.0"/>
</elevationProfile>
<lanes>
<laneSection s="0.0">
<right>
<lane id="-1" type="driving">
<width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0"/>
</lane>
</right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn straight_one_forward_lane() {
let net = load_str(STRAIGHT).expect("import");
assert_eq!(net.driving_lanes().count(), 1);
let lane = net.lanes().first().unwrap();
assert_eq!(lane.direction, Direction::Forward);
let start = lane.center.pose_at(0.0);
let end = lane.center.pose_at(lane.center.length());
assert!(start.heading.x > 0.9, "start heading {:?}", start.heading);
assert!(
(start.position.y + 1.75).abs() < 0.1,
"y {}",
start.position.y
);
assert!(end.position.z > start.position.z + 1.0, "no climb");
}
const STRAIGHT_ARC: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="sa" length="87.12" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="40.0"><line/></geometry>
<geometry s="40.0" x="40.0" y="0.0" hdg="0.0" length="47.12"><arc curvature="0.03333"/></geometry>
</planView>
<lanes>
<laneSection s="0.0">
<left>
<lane id="1" type="driving"><width sOffset="0.0" a="3.5"/></lane>
</left>
<right>
<lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane>
</right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn straight_then_left_arc_two_lanes() {
let net = load_str(STRAIGHT_ARC).expect("import");
assert_eq!(net.driving_lanes().count(), 2);
let fwd = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Forward)
.unwrap();
let start = fwd.center.pose_at(0.0);
let end = fwd.center.pose_at(fwd.center.length());
assert!(start.heading.x > 0.9, "start {:?}", start.heading);
assert!(end.heading.y > 0.9, "end {:?}", end.heading);
}
#[test]
fn lanes_sit_a_width_apart() {
let net = load_str(STRAIGHT_ARC).expect("import");
let fwd = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Forward)
.unwrap();
let bwd = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Backward)
.unwrap();
let a = fwd.center.point_at(10.0);
let gap = (a - bwd.center.project(a).point).length();
assert!((gap - 3.5).abs() < 0.2, "gap {gap}");
}
const PLACED: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="placed" length="30.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="100.0" y="50.0" hdg="1.5707963267948966" length="30.0"><line/></geometry>
</planView>
<elevationProfile>
<elevation s="0.0" a="3.0" b="0.0" c="0.0" d="0.0"/>
</elevationProfile>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="4.0"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn a_placed_road_bakes_to_its_opendrive_coordinates() {
let net = load_str(PLACED).expect("import");
let lane = &net.lanes()[0];
for (s, want) in [
(0.0, Point::new(102.0, 50.0, 3.0)),
(15.0, Point::new(102.0, 65.0, 3.0)),
(30.0, Point::new(102.0, 80.0, 3.0)),
] {
let got = lane.center.point_at(s);
assert!(
(got - want).length() < 1e-3,
"s={s}: baked {got:?}, want {want:?}"
);
}
assert!(lane
.center
.pose_at(15.0)
.heading
.abs_diff_eq(Vector::Y, 1e-4));
assert!(lane.sample_at(15.0).up.abs_diff_eq(Vector::Z, 1e-5));
}
const STRAIGHT_OFFSET: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="o" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lanes>
<laneOffset s="0.0" a="2.0" b="0.0" c="0.0" d="0.0"/>
<laneSection s="0.0">
<right>
<lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane>
</right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn lane_offset_shifts_the_cross_section() {
let net = load_str(STRAIGHT_OFFSET).expect("import");
let y = net.lanes()[0].center.pose_at(0.0).position.y;
assert!((y - 0.25).abs() < 0.05, "y {y}");
}
const SHOULDER_INBOARD: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="s" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lanes>
<laneSection s="0.0">
<right>
<lane id="-1" type="shoulder"><width sOffset="0.0" a="2.0"/></lane>
<lane id="-2" type="driving"><width sOffset="0.0" a="3.0"/></lane>
</right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn a_non_driving_inner_lane_still_offsets_the_driving_lane() {
let net = load_str(SHOULDER_INBOARD).expect("import");
assert_eq!(net.lanes().len(), 2, "the shoulder is a lane too");
let driving = net.driving_lanes().next().expect("the driving lane");
let y = driving.center.pose_at(0.0).position.y;
assert!((y + 3.5).abs() < 0.05, "y {y}, expected -3.5");
assert!(
driving.neighbors.is_empty(),
"a shoulder is not a lane you change into: {:?}",
driving.neighbors
);
}
const TWO_SECTIONS: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="ms" length="40.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="40.0"><line/></geometry>
</planView>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
<laneSection s="25.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn each_lane_section_becomes_its_own_lane() {
let net = load_str(TWO_SECTIONS).expect("import");
assert_eq!(net.driving_lanes().count(), 2, "one lane per section");
let mut lens: Vec<f32> = net.lanes().iter().map(|l| l.center.length()).collect();
lens.sort_by(|a, b| a.total_cmp(b));
assert!((lens[0] - 15.0).abs() < 1.0, "short section {}", lens[0]);
assert!((lens[1] - 25.0).abs() < 1.0, "long section {}", lens[1]);
}
const MANY_TYPES: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="m" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lanes>
<laneSection s="0.0">
<left>
<lane id="1" type="median"><width sOffset="0.0" a="1.0"/></lane>
<lane id="2" type="special1"><width sOffset="0.0" a="1.0"/></lane>
<lane id="3" type="driving"><width sOffset="0.0" a="3.5"/></lane>
<lane id="4" type="banana"><width sOffset="0.0" a="1.5"/></lane>
</left>
<right>
<lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane>
<lane id="-2" type="onRamp"><width sOffset="0.0" a="3.0"/></lane>
<lane id="-3" type="none"><width sOffset="0.0" a="0.5"/></lane>
<lane id="-4" type="parking"><width sOffset="0.0" a="2.5"/></lane>
<lane id="-5" type="curb"><width sOffset="0.0" a="0.3"/></lane>
<lane id="-6" type="sidewalk"><width sOffset="0.0" a="2.0"/></lane>
<lane id="-7" type="walking"><width sOffset="0.0" a="2.0"/></lane>
<lane id="-8" type="shared"><width sOffset="0.0" a="3.0"/></lane>
</right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn every_named_lane_type_bakes_as_itself() {
let (net, Provenance { lanes: prov, .. }) =
load_str_with_provenance(MANY_TYPES).expect("import");
let kind = |od_id: i32| {
prov.iter()
.find(|p| p.od_id == od_id)
.and_then(|p| net.lane(p.lane))
.map(|l| l.kind)
};
assert_eq!(kind(1), Some(LaneType::Median));
assert_eq!(kind(3), Some(LaneType::Driving));
assert_eq!(kind(-1), Some(LaneType::Driving));
assert_eq!(kind(-2), Some(LaneType::OnRamp));
assert_eq!(kind(-3), Some(LaneType::None), "`none` is paved surface");
assert_eq!(kind(-4), Some(LaneType::Parking));
assert_eq!(kind(-5), Some(LaneType::Curb));
assert_eq!(kind(-6), Some(LaneType::Sidewalk));
assert_eq!(
kind(-7),
Some(LaneType::Sidewalk),
"1.8 calls a sidewalk `walking`"
);
assert_eq!(kind(-8), Some(LaneType::Shared));
assert_eq!(kind(2), Some(LaneType::Special1));
assert_eq!(kind(4), Some(LaneType::Unknown), "banana is not a type");
assert_eq!(net.lanes().len(), 12);
assert_eq!(net.driving_lanes().count(), 2);
}
const TAPERED: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="t" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lanes>
<laneSection s="0.0">
<right>
<lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane>
<lane id="-2" type="none">
<width sOffset="0.0" a="0.0" b="0.0" c="0.04" d="0.0"/>
<width sOffset="10.0" a="4.0" b="0.0" c="0.0" d="0.0"/>
</lane>
</right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn a_lane_that_opens_out_of_nothing_keeps_its_width_along_its_length() {
let (net, Provenance { lanes: prov, .. }) =
load_str_with_provenance(TAPERED).expect("import");
let lane = prov
.iter()
.find(|p| p.od_id == -2)
.and_then(|p| net.lane(p.lane))
.expect("the tapered lane");
assert!((lane.width - 4.0).abs() < 0.01, "nominal {}", lane.width);
assert!(
lane.width_at(0.0) < 0.01,
"it starts as a point: {}",
lane.width_at(0.0)
);
assert!(
(lane.width_at(lane.center.length()) - 4.0).abs() < 0.01,
"and ends at full width: {}",
lane.width_at(lane.center.length())
);
let mut last = -1.0;
for k in 0..=20 {
let w = lane.width_at(lane.center.length() * k as f32 / 20.0);
assert!(w >= last - 1e-4, "width dipped at step {k}: {last} -> {w}");
last = w;
}
}
#[test]
fn a_tapered_lane_tessellates_to_a_wedge_not_a_sliver() {
let (net, Provenance { lanes: prov, .. }) =
load_str_with_provenance(TAPERED).expect("import");
let id = prov
.iter()
.find(|p| p.od_id == -2)
.expect("provenance")
.lane;
let mesh = net.surface_mesh();
let span = mesh
.lanes
.iter()
.find(|s| s.lane == id)
.expect("the lane owns a mesh slice");
let ribs: Vec<f32> = mesh.vertices
[span.vertices.start as usize..span.vertices.end as usize]
.chunks_exact(2)
.map(|p| (p[0] - p[1]).length())
.collect();
assert!(ribs.first().copied().unwrap_or(1.0) < 0.01, "starts sharp");
assert!(
ribs.last().copied().unwrap_or(0.0) > 3.9,
"opens to full width: {:?}",
ribs.last()
);
}
#[test]
fn an_unrecognised_lane_type_is_a_lane_rather_than_a_hole() {
let (net, Provenance { lanes: prov, .. }) =
load_str_with_provenance(MANY_TYPES).expect("import");
let lane = prov
.iter()
.find(|p| p.od_id == 4)
.and_then(|p| net.lane(p.lane))
.expect("the unrecognised lane is baked");
assert_eq!(lane.kind, LaneType::Unknown);
assert!(
!lane.kind.is_drivable(),
"and nothing may be routed onto it"
);
assert!((lane.width - 1.5).abs() < 1e-6, "width {}", lane.width);
assert!(lane.center.length() > 19.0, "it has the road's length");
}
#[test]
fn inner_lane_widths_accumulate_across_the_cross_section() {
let (net, Provenance { lanes: prov, .. }) =
load_str_with_provenance(MANY_TYPES).expect("import");
let driving = prov
.iter()
.find(|p| p.od_id == 3)
.and_then(|p| net.lane(p.lane))
.expect("the left driving lane");
let y = driving.center.pose_at(0.0).position.y;
assert!((y - 3.75).abs() < 0.05, "y {y}, expected 3.75");
}
#[test]
fn lane_change_edges_stop_at_the_first_lane_traffic_cannot_use() {
let (net, Provenance { lanes: prov, .. }) =
load_str_with_provenance(MANY_TYPES).expect("import");
let lane = |od_id: i32| {
prov.iter()
.find(|p| p.od_id == od_id)
.and_then(|p| net.lane(p.lane))
.expect("a baked lane")
};
let (driving, ramp) = (lane(-1), lane(-2));
assert_eq!(driving.neighbors, vec![ramp.id]);
assert_eq!(ramp.neighbors, vec![driving.id]);
assert!(
lane(-4).neighbors.is_empty(),
"parking is not a lane change"
);
assert!(lane(-6).neighbors.is_empty(), "sidewalk is not either");
assert!(lane(-3).neighbors.is_empty(), "nor is unnamed surface");
assert!(lane(3).neighbors.is_empty(), "a median is not crossable");
}
#[test]
fn provenance_names_each_baked_lane_by_road_section_and_od_id() {
let (net, Provenance { lanes: prov, .. }) =
load_str_with_provenance(TWO_SECTIONS).expect("import with provenance");
assert_eq!(prov.len(), net.lanes().len(), "one record per baked lane");
for p in &prov {
assert!(net.lane(p.lane).is_some(), "{:?} names no lane", p.lane);
assert_eq!(p.road_id, "1");
assert_eq!(p.od_id, -1);
}
let mut sections: Vec<usize> = prov.iter().map(|p| p.section).collect();
sections.sort_unstable();
assert_eq!(sections, vec![0, 1], "one lane per section, indexed 0,1");
}
const SPIRAL_ONLY: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="sp" length="10.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="10.0">
<spiral curvStart="0.0" curvEnd="0.1"/>
</geometry>
</planView>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn spiral_heading_matches_closed_form() {
let net = load_str(SPIRAL_ONLY).expect("import");
let h = net.lanes()[0].center.pose_at(5.0).heading;
let theta = h.y.atan2(h.x);
assert!((theta - 0.125).abs() < 0.03, "mid heading angle {theta}");
}
const PARAM_POLY3: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="pp" length="15.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="15.0">
<paramPoly3 pRange="normalized" aU="0" bU="10" cU="0" dU="0" aV="0" bV="0" cV="5" dV="0"/>
</geometry>
</planView>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn param_poly3_follows_the_v_deviation() {
let net = load_str(PARAM_POLY3).expect("import");
let end = net.lanes()[0]
.center
.pose_at(net.lanes()[0].center.length())
.position;
assert!(end.x > 8.0, "end x {}", end.x);
assert!(end.y > 3.0, "end y {} (should follow v to ~+5)", end.y);
}
const POLY3: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="p3" length="10.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="10.0">
<poly3 a="0" b="0" c="0.05" d="0"/>
</geometry>
</planView>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn poly3_curves_laterally() {
let net = load_str(POLY3).expect("import");
let end = net.lanes()[0]
.center
.pose_at(net.lanes()[0].center.length())
.position;
assert!(end.y > 2.0, "end y {} (poly3 should curve)", end.y);
}
const TWO_SECTION_LINKED: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="r" length="40.0" id="1" junction="-1">
<planView><geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="40.0"><line/></geometry></planView>
<lanes>
<laneSection s="0.0"><right><lane id="-1" type="driving">
<link><successor id="-1"/></link><width sOffset="0.0" a="3.5"/>
</lane></right></laneSection>
<laneSection s="15.0"><right><lane id="-1" type="driving">
<link><predecessor id="-1"/></link><width sOffset="0.0" a="3.5"/>
</lane></right></laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn cross_section_link() {
let net = load_str(TWO_SECTION_LINKED).expect("import");
assert_eq!(net.lanes().len(), 2);
assert_eq!(net.lanes()[0].successors, vec![net.lanes()[1].id]);
assert_eq!(net.lanes()[1].predecessors, vec![net.lanes()[0].id]);
}
const TWO_ROADS_LINKED: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="a" length="20.0" id="1" junction="-1">
<link><successor elementType="road" elementId="2" contactPoint="start"/></link>
<planView><geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry></planView>
<lanes><laneSection s="0.0"><right><lane id="-1" type="driving">
<link><successor id="-1"/></link><width sOffset="0.0" a="3.5"/>
</lane></right></laneSection></lanes>
</road>
<road name="b" length="20.0" id="2" junction="-1">
<link><predecessor elementType="road" elementId="1" contactPoint="end"/></link>
<planView><geometry s="0.0" x="20.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry></planView>
<lanes><laneSection s="0.0"><right><lane id="-1" type="driving">
<link><predecessor id="-1"/></link><width sOffset="0.0" a="3.5"/>
</lane></right></laneSection></lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn road_to_road_link() {
let net = load_str(TWO_ROADS_LINKED).expect("import");
assert_eq!(net.lanes().len(), 2);
assert_eq!(net.lanes()[0].successors, vec![net.lanes()[1].id], "A -> B");
assert_eq!(net.lanes()[1].predecessors, vec![net.lanes()[0].id]);
}
const JUNCTION: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="in" length="20.0" id="1" junction="-1">
<link><successor elementType="junction" elementId="100"/></link>
<planView><geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry></planView>
<lanes><laneSection s="0.0"><right><lane id="-1" type="driving">
<link><successor id="-1"/></link><width sOffset="0.0" a="3.5"/>
</lane></right></laneSection></lanes>
</road>
<road name="conn" length="20.0" id="2" junction="100">
<link><predecessor elementType="road" elementId="1" contactPoint="end"/></link>
<planView><geometry s="0.0" x="20.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry></planView>
<lanes><laneSection s="0.0"><right><lane id="-1" type="driving">
<width sOffset="0.0" a="3.5"/>
</lane></right></laneSection></lanes>
</road>
<junction id="100">
<connection id="0" incomingRoad="1" connectingRoad="2" contactPoint="start">
<laneLink from="-1" to="-1"/>
</connection>
</junction>
</OpenDRIVE>"#;
#[test]
fn junction_link() {
let net = load_str(JUNCTION).expect("import");
assert_eq!(net.lanes().len(), 2);
assert_eq!(
net.lanes()[0].successors,
vec![net.lanes()[1].id],
"road -> junction -> connecting road"
);
}
#[test]
fn only_a_leading_declaration_is_stripped() {
let carla = "<!-- licence -->\n<?xml version=\"1.0\"?><OpenDRIVE/>";
assert_eq!(sanitize(carla), "<!-- licence -->\n<OpenDRIVE/>");
let quoted = "<OpenDRIVE><!-- see <?xml notes --><header/><?app x?></OpenDRIVE>";
assert_eq!(sanitize(quoted), quoted);
}
#[test]
fn empty_or_junk_is_an_error() {
assert!(load_str("<OpenDRIVE></OpenDRIVE>").is_err());
assert!(load_str("not xml at all <<<").is_err());
}
#[test]
fn no_lateral_profile_leaves_bank_empty() {
let net = load_str(STRAIGHT).expect("import");
assert!(
net.lanes()[0].bank.is_empty(),
"a flat road carries no bank"
);
assert_eq!(net.lanes()[0].bank_at(10.0), 0.0);
}
const SUPERELEV_CONST: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="se" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="0.1" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<left><lane id="1" type="driving"><width sOffset="0.0" a="3.5"/></lane></left>
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn constant_superelevation_pivots_about_the_reference_line() {
let net = load_str(SUPERELEV_CONST).expect("import");
let left = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Backward)
.expect("a left lane");
let right = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Forward)
.expect("a right lane");
assert!(
(left.bank_at(10.0) - 0.1).abs() < 1e-4,
"{}",
left.bank_at(10.0)
);
assert!(
(right.bank_at(10.0) - 0.1).abs() < 1e-4,
"{}",
right.bank_at(10.0)
);
assert!(!left.bank.is_empty(), "a banked lane carries a profile");
let expect = 1.75 * 0.1_f32.sin();
let ly = left.center.point_at(10.0).z;
let ry = right.center.point_at(10.0).z;
assert!((ly - expect).abs() < 0.02, "left z {ly}, want {expect}");
assert!((ry + expect).abs() < 0.02, "right z {ry}, want {}", -expect);
assert!(ly > ry, "left {ly} should ride above right {ry}");
let ly = left.center.point_at(10.0).y.abs();
assert!(ly < 1.75 && ly > 1.75 * 0.1_f32.cos() - 0.02, "y {ly}");
}
const SUPERELEV_RAMP: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="ser" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="0.0" b="0.01" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn ramped_superelevation_grows_along_s() {
let net = load_str(SUPERELEV_RAMP).expect("import");
let lane = &net.lanes()[0];
assert!(
(lane.bank_at(5.0) - 0.05).abs() < 5e-3,
"{}",
lane.bank_at(5.0)
);
assert!(
(lane.bank_at(15.0) - 0.15).abs() < 5e-3,
"{}",
lane.bank_at(15.0)
);
assert!(lane.bank_at(15.0) > lane.bank_at(5.0));
}
const SUPERELEV_TWO_LEFT: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="se2" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="0.1" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<left>
<lane id="1" type="driving"><width sOffset="0.0" a="3.5"/></lane>
<lane id="2" type="driving"><width sOffset="0.0" a="3.5"/></lane>
</left>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn outer_lane_of_a_banked_road_rides_higher() {
let net = load_str(SUPERELEV_TWO_LEFT).expect("import");
let mut zs: Vec<f32> = net
.lanes()
.iter()
.map(|l| l.center.point_at(10.0).z)
.collect();
zs.sort_by(|a, b| a.total_cmp(b));
let inner_z = 1.75 * 0.1_f32.sin();
let outer_z = 5.25 * 0.1_f32.sin();
assert!((zs[0] - inner_z).abs() < 0.02, "inner z {}", zs[0]);
assert!((zs[1] - outer_z).abs() < 0.02, "outer z {}", zs[1]);
assert!(
zs[1] - zs[0] > 0.3,
"outer lane {} should ride well above inner {}",
zs[1],
zs[0]
);
}
const SUPERELEV_NEG: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="sen" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="-0.1" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<left><lane id="1" type="driving"><width sOffset="0.0" a="3.5"/></lane></left>
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn negative_superelevation_raises_the_right_edge() {
let net = load_str(SUPERELEV_NEG).expect("import");
let left = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Backward)
.expect("a left lane");
let right = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Forward)
.expect("a right lane");
assert!(
right.center.point_at(10.0).z > left.center.point_at(10.0).z,
"right {} should ride above left {} for negative bank",
right.center.point_at(10.0).z,
left.center.point_at(10.0).z
);
assert!(
(left.bank_at(10.0) + 0.1).abs() < 1e-4,
"{}",
left.bank_at(10.0)
);
}
const SUPERELEV_ARC: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="sa" length="87.12" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="40.0"><line/></geometry>
<geometry s="40.0" x="40.0" y="0.0" hdg="0.0" length="47.12"><arc curvature="0.03333"/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="0.15" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<left><lane id="1" type="driving"><width sOffset="0.0" a="3.5"/></lane></left>
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn superelevation_on_a_banked_arc() {
let net = load_str(SUPERELEV_ARC).expect("import");
let left = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Backward)
.expect("a left lane");
let right = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Forward)
.expect("a right lane");
for lane in [left, right] {
assert!(!lane.bank.is_empty(), "banked lane carries a profile");
for &b in &lane.bank {
assert!(b.is_finite(), "bank entry not finite: {b}");
}
for p in lane.center.points() {
assert!(p.is_finite(), "centerline point not finite: {p:?}");
}
}
for &s in &[0.0_f32, 20.0, 60.0, 85.0] {
assert!(
(left.bank_at(s) - 0.15).abs() < 1e-3,
"left bank_at({s}) = {}",
left.bank_at(s)
);
assert!(
(right.bank_at(s) - 0.15).abs() < 1e-3,
"right bank_at({s}) = {}",
right.bank_at(s)
);
}
let expect = 1.75 * 0.15_f32.sin();
for &s in &[20.0_f32, 60.0, 85.0] {
let ly = left.center.point_at(s).z;
let ry = right.center.point_at(s).z;
assert!(
(ly - expect).abs() < 0.05,
"left z@{s} = {ly}, want {expect}"
);
assert!((ry + expect).abs() < 0.05, "right z@{s} = {ry}");
assert!(ly > ry, "left {ly} should ride above right {ry} @ s={s}");
}
}
#[test]
fn sample_near_on_an_imported_banked_road_is_sane() {
let net = load_str(SUPERELEV_ARC).expect("import");
let left = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Backward)
.expect("a left lane");
for &s in &[20.0_f32, 60.0] {
let on_lane = left.center.point_at(s);
let rs = net.sample_near(on_lane).expect("a sample near the road");
assert!(rs.point.is_finite(), "sample point not finite @ s={s}");
assert!(
(rs.bank.abs() - 0.15).abs() < 1e-2,
"bank @ s={s} = {} (want |0.15|)",
rs.bank
);
assert!(
(rs.up.length() - 1.0).abs() < 1e-4,
"up not unit @ s={s}: {:?}",
rs.up
);
assert!(rs.up.z > 0.9, "up.z too low @ s={s}: {}", rs.up.z);
assert!(
rs.up.dot(rs.heading).abs() < 1e-5,
"up.heading @ s={s} = {}",
rs.up.dot(rs.heading)
);
assert!(
(rs.point - on_lane).length() < 0.5,
"sample drifted from the query @ s={s}: {:?} vs {on_lane:?}",
rs.point
);
}
}
const SUPERELEV_PLUS_GRADE: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="sg" length="40.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="40.0"><line/></geometry>
</planView>
<elevationProfile>
<elevation s="0.0" a="0.0" b="0.04" c="0.0" d="0.0"/>
</elevationProfile>
<lateralProfile>
<superelevation s="0.0" a="0.1" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<left><lane id="1" type="driving"><width sOffset="0.0" a="3.5"/></lane></left>
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn superelevation_composes_with_elevation_grade() {
let net = load_str(SUPERELEV_PLUS_GRADE).expect("import");
let left = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Backward)
.expect("a left lane");
let right = net
.lanes()
.iter()
.find(|l| l.direction == Direction::Forward)
.expect("a right lane");
let cant = 1.75 * 0.1_f32.sin();
for &s in &[10.0_f32, 20.0, 30.0] {
let grade = 0.04 * s; let ly = left.center.point_at(s).z;
let ry = right.center.point_at(s).z;
assert!(
(ly - (grade + cant)).abs() < 0.02,
"left z@{s} = {ly}, want {}",
grade + cant
);
assert!(
(ry - (grade - cant)).abs() < 0.02,
"right z@{s} = {ry}, want {}",
grade - cant
);
assert!(((ly + ry) / 2.0 - grade).abs() < 0.02, "grade midline @{s}");
}
}
const SUPERELEV_PLUS_OFFSET: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="so" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="0.1" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneOffset s="0.0" a="2.0" b="0.0" c="0.0" d="0.0"/>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn superelevation_pivots_about_the_offset_shifted_cross_section() {
let net = load_str(SUPERELEV_PLUS_OFFSET).expect("import");
let lane = &net.lanes()[0];
let t = 2.0 - 1.75; let want_z = t * 0.1_f32.sin();
let z = lane.center.point_at(10.0).z;
assert!(
(z - want_z).abs() < 5e-3,
"z = {z}, want {want_z} (pivot on shifted t=+0.25, not own -1.75 nor offset +2.0)"
);
assert!(
z > 0.0,
"shifted t is +0.25 -> height must be positive, got {z}"
);
let y = lane.center.point_at(10.0).y;
assert!(
(y - t * 0.1_f32.cos()).abs() < 5e-3,
"y = {y}, want {}",
t * 0.1_f32.cos()
);
}
#[test]
fn banked_import_is_deterministic() {
let a = load_str(SUPERELEV_ARC).expect("import a");
let b = load_str(SUPERELEV_ARC).expect("import b");
assert_eq!(a.lanes().len(), b.lanes().len());
for (la, lb) in a.lanes().iter().zip(b.lanes().iter()) {
assert_eq!(la.bank, lb.bank, "bank vectors differ between imports");
assert_eq!(
la.center.points(),
lb.center.points(),
"centerline points differ between imports"
);
}
}
const SUPERELEV_STEEP: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="st" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="1.5" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn steep_superelevation_stays_finite() {
let net = load_str(SUPERELEV_STEEP).expect("import");
let lane = &net.lanes()[0];
assert!(
(lane.bank_at(10.0) - 1.5).abs() < 1e-3,
"{}",
lane.bank_at(10.0)
);
let p = lane.center.point_at(10.0);
assert!(p.is_finite(), "point not finite at steep bank: {p:?}");
assert!((p.z - (-1.75 * 1.5_f32.sin())).abs() < 0.02, "z {}", p.z);
assert!(
p.y.abs() < 0.2,
"horizontal reach should collapse, y {}",
p.y
);
}
const SUPERELEV_EXPLICIT_ZERO: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="sz" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="0.0" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn explicit_zero_superelevation_collapses_to_empty() {
let net = load_str(SUPERELEV_EXPLICIT_ZERO).expect("import");
assert!(
net.lanes()[0].bank.is_empty(),
"an all-zero profile must collapse to the flat sentinel"
);
assert_eq!(net.lanes()[0].bank_at(10.0), 0.0);
let p = net.lanes()[0].center.point_at(10.0);
assert!(p.z.abs() < 1e-4, "flat road, z {}", p.z);
}
const SUPERELEV_LATE_START: &str = r#"<?xml version="1.0"?>
<OpenDRIVE>
<road name="sl" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="10.0" a="0.1" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
</road>
</OpenDRIVE>"#;
#[test]
fn superelevation_starting_late_leaves_early_stations_flat() {
let net = load_str(SUPERELEV_LATE_START).expect("import");
let lane = &net.lanes()[0];
assert!(!lane.bank.is_empty());
assert!(
lane.bank_at(2.0).abs() < 1e-4,
"early bank {}",
lane.bank_at(2.0)
);
assert!(
lane.center.point_at(2.0).z.abs() < 1e-3,
"early height should be flat, z {}",
lane.center.point_at(2.0).z
);
assert!(
(lane.bank_at(18.0) - 0.1).abs() < 1e-3,
"late bank {}",
lane.bank_at(18.0)
);
assert!(
lane.center.point_at(18.0).z < -0.1,
"late height should be banked (t=-1.75), z {}",
lane.center.point_at(18.0).z
);
}
fn banked_road_with(objects: &str) -> String {
format!(
r#"<OpenDRIVE>
<road name="o" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lateralProfile>
<superelevation s="0.0" a="0.1" b="0.0" c="0.0" d="0.0"/>
</lateralProfile>
<lanes>
<laneSection s="0.0">
<right><lane id="-1" type="driving"><width sOffset="0.0" a="3.5"/></lane></right>
</laneSection>
</lanes>
<objects>{objects}</objects>
</road>
</OpenDRIVE>"#
)
}
fn two_section_road_with(objects: &str) -> String {
let section = |s| {
format!(
r#"<laneSection s="{s}">
<left><lane id="1" type="driving"><width sOffset="0.0" a="3"/></lane></left>
<right>
<lane id="-1" type="driving"><width sOffset="0.0" a="3"/></lane>
<lane id="-2" type="sidewalk"><width sOffset="0.0" a="2"/></lane>
</right>
</laneSection>"#
)
};
format!(
r#"<OpenDRIVE>
<road name="o" length="20.0" id="1" junction="-1">
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="20.0"><line/></geometry>
</planView>
<lanes>{}{}</lanes>
<objects>{objects}</objects>
</road>
</OpenDRIVE>"#,
section("0.0"),
section("10.0")
)
}
fn object_lanes(xml: &str) -> Vec<Vec<(usize, i32)>> {
let (net, prov) = load_str_with_provenance(xml).expect("import");
net.objects()
.iter()
.map(|o| {
o.lanes
.iter()
.map(|id| {
let p = prov.lanes.iter().find(|p| p.lane == *id).unwrap();
(p.section, p.od_id)
})
.collect()
})
.collect()
}
#[test]
fn an_object_applies_to_the_lanes_of_the_sections_it_spans() {
let xml = two_section_road_with(
r#"<object id="a" s="5" t="0"/>
<object id="b" s="10" t="0">
<validity fromLane="-2" toLane="-1"/>
</object>
<object id="c" s="0" t="0">
<repeat s="2" length="16" distance="0" height="1"/>
<validity fromLane="1" toLane="1"/>
</object>
<object id="d" s="20" t="0">
<validity fromLane="1" toLane="1"/>
<validity fromLane="-2" toLane="-2"/>
</object>"#,
);
assert_eq!(
object_lanes(&xml),
[
vec![(0, 1), (0, -1), (0, -2)],
vec![(1, -1), (1, -2)],
vec![(0, 1), (1, 1)],
vec![(1, 1), (1, -2)],
]
);
}
#[test]
fn an_object_on_a_road_without_lanes_applies_to_none() {
let xml = r#"<OpenDRIVE>
<road length="20.0" id="1">
<planView><geometry s="0" x="0" y="0" hdg="0" length="20"><line/></geometry></planView>
<objects><object id="a" s="5" t="0"/></objects>
</road>
<road length="20.0" id="2">
<planView><geometry s="0" x="0" y="9" hdg="0" length="20"><line/></geometry></planView>
<lanes><laneSection s="0"><right><lane id="-1" type="driving"><width sOffset="0" a="3"/></lane></right></laneSection></lanes>
</road>
</OpenDRIVE>"#;
let net = load_str(xml).expect("import");
assert_eq!(net.objects().len(), 1);
assert!(net.objects()[0].lanes.is_empty());
}
#[test]
fn a_marking_goes_to_the_outline_holding_its_corners() {
let square = |id: &str| {
let corners: String = [(0, 0), (1, 0), (1, 1), (0, 1)]
.iter()
.enumerate()
.map(|(k, (u, v))| {
format!(r#"<cornerLocal u="{u}" v="{v}" height="0" id="{id}{k}"/>"#)
})
.collect();
format!("<outline>{corners}</outline>")
};
let marking = |refs: &[&str], width: &str| {
let refs: String = refs
.iter()
.map(|r| format!(r#"<cornerReference id="{r}"/>"#))
.collect();
format!(r#"<marking width="{width}">{refs}</marking>"#)
};
let xml = banked_road_with(&format!(
r#"<object id="o" s="5" t="0"><outlines>{}{}</outlines><markings>{}{}{}{}{}{}</markings></object>"#,
square("a"),
square("b"),
marking(&["a0", "a1"], "0.1"),
marking(&["b1", "b2", "b3"], "0.1"),
marking(&["a0", "b1"], "0.1"),
marking(&["a0", "z9"], "0.1"),
marking(&["a0"], "0.1"),
marking(&["a0", "a1"], "0"),
));
let objects = objects_of(&xml);
let pieces = |o: &Object| {
o.markings
.iter()
.map(|m| m.pieces.len())
.collect::<Vec<_>>()
};
assert_eq!(pieces(&objects[0]), [1]);
assert_eq!(pieces(&objects[1]), [2]);
}
#[test]
fn a_band_lies_in_a_banked_road_rather_than_level() {
let xml = banked_road_with(
r#"<object id="a" s="5" t="-1">
<outlines><outline id="0">
<cornerRoad id="1" s="2" t="-1" dz="0"/><cornerRoad id="2" s="8" t="-1" dz="0"/>
<cornerRoad id="3" s="8" t="-3" dz="0"/></outline></outlines>
<markings><marking width="0.4"><cornerReference id="1"/><cornerReference id="2"/></marking></markings>
<borders><border width="1" useCompleteOutline="true"/></borders>
</object>
<object id="b" s="12" t="-2" hdg="0.5" perpToRoad="true">
<outlines><outline><cornerLocal u="0" v="0"/><cornerLocal u="3" v="0"/>
<cornerLocal u="3" v="2"/></outline></outlines>
<borders><border width="1" useCompleteOutline="true"/></borders>
</object>
<object id="c" s="16" t="-2" length="2" width="1" height="1" perpToRoad="true">
<markings><marking side="left" width="0.4"/></markings>
</object>"#,
);
let tan = 0.1_f32.tan();
let off = |p: Point| p.z - p.y * tan;
let objects = objects_of(&xml);
let mut bands = 0;
for object in &objects {
for border in &object.borders {
for p in border.pieces.iter().flatten() {
assert!(off(*p).abs() < 1e-5, "{} border {p:?}", object.id.0);
bands += 1;
}
}
for marking in &object.markings {
for p in marking.pieces.iter().flatten() {
let want = 0.005 / 0.1_f32.cos();
assert!((off(*p) - want).abs() < 1e-5, "{} paint {p:?}", object.id.0);
bands += 1;
}
}
}
assert_eq!(bands, 4 * (3 + 1 + 3 + 1));
}
#[test]
fn a_border_goes_to_the_outline_it_names() {
let square = |id: &str| {
format!(
r#"<outline id="{id}"><cornerLocal u="0" v="0"/><cornerLocal u="1" v="0"/><cornerLocal u="1" v="1"/></outline>"#
)
};
let border =
|outline: &str| format!(r#"<border width="0.2" {outline} useCompleteOutline="true"/>"#);
let xml = banked_road_with(&format!(
r#"<object id="o" s="5" t="0"><outlines>{}{}</outlines><borders>{}{}{}</borders></object>"#,
square("a"),
square("b"),
border(r#"outlineId="b""#),
border(""),
border(r#"outlineId="z""#),
));
let objects = objects_of(&xml);
let pieces = |o: &Object| o.borders.iter().map(|b| b.pieces.len()).collect::<Vec<_>>();
assert_eq!(pieces(&objects[0]), [3]);
assert_eq!(pieces(&objects[1]), [3, 3]);
}
fn ends(pieces: &[[Point; 4]]) -> Vec<(f32, f32)> {
pieces
.iter()
.map(|[a, b, c, d]| (a.lerp(*d, 0.5).x, b.lerp(*c, 0.5).x))
.collect()
}
fn level(points: &[Point]) -> Vec<(Point, Vector)> {
points.iter().map(|&p| (p, Vector::Z)).collect()
}
#[test]
fn a_strip_steps_over_an_edge_with_no_length() {
let (a, b, c) = (
Point::ORIGIN,
Point::new(2.0, 0.0, 0.0),
Point::new(4.0, 0.0, 0.0),
);
let up = Point::new(2.0, 0.0, 1.0);
assert_eq!(
ends(&strip(&level(&[a, a, b]), 0.0, 0.0, None, 0.1, usize::MAX).unwrap()),
[(0.0, 2.0)]
);
assert_eq!(
ends(&strip(&level(&[a, b, up]), 0.0, 0.0, None, 0.1, usize::MAX).unwrap()),
[(0.0, 2.0)]
);
let dashes = strip(
&level(&[a, b, b, c]),
1.0,
0.0,
Some((2.0, 1.0)),
0.1,
usize::MAX,
)
.unwrap();
assert_eq!(ends(&dashes), [(1.0, 2.0), (2.0, 3.0)]);
}
#[test]
fn a_strip_of_too_many_dashes_is_refused() {
let path = level(&[Point::ORIGIN, Point::new(10.0, 0.0, 0.0)]);
assert!(strip(&path, 0.0, 0.0, Some((1e-5, 1e-5)), 0.1, usize::MAX).is_none());
assert_eq!(
strip(&path, 0.0, 0.0, Some((0.5, 0.5)), 0.1, usize::MAX)
.unwrap()
.len(),
10
);
}
#[test]
fn dashes_from_far_before_a_strip_count_only_those_on_it() {
let path = level(&[Point::ORIGIN, Point::new(10.0, 0.0, 0.0)]);
let pieces = strip(&path, -1e6, 0.0, Some((1.0, 1.0)), 0.1, usize::MAX).unwrap();
assert_eq!(pieces.len(), 5);
}
#[test]
fn a_strip_lies_half_its_width_either_side_anticlockwise() {
let path = level(&[Point::ORIGIN, Point::new(10.0, 0.0, 0.0)]);
let pieces = strip(&path, 0.0, 0.0, None, 0.1, usize::MAX).unwrap();
let want = [(0.0, -0.1), (10.0, -0.1), (10.0, 0.1), (0.0, 0.1)];
for (p, (x, y)) in pieces[0].iter().zip(want) {
assert!((p.x - x).abs() < 1e-6 && (p.y - y).abs() < 1e-6, "{p:?}");
}
assert!(strip(&path, 0.0, 0.0, Some((1.0, 1.0)), 0.1, 3).is_none());
assert_eq!(
strip(&path, 0.0, 0.0, Some((1.0, 1.0)), 0.1, 5)
.unwrap()
.len(),
5
);
}
#[test]
fn offsets_past_the_end_of_a_strip_leave_nothing() {
let path = level(&[Point::ORIGIN, Point::new(10.0, 0.0, 0.0)]);
for (start, stop) in [(20.0, 0.0), (0.0, 20.0), (6.0, 6.0)] {
for dashes in [None, Some((1.0, 1.0))] {
let pieces = strip(&path, start, stop, dashes, 0.1, usize::MAX).unwrap();
assert!(pieces.is_empty(), "{start}, {stop}, {dashes:?}: {pieces:?}");
}
}
assert_eq!(
ends(&strip(&path, 4.0, 4.0, None, 0.1, usize::MAX).unwrap()),
[(4.0, 6.0)]
);
}
#[test]
fn a_side_marking_paints_every_solid_with_a_box() {
let markings = r#"<markings>
<marking side="front" width="0.1"/>
<marking side="top" width="0.1"/>
<marking side="left" width="0.1"><cornerReference id="0"/><cornerReference id="1"/></marking>
</markings>"#;
let xml = banked_road_with(&format!(
r#"<object id="a" s="5" t="0" length="2" width="1">
<repeat s="5" length="4" distance="2"/>{markings}</object>
<object id="b" s="5" t="0" radius="1">{markings}</object>
<object id="c" s="5" t="0">{markings}</object>"#
));
let counts: Vec<usize> = objects_of(&xml)
.iter()
.map(|o| o.markings.iter().map(|m| m.pieces.len()).sum())
.collect();
assert_eq!(counts, [1, 1, 1, 1, 0]);
}
#[test]
fn a_hole_goes_to_the_closed_outline_round_it() {
let square = |attrs: &str, (u, v): (f64, f64), size: f64| {
let corners: String = [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)]
.map(|(du, dv)| {
let (u, v) = (u + du * size, v + dv * size);
format!(r#"<cornerLocal u="{u}" v="{v}"/>"#)
})
.concat();
format!("<outline {attrs}>{corners}</outline>")
};
let hole = |at| square(r#"outer="false""#, at, 1.0);
let xml = banked_road_with(&format!(
r#"<object id="o" s="5" t="0"><outlines>{}{}{}{}{}{}</outlines></object>"#,
hole((11.0, 1.0)),
square("", (0.0, 0.0), 3.0),
square("", (10.0, 0.0), 3.0),
square(r#"closed="false""#, (20.0, 0.0), 3.0),
hole((21.0, 1.0)),
hole((30.0, 0.0)),
));
let holes: Vec<usize> = objects_of(&xml)
.iter()
.map(|o| match &o.shape {
Shape::Outline { holes, .. } => holes.len(),
_ => panic!("an outline: {:?}", o.shape),
})
.collect();
assert_eq!(holes, [0, 1, 0]);
}
#[test]
fn a_structure_covers_each_section_it_spans_as_far_along_each_lane() {
let xml = two_section_road_with(
r#"<tunnel s="5" length="10" name="t"/>
<bridge s="12" length="3" name="b"><validity fromLane="-2" toLane="-2"/></bridge>
<tunnel s="5" name="no length"/>
<bridge s="5" length="-1" name="negative"/>"#,
);
let (net, prov) = load_str_with_provenance(&xml).expect("import");
let section_of = |id: LaneId| {
let p = prov.lanes.iter().find(|p| p.lane == id).unwrap();
(p.section, p.od_id)
};
let covered = |i: usize| {
net.structures()[i]
.lanes
.iter()
.map(|c| (section_of(c.lane), c.from, c.to))
.collect::<Vec<_>>()
};
assert_eq!(net.structures().len(), 2);
assert_eq!(
covered(0),
[
((0, 1), 5.0, 10.0),
((0, -1), 5.0, 10.0),
((0, -2), 5.0, 10.0),
((1, 1), 0.0, 5.0),
((1, -1), 0.0, 5.0),
((1, -2), 0.0, 5.0),
]
);
assert_eq!(covered(1), [((1, -2), 2.0, 5.0)]);
}
fn objects_of(xml: &str) -> Vec<Object> {
load_str(xml).expect("import").objects().to_vec()
}
fn solid(object: &Object) -> (Point, f32, f32, Option<Extent>) {
match object.shape {
Shape::Solid {
position,
pitch,
roll,
extent,
..
} => (position, pitch, roll, extent),
ref other => panic!("not a solid: {other:?}"),
}
}
#[test]
fn an_object_stands_upright_on_a_banked_road() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" s="10" t="3" zOffset="0.5" roll="0.05"/>"#,
));
let (p, pitch, roll, _) = solid(&objects[0]);
let (sin, cos) = 0.1_f32.sin_cos();
assert!((p.x - 10.0).abs() < 1e-5, "x {}", p.x);
assert!((p.y - 3.0 * cos).abs() < 1e-5, "y {}", p.y);
assert!((p.z - (3.0 * sin + 0.5)).abs() < 1e-5, "z {}", p.z);
assert!(pitch.abs() < 1e-6, "pitch {pitch}");
assert!((roll - 0.05).abs() < 1e-6, "roll {roll}");
}
#[test]
fn an_object_perpendicular_to_the_road_leans_with_the_bank_and_ignores_pitch_and_roll() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" s="10" t="3" zOffset="0.5" pitch="0.2" roll="0.05" perpToRoad="true"/>"#,
));
let (p, pitch, roll, _) = solid(&objects[0]);
let (sin, cos) = 0.1_f32.sin_cos();
assert!((p.x - 10.0).abs() < 1e-5, "x {}", p.x);
assert!((p.y - (3.0 * cos - 0.5 * sin)).abs() < 1e-5, "y {}", p.y);
assert!((p.z - (3.0 * sin + 0.5 * cos)).abs() < 1e-5, "z {}", p.z);
assert!(pitch.abs() < 1e-6, "pitch {pitch}");
assert!((roll - 0.1).abs() < 1e-6, "roll {roll}");
}
#[test]
fn an_object_without_a_station_is_skipped() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" t="3"/><object id="2" s="NaN" t="3"/><object id="3" s="4" t="0"/>"#,
));
assert_eq!(objects.len(), 1);
assert_eq!(solid(&objects[0]).0.x, 4.0);
}
#[test]
fn instances_past_the_ends_of_the_road_are_dropped() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" s="0" t="0">
<repeat s="15" length="15" distance="5" tStart="0" tEnd="0"/>
</object>
<object id="2" s="-1" t="0"/>"#,
));
let xs: Vec<f32> = objects.iter().map(|o| solid(o).0.x).collect();
assert_eq!(xs, vec![15.0, 20.0]);
}
#[test]
fn a_runaway_repeat_is_refused_rather_than_expanded() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" s="0" t="0">
<repeat s="0" length="20" distance="0.00000002"/>
</object>"#,
));
assert!(objects.is_empty());
}
#[test]
fn each_repeat_of_an_object_contributes_its_own_row() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" type="pole" s="0" t="0" radius="0.1">
<repeat s="0" length="10" distance="10" tStart="2" tEnd="2"/>
<repeat s="0" length="10" distance="10" tStart="-2" tEnd="-2"/>
</object>"#,
));
assert_eq!(objects.len(), 4);
assert!(objects.iter().all(|o| o.kind == ObjectType::Pole));
assert!(objects.iter().all(|o| solid(o).3
== Some(Extent::Cylinder {
radius: 0.1,
height: 0.0
})));
}
#[test]
fn a_repeat_moves_the_outlines_to_each_step() {
let outlines = |ds: f64, dt: f64| {
let road = |s: f64, t: f64| format!(r#"<cornerRoad s="{}" t="{}"/>"#, s + ds, t + dt);
format!(
r#"<outlines>
<outline><cornerLocal u="0" v="0"/><cornerLocal u="2" v="0"/>
<cornerLocal u="2" v="2"/><cornerLocal u="0" v="2"/></outline>
<outline outer="false"><cornerLocal u="0.5" v="0.5"/>
<cornerLocal u="1.5" v="0.5"/><cornerLocal u="1" v="1.5"/></outline>
<outline>{}{}{}</outline>
</outlines>"#,
road(2.0, 3.0),
road(4.0, 3.0),
road(3.0, 5.0),
)
};
let repeated = objects_of(&banked_road_with(&format!(
r#"<object id="1" s="2" t="3" hdg="0.3">
<repeat s="1" length="10" distance="5" tStart="1" tEnd="2"/>{}
</object>"#,
outlines(0.0, 0.0),
)));
let mut want = Vec::new();
for (s, t) in [(1.0, 1.0), (6.0, 1.5), (11.0, 2.0)] {
want.extend(objects_of(&banked_road_with(&format!(
r#"<object id="1" s="{s}" t="{t}" hdg="0.3">{}</object>"#,
outlines(s - 2.0, t - 3.0),
))));
}
assert_eq!(want.len(), 6);
let shapes =
|objects: &[Object]| objects.iter().map(|o| o.shape.clone()).collect::<Vec<_>>();
assert_eq!(shapes(&repeated), shapes(&want));
assert!(matches!(&want[0].shape, Shape::Outline { holes, .. } if holes.len() == 1));
}
#[test]
fn a_section_bends_as_sharply_as_its_sharpest_lane_edge() {
let width = |a, c| Cubic {
start: 0.0,
a,
b: 0.0,
c,
d: 0.0,
};
let lane = |widths| LaneGeom {
id: -1,
extent: LaneExtent::Width(vec![widths]),
heights: Vec::new(),
level: false,
};
let road = |shape| [GeomRec::new(0.0, 0.0, 0.0, 0.0, 10.0, shape)];
let bend = |shape, right: &[LaneGeom]| {
section_bends(&road(shape), &[], [&[], right], 0.0, 10.0)
.into_iter()
.fold(0.0, f64::max)
};
let even = [lane(width(3.0, 0.0))];
assert!(bend(GeomShape::Line, &even) < 1e-9);
let arc = bend(GeomShape::Arc { curvature: 0.1 }, &even);
assert!((arc - 0.1).abs() < 1e-6, "{arc}");
let flare = bend(GeomShape::Line, &[lane(width(3.0, 0.1))]);
assert!((flare - 0.2).abs() < 0.01, "{flare}");
assert_eq!(
sample_positions(0.0, 6.0, &[0.0; 25], &[]),
[0.0, 2.0, 4.0, 6.0]
);
assert_eq!(sample_positions(0.0, 1.0, &[0.1; 5], &[]), [0.0, 0.5, 1.0]);
assert_eq!(sample_positions(0.0, 1.0, &[10.0; 5], &[]).len(), 5);
}
#[test]
fn only_the_road_near_a_tight_spot_is_sampled_finely() {
let mut bends = vec![0.0; 101];
bends[50] = 10.0;
let ss = sample_positions(0.0, 100.0, &bends, &[]);
let steps: Vec<f64> = ss.windows(2).map(|w| w[1] - w[0]).collect();
let at = |s: f64| steps[ss.iter().rposition(|&x| x <= s).unwrap()];
assert!((at(50.0) - SAMPLE_MIN_STEP).abs() < 1e-9, "{}", at(50.0));
assert!((at(5.0) - SAMPLE_STEP).abs() < 1e-9, "{}", at(5.0));
assert!((at(95.0) - SAMPLE_STEP).abs() < 1e-9, "{}", at(95.0));
assert!(ss.len() < 100, "{} samples", ss.len());
for w in steps[..steps.len() - 1].windows(2) {
assert!(w[1] > 0.5 * w[0], "{} after {}", w[1], w[0]);
}
}
#[test]
fn a_road_without_objects_has_none() {
assert!(load_str(STRAIGHT).expect("import").objects().is_empty());
}
#[test]
fn a_sweep_rises_straight_up_off_a_banked_road() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" s="0" t="0">
<repeat s="0" length="20" distance="0" tStart="3" tEnd="3" widthStart="1"
heightStart="2" zOffsetStart="0.5"/>
</object>"#,
));
let Shape::Sweep { sections, .. } = &objects[0].shape else {
panic!("not a sweep: {:?}", objects[0].shape);
};
assert_eq!(sections.len(), 3, "every 10 m, as it runs straight");
let (sin, cos) = 0.1_f32.sin_cos();
for section in sections {
for (corner, t) in [(section.left, 3.5_f32), (section.right, 2.5)] {
assert!(
(corner.base.y - t * cos).abs() < 1e-5,
"y {}",
corner.base.y
);
assert!((corner.base.z - (t * sin + 0.5)).abs() < 1e-5);
assert_eq!(corner.top - corner.base, Vector::new(0.0, 0.0, 2.0));
}
}
}
#[test]
fn a_sweep_is_sampled_closer_where_it_bends() {
let xml = r#"<OpenDRIVE><road length="20" id="1" junction="-1">
<planView>
<geometry s="0" x="0" y="0" hdg="0" length="10"><line/></geometry>
<geometry s="10" x="10" y="0" hdg="0" length="10"><arc curvature="0.1"/></geometry>
</planView>
<lanes><laneSection s="0"><right><lane id="-1" type="driving">
<width sOffset="0" a="3.5"/></lane></right></laneSection></lanes>
<objects><object id="1" s="0" t="0">
<repeat s="0" length="20" distance="0" heightStart="1"/>
</object></objects></road></OpenDRIVE>"#;
let objects = objects_of(xml);
let Shape::Sweep { sections, .. } = &objects[0].shape else {
panic!("not a sweep: {:?}", objects[0].shape);
};
let bases: Vec<Point> = sections.iter().map(|s| s.left.base).collect();
let on_arc = bases.iter().filter(|p| p.x > 10.0 + 1e-3).count();
assert_eq!(bases.len() - on_arc, 2, "the straight is one piece");
let centre = Point::new(10.0, 10.0, 0.0);
for pair in bases.windows(2).skip(1) {
let middle = pair[0].lerp(pair[1], 0.5);
let strays = 10.0 - middle.distance_to(centre);
assert!(strays < SWEEP_TOLERANCE, "{pair:?} strays {strays} m");
}
}
#[test]
fn a_sweep_starting_before_the_road_is_anchored_where_the_road_starts() {
let (_, prov) = load_str_with_provenance(&banked_road_with(
r#"<object id="1" s="0" t="0">
<repeat s="-10" length="20" distance="0" tStart="0" tEnd="4"/>
</object>"#,
))
.expect("import");
let p = &prov.objects[0];
assert_eq!((p.s, p.t), (0.0, 2.0));
}
#[test]
fn a_sweep_entirely_off_the_road_bakes_nothing() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" s="0" t="0">
<repeat s="25" length="10" distance="0" tStart="3"/>
</object>"#,
));
assert!(objects.is_empty());
}
#[test]
fn an_outline_straight_under_the_object_is_read_as_opendrive_1_4_writes_it() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" s="5" t="0">
<outline><cornerRoad s="5" t="0"/><cornerRoad s="6" t="0"/></outline>
</object>"#,
));
assert!(matches!(
&objects[0].shape,
Shape::Outline { corners, closed: true, .. } if corners.len() == 2
));
}
#[test]
fn an_outline_with_a_corner_off_the_road_is_dropped_whole() {
let objects = objects_of(&banked_road_with(
r#"<object id="1" s="5" t="0"><outlines><outline>
<cornerRoad s="18" t="0"/><cornerRoad s="22" t="0"/>
<cornerRoad s="22" t="2"/><cornerRoad s="18" t="2"/>
</outline></outlines></object>"#,
));
assert_eq!(objects.len(), 1);
assert!(matches!(objects[0].shape, Shape::Solid { .. }));
}
const FLAT: [[f64; 3]; 3] = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
#[test]
fn a_frames_angles_turn_it_as_it_is() {
let cubic = |a, b| Cubic {
start: 0.0,
a,
b,
c: 0.0,
d: 0.0,
};
let road = Road {
id: RoadId(0),
od_id: String::new(),
name: String::new(),
junction: None,
length: 10.0,
rule: TrafficRule::RightHand,
geoms: vec![GeomRec::new(0.0, 0.0, 0.0, 0.4, 10.0, GeomShape::Line)],
elevations: vec![cubic(0.0, 0.08)],
superelevations: vec![cubic(-0.2, 0.0)],
lateral: Lateral::default(),
lane_offsets: Vec::new(),
temporary_lane_offsets: Vec::new(),
sections: Vec::new(),
}
.axes(5.0);
let half = std::f64::consts::FRAC_PI_2;
for (axes, turn) in [
(FLAT, (0.3, -0.2, 0.5)),
(road, (0.3, -0.2, 0.5)),
(FLAT, (0.3, half, 0.5)),
(road, (1.0, 0.0, 0.0)),
] {
let frame = Frame::on_road(Point::ORIGIN, axes, turn);
let again = Frame::on_road(Point::ORIGIN, FLAT, frame.angles());
for (a, b) in frame.axes.iter().zip(again.axes) {
for (x, y) in a.iter().zip(b) {
assert!((x - y).abs() < 1e-9, "{:?} != {:?}", frame.axes, again.axes);
}
}
}
}
#[test]
fn a_frame_turns_yaw_then_pitch_then_roll() {
let frame = |yaw: f64, pitch: f64, roll: f64| {
Frame::on_road(Point::new(1.0, 2.0, 3.0), FLAT, (yaw, pitch, roll))
};
let near = |got: Point, want: [f32; 3]| {
assert!(
(got - Point::from_array(want)).length() < 1e-6,
"{got:?} != {want:?}"
);
};
let half = std::f64::consts::FRAC_PI_2;
near(
frame(half, 0.0, 0.0).point([1.0, 0.0, 0.0]),
[1.0, 3.0, 3.0],
);
near(
frame(0.0, half, 0.0).point([1.0, 0.0, 0.0]),
[1.0, 2.0, 2.0],
);
near(
frame(0.0, 0.0, half).point([0.0, 1.0, 0.0]),
[1.0, 2.0, 4.0],
);
near(
frame(half, half, half).point([0.0, 1.0, 0.0]),
[1.0, 3.0, 3.0],
);
}
fn raised_road(heights: &str, center: &str) -> String {
format!(
r#"<OpenDRIVE><road id="1" length="20" junction="-1">
<planView><geometry s="0" x="0" y="0" hdg="0" length="20"><line/></geometry></planView>
<lanes><laneSection s="0">
<center><lane id="0" type="none">{center}</lane></center>
<right>
<lane id="-1" type="driving"><width sOffset="0" a="3" b="0" c="0" d="0"/></lane>
<lane id="-2" type="sidewalk"><width sOffset="0" a="2" b="0" c="0" d="0"/>{heights}</lane>
</right>
</laneSection></lanes>
</road></OpenDRIVE>"#
)
}
fn sidewalk_z(xml: &str, x: f32) -> f32 {
let net = load_str(xml).unwrap();
let sidewalk = net
.lanes()
.iter()
.find(|l| l.kind == LaneType::Sidewalk)
.unwrap();
sidewalk
.center
.points()
.iter()
.find(|p| (p.x - x).abs() < 1e-4)
.unwrap_or_else(|| panic!("no station at {x}"))
.z
}
fn assert_z(got: f32, want: f32) {
assert!((got - want).abs() < 1e-6, "z {got}, want {want}");
}
#[test]
fn heights_go_straight_from_one_to_the_next_rather_than_step() {
let xml = raised_road(
r#"<height sOffset="0" inner="0" outer="0.2"/>
<height sOffset="10" inner="0.2" outer="0.2"/>"#,
"",
);
assert_z(sidewalk_z(&xml, 4.0), 0.14);
assert_z(sidewalk_z(&xml, 10.0), 0.2);
}
#[test]
fn the_last_height_holds_after_it() {
let xml = raised_road(
r#"<height sOffset="0" inner="0" outer="0"/>
<height sOffset="10" inner="0.2" outer="0.2"/>"#,
"",
);
assert_z(sidewalk_z(&xml, 16.0), 0.2);
}
#[test]
fn the_first_height_holds_before_it() {
let xml = raised_road(
r#"<height sOffset="6" inner="0.1" outer="0.1"/>
<height sOffset="10" inner="0.2" outer="0.2"/>"#,
"",
);
assert_z(sidewalk_z(&xml, 4.0), 0.1);
}
#[test]
fn a_height_missing_an_attribute_reads_it_as_0() {
let no_inner = raised_road(r#"<height sOffset="0" outer="0.2"/>"#, "");
assert_z(sidewalk_z(&no_inner, 4.0), 0.1);
let no_outer = raised_road(r#"<height sOffset="0" inner="0.2"/>"#, "");
assert_z(sidewalk_z(&no_outer, 4.0), 0.1);
let no_s_offset = raised_road(
r#"<height inner="0.1" outer="0.1"/>
<height sOffset="10" inner="0.3" outer="0.3"/>"#,
"",
);
assert_z(sidewalk_z(&no_s_offset, 0.0), 0.1);
assert_z(sidewalk_z(&no_s_offset, 4.0), 0.18);
}
#[test]
fn a_negative_height_s_offset_reads_as_0() {
let xml = raised_road(
r#"<height sOffset="-5" inner="0.1" outer="0.1"/>
<height sOffset="10" inner="0.3" outer="0.3"/>"#,
"",
);
assert_z(sidewalk_z(&xml, 0.0), 0.1);
assert_z(sidewalk_z(&xml, 4.0), 0.18);
}
#[test]
fn heights_out_of_order_are_sorted() {
let xml = raised_road(
r#"<height sOffset="10" inner="0.2" outer="0.2"/>
<height sOffset="0" inner="0" outer="0"/>"#,
"",
);
assert_z(sidewalk_z(&xml, 4.0), 0.08);
assert_z(sidewalk_z(&xml, 16.0), 0.2);
}
#[test]
fn a_second_height_at_the_same_s_offset_takes_over_there() {
let xml = raised_road(
r#"<height sOffset="0" inner="0.1" outer="0.1"/>
<height sOffset="10" inner="0.1" outer="0.1"/>
<height sOffset="10" inner="0.3" outer="0.3"/>"#,
"",
);
assert_z(sidewalk_z(&xml, 8.0), 0.1);
assert_z(sidewalk_z(&xml, 10.0), 0.3);
assert_z(sidewalk_z(&xml, 16.0), 0.3);
}
#[test]
fn heights_on_the_center_lane_are_ignored() {
let xml = raised_road("", r#"<height sOffset="0" inner="0.5" outer="0.5"/>"#);
let net = load_str(&xml).unwrap();
for lane in net.lanes() {
assert!(lane.bank.is_empty());
assert!(lane.center.points().iter().all(|p| p.z == 0.0));
}
}
#[test]
fn a_lane_without_heights_bakes_level_with_the_road() {
let (net, prov) = load_str_with_provenance(&raised_road("", "")).unwrap();
assert!(net.lanes().iter().all(|l| l.bank.is_empty()));
assert!(prov.lanes.iter().all(|p| p.heights.is_empty()));
}
#[test]
fn a_knot_becomes_a_station_and_never_a_stub() {
let ss = sample_positions(0.0, 10.0, &[0.0; 11], &[2.1, 5.0, 10.0, 12.0]);
assert_eq!(ss, [0.0, 1.05, 2.1, 3.55, 5.0, 7.0, 9.0, 10.0]);
let ss = sample_positions(0.0, 10.0, &[0.0; 11], &[4.0]);
assert_eq!(ss, [0.0, 2.0, 4.0, 6.0, 8.0, 10.0]);
}
fn with_objects(xml: &str, objects: &str) -> String {
xml.replace("</road>", &format!("<objects>{objects}</objects></road>"))
}
#[test]
fn a_point_on_a_border_stands_on_the_inner_lane() {
let xml = with_objects(
&raised_road(r#"<height sOffset="0" inner="0.2" outer="0.2"/>"#, ""),
r#"<object id="1" s="10" t="-3"/><object id="2" s="10" t="-3.001"/>"#,
);
let objects = objects_of(&xml);
assert_z(solid(&objects[0]).0.z, 0.0);
assert_z(solid(&objects[1]).0.z, 0.2);
}
#[test]
fn a_point_past_the_outermost_lane_takes_its_outer_height() {
let xml = with_objects(
&raised_road(r#"<height sOffset="0" inner="0.1" outer="0.2"/>"#, ""),
r#"<object id="1" s="10" t="-7"/>"#,
);
assert_z(solid(&objects_of(&xml)[0]).0.z, 0.2);
}
#[test]
fn an_outline_across_a_kerb_has_its_corners_on_either_side() {
let xml = with_objects(
&raised_road(r#"<height sOffset="0" inner="0.2" outer="0.2"/>"#, ""),
r#"<object id="1" s="10" t="-3"><outlines><outline id="0" closed="true">
<cornerRoad s="9" t="-2" dz="0" height="1"/>
<cornerRoad s="9" t="-4" dz="0" height="1"/>
<cornerRoad s="11" t="-4" dz="0" height="1"/>
</outline></outlines></object>"#,
);
let Shape::Outline { corners, .. } = &objects_of(&xml)[0].shape else {
panic!("not an outline");
};
let z: Vec<f32> = corners.iter().map(|c| c.base.z).collect();
assert_z(z[0], 0.0);
assert_z(z[1], 0.2);
assert_z(z[2], 0.2);
}
#[test]
fn a_sweep_along_a_sidewalk_stands_on_it() {
let xml = with_objects(
&raised_road(r#"<height sOffset="0" inner="0.2" outer="0.2"/>"#, ""),
r#"<object id="1" s="2" t="-4" height="0.8">
<repeat s="2" length="10" distance="0" tStart="-4" tEnd="-4"
heightStart="0.8" heightEnd="0.8" zOffsetStart="0" zOffsetEnd="0"/>
</object>"#,
);
let Shape::Sweep { sections, .. } = &objects_of(&xml)[0].shape else {
panic!("not a sweep");
};
assert!(!sections.is_empty());
for section in sections {
assert_z(section.left.base.z, 0.2);
assert_z(section.right.base.z, 0.2);
}
}
}