use super::links::LaneMeta;
use super::Warning;
use crate::coords::{Point, Vector};
use crate::road::Road;
use crate::{Direction, Lane};
pub(super) const LENGTH_TOLERANCE: f64 = 0.01;
const GAP_TOLERANCE: f32 = 0.1;
pub(super) fn road_length(road: &Road) -> Option<Warning> {
let last = road.geoms.last()?;
let plan_view = last.s + last.length;
((road.length - plan_view).abs() > LENGTH_TOLERANCE).then(|| Warning::RoadLengthMismatch {
road_id: road.od_id.clone(),
length: road.length,
plan_view,
})
}
pub(super) fn link_gaps(lanes: &[Lane], metas: &[LaneMeta]) -> Vec<Warning> {
let position = |id: crate::LaneId| match lanes.get(id.0) {
Some(l) if l.id == id => Some(id.0),
_ => lanes.iter().position(|l| l.id == id),
};
let mut out = Vec::new();
for (lane, from) in lanes.iter().zip(metas) {
for &next in &lane.successors {
let Some(i) = position(next) else {
continue;
};
let (to, into) = (&lanes[i], &metas[i]);
let gap = ends(lane, Direction::Forward)
.flat_map(|exit| {
ends(to, Direction::Backward).map(move |entry| distance(exit, entry))
})
.fold(f32::INFINITY, f32::min);
if gap <= GAP_TOLERANCE {
continue;
}
out.push(Warning::LinkGap {
road_id: from.road.clone(),
section: from.section,
lane: from.od_id,
to_road_id: into.road.clone(),
to_section: into.section,
to_lane: into.od_id,
gap: f64::from(gap),
});
}
}
out
}
fn ends(lane: &Lane, direction: Direction) -> impl Iterator<Item = [Point; 2]> + '_ {
[true, false].into_iter().filter_map(move |end| {
let leaves = match lane.direction {
Direction::Both => true,
d => (d == direction) == end,
};
leaves.then(|| across(lane, end))
})
}
fn across(lane: &Lane, end: bool) -> [Point; 2] {
let points = lane.center.points();
let (i, s) = if end {
(points.len() - 1, lane.center.length())
} else {
(0, 0.0)
};
let tangent = lane.center.tangents()[i];
let half = lane.width_at(s) / 2.0;
let bank = lane.bank_at(s);
let left = Vector::new(-tangent.y, tangent.x, 0.0).normalize_or_zero();
let reach = left * (half * bank.cos()) + Vector::Z * (half * bank.sin());
[points[i] + reach, points[i] - reach]
}
fn distance([p, q]: [Point; 2], [r, s]: [Point; 2]) -> f32 {
let (d1, d2, w) = (q - p, s - r, p - r);
let (a, e, f) = (d1.dot(d1), d2.dot(d2), d2.dot(w));
let (u, v) = if a <= f32::EPSILON && e <= f32::EPSILON {
(0.0, 0.0)
} else if a <= f32::EPSILON {
(0.0, (f / e).clamp(0.0, 1.0))
} else if e <= f32::EPSILON {
((-d1.dot(w) / a).clamp(0.0, 1.0), 0.0)
} else {
let (b, c) = (d1.dot(d2), d1.dot(w));
let denom = a * e - b * b;
let u = if denom > f32::EPSILON * a * e {
((b * f - c * e) / denom).clamp(0.0, 1.0)
} else {
0.0
};
let v = (b * u + f) / e;
if v < 0.0 {
((-c / a).clamp(0.0, 1.0), 0.0)
} else if v > 1.0 {
(((b - c) / a).clamp(0.0, 1.0), 1.0)
} else {
(u, v)
}
};
((p + d1 * u) - (r + d2 * v)).length()
}
#[cfg(test)]
mod tests {
use super::*;
fn seg(a: [f32; 3], b: [f32; 3]) -> [Point; 2] {
[Point::from_array(a), Point::from_array(b)]
}
#[test]
fn segments_that_cross_touch_and_parallel_ones_are_their_spacing_apart() {
let crossing = distance(
seg([0., -1., 0.], [0., 1., 0.]),
seg([-1., 0., 0.], [1., 0., 0.]),
);
assert!(crossing < 1e-6);
let parallel = distance(
seg([0., 0., 0.], [0., 2., 0.]),
seg([3., 1., 0.], [3., 4., 0.]),
);
assert!((parallel - 3.0).abs() < 1e-6);
let past_the_end = distance(
seg([0., 0., 0.], [0., 1., 0.]),
seg([0., 3., 0.], [0., 5., 0.]),
);
assert!((past_the_end - 2.0).abs() < 1e-6);
let above = distance(
seg([0., -1., 0.], [0., 1., 0.]),
seg([-1., 0., 1.], [1., 0., 1.]),
);
assert!((above - 1.0).abs() < 1e-6);
let point = distance(
seg([0., 0., 0.], [0., 0., 0.]),
seg([1., -1., 0.], [1., 1., 0.]),
);
assert!((point - 1.0).abs() < 1e-6);
let end_on_point = distance(
seg([0., -3.5, 0.], [0., -7., 0.]),
seg([0., -7., 0.], [0., -7., 0.]),
);
assert!(end_on_point < 1e-6);
}
}