use std::collections::HashMap;
use std::ops::Range;
use crate::along::{self, Access, Along, RoadType, SpeedLimit, Visibility};
use crate::coords::Point;
use crate::crg::CrgSurface;
use crate::geo::GeoReference;
use crate::geometry::{Polyline, Projection, RoadSample};
use crate::grid::{Aabb, Grid};
use crate::header::Header;
use crate::junction::{CrossPath, Crossing, JunctionArea, JunctionGroup, VirtualJunction};
use crate::object::{Material, Object, ObjectId};
use crate::railway::{Station, Switch};
use crate::road::{LanePosition, Priority, Road, RoadId, RoadLane, RoadNeighbor, RoadPosition};
use crate::road_mark::{RoadMark, RoadMarkId};
use crate::signal::{Controller, ControllerId, Signal, SignalId, VmsGroup};
use crate::structure::{Coverage, Structure, StructureId};
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct LaneId(pub usize);
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub enum LaneType {
None,
Driving,
Bidirectional,
Bus,
Taxi,
Hov,
Entry,
Exit,
OnRamp,
OffRamp,
ConnectingRamp,
SlipLane,
Parking,
Stop,
Restricted,
Biking,
Sidewalk,
Shared,
Shoulder,
Border,
Curb,
Median,
RoadWorks,
Tram,
Rail,
Special1,
Special2,
Special3,
Unknown,
}
impl LaneType {
pub fn is_drivable(&self) -> bool {
matches!(
self,
Self::Driving
| Self::Bidirectional
| Self::Bus
| Self::Taxi
| Self::Hov
| Self::Entry
| Self::Exit
| Self::OnRamp
| Self::OffRamp
| Self::ConnectingRamp
| Self::SlipLane
| Self::Shared
)
}
pub fn as_str(&self) -> &'static str {
match self {
Self::None => "none",
Self::Driving => "driving",
Self::Bidirectional => "bidirectional",
Self::Bus => "bus",
Self::Taxi => "taxi",
Self::Hov => "hov",
Self::Entry => "entry",
Self::Exit => "exit",
Self::OnRamp => "on-ramp",
Self::OffRamp => "off-ramp",
Self::ConnectingRamp => "connecting-ramp",
Self::SlipLane => "slip-lane",
Self::Parking => "parking",
Self::Stop => "stop",
Self::Restricted => "restricted",
Self::Biking => "biking",
Self::Sidewalk => "sidewalk",
Self::Shared => "shared",
Self::Shoulder => "shoulder",
Self::Border => "border",
Self::Curb => "curb",
Self::Median => "median",
Self::RoadWorks => "road-works",
Self::Tram => "tram",
Self::Rail => "rail",
Self::Special1 => "special1",
Self::Special2 => "special2",
Self::Special3 => "special3",
Self::Unknown => "unknown",
}
}
}
impl std::fmt::Display for LaneType {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
f.write_str(self.as_str())
}
}
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Hash, PartialOrd, Ord)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum Layer {
#[default]
Permanent,
Temporary,
}
impl Layer {
pub(crate) fn from_attr(name: Option<&str>) -> Self {
match name {
Some("temporary") => Self::Temporary,
_ => Self::Permanent,
}
}
pub fn as_str(&self) -> &'static str {
match self {
Self::Permanent => "permanent",
Self::Temporary => "temporary",
}
}
}
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Hash)]
#[non_exhaustive]
pub enum LayerPolicy {
#[default]
Both,
PermanentOnly,
}
impl LayerPolicy {
pub fn admits(&self, lane: &Lane) -> bool {
match self {
Self::Both => true,
Self::PermanentOnly => lane.layer == Layer::Permanent,
}
}
}
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub enum LaneAdvisory {
#[default]
None,
Inner,
Outer,
Both,
}
impl LaneAdvisory {
pub fn as_str(&self) -> &'static str {
match self {
Self::None => "none",
Self::Inner => "inner",
Self::Outer => "outer",
Self::Both => "both",
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum Direction {
Forward,
Backward,
Both,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Lane {
pub id: LaneId,
pub kind: LaneType,
#[cfg_attr(feature = "serde", serde(default))]
pub layer: Layer,
pub direction: Direction,
pub center: Polyline,
pub width: f32,
pub widths: Vec<f32>,
pub bank: Vec<f32>,
pub successors: Vec<LaneId>,
pub predecessors: Vec<LaneId>,
pub neighbors: Vec<LaneId>,
}
#[derive(Debug, Clone, Default)]
#[cfg_attr(
feature = "serde",
derive(serde::Serialize, serde::Deserialize),
serde(into = "NetworkData", from = "NetworkData")
)]
pub struct RoadNetwork {
lanes: Vec<Lane>,
objects: Vec<Object>,
structures: Vec<Structure>,
signals: Vec<Signal>,
controllers: Vec<Controller>,
vms_groups: Vec<VmsGroup>,
road_marks: Vec<RoadMark>,
crg: Vec<CrgSurface>,
speed_limits: Vec<Along<SpeedLimit>>,
road_types: Vec<Along<RoadType>>,
road_countries: Vec<Along<String>>,
lane_rules: Vec<Along<String>>,
lane_access: Vec<Along<Access>>,
lane_materials: Vec<Along<Material>>,
lane_visibility: Vec<Along<Visibility>>,
geo: GeoReference,
header: Header,
roads: Vec<Road>,
priorities: Vec<Priority>,
road_neighbors: Vec<RoadNeighbor>,
junction_areas: Vec<JunctionArea>,
cross_paths: Vec<CrossPath>,
junction_groups: Vec<JunctionGroup>,
crossings: Vec<Crossing>,
virtual_junctions: Vec<VirtualJunction>,
switches: Vec<Switch>,
stations: Vec<Station>,
index: LaneIndex,
temporary_index: LaneIndex,
closing_index: LaneIndex,
road_lanes: HashMap<LaneId, RoadLane>,
footprints: LaneIndex,
}
impl PartialEq for RoadNetwork {
fn eq(&self, other: &Self) -> bool {
self.lanes == other.lanes
&& self.objects == other.objects
&& self.structures == other.structures
&& self.signals == other.signals
&& self.controllers == other.controllers
&& self.vms_groups == other.vms_groups
&& self.road_marks == other.road_marks
&& self.crg == other.crg
&& self.speed_limits == other.speed_limits
&& self.road_types == other.road_types
&& self.road_countries == other.road_countries
&& self.lane_rules == other.lane_rules
&& self.lane_access == other.lane_access
&& self.lane_materials == other.lane_materials
&& self.lane_visibility == other.lane_visibility
&& self.geo == other.geo
&& self.header == other.header
&& self.roads == other.roads
&& self.priorities == other.priorities
&& self.road_neighbors == other.road_neighbors
&& self.junction_areas == other.junction_areas
&& self.cross_paths == other.cross_paths
&& self.junction_groups == other.junction_groups
&& self.crossings == other.crossings
&& self.virtual_junctions == other.virtual_junctions
&& self.switches == other.switches
&& self.stations == other.stations
}
}
#[cfg(feature = "serde")]
#[derive(serde::Serialize, serde::Deserialize)]
struct NetworkData {
lanes: Vec<Lane>,
objects: Vec<Object>,
structures: Vec<Structure>,
signals: Vec<Signal>,
controllers: Vec<Controller>,
#[serde(default)]
vms_groups: Vec<VmsGroup>,
road_marks: Vec<RoadMark>,
crg: Vec<CrgSurface>,
speed_limits: Vec<Along<SpeedLimit>>,
road_types: Vec<Along<RoadType>>,
#[serde(default)]
road_countries: Vec<Along<String>>,
#[serde(default)]
lane_rules: Vec<Along<String>>,
#[serde(default)]
lane_access: Vec<Along<Access>>,
#[serde(default)]
lane_materials: Vec<Along<Material>>,
#[serde(default)]
lane_visibility: Vec<Along<Visibility>>,
#[serde(default)]
geo: GeoReference,
#[serde(default)]
header: Header,
#[serde(default)]
roads: Vec<Road>,
#[serde(default)]
priorities: Vec<Priority>,
#[serde(default)]
road_neighbors: Vec<RoadNeighbor>,
#[serde(default)]
junction_areas: Vec<JunctionArea>,
#[serde(default)]
cross_paths: Vec<CrossPath>,
#[serde(default)]
junction_groups: Vec<JunctionGroup>,
#[serde(default)]
crossings: Vec<Crossing>,
#[serde(default)]
virtual_junctions: Vec<VirtualJunction>,
#[serde(default)]
switches: Vec<Switch>,
#[serde(default)]
stations: Vec<Station>,
}
#[cfg(feature = "serde")]
impl From<NetworkData> for RoadNetwork {
fn from(data: NetworkData) -> Self {
Self::new(data.lanes)
.with_objects(data.objects)
.with_structures(data.structures)
.with_signals(data.signals)
.with_controllers(data.controllers)
.with_vms_groups(data.vms_groups)
.with_road_marks(data.road_marks)
.with_crg_surfaces(data.crg)
.with_speed_limits(data.speed_limits)
.with_road_types(data.road_types)
.with_road_countries(data.road_countries)
.with_lane_rules(data.lane_rules)
.with_lane_access(data.lane_access)
.with_lane_materials(data.lane_materials)
.with_lane_visibility(data.lane_visibility)
.with_geo_reference(data.geo)
.with_header(data.header)
.with_roads(data.roads)
.with_priorities(data.priorities)
.with_road_neighbors(data.road_neighbors)
.with_junction_areas(data.junction_areas)
.with_cross_paths(data.cross_paths)
.with_junction_groups(data.junction_groups)
.with_crossings(data.crossings)
.with_virtual_junctions(data.virtual_junctions)
.with_railways(data.switches, data.stations)
}
}
#[cfg(feature = "serde")]
impl From<RoadNetwork> for NetworkData {
fn from(net: RoadNetwork) -> Self {
Self {
lanes: net.lanes,
objects: net.objects,
structures: net.structures,
signals: net.signals,
controllers: net.controllers,
vms_groups: net.vms_groups,
road_marks: net.road_marks,
crg: net.crg,
speed_limits: net.speed_limits,
road_types: net.road_types,
road_countries: net.road_countries,
lane_rules: net.lane_rules,
lane_access: net.lane_access,
lane_materials: net.lane_materials,
lane_visibility: net.lane_visibility,
geo: net.geo,
header: net.header,
roads: net.roads,
priorities: net.priorities,
road_neighbors: net.road_neighbors,
junction_areas: net.junction_areas,
cross_paths: net.cross_paths,
junction_groups: net.junction_groups,
crossings: net.crossings,
virtual_junctions: net.virtual_junctions,
switches: net.switches,
stations: net.stations,
}
}
}
impl From<Vec<Lane>> for RoadNetwork {
fn from(lanes: Vec<Lane>) -> Self {
Self::new(lanes)
}
}
const SPAN_SEGMENTS: usize = 4;
const FOOTPRINT_MARGIN: f32 = 0.5;
#[derive(Debug, Clone, Default)]
struct LaneIndex {
bounds: Vec<Aabb>,
spans: Vec<(usize, Range<usize>)>,
grid: Grid,
}
const HOLD_MARGIN: f32 = 0.05;
const HOLD_HEIGHT: f32 = 1.5;
fn holds(lane: &Lane, point: Point, projection: &Projection) -> bool {
let along = (point.x - projection.point.x).hypot(point.y - projection.point.y);
let across = projection.offset.abs();
across <= lane.width_at(projection.s) * 0.5 + HOLD_MARGIN
&& along <= across + HOLD_MARGIN
&& (point.z - projection.point.z).abs() <= HOLD_HEIGHT
}
impl LaneIndex {
fn is_empty(&self) -> bool {
self.spans.is_empty()
}
fn closing(lanes: &[Lane]) -> Self {
Self::build(
lanes,
|lane| lane.layer == Layer::Temporary && !lane.kind.is_drivable(),
|_, _| 0.0,
)
}
fn driving(lanes: &[Lane], layer: Layer) -> Self {
Self::build(
lanes,
|lane| lane.kind == LaneType::Driving && lane.layer == layer,
|_, _| 0.0,
)
}
fn footprints(lanes: &[Lane]) -> Self {
Self::build(
lanes,
|_| true,
|lane, span| {
let widest = match lane.widths.get(span.start..=span.end) {
Some(widths) => widths.iter().copied().fold(0.0, f32::max),
None => lane.width,
};
widest / 2.0 + FOOTPRINT_MARGIN
},
)
}
fn build(
lanes: &[Lane],
keep: impl Fn(&Lane) -> bool,
pad: impl Fn(&Lane, &Range<usize>) -> f32,
) -> Self {
let (mut bounds, mut spans) = (Vec::new(), Vec::new());
for (i, lane) in lanes.iter().enumerate() {
if !keep(lane) {
continue;
}
let points = lane.center.points();
let segments = points.len() - 1;
for first in (0..segments).step_by(SPAN_SEGMENTS) {
let span = first..(first + SPAN_SEGMENTS).min(segments);
if let Some(b) =
Aabb::around(points[span.start..=span.end].iter().map(|p| (p.x, p.y)))
{
bounds.push(b.padded(pad(lane, &span)));
spans.push((i, span));
}
}
}
let grid = Grid::build(&bounds);
Self {
bounds,
spans,
grid,
}
}
}
fn horizontal_distance(a: Point, b: Point) -> f32 {
(a.x - b.x).hypot(a.y - b.y)
}
fn along_profile(profile: &[f32], (i, t): (usize, f32)) -> f32 {
let at = |k: usize| profile[k.min(profile.len() - 1)];
at(i) + (at(i + 1) - at(i)) * t
}
impl Lane {
pub fn bank_at(&self, s: f32) -> f32 {
if self.bank.is_empty() {
return 0.0;
}
along_profile(&self.bank, self.center.locate(s))
}
pub fn width_at(&self, s: f32) -> f32 {
if self.widths.is_empty() {
return self.width;
}
along_profile(&self.widths, self.center.locate(s))
}
pub fn sample_at(&self, s: f32) -> RoadSample {
let pose = self.center.pose_at(s);
RoadSample::new(pose.position, pose.heading, self.bank_at(s))
}
}
impl RoadNetwork {
pub fn new(lanes: Vec<Lane>) -> Self {
let index = LaneIndex::driving(&lanes, Layer::Permanent);
let temporary_index = LaneIndex::driving(&lanes, Layer::Temporary);
let closing_index = LaneIndex::closing(&lanes);
Self {
lanes,
objects: Vec::new(),
structures: Vec::new(),
signals: Vec::new(),
controllers: Vec::new(),
vms_groups: Vec::new(),
road_marks: Vec::new(),
crg: Vec::new(),
speed_limits: Vec::new(),
road_types: Vec::new(),
road_countries: Vec::new(),
lane_rules: Vec::new(),
lane_access: Vec::new(),
lane_materials: Vec::new(),
lane_visibility: Vec::new(),
geo: GeoReference::default(),
header: Header::default(),
roads: Vec::new(),
priorities: Vec::new(),
road_neighbors: Vec::new(),
junction_areas: Vec::new(),
cross_paths: Vec::new(),
junction_groups: Vec::new(),
crossings: Vec::new(),
virtual_junctions: Vec::new(),
switches: Vec::new(),
stations: Vec::new(),
index,
temporary_index,
closing_index,
road_lanes: HashMap::new(),
footprints: LaneIndex::default(),
}
}
pub fn with_roads(mut self, roads: Vec<Road>) -> Self {
self.road_lanes = roads
.iter()
.flat_map(|road| {
road.sections.iter().flat_map(move |section| {
section.lanes.iter().map(move |&(od_id, lane)| {
let at = RoadLane {
road: road.id,
section: section.index,
od_id,
};
(lane, at)
})
})
})
.collect();
self.footprints = if roads.is_empty() {
LaneIndex::default()
} else {
LaneIndex::footprints(&self.lanes)
};
self.roads = roads;
self
}
pub fn with_priorities(mut self, priorities: Vec<Priority>) -> Self {
self.priorities = priorities;
self
}
pub fn priorities(&self) -> &[Priority] {
&self.priorities
}
pub fn yields_to(&self, road: RoadId) -> impl Iterator<Item = RoadId> + '_ {
self.priorities
.iter()
.filter(move |p| p.low == road)
.map(|p| p.high)
}
pub fn with_road_neighbors(mut self, road_neighbors: Vec<RoadNeighbor>) -> Self {
self.road_neighbors = road_neighbors;
self
}
pub fn road_neighbors(&self) -> &[RoadNeighbor] {
&self.road_neighbors
}
pub fn with_junction_areas(mut self, junction_areas: Vec<JunctionArea>) -> Self {
self.junction_areas = junction_areas;
self
}
pub fn junction_areas(&self) -> &[JunctionArea] {
&self.junction_areas
}
pub fn with_cross_paths(mut self, cross_paths: Vec<CrossPath>) -> Self {
self.cross_paths = cross_paths;
self
}
pub fn cross_paths(&self) -> &[CrossPath] {
&self.cross_paths
}
pub fn with_crossings(mut self, crossings: Vec<Crossing>) -> Self {
self.crossings = crossings;
self
}
pub fn crossings(&self) -> &[Crossing] {
&self.crossings
}
pub fn with_junction_groups(mut self, junction_groups: Vec<JunctionGroup>) -> Self {
self.junction_groups = junction_groups;
self
}
pub fn junction_groups(&self) -> &[JunctionGroup] {
&self.junction_groups
}
pub fn junction_groups_of(&self, road: RoadId) -> impl Iterator<Item = &JunctionGroup> {
let junction = self.road(road).and_then(|r| r.junction());
self.junction_groups
.iter()
.filter(move |g| junction.is_some_and(|j| g.junctions.iter().any(|m| m == j)))
}
pub fn with_virtual_junctions(mut self, virtual_junctions: Vec<VirtualJunction>) -> Self {
self.virtual_junctions = virtual_junctions;
self
}
pub fn virtual_junctions(&self) -> &[VirtualJunction] {
&self.virtual_junctions
}
pub fn with_railways(mut self, switches: Vec<Switch>, stations: Vec<Station>) -> Self {
self.switches = switches;
self.stations = stations;
self
}
pub fn switches(&self) -> &[Switch] {
&self.switches
}
pub fn stations(&self) -> &[Station] {
&self.stations
}
pub fn roads(&self) -> &[Road] {
&self.roads
}
pub fn road(&self, id: RoadId) -> Option<&Road> {
match self.roads.get(id.0) {
Some(road) if road.id == id => Some(road),
_ => self.roads.iter().find(|r| r.id == id),
}
}
pub fn road_by_od_id(&self, od_id: &str) -> Option<&Road> {
self.roads.iter().find(|road| road.od_id == od_id)
}
pub fn road_lane(&self, lane: LaneId) -> Option<RoadLane> {
self.road_lanes.get(&lane).copied()
}
pub fn road_point(&self, at: RoadPosition) -> Option<Point> {
let road = self.road(at.road)?;
(road.on_road(at.s) && at.t.is_finite()).then(|| road.surface(at.s, at.t).0)
}
pub fn road_position(&self, point: Point) -> Option<RoadPosition> {
self.nearest_road_position(point, None)
}
pub fn road_position_on(&self, road: RoadId, point: Point) -> Option<RoadPosition> {
self.road(road)?;
self.nearest_road_position(point, Some(road))
}
fn nearest_road_position(&self, point: Point, only: Option<RoadId>) -> Option<RoadPosition> {
let index = &self.footprints;
let on_road = |item: u32| {
let lane = self.lanes[index.spans[item as usize].0].id;
only.is_none_or(|road| self.road_lanes.get(&lane).is_some_and(|at| at.road == road))
};
let seed = index.grid.nearest(point.x, point.y, |item, best| {
if !on_road(item) {
return None;
}
let (lane, segments) = &index.spans[item as usize];
let off = self.lane_reach(&self.lanes[*lane], segments, point).1;
Some((off.max(0.0).powi(2), item)).filter(|&(d2, _)| d2 <= best)
})?;
let seeded = self.footprint_position(seed, point);
let ceiling = seeded.map_or(f32::INFINITY, |(d2, _)| d2);
index.grid.nearest(point.x, point.y, |item, best| {
if item == seed {
return seeded;
}
let best = best.min(ceiling);
if index.bounds[item as usize].dist2(point.x, point.y) > best || !on_road(item) {
return None;
}
let (lane, segments) = &index.spans[item as usize];
let off = self.lane_reach(&self.lanes[*lane], segments, point).1;
if off > 0.0 && off * off > best {
return None;
}
self.footprint_position(item, point)
})
}
pub fn lane_point(&self, at: LanePosition) -> Option<Point> {
let on = self.road_lane(at.lane)?;
let road = self.road(on.road)?;
let section = road.section(on.section)?;
if !(section.start..=section.end).contains(&at.s) || !at.offset.is_finite() {
return None;
}
let lane = road.across(section, at.s).find(|a| a.od_id == on.od_id)?;
let t = (lane.inner_t + lane.outer_t) / 2.0 + at.offset;
let (low, high) = (
lane.inner_t.min(lane.outer_t),
lane.inner_t.max(lane.outer_t),
);
Some(road.raised(at.s, t, lane.height(t.clamp(low, high))).0)
}
pub fn lane_position(&self, point: Point) -> Option<LanePosition> {
self.lane_position_with(point, LayerPolicy::default())
}
pub fn lane_position_with(&self, point: Point, policy: LayerPolicy) -> Option<LanePosition> {
let at = self.road_position(point)?;
let road = self.road(at.road)?;
if policy == LayerPolicy::Both {
let held = road
.section_of(Layer::Temporary, at.s)
.and_then(|section| Some((section, road.lane_at(section, at.s, at.t)?)))
.filter(|(_, lane)| {
let (lo, hi) = (
lane.inner_t.min(lane.outer_t),
lane.inner_t.max(lane.outer_t),
);
(lo..=hi).contains(&at.t)
})
.and_then(|(section, lane)| {
let (_, id) = *section.lanes.iter().find(|(od, _)| *od == lane.od_id)?;
Some(LanePosition {
lane: id,
s: at.s,
offset: at.t - (lane.inner_t + lane.outer_t) / 2.0,
})
});
if held.is_some() {
return held;
}
}
let section = road
.section_at(at.s)
.filter(|section| (section.start - 1e-9..=section.end).contains(&at.s))?;
let innermost = |od_id: i32| road.across(section, at.s).find(|a| a.od_id == od_id);
let lane = road
.lane_at(section, at.s, at.t)
.or_else(|| innermost(-1))
.or_else(|| innermost(1))?;
let (_, id) = *section
.lanes
.iter()
.find(|(od_id, _)| *od_id == lane.od_id)?;
Some(LanePosition {
lane: id,
s: at.s.max(section.start).min(section.end),
offset: at.t - (lane.inner_t + lane.outer_t) / 2.0,
})
}
pub fn centerline_s(&self, at: LanePosition) -> Option<f32> {
let on = self.road_lane(at.lane)?;
let section = self.road(on.road)?.section(on.section)?;
let lane = self.lane(at.lane)?;
(section.start..=section.end)
.contains(&at.s)
.then(|| crate::road::along(lane.center.points(), §ion.stations, at.s))
}
fn lane_reach(&self, lane: &Lane, segments: &Range<usize>, point: Point) -> (Projection, f32) {
let projection = lane.center.project_segments(point, segments.clone());
let reach = lane.width_at(projection.s) / 2.0 + FOOTPRINT_MARGIN;
let off = horizontal_distance(point, projection.point) - reach;
(projection, off)
}
fn footprint_position(&self, item: u32, point: Point) -> Option<(f32, RoadPosition)> {
let (lane, segments) = &self.footprints.spans[item as usize];
let lane = &self.lanes[*lane];
let at = self.road_lanes.get(&lane.id)?;
let road = self.road(at.road)?;
let section = road.sections.iter().find(|sec| sec.index == at.section)?;
let (projection, _) = self.lane_reach(lane, segments, point);
let (k, f) = lane.center.locate(projection.s);
let (from, to) = (*section.stations.get(k)?, *section.stations.get(k + 1)?);
let guess = from + (to - from) * f64::from(f);
let (s, t) = road.locate(f64::from(point.x), f64::from(point.y), guess);
let on = s.max(section.start).min(section.end);
let across = road.across(section, on).find(|a| a.od_id == at.od_id)?;
let (low, high) = (
across.inner_t.min(across.outer_t),
across.inner_t.max(across.outer_t),
);
let nearest = road.surface(on, t.clamp(low, high)).0;
let position = RoadPosition {
road: road.id,
s,
t,
};
Some(((point - nearest).length_squared(), position))
}
pub fn with_objects(mut self, objects: Vec<Object>) -> Self {
self.objects = objects;
self
}
pub fn with_structures(mut self, structures: Vec<Structure>) -> Self {
self.structures = structures;
self
}
pub fn with_signals(mut self, signals: Vec<Signal>) -> Self {
self.signals = signals;
self
}
pub fn with_vms_groups(mut self, vms_groups: Vec<VmsGroup>) -> Self {
self.vms_groups = vms_groups;
self
}
pub fn vms_groups(&self) -> &[VmsGroup] {
&self.vms_groups
}
pub fn with_controllers(mut self, controllers: Vec<Controller>) -> Self {
self.controllers = controllers;
self
}
pub fn with_road_marks(mut self, road_marks: Vec<RoadMark>) -> Self {
self.road_marks = road_marks;
self
}
pub fn with_crg_surfaces(mut self, crg: Vec<CrgSurface>) -> Self {
self.crg = crg;
self
}
pub fn with_speed_limits(mut self, speed_limits: Vec<Along<SpeedLimit>>) -> Self {
self.speed_limits = along::sorted(speed_limits);
self
}
pub fn with_road_types(mut self, road_types: Vec<Along<RoadType>>) -> Self {
self.road_types = along::sorted(road_types);
self
}
pub fn with_road_countries(mut self, road_countries: Vec<Along<String>>) -> Self {
self.road_countries = along::sorted(road_countries);
self
}
pub fn with_lane_rules(mut self, lane_rules: Vec<Along<String>>) -> Self {
self.lane_rules = along::sorted(lane_rules);
self
}
pub fn with_lane_access(mut self, lane_access: Vec<Along<Access>>) -> Self {
self.lane_access = along::sorted(lane_access);
self
}
pub fn with_lane_materials(mut self, lane_materials: Vec<Along<Material>>) -> Self {
self.lane_materials = along::sorted(lane_materials);
self
}
pub fn speed_limits(&self) -> &[Along<SpeedLimit>] {
&self.speed_limits
}
pub fn speed_limit_at(&self, lane: LaneId, s: f32) -> Option<SpeedLimit> {
along::at(&self.speed_limits, lane, s).copied()
}
pub fn road_types(&self) -> &[Along<RoadType>] {
&self.road_types
}
pub fn road_type_at(&self, lane: LaneId, s: f32) -> Option<RoadType> {
along::at(&self.road_types, lane, s).copied()
}
pub fn road_countries(&self) -> &[Along<String>] {
&self.road_countries
}
pub fn road_country_at(&self, lane: LaneId, s: f32) -> Option<&str> {
along::at(&self.road_countries, lane, s).map(String::as_str)
}
pub fn lane_rules(&self) -> &[Along<String>] {
&self.lane_rules
}
pub fn lane_rule_at(&self, lane: LaneId, s: f32) -> Option<&str> {
along::at(&self.lane_rules, lane, s).map(String::as_str)
}
pub fn lane_access(&self) -> &[Along<Access>] {
&self.lane_access
}
pub fn lane_access_at(&self, lane: LaneId, s: f32) -> Option<&Access> {
along::at(&self.lane_access, lane, s)
}
pub fn lane_materials(&self) -> &[Along<Material>] {
&self.lane_materials
}
pub fn lane_material_at(&self, lane: LaneId, s: f32) -> Option<&Material> {
along::at(&self.lane_materials, lane, s)
}
pub fn with_lane_visibility(mut self, lane_visibility: Vec<Along<Visibility>>) -> Self {
self.lane_visibility = along::sorted(lane_visibility);
self
}
pub fn lane_visibility(&self) -> &[Along<Visibility>] {
&self.lane_visibility
}
pub fn lane_visibility_at(&self, lane: LaneId, s: f32) -> Option<&Visibility> {
along::at(&self.lane_visibility, lane, s)
}
pub fn with_geo_reference(mut self, geo: GeoReference) -> Self {
self.geo = geo;
self
}
pub fn geo_reference(&self) -> &GeoReference {
&self.geo
}
pub fn with_header(mut self, header: Header) -> Self {
self.header = header;
self
}
pub fn header(&self) -> &Header {
&self.header
}
pub fn crg_surfaces(&self) -> &[CrgSurface] {
&self.crg
}
pub fn structures(&self) -> &[Structure] {
&self.structures
}
pub fn structure(&self, id: StructureId) -> Option<&Structure> {
match self.structures.get(id.0) {
Some(structure) if structure.id == id => Some(structure),
_ => self.structures.iter().find(|s| s.id == id),
}
}
pub fn structures_over(&self, lane: LaneId) -> impl Iterator<Item = (&Structure, &Coverage)> {
self.structures.iter().flat_map(move |s| {
s.lanes
.iter()
.filter(move |c| c.lane == lane)
.map(move |c| (s, c))
})
}
pub fn lanes(&self) -> &[Lane] {
&self.lanes
}
pub fn objects(&self) -> &[Object] {
&self.objects
}
pub fn object(&self, id: ObjectId) -> Option<&Object> {
match self.objects.get(id.0) {
Some(object) if object.id == id => Some(object),
_ => self.objects.iter().find(|o| o.id == id),
}
}
pub fn signals(&self) -> &[Signal] {
&self.signals
}
pub fn signal(&self, id: SignalId) -> Option<&Signal> {
match self.signals.get(id.0) {
Some(signal) if signal.id == id => Some(signal),
_ => self.signals.iter().find(|s| s.id == id),
}
}
pub fn controllers(&self) -> &[Controller] {
&self.controllers
}
pub fn controller(&self, id: ControllerId) -> Option<&Controller> {
match self.controllers.get(id.0) {
Some(controller) if controller.id == id => Some(controller),
_ => self.controllers.iter().find(|c| c.id == id),
}
}
pub fn road_marks(&self) -> &[RoadMark] {
&self.road_marks
}
pub fn road_mark(&self, id: RoadMarkId) -> Option<&RoadMark> {
match self.road_marks.get(id.0) {
Some(mark) if mark.id == id => Some(mark),
_ => self.road_marks.iter().find(|m| m.id == id),
}
}
pub fn lane(&self, id: LaneId) -> Option<&Lane> {
match self.lanes.get(id.0) {
Some(lane) if lane.id == id => Some(lane),
_ => self.lanes.iter().find(|l| l.id == id),
}
}
pub fn driving_lanes(&self) -> impl Iterator<Item = &Lane> {
self.lanes.iter().filter(|l| l.kind == LaneType::Driving)
}
pub fn min_elevation(&self) -> Option<f32> {
self.lanes
.iter()
.flat_map(|l| l.center.points())
.map(|p| p.z)
.filter(|z| !z.is_nan())
.reduce(f32::min)
}
pub fn successors(&self, id: LaneId) -> impl Iterator<Item = &Lane> {
self.lane(id)
.into_iter()
.flat_map(|l| l.successors.iter())
.filter_map(|s| self.lane(*s))
}
pub fn nearest_lane(&self, point: Point) -> Option<(LaneId, Projection)> {
self.nearest_lane_with(point, LayerPolicy::default())
}
pub fn nearest_lane_with(
&self,
point: Point,
policy: LayerPolicy,
) -> Option<(LaneId, Projection)> {
if !point.to_array().iter().all(|c| c.is_finite()) {
return None;
}
let both = policy == LayerPolicy::Both;
if both && !self.temporary_index.is_empty() {
let held = self.nearest_in(&self.temporary_index, point, true, false);
if held.is_some() {
return held;
}
}
let hidden = both && !self.closing_index.is_empty();
self.nearest_in(&self.index, point, false, hidden)
}
fn nearest_in(
&self,
index: &LaneIndex,
point: Point,
must_hold: bool,
skip_closed: bool,
) -> Option<(LaneId, Projection)> {
index.grid.nearest(point.x, point.y, |item, best| {
let i = item as usize;
if index.bounds[i].dist2(point.x, point.y) > best {
return None;
}
let (lane, segments) = &index.spans[i];
let lane = &self.lanes[*lane];
let projection = lane.center.project_segments(point, segments.clone());
if must_hold && !holds(lane, point, &projection) {
return None;
}
if skip_closed {
let under = lane.center.project(point).point;
if self
.nearest_in(&self.closing_index, under, true, false)
.is_some()
{
return None;
}
}
Some((
(point - projection.point).length_squared(),
(lane.id, projection),
))
})
}
pub fn sample_near(&self, point: Point) -> Option<RoadSample> {
let (id, proj) = self.nearest_lane(point)?;
self.lane(id).map(|lane| lane.sample_at(proj.s))
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::coords::Vector;
fn lane(id: usize, points: &[[f32; 3]]) -> Lane {
Lane {
id: LaneId(id),
kind: LaneType::Driving,
layer: crate::Layer::Permanent,
direction: Direction::Forward,
center: Polyline::new(points.iter().map(|p| Point::from_array(*p)).collect()),
width: 3.5,
widths: Vec::new(),
bank: Vec::new(),
successors: Vec::new(),
predecessors: Vec::new(),
neighbors: Vec::new(),
}
}
#[test]
fn nearest_lane_picks_the_closer_centerline() {
let net = RoadNetwork::new(vec![
lane(0, &[[0.0, 2.0, 0.0], [10.0, 2.0, 0.0]]),
lane(1, &[[0.0, -2.0, 0.0], [10.0, -2.0, 0.0]]),
]);
let (id, proj) = net.nearest_lane(Point::new(5.0, 1.5, 0.0)).expect("a lane");
assert_eq!(id, LaneId(0));
assert!((proj.point - Point::new(5.0, 2.0, 0.0)).length() < 1e-4);
}
#[test]
fn nearest_lane_is_none_when_empty() {
assert!(RoadNetwork::default().nearest_lane(Point::ORIGIN).is_none());
}
#[test]
fn sample_at_reads_bank_and_tilts_the_up_normal() {
let mut banked = lane(0, &[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]]);
banked.bank = vec![0.2, 0.2];
let s = banked.sample_at(5.0);
assert!((s.bank - 0.2).abs() < 1e-5, "bank {}", s.bank);
assert!(
s.heading.abs_diff_eq(Vector::X, 1e-4),
"heading {:?}",
s.heading
);
assert!(s.up.z < 1.0 && s.up.z > 0.9, "up {:?}", s.up);
assert!((s.up - Vector::Z).length() > 0.05, "up should tilt");
let flat = lane(1, &[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]]);
let fs = flat.sample_at(5.0);
assert_eq!(fs.bank, 0.0);
assert!(fs.up.abs_diff_eq(Vector::Z, 1e-5), "flat up {:?}", fs.up);
}
#[test]
fn sample_near_samples_the_nearest_lane() {
let mut banked = lane(0, &[[0.0, 2.0, 0.0], [10.0, 2.0, 0.0]]);
banked.bank = vec![0.1, 0.1];
let flat = lane(1, &[[0.0, -2.0, 0.0], [10.0, -2.0, 0.0]]);
let net = RoadNetwork::new(vec![banked, flat]);
let a = net
.sample_near(Point::new(5.0, 1.8, 0.0))
.expect("a sample");
assert!((a.bank - 0.1).abs() < 1e-5, "bank {}", a.bank);
let b = net
.sample_near(Point::new(5.0, -1.8, 0.0))
.expect("a sample");
assert_eq!(b.bank, 0.0);
}
#[test]
fn sample_near_is_none_when_empty() {
assert!(RoadNetwork::default().sample_near(Point::ORIGIN).is_none());
}
#[test]
fn sample_at_up_follows_stored_tangent_not_travel() {
let pts = &[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]];
let mut fwd = lane(0, pts);
fwd.bank = vec![0.2, 0.2];
let mut bwd = lane(1, pts);
bwd.direction = Direction::Backward;
bwd.bank = vec![0.2, 0.2];
let f = fwd.sample_at(5.0);
let b = bwd.sample_at(5.0);
assert_eq!(f.up, b.up, "up must not depend on travel direction");
assert_eq!(f.bank, b.bank);
assert!(f.heading.abs_diff_eq(b.heading, 1e-6));
assert!(
b.up.y < -0.05,
"up should lean away from the raised +Y edge: {:?}",
b.up
);
}
#[test]
fn sample_at_on_a_curved_banked_lane() {
let mut l = lane(0, &[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0], [20.0, 10.0, 0.0]]);
l.bank = vec![0.0, 0.1, 0.2];
let seg1 = 10.0_f32;
let seg2 = (100.0_f32 + 100.0).sqrt();
assert!(
l.sample_at(0.0).heading.abs_diff_eq(Vector::X, 1e-5),
"start heading {:?}",
l.sample_at(0.0).heading
);
let end_dir = Vector::new(1.0, 1.0, 0.0).normalize_or_zero();
assert!(
l.sample_at(seg1 + seg2).heading.abs_diff_eq(end_dir, 1e-4),
"end heading {:?} want {end_dir:?}",
l.sample_at(seg1 + seg2).heading
);
assert!((l.sample_at(seg1).bank - 0.1).abs() < 1e-5);
assert!((l.sample_at(seg1 + seg2).bank - 0.2).abs() < 1e-5);
assert!(
(l.sample_at(seg1 + seg2 * 0.5).bank - 0.15).abs() < 1e-5,
"mid-seg2 bank {}",
l.sample_at(seg1 + seg2 * 0.5).bank
);
for s in [0.0, 3.0, seg1, seg1 + 4.0, seg1 + seg2] {
let rs = l.sample_at(s);
assert!((rs.up.length() - 1.0).abs() < 1e-5, "up not unit @ {s}");
assert!(rs.up.dot(rs.heading).abs() < 1e-6, "up.heading != 0 @ {s}");
assert!(rs.up.z > 0.9, "up.z too low @ {s}: {}", rs.up.z);
if rs.bank.abs() > 1e-3 {
assert!(
(rs.up - Vector::Z).length() > 0.02,
"up should tilt where banked @ {s}: {:?}",
rs.up
);
}
}
}
#[test]
fn sample_at_negative_bank_flips_the_up_normal() {
let mut pos = lane(0, &[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]]);
pos.bank = vec![0.25, 0.25];
let mut neg = lane(1, &[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]]);
neg.bank = vec![-0.25, -0.25];
let p = pos.sample_at(5.0);
let n = neg.sample_at(5.0);
assert!(p.up.y < -0.05, "+bank up {:?}", p.up);
assert!(n.up.y > 0.05, "-bank up {:?}", n.up);
assert!((p.up.y + n.up.y).abs() < 1e-6, "should be mirrored in y");
assert!((p.up.z - n.up.z).abs() < 1e-6, "same height component");
}
#[test]
fn sample_near_steps_at_a_banked_lane_boundary() {
let mut a = lane(0, &[[0.0, 2.0, 0.3], [10.0, 2.0, 0.3]]);
a.bank = vec![0.1, 0.1];
let mut b = lane(1, &[[0.0, -2.0, -0.3], [10.0, -2.0, -0.3]]);
b.bank = vec![-0.15, -0.15];
let net = RoadNetwork::new(vec![a, b]);
let sa = net
.sample_near(Point::new(5.0, 0.1, 0.0))
.expect("sample A");
assert!((sa.bank - 0.1).abs() < 1e-5, "A bank {}", sa.bank);
assert!((sa.point.z - 0.3).abs() < 1e-5, "A height {}", sa.point.z);
let sb = net
.sample_near(Point::new(5.0, -0.1, 0.0))
.expect("sample B");
assert!((sb.bank + 0.15).abs() < 1e-5, "B bank {}", sb.bank);
assert!((sb.point.z + 0.3).abs() < 1e-5, "B height {}", sb.point.z);
assert!(
(sa.point.z - sb.point.z).abs() > 0.5,
"expected a vertical step across the boundary: {} vs {}",
sa.point.z,
sb.point.z
);
}
#[test]
fn sample_at_reads_vertex_bank_exactly() {
let mut l = lane(0, &[[0.0, 0.0, 0.0], [4.0, 0.0, 0.0], [4.0, 0.0, 6.0]]);
l.bank = vec![0.05, 0.12, -0.08];
for (s, want) in [(0.0, 0.05), (4.0, 0.12), (10.0, -0.08)] {
assert!(
(l.sample_at(s).bank - want).abs() < 1e-6,
"vertex bank @ {s} = {}, want {want}",
l.sample_at(s).bank
);
}
}
#[test]
fn min_elevation_is_the_lowest_centerline_point() {
let net = RoadNetwork::new(vec![
lane(0, &[[0.0, 0.0, 5.0], [10.0, 0.0, 2.0]]),
lane(1, &[[0.0, 0.0, -40.0], [10.0, 0.0, -12.0]]),
]);
assert_eq!(net.min_elevation(), Some(-40.0));
assert_eq!(RoadNetwork::default().min_elevation(), None);
}
#[test]
fn lane_lookup_is_by_id_not_position() {
let net = RoadNetwork::new(vec![
Lane {
id: LaneId(17),
..lane(0, &[[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]])
},
Lane {
id: LaneId(4),
..lane(1, &[[0.0, 5.0, 0.0], [1.0, 5.0, 0.0]])
},
]);
assert_eq!(net.lane(LaneId(17)).map(|l| l.id), Some(LaneId(17)));
assert_eq!(net.lane(LaneId(4)).map(|l| l.id), Some(LaneId(4)));
assert!(net.lane(LaneId(0)).is_none()); let (id, _) = net.nearest_lane(Point::new(0.5, 0.0, 0.0)).unwrap();
assert!(net.lane(id).is_some());
}
#[test]
fn bank_at_of_a_flat_lane_is_zero_and_never_panics() {
let l = lane(0, &[[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [4.0, 0.0, 0.0]]);
assert!(l.bank.is_empty());
for s in [-10.0, -0.0, 0.0, 1.0, 2.0, 4.0, 4.0001, 1000.0] {
assert_eq!(l.bank_at(s), 0.0, "flat bank_at({s})");
}
}
#[test]
fn bank_at_interpolates_between_vertices_and_clamps() {
let l = Lane {
bank: vec![0.0, 0.1, 0.2],
..lane(0, &[[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [4.0, 0.0, 0.0]])
};
assert!((l.bank_at(0.0) - 0.0).abs() < 1e-6, "{}", l.bank_at(0.0));
assert!((l.bank_at(2.0) - 0.1).abs() < 1e-6, "{}", l.bank_at(2.0));
assert!((l.bank_at(4.0) - 0.2).abs() < 1e-6, "{}", l.bank_at(4.0));
assert!((l.bank_at(1.0) - 0.05).abs() < 1e-6, "{}", l.bank_at(1.0));
assert!((l.bank_at(3.0) - 0.15).abs() < 1e-6, "{}", l.bank_at(3.0));
assert!(
(l.bank_at(100.0) - 0.2).abs() < 1e-6,
"{}",
l.bank_at(100.0)
);
assert!(
(l.bank_at(-100.0) - 0.0).abs() < 1e-6,
"{}",
l.bank_at(-100.0)
);
let len = l.center.length();
assert!((l.bank_at(len) - 0.2).abs() < 1e-6);
assert!((l.bank_at(len + 5.0) - 0.2).abs() < 1e-6);
}
#[test]
fn bank_at_preserves_a_negative_profile() {
let l = Lane {
bank: vec![-0.2, -0.1, 0.0],
..lane(0, &[[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [4.0, 0.0, 0.0]])
};
assert!((l.bank_at(0.0) + 0.2).abs() < 1e-6, "{}", l.bank_at(0.0));
assert!((l.bank_at(1.0) + 0.15).abs() < 1e-6, "{}", l.bank_at(1.0));
assert!((l.bank_at(-5.0) + 0.2).abs() < 1e-6, "clamp low");
}
}