use std::collections::HashMap;
use crate::coords::{Point, Vector};
use crate::mesh::Mesh;
use crate::network::LaneId;
use crate::road::RoadId;
use crate::structure::Coverage;
use crate::{Orientation, RoadEnd};
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct VirtualJunction {
pub od_id: String,
pub name: String,
pub main: Option<MainRoad>,
pub orientation: Orientation,
pub links: Vec<VirtualLink>,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MainRoad {
pub road: RoadId,
pub s_start: f64,
pub s_end: f64,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct VirtualLink {
pub from: LinkPoint,
pub to: LinkPoint,
pub lanes: Vec<(LaneId, LaneId)>,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum LinkPoint {
End {
road: RoadId,
end: RoadEnd,
},
Along {
road: RoadId,
s: f64,
forward: bool,
},
}
impl LinkPoint {
pub fn road(&self) -> RoadId {
match *self {
Self::End { road, .. } | Self::Along { road, .. } => road,
}
}
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Crossing {
pub od_id: String,
pub name: String,
pub sections: Vec<CrossingSection>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct CrossingSection {
pub od_id: String,
pub road: RoadId,
pub s_start: f64,
pub s_end: f64,
pub lanes: Vec<Coverage>,
}
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct JunctionGroup {
pub od_id: String,
pub name: String,
pub kind: JunctionGroupKind,
pub junctions: Vec<String>,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub enum JunctionGroupKind {
Roundabout,
ComplexJunction,
HighwayInterchange,
Unknown,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct CrossPath {
pub crossing: RoadId,
pub start: CrossPathEnd,
pub end: CrossPathEnd,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct CrossPathEnd {
pub lane: LaneId,
pub s: f64,
pub crossing_lane: LaneId,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct JunctionArea {
pub od_id: String,
pub boundary: Vec<Point>,
pub grid: Option<ElevationGrid>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct ElevationGrid {
pub origin: [f64; 2],
pub heading: f64,
pub s_start: f64,
pub spacing: f64,
pub rows: Vec<GridRow>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct GridRow {
pub center: f64,
pub left: Vec<f64>,
pub right: Vec<f64>,
}
impl GridRow {
fn at(&self, j: i64) -> Option<f64> {
match j {
0 => Some(self.center),
j if j > 0 => self.left.get(j as usize - 1).copied(),
j => self.right.get((-j) as usize - 1).copied(),
}
}
}
impl ElevationGrid {
pub fn height(&self, s: f64, t: f64) -> Option<f64> {
if self.spacing.is_nan() || self.spacing <= 0.0 {
return None;
}
let snap = |at: f64| {
if (at - at.round()).abs() < 1e-6 {
at.round()
} else {
at
}
};
let (u, v) = (
snap((s - self.s_start) / self.spacing),
snap(t / self.spacing),
);
let last_row = self.rows.len().checked_sub(1)? as f64;
let widest = self
.rows
.iter()
.map(|row| row.left.len().max(row.right.len()))
.max()? as f64;
if !(0.0..=last_row).contains(&u) || !(-widest..=widest).contains(&v) {
return None;
}
let squares = |at: f64| {
let i = at.floor() as i64;
let frac = at - i as f64;
if frac == 0.0 {
vec![(i, 0.0), (i - 1, 1.0)]
} else {
vec![(i, frac)]
}
};
let (rows, columns) = (squares(u), squares(v));
rows.iter()
.flat_map(|&row| columns.iter().map(move |&column| (row, column)))
.find_map(|((i, x), (j, y))| self.in_square(i, x, j, y))
}
fn in_square(&self, i: i64, x: f64, j: i64, y: f64) -> Option<f64> {
let z = |i: i64, j: i64| self.value(i, j);
let dt = |i: i64| slopes(|k| z(i, j - 1 + k));
let [z00, z01, z10, z11] = [z(i, j)?, z(i, j + 1)?, z(i + 1, j)?, z(i + 1, j + 1)?];
let (t0, t1) = (dt(i)?, dt(i + 1)?);
let (s0, s1) = (
slopes(|k| z(i - 1 + k, j))?,
slopes(|k| z(i - 1 + k, j + 1))?,
);
let (st0, st1) = (
slopes(|k| dt(i - 1 + k).map(|d| d.0))?,
slopes(|k| dt(i - 1 + k).map(|d| d.1))?,
);
let f = [
[z00, z01, t0.0, t0.1],
[z10, z11, t1.0, t1.1],
[s0.0, s1.0, st0.0, st1.0],
[s0.1, s1.1, st0.1, st1.1],
];
let alpha = mul(&mul(&A, &f), &transpose(&A));
let powers = |x: f64| [1.0, x, x * x, x * x * x];
let (px, py) = (powers(x), powers(y));
Some(
(0..4)
.flat_map(|r| (0..4).map(move |c| (r, c)))
.map(|(r, c)| px[r] * alpha[r][c] * py[c])
.sum(),
)
}
fn value(&self, i: i64, j: i64) -> Option<f64> {
usize::try_from(i)
.ok()
.and_then(|i| self.rows.get(i))
.and_then(|row| row.at(j))
}
pub(crate) fn height_at(&self, x: f64, y: f64) -> Option<f64> {
let (s, t) = self.locate(x, y);
self.height(s, t)
}
fn locate(&self, x: f64, y: f64) -> (f64, f64) {
let (sin, cos) = self.heading.sin_cos();
let (dx, dy) = (x - self.origin[0], y - self.origin[1]);
(dx * cos + dy * sin, dy * cos - dx * sin)
}
}
const A: [[f64; 4]; 4] = [
[1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[-3.0, 3.0, -2.0, -1.0],
[2.0, -2.0, 1.0, 1.0],
];
fn mul(a: &[[f64; 4]; 4], b: &[[f64; 4]; 4]) -> [[f64; 4]; 4] {
let mut out = [[0.0; 4]; 4];
for (r, row) in out.iter_mut().enumerate() {
for (c, cell) in row.iter_mut().enumerate() {
*cell = (0..4).map(|k| a[r][k] * b[k][c]).sum();
}
}
out
}
fn transpose(a: &[[f64; 4]; 4]) -> [[f64; 4]; 4] {
let mut out = [[0.0; 4]; 4];
for (r, row) in a.iter().enumerate() {
for (c, &cell) in row.iter().enumerate() {
out[c][r] = cell;
}
}
out
}
fn slopes(value: impl Fn(i64) -> Option<f64>) -> Option<(f64, f64)> {
let (b, c) = (value(1)?, value(2)?);
match (value(0), value(3)) {
(Some(a), Some(d)) => Some((
(-2.0 * a - 3.0 * b + 6.0 * c - d) / 6.0,
(a - 6.0 * b + 3.0 * c + 2.0 * d) / 6.0,
)),
_ => Some((c - b, c - b)),
}
}
impl JunctionArea {
pub fn height_at(&self, x: f64, y: f64) -> Option<f64> {
self.grid.as_ref()?.height_at(x, y)
}
pub fn mesh(&self) -> Mesh {
let ring = &self.boundary;
let step = self.grid.as_ref().map_or(f64::INFINITY, |g| g.spacing);
let triangles = ear_clip(ring);
let counts: Vec<usize> = triangles
.iter()
.map(|&[a, b, c]| {
let longest = [(a, b), (b, c), (c, a)]
.map(|(p, q)| ring[p].distance_to(ring[q]))
.into_iter()
.fold(0.0_f32, f32::max);
(f64::from(longest) / step).ceil().clamp(1.0, 64.0) as usize
})
.collect();
let mut splits: HashMap<(usize, usize), Vec<usize>> = HashMap::new();
for (&[a, b, c], &n) in triangles.iter().zip(&counts) {
for (p, q) in [(a, b), (b, c), (c, a)] {
splits.entry((p.min(q), p.max(q))).or_default().push(n);
}
}
let stand = |p: Point| {
let z = self.height_at(f64::from(p.x), f64::from(p.y));
Point::new(p.x, p.y, z.map_or(p.z, |z| z as f32))
};
let on_edge = |a: usize, b: usize, num: usize, den: usize| {
let (from, to, num) = if a < b {
(a, b, num)
} else {
(b, a, den - num)
};
match num {
0 => stand(ring[from]),
_ if num == den => stand(ring[to]),
_ => stand(ring[from].lerp(ring[to], num as f32 / den as f32)),
}
};
let between = |a: usize, b: usize, lo: usize, n: usize| {
let mut found: Vec<(usize, usize)> = Vec::new();
for &m in &splits[&(a.min(b), a.max(b))] {
found.extend(
(lo * m / n + 1..)
.take_while(|k| k * n < (lo + 1) * m)
.map(|k| (k, m)),
);
}
found.sort_by(|x, y| (x.0 * y.1).cmp(&(y.0 * x.1)));
found.dedup_by(|x, y| x.0 * y.1 == y.0 * x.1);
found.into_iter().map(move |(k, m)| on_edge(a, b, k, m))
};
let mut mesh = Mesh::default();
for (&[a, b, c], &n) in triangles.iter().zip(&counts) {
let (pa, pb, pc) = (ring[a], ring[b], ring[c]);
let vertex = |i: usize, j: usize| match (i, j) {
(_, 0) => on_edge(a, b, i, n),
(0, _) => on_edge(c, a, n - j, n),
_ if i + j == n => on_edge(b, c, j, n),
_ => {
let (u, v) = (i as f32 / n as f32, j as f32 / n as f32);
stand(pa + (pb - pa) * u + (pc - pa) * v)
}
};
for i in 0..n {
for j in 0..n - i {
let mut ring_of = vec![vertex(i, j)];
if j == 0 {
ring_of.extend(between(a, b, i, n));
}
ring_of.push(vertex(i + 1, j));
if i + j + 1 == n {
ring_of.extend(between(b, c, j, n));
}
ring_of.push(vertex(i, j + 1));
if i == 0 {
ring_of.extend(between(c, a, n - j - 1, n));
}
fan(&mut mesh, &ring_of);
if i + j + 1 < n {
push(
&mut mesh,
[vertex(i + 1, j), vertex(i + 1, j + 1), vertex(i, j + 1)],
);
}
}
}
}
mesh
}
}
fn fan(mesh: &mut Mesh, corners: &[Point]) {
if let [a, b, c] = corners {
return push(mesh, [*a, *b, *c]);
}
let sum = corners
.iter()
.fold(Vector::ZERO, |s, p| s + (*p - Point::ORIGIN));
let centre = Point::ORIGIN + sum * (1.0 / corners.len() as f32);
for (k, p) in corners.iter().enumerate() {
push(mesh, [*p, corners[(k + 1) % corners.len()], centre]);
}
}
fn push(mesh: &mut Mesh, corners: [Point; 3]) {
let normal = (corners[1] - corners[0])
.cross(corners[2] - corners[0])
.normalize_or(Vector::Z);
let normal = if normal.z < 0.0 {
normal * -1.0
} else {
normal
};
let first = mesh.vertices.len() as u32;
mesh.vertices.extend(corners);
mesh.normals.extend([normal; 3]);
mesh.indices.extend([first, first + 1, first + 2]);
}
const NEAR: f32 = 0.01;
fn ear_clip(ring: &[Point]) -> Vec<[usize; 3]> {
let cross =
|o: Point, a: Point, b: Point| (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x);
let inside = |p: Point, [a, b, c]: [Point; 3]| {
cross(a, b, p) > 0.0 && cross(b, c, p) > 0.0 && cross(c, a, p) > 0.0
};
let n = ring.len();
let straight = |k: usize| {
let (a, b, c) = (ring[(k + n - 1) % n], ring[k], ring[(k + 1) % n]);
let scale = (c.x - a.x).hypot(c.y - a.y).powi(2);
cross(a, b, c).abs() <= 1e-6 * scale
};
let mut left: Vec<usize> = Vec::new();
for k in (0..n).filter(|&k| !straight(k)) {
let near = |j: &usize| ring[*j].distance_to(ring[k]) < NEAR;
if !left.last().is_some_and(near) && !(k + 1 == n && left.first().is_some_and(near)) {
left.push(k);
}
}
let mut out = Vec::new();
while left.len() > 3 {
let n = left.len();
let corner = |k: usize| (left[(k + n - 1) % n], left[k % n], left[(k + 1) % n]);
let turn = |k: usize| {
let (a, b, c) = corner(k);
cross(ring[a], ring[b], ring[c])
};
let reflex: Vec<usize> = (0..n).filter(|&k| turn(k) < 0.0).map(|k| left[k]).collect();
let ear = (0..n).find(|&k| {
let (a, b, c) = corner(k);
let corners = [ring[a], ring[b], ring[c]];
turn(k) > 0.0
&& !reflex
.iter()
.any(|&p| p != a && p != c && inside(ring[p], corners))
});
if let Some(k) = ear {
let (a, b, c) = corner(k);
out.push([a, b, c]);
left.remove(k);
} else if let Some(k) = (0..n).find(|&k| turn(k) == 0.0) {
left.remove(k);
} else {
break;
}
}
if left.len() == 3 && cross(ring[left[0]], ring[left[1]], ring[left[2]]) > 0.0 {
out.push([left[0], left[1], left[2]]);
}
out
}
#[cfg(test)]
mod tests {
use super::*;
fn grid(rows: Vec<GridRow>) -> ElevationGrid {
ElevationGrid {
origin: [0.0, 0.0],
heading: 0.0,
s_start: 0.0,
spacing: 2.0,
rows,
}
}
fn row(center: f64, left: &[f64], right: &[f64]) -> GridRow {
GridRow {
center,
left: left.to_vec(),
right: right.to_vec(),
}
}
#[test]
fn a_flat_grid_is_flat_and_ends_at_its_last_whole_square() {
let g = grid(vec![row(5.0, &[5.0], &[5.0]); 3]);
for (s, t) in [(0.0, 0.0), (1.3, -1.7), (3.9, 1.9), (4.0, 2.0)] {
assert!((g.height(s, t).unwrap() - 5.0).abs() < 1e-12, "({s}, {t})");
}
assert!(g.height(4.1, 0.0).is_none());
assert!(g.height(1.0, 2.1).is_none());
assert!(g.height(-0.1, 0.0).is_none());
}
#[test]
fn a_coordinate_far_off_or_not_finite_has_no_height() {
let g = grid(vec![row(5.0, &[5.0], &[5.0]); 3]);
for (s, t) in [
(f64::NAN, 0.0),
(0.0, f64::NAN),
(f64::INFINITY, 0.0),
(0.0, f64::NEG_INFINITY),
(1e30, 0.0),
(2.0, -1e30),
] {
assert!(g.height(s, t).is_none(), "({s}, {t})");
}
let fine = ElevationGrid {
spacing: 1e-300,
..g.clone()
};
assert!(fine.height(1.0, 0.0).is_none());
assert!(grid(Vec::new()).height(0.0, 0.0).is_none());
}
#[test]
fn a_grid_passes_through_its_points_and_is_exact_on_a_plane() {
let plane = |s: f64, t: f64| 1.0 + 0.1 * s - 0.05 * t;
let rows = (0..5)
.map(|i| {
let s = 2.0 * i as f64;
row(
plane(s, 0.0),
&[plane(s, 2.0), plane(s, 4.0)],
&[plane(s, -2.0), plane(s, -4.0)],
)
})
.collect();
let g = grid(rows);
for (s, t) in [(2.0, 2.0), (3.1, -0.7), (5.5, 3.3), (7.9, -3.9)] {
assert!(
(g.height(s, t).unwrap() - plane(s, t)).abs() < 1e-9,
"({s}, {t})"
);
}
}
#[test]
fn a_grid_follows_a_cubic_along_the_line_between_its_points() {
let f = |s: f64| 0.01 * s * s * s - 0.2 * s + 1.0;
let rows = (0..4).map(|i| {
let z = f(2.0 * i as f64);
row(z, &[z], &[z])
});
let g = grid(rows.collect());
for s in [2.0, 2.5, 3.0, 3.7] {
assert!((g.height(s, 0.5).unwrap() - f(s)).abs() < 1e-9, "{s}");
}
}
#[test]
fn a_square_ring_clips_to_two_triangles_and_meshes_at_the_grid_spacing() {
let ring = vec![
Point::new(0.0, 0.0, 0.0),
Point::new(4.0, 0.0, 0.0),
Point::new(4.0, 4.0, 0.0),
Point::new(0.0, 4.0, 0.0),
];
assert_eq!(ear_clip(&ring).len(), 2);
let mut area = JunctionArea {
od_id: "1".into(),
boundary: ring.clone(),
grid: None,
};
assert_eq!(area.mesh().indices.len(), 6);
area.grid = Some(ElevationGrid {
origin: [0.0, 0.0],
heading: 0.0,
s_start: 0.0,
spacing: 2.0,
rows: vec![row(3.0, &[3.0, 3.0], &[]); 3],
});
let mesh = area.mesh();
mesh.validate().unwrap();
assert!(mesh.indices.len() > 6);
assert!(mesh.vertices.iter().all(|v| (v.z - 3.0).abs() < 1e-6));
assert!(mesh.normals.iter().all(|n| n.z > 0.99));
}
#[test]
fn a_concave_ring_clips_without_covering_its_notch() {
let ring = vec![
Point::new(0.0, 0.0, 0.0),
Point::new(4.0, 0.0, 0.0),
Point::new(4.0, 4.0, 0.0),
Point::new(2.0, 1.0, 0.0),
Point::new(0.0, 4.0, 0.0),
];
let triangles = ear_clip(&ring);
assert_eq!(triangles.len(), 3);
let area: f32 = triangles
.iter()
.map(|&[a, b, c]| {
let (a, b, c) = (ring[a], ring[b], ring[c]);
((b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x)) / 2.0
})
.sum();
assert!((area - 10.0).abs() < 1e-5, "{area}");
}
}