use nalgebra::{self as na, ComplexField};
use num_dual::{DualNum, DualNumFloat};
pub fn circular_cumulative_distance<D, F>(vertices: &[na::Point2<D>]) -> Vec<D>
where
D: DualNum<F> + ComplexField<RealField = D>,
F: DualNumFloat,
{
vertices
.iter()
.zip(vertices.iter().skip(1).chain(vertices.iter().take(1)))
.fold(vec![D::zero()], |mut acc, (p1, p2)| {
acc.push(acc.last().cloned().unwrap_or(D::zero()) + (p2 - p1).norm());
acc
})
}
#[cfg(test)]
mod tests {
use super::*;
use approx::assert_relative_eq;
use nalgebra::{Isometry2, Point2, Vector2};
fn assert_vec_rel_eq<T: na::RealField>(a: &[T], b: &[T]) {
a.iter().zip(b.iter()).for_each(|(a, b)| {
assert_relative_eq!(a, b);
});
}
#[test]
fn test_compute_distance_square() {
let iso = Isometry2::new(Vector2::new(1.1, -2.2), 1.23);
let vertices = vec![
iso * Point2::new(0.0, 0.0),
iso * Point2::new(1.0, 0.0),
iso * Point2::new(1.0, 1.0),
iso * Point2::new(0.0, 1.0),
];
let distances = circular_cumulative_distance(&vertices);
let expected_distances = vec![0.0, 1.0, 2.0, 3.0, 4.0];
assert_vec_rel_eq(&distances, &expected_distances);
}
#[test]
fn test_compute_distance_triangle() {
let vertices = vec![
Point2::new(0.0, 0.0),
Point2::new(3.0, 0.0),
Point2::new(3.0, 4.0),
];
let distances = circular_cumulative_distance(&vertices);
let expected_distances = vec![0.0, 3.0, 7.0, 12.0];
assert_vec_rel_eq(&distances, &expected_distances);
}
#[test]
fn test_compute_distance_line() {
let vertices = vec![Point2::new(0.0, 0.0), Point2::new(1.0, 0.0)];
let distances = circular_cumulative_distance(&vertices);
let expected_distances = vec![0.0, 1.0, 2.0];
assert_vec_rel_eq(&distances, &expected_distances);
}
}