use std::ops::Range;
use crate::coords::{Point, Vector};
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Pose {
pub position: Point,
pub heading: Vector,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RoadSample {
pub point: Point,
pub heading: Vector,
pub bank: f32,
pub up: Vector,
}
impl RoadSample {
pub fn new(point: Point, heading: Vector, bank: f32) -> Self {
let up = Vector::Z
.rotate_about(heading, bank)
.normalize_or(Vector::Z);
Self {
point,
heading,
bank,
up,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Projection {
pub s: f32,
pub point: Point,
pub offset: f32,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(
feature = "serde",
derive(serde::Serialize, serde::Deserialize),
serde(into = "PolylineData", try_from = "PolylineData")
)]
pub struct Polyline {
points: Vec<Point>,
cumulative: Vec<f32>,
tangents: Vec<Vector>,
}
impl Polyline {
pub fn try_new(points: Vec<Point>) -> Option<Self> {
Self::build(points, None)
}
pub fn try_new_with_tangents(
points: Vec<Point>,
start_tangent: Vector,
end_tangent: Vector,
) -> Option<Self> {
Self::build(points, Some((start_tangent, end_tangent)))
}
fn build(points: Vec<Point>, ends: Option<(Vector, Vector)>) -> Option<Self> {
if points.len() < 2 {
return None;
}
let mut cumulative = Vec::with_capacity(points.len());
let mut acc = 0.0_f64;
cumulative.push(0.0);
for pair in points.windows(2) {
acc += f64::from((pair[1] - pair[0]).length());
cumulative.push(acc as f32);
}
let tangents = vertex_tangents(&points, ends);
Some(Self {
points,
cumulative,
tangents,
})
}
pub fn new(points: Vec<Point>) -> Self {
Self::try_new(points).expect("a polyline needs at least two points")
}
pub fn points(&self) -> &[Point] {
&self.points
}
pub fn tangents(&self) -> &[Vector] {
&self.tangents
}
pub fn length(&self) -> f32 {
self.cumulative.last().copied().unwrap_or(0.0)
}
fn segment(&self, s: f32) -> usize {
let last = self.points.len() - 2;
self.cumulative
.partition_point(|&c| c <= s)
.saturating_sub(1)
.min(last)
}
fn local_t(&self, i: usize, s: f32) -> f32 {
let seg_len = self.cumulative[i + 1] - self.cumulative[i];
if seg_len > 1e-6 {
(s - self.cumulative[i]) / seg_len
} else {
0.0
}
}
pub(crate) fn locate(&self, s: f32) -> (usize, f32) {
let s = s.max(0.0).min(self.length());
let i = self.segment(s);
(i, self.local_t(i, s))
}
pub fn point_at(&self, s: f32) -> Point {
let (i, t) = self.locate(s);
self.points[i].lerp(self.points[i + 1], t)
}
pub fn pose_at(&self, s: f32) -> Pose {
let (i, t) = self.locate(s);
Pose {
position: self.points[i].lerp(self.points[i + 1], t),
heading: self.tangents[i]
.lerp(self.tangents[i + 1], t)
.normalize_or_zero(),
}
}
pub fn project(&self, point: Point) -> Projection {
self.project_segments(point, 0..self.points.len() - 1)
}
pub(crate) fn project_segments(&self, point: Point, segments: Range<usize>) -> Projection {
let mut best = Projection {
s: self.cumulative[segments.start],
point: self.points[segments.start],
offset: 0.0,
};
if !point.is_finite() {
return Projection {
s: f32::NAN,
offset: f32::NAN,
..best
};
}
let mut best_dist = f32::INFINITY;
for i in segments {
let a = self.points[i];
let ab = self.points[i + 1] - a;
let len2 = ab.length_squared();
if len2 <= 1e-9 {
continue;
}
let t = ((point - a).dot(ab) / len2).clamp(0.0, 1.0);
let closest = a + ab * t;
let dist = (point - closest).length_squared();
if dist < best_dist {
best_dist = dist;
let heading = horizontal(ab).normalize_or_zero();
best = Projection {
s: self.cumulative[i] + ab.length() * t,
point: closest,
offset: horizontal(point - closest).dot(left_normal(heading)),
};
}
}
best
}
}
impl From<Polyline> for Vec<Point> {
fn from(line: Polyline) -> Self {
line.points
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
#[error("a polyline needs at least two points")]
pub struct TooFewPoints;
impl TryFrom<Vec<Point>> for Polyline {
type Error = TooFewPoints;
fn try_from(points: Vec<Point>) -> Result<Self, Self::Error> {
Self::try_new(points).ok_or(TooFewPoints)
}
}
#[cfg(feature = "serde")]
#[derive(serde::Serialize, serde::Deserialize)]
struct PolylineData {
points: Vec<Point>,
tangents: (Vector, Vector),
}
#[cfg(feature = "serde")]
impl From<Polyline> for PolylineData {
fn from(line: Polyline) -> Self {
let tangents = (line.tangents[0], line.tangents[line.tangents.len() - 1]);
Self {
points: line.points,
tangents,
}
}
}
#[cfg(feature = "serde")]
impl TryFrom<PolylineData> for Polyline {
type Error = TooFewPoints;
fn try_from(data: PolylineData) -> Result<Self, Self::Error> {
let (start, end) = data.tangents;
Self::try_new_with_tangents(data.points, start, end).ok_or(TooFewPoints)
}
}
fn horizontal(v: Vector) -> Vector {
Vector::new(v.x, v.y, 0.0)
}
fn vertex_tangents(points: &[Point], ends: Option<(Vector, Vector)>) -> Vec<Vector> {
let n = points.len();
(0..n)
.map(|i| {
let incoming = if i > 0 {
horizontal(points[i] - points[i - 1]).normalize_or_zero()
} else {
Vector::ZERO
};
let outgoing = if i + 1 < n {
horizontal(points[i + 1] - points[i]).normalize_or_zero()
} else {
Vector::ZERO
};
let chord = (incoming + outgoing).normalize_or_zero();
match ends {
Some((start, _)) if i == 0 => horizontal(start).normalize_or(chord),
Some((_, end)) if i == n - 1 => horizontal(end).normalize_or(chord),
_ => chord,
}
})
.collect()
}
pub(crate) fn left_normal(heading: Vector) -> Vector {
Vector::Z.cross(horizontal(heading)).normalize_or_zero()
}
#[cfg(test)]
mod tests {
use super::*;
fn line(points: &[[f32; 3]]) -> Polyline {
Polyline::new(points.iter().map(|p| Point::from_array(*p)).collect())
}
#[test]
fn supplied_tangents_replace_the_end_chords() {
let pts = vec![
Point::ORIGIN,
Point::new(10.0, 0.0, 0.0),
Point::new(10.0, 10.0, 0.0),
];
let diag = Vector::new(1.0, 1.0, 0.0).normalize_or_zero();
let l = Polyline::try_new_with_tangents(pts.clone(), diag, diag).unwrap();
assert!(l.tangents()[0].abs_diff_eq(diag, 1e-6));
assert!(l.tangents()[2].abs_diff_eq(diag, 1e-6));
assert!(l.tangents()[1].abs_diff_eq(diag, 1e-6));
let plain = Polyline::new(pts);
assert!(plain.tangents()[0].abs_diff_eq(Vector::new(1.0, 0.0, 0.0), 1e-6));
assert!(plain.tangents()[2].abs_diff_eq(Vector::new(0.0, 1.0, 0.0), 1e-6));
}
#[test]
fn a_degenerate_supplied_tangent_falls_back_to_the_chord() {
let pts = vec![Point::ORIGIN, Point::new(10.0, 0.0, 0.0)];
let l = Polyline::try_new_with_tangents(pts, Vector::ZERO, Vector::new(f32::NAN, 0.0, 0.0))
.unwrap();
assert!(l.tangents()[0].abs_diff_eq(Vector::new(1.0, 0.0, 0.0), 1e-6));
assert!(l.tangents()[1].abs_diff_eq(Vector::new(1.0, 0.0, 0.0), 1e-6));
}
#[test]
fn try_new_rejects_too_few_points() {
assert!(Polyline::try_new(vec![Point::ORIGIN]).is_none());
assert!(Polyline::try_new(vec![]).is_none());
assert!(Polyline::try_new(vec![Point::ORIGIN, Point::new(1.0, 0.0, 0.0)]).is_some());
}
#[test]
fn length_sums_the_segments() {
let l = line(&[[0.0, 0.0, 0.0], [3.0, 0.0, 0.0], [3.0, 4.0, 0.0]]);
assert!((l.length() - 7.0).abs() < 1e-5);
}
#[test]
fn point_at_interpolates_and_clamps() {
let l = line(&[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]]);
assert!((l.point_at(5.0) - Point::new(5.0, 0.0, 0.0)).length() < 1e-5);
assert!((l.point_at(100.0) - Point::new(10.0, 0.0, 0.0)).length() < 1e-5);
}
#[test]
fn pose_heading_exact_at_ends_and_continuous_across_a_vertex() {
let l = line(&[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0], [10.0, 10.0, 0.0]]);
assert!(l.pose_at(0.0).heading.abs_diff_eq(Vector::X, 1e-5));
assert!(l.pose_at(l.length()).heading.abs_diff_eq(Vector::Y, 1e-5));
let before = l.pose_at(10.0 - 0.01).heading;
let after = l.pose_at(10.0 + 0.01).heading;
assert!((before - after).length() < 0.02, "{before:?} vs {after:?}");
}
#[test]
fn project_gives_arc_length_and_signed_offset() {
let l = line(&[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]]);
let left = l.project(Point::new(5.0, 3.0, 0.0));
assert!((left.s - 5.0).abs() < 1e-4);
assert!((left.offset - 3.0).abs() < 1e-4, "offset {}", left.offset);
let right = l.project(Point::new(5.0, -3.0, 0.0));
assert!((right.offset + 3.0).abs() < 1e-4, "offset {}", right.offset);
}
#[test]
fn project_arc_length_spans_multiple_segments() {
let l = line(&[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0], [10.0, 10.0, 0.0]]);
let p = l.project(Point::new(12.0, 6.0, 0.0));
assert!((p.s - 16.0).abs() < 1e-4, "s {}", p.s);
}
#[test]
fn left_normal_of_plus_x_is_plus_y() {
assert!(left_normal(Vector::X).abs_diff_eq(Vector::Y, 1e-5));
}
#[test]
fn road_sample_up_is_unit_orthogonal_and_plumb_at_zero() {
let headings = [
Vector::X,
Vector::Y,
Vector::new(1.0, 1.0, 0.0).normalize_or_zero(),
Vector::new(-2.0, 1.0, 0.0).normalize_or_zero(),
];
for h in headings {
for bank in [0.0_f32, 0.2, -0.2, 1.5, -1.5] {
let s = RoadSample::new(Point::new(3.0, 4.0, 1.0), h, bank);
assert!(
(s.up.length() - 1.0).abs() < 1e-5,
"up not unit for heading {h:?} bank {bank}: len {}",
s.up.length()
);
assert!(
s.up.dot(h).abs() < 1e-6,
"up.heading = {} for heading {h:?} bank {bank}",
s.up.dot(h)
);
}
assert_eq!(
RoadSample::new(Point::ORIGIN, h, 0.0).up,
Vector::Z,
"bank 0 up must be exactly +Z for heading {h:?}"
);
}
}
#[test]
fn road_sample_up_sign_flips_with_bank_sign() {
let pos = RoadSample::new(Point::ORIGIN, Vector::X, 0.3);
let neg = RoadSample::new(Point::ORIGIN, Vector::X, -0.3);
assert!(pos.up.y < -0.05, "+bank should lean -Y: {:?}", pos.up);
assert!(neg.up.y > 0.05, "-bank should lean +Y: {:?}", neg.up);
assert!((pos.up.z - neg.up.z).abs() < 1e-6);
assert!((pos.up.y + neg.up.y).abs() < 1e-6);
}
#[test]
fn locate_finds_segment_and_fraction() {
let l = line(&[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0], [10.0, 10.0, 0.0]]);
assert_eq!(l.locate(5.0), (0, 0.5));
let (i, t) = l.locate(15.0);
assert_eq!(i, 1);
assert!((t - 0.5).abs() < 1e-5, "t {t}");
assert_eq!(l.locate(-3.0), (0, 0.0));
let (i, t) = l.locate(100.0);
assert_eq!(i, 1);
assert!((t - 1.0).abs() < 1e-5, "t {t}");
}
#[test]
fn a_line_of_no_finite_length_samples_without_panicking() {
for bad in [f32::NAN, f32::INFINITY] {
let l = line(&[[0.0, 0.0, 0.0], [bad, 0.0, 0.0]]);
let _ = (l.point_at(1.0), l.pose_at(0.0));
}
}
#[test]
fn a_query_that_is_not_finite_projects_to_nan() {
let p = line(&[[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]]).project(Point::new(f32::NAN, 0.0, 0.0));
assert!(p.s.is_nan() && p.offset.is_nan());
}
#[test]
fn a_repeated_point_does_not_hide_the_offset_beside_it() {
let l = line(&[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [10.0, 0.0, 0.0]]);
let p = l.project(Point::new(0.0, 5.0, 0.0));
assert_eq!((p.s, p.offset), (0.0, 5.0));
}
#[test]
fn a_long_line_sums_its_length_without_drift() {
let step = 0.3 / std::f32::consts::SQRT_2;
let points = (0..=53_333)
.map(|k| Point::new(k as f32 * step, k as f32 * step, 0.0))
.collect();
let l = Polyline::new(points);
assert!((l.length() - 15_999.9).abs() < 0.1, "{}", l.length());
}
}