use crate::Result;
use crate::common::kd_tree::{KdTreeSearch, PartialKdTree};
use crate::common::surface_point::SurfacePoint;
use crate::common::{IndexMask, PCoords};
use parry3d_f64::na::{AbstractRotation, Isometry, Point, SVector};
pub fn to_points<const D: usize>(items: &[[f64; D]]) -> Vec<Point<f64, D>> {
items.iter().map(|arr| Point::from(*arr)).collect()
}
pub fn clone_points<const D: usize>(collection: &[impl PCoords<D>]) -> Vec<Point<f64, D>> {
collection
.iter()
.map(|pc| Point::from(pc.coords()))
.collect()
}
pub fn triangle_area<const D: usize>(
pa: &impl PCoords<D>,
pb: &impl PCoords<D>,
pc: &impl PCoords<D>,
) -> f64 {
let ab = pb.coords() - pa.coords();
let ac = pc.coords() - pa.coords();
if D == 2 {
0.5 * (ab[0] * ac[1] - ab[1] * ac[0]).abs()
} else if D == 3 {
0.5 * ab.cross(&ac).norm()
} else {
panic!("Area calculation is only implemented for 2D and 3D points");
}
}
pub fn three_point_angle<const D: usize>(
pc: &impl PCoords<D>,
p1: &impl PCoords<D>,
p2: &impl PCoords<D>,
) -> f64 {
let v1 = p1.coords() - pc.coords();
let v2 = p2.coords() - pc.coords();
v1.angle(&v2)
}
pub fn dist<const D: usize>(a: &impl PCoords<D>, b: &impl PCoords<D>) -> f64 {
(a.coords() - b.coords()).norm()
}
pub fn mid_point<const D: usize>(a: &impl PCoords<D>, b: &impl PCoords<D>) -> Point<f64, D> {
Point::from(b.coords() + (a.coords() - b.coords()) * 0.5)
}
pub fn mean_point<const D: usize>(points: &[impl PCoords<D>]) -> Point<f64, D> {
let mut sum = SVector::<f64, D>::zeros();
for p in points {
sum += p.coords();
}
Point::from(sum / points.len() as f64)
}
pub fn mean_point_weighted<const D: usize>(
points: &[impl PCoords<D>],
weights: &[f64],
) -> Point<f64, D> {
let mut sum = SVector::<f64, D>::zeros();
let mut total_weight = 0.0;
for (p, w) in points.iter().zip(weights) {
sum += p.coords() * *w;
total_weight += *w;
}
Point::from(sum / total_weight)
}
pub fn evenly_spaced_points<const D: usize>(
start: &impl PCoords<D>,
end: &impl PCoords<D>,
num_points: usize,
) -> Vec<Point<f64, D>> {
let mut result = Vec::new();
let step = (end.coords() - start.coords()) / (num_points - 1) as f64;
for i in 0..num_points {
result.push((start.coords() + step * i as f64).into());
}
result
}
pub fn evenly_spaced_points_between<const D: usize>(
start: &impl PCoords<D>,
end: &impl PCoords<D>,
num_points: usize,
) -> Vec<Point<f64, D>> {
let mut result = Vec::new();
let step = (end.coords() - start.coords()) / (num_points + 1) as f64;
for i in 1..num_points + 1 {
result.push((start.coords() + step * i as f64).into());
}
result
}
pub fn fill_gaps<const D: usize>(original: &[Point<f64, D>], max_dist: f64) -> Vec<Point<f64, D>> {
if original.len() < 2 {
return original.to_vec();
}
let mut result = vec![original[0]];
for p in original.iter().skip(1) {
let d = dist(p, result.last().unwrap());
if d > max_dist {
let mut n = 1;
while d / (n + 1) as f64 > max_dist {
n += 1;
}
for x in evenly_spaced_points_between(result.last().unwrap(), p, n) {
result.push(x);
}
}
result.push(*p);
}
result
}
pub fn max_point_in_direction<const D: usize>(
points: &[impl PCoords<D>],
vector: &SVector<f64, D>,
) -> Option<(usize, Point<f64, D>)> {
let mut max_dist = f64::MIN;
let mut max_i = None;
for (i, p) in points.iter().enumerate() {
let dist = p.coords().dot(vector);
if dist > max_dist {
max_dist = dist;
max_i = Some(i);
}
}
max_i.map(|max_i| (max_i, points[max_i].coords().into()))
}
pub fn linear_interpolation_error<const D: usize>(
p0: &impl PCoords<D>,
p1: &impl PCoords<D>,
p_test: &Point<f64, D>,
) -> f64 {
let sp = SurfacePoint::new_normalize(p0.coords().into(), p1.coords() - p0.coords());
let proj = sp.projection(p_test);
dist(p_test, &proj)
}
pub fn ramer_douglas_peucker<const D: usize>(
points: &[Point<f64, D>],
tol: f64,
) -> Vec<Point<f64, D>> {
let mut rdp = Rdp::new(points, tol);
rdp.simplify(0, points.len() - 1);
rdp.generate_points()
}
struct Rdp<'a, const D: usize> {
points: &'a [Point<f64, D>],
keep: Vec<bool>,
tol: f64,
}
impl<'a, const D: usize> Rdp<'a, D> {
fn new(points: &'a [Point<f64, D>], tol: f64) -> Self {
let keep = points.iter().map(|_| false).collect();
Rdp { points, keep, tol }
}
fn simplify(&mut self, i0: usize, i1: usize) {
self.keep[i0] = true;
self.keep[i1] = true;
if i1 - i0 < 2 {
return;
}
let sp = SurfacePoint::new_normalize(self.points[i0], self.points[i1] - self.points[i0]);
let mut max_dist = 0.0;
let mut max_i = 0;
for i in i0 + 1..i1 {
let dist = (sp.projection(&self.points[i]) - self.points[i]).norm();
if dist > max_dist {
max_dist = dist;
max_i = i;
}
}
if max_dist > self.tol {
self.simplify(i0, max_i);
self.simplify(max_i, i1);
}
}
fn generate_points(&self) -> Vec<Point<f64, D>> {
let mut result = Vec::new();
for i in 0..self.points.len() {
if self.keep[i] {
result.push(self.points[i]);
}
}
result
}
}
pub fn transform_points<R, const D: usize>(
points: &[Point<f64, D>],
transform: &Isometry<f64, R, D>,
) -> Vec<Point<f64, D>>
where
R: AbstractRotation<f64, D>,
{
points.iter().map(|p| transform * p).collect()
}
pub fn cluster_points_by_tol<const D: usize>(
points: &[Point<f64, D>],
tol: f64,
) -> Result<Vec<Vec<usize>>> {
let mut clusters = Vec::new();
let mut working = IndexMask::new(points.len(), true);
while !working.is_empty() {
let mut group_mask = IndexMask::new(points.len(), false);
group_mask.set(working.pop_true().unwrap(), true);
loop {
let tree = PartialKdTree::try_new(points, &group_mask)?;
let mut to_add = Vec::new();
for i in working.iter_true() {
let (_, d) = tree.nearest_one(&points[i]);
if d <= tol {
to_add.push(i);
}
}
if to_add.is_empty() {
break;
} else {
for i in to_add.iter().rev() {
group_mask.set(*i, true);
working.set(*i, false);
}
}
}
clusters.push(group_mask.to_indices());
}
Ok(clusters)
}
pub fn merge_points_by_tol<const D: usize>(
points: &[Point<f64, D>],
tol: f64,
) -> Result<Vec<Point<f64, D>>> {
let clusters = cluster_points_by_tol(points, tol)?;
Ok(clusters
.iter()
.map(|c| mean_point(&c.iter().map(|i| points[*i]).collect::<Vec<_>>()))
.collect())
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom2::{Curve2, Point2};
use crate::{Point3, Vector2};
use approx::assert_relative_eq;
#[test]
fn distance_calc() {
let p0 = Point2::new(0.0, 0.0);
let p1 = Point2::new(3.0, 4.0);
let d = dist(&p0, &p1);
assert_eq!(d, 5.0); }
#[test]
fn test_simple_reduce() {
let mut c = curve_from(&[(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)]);
c = insert(&mut c, 0.25);
c = insert(&mut c, 0.75);
let points = c.clone_points();
assert_eq!(points.len(), 5);
let reduced = ramer_douglas_peucker(&points, 0.001);
assert_eq!(reduced.len(), 3);
assert_relative_eq!(reduced[0], Point2::new(0.0, 0.0));
assert_relative_eq!(reduced[1], Point2::new(1.0, 0.0));
assert_relative_eq!(reduced[2], Point2::new(1.0, 1.0));
}
fn curve_from(points: &[(f64, f64)]) -> Curve2 {
let points = points
.iter()
.map(|(x, y)| Point2::new(*x, *y))
.collect::<Vec<_>>();
Curve2::from_points(&points, 0.001, false).unwrap()
}
fn insert(c: &mut Curve2, l: f64) -> Curve2 {
let points = c.clone_points();
let lengths = c.lengths();
let mut working = points
.iter()
.zip(lengths.iter())
.map(|(p, l)| (*l, *p))
.collect::<Vec<_>>();
working.push((l, c.at_length(l).unwrap().point()));
working.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap());
let points = working.iter().map(|(_, p)| *p).collect::<Vec<_>>();
Curve2::from_points(&points, 0.001, false).unwrap()
}
#[test]
fn single_point_between() {
let p0 = Point2::new(0.0, 0.0);
let p1 = Point2::new(2.0, 0.0);
let points = evenly_spaced_points_between(&p0, &p1, 1);
assert_eq!(points.len(), 1);
assert_relative_eq!(points[0], Point2::new(1.0, 0.0));
}
#[test]
fn test_fill_gaps() {
let points = vec![Point2::new(0.0, 0.0), Point2::new(2.0, 0.0)];
let filled = fill_gaps(&points, 1.5);
assert_eq!(filled.len(), 3);
assert_relative_eq!(filled[0], Point2::new(0.0, 0.0));
assert_relative_eq!(filled[1], Point2::new(1.0, 0.0));
assert_relative_eq!(filled[2], Point2::new(2.0, 0.0));
}
#[test]
fn max_point_in_dir() {
let dir = Vector2::new(-10.0, -10.0);
let points = vec![
Point2::new(10.0, 0.0),
Point2::new(11.0, 1.0),
Point2::new(12.0, 2.0),
Point2::new(13.0, 3.0),
Point2::new(14.0, 4.0),
];
let (_, max) = max_point_in_direction(&points, &dir).unwrap();
assert_relative_eq!(max, Point2::new(10.0, 0.0));
}
#[test]
fn clustering_by_tolerance_0() -> Result<()> {
let points = vec![
Point2::new(0.0, 0.0),
Point2::new(1.0, 0.0),
Point2::new(2.0, 0.0),
Point2::new(0.0, 5.0),
Point2::new(1.0, 5.0),
Point2::new(0.0, 20.0),
];
let mut c = cluster_points_by_tol(&points, 5.1)?;
c.sort_by(|a, b| a.len().partial_cmp(&b.len()).unwrap());
assert_eq!(c.len(), 2);
assert_eq!(c[0].len(), 1);
assert_eq!(c[1].len(), 5);
let mut c0 = c[0].to_vec();
c0.sort();
let mut c1 = c[1].to_vec();
c1.sort();
assert_eq!(c0, vec![5]);
assert_eq!(c1, vec![0, 1, 2, 3, 4]);
Ok(())
}
#[test]
fn clustering_by_tolerance_1() -> Result<()> {
let points = vec![
Point2::new(0.0, 0.0),
Point2::new(1.0, 0.0),
Point2::new(2.0, 0.0),
Point2::new(0.0, 5.0),
Point2::new(1.0, 5.0),
Point2::new(0.0, 20.0),
];
let mut c = cluster_points_by_tol(&points, 1.1)?;
assert_eq!(c.len(), 3);
c.sort_by(|a, b| a.len().partial_cmp(&b.len()).unwrap());
let mut c0 = c[0].to_vec();
c0.sort();
let mut c1 = c[1].to_vec();
c1.sort();
let mut c2 = c[2].to_vec();
c2.sort();
assert_eq!(c0, vec![5]);
assert_eq!(c1, vec![3, 4]);
assert_eq!(c2, vec![0, 1, 2]);
Ok(())
}
#[test]
fn merge_points_by_tolerance() -> Result<()> {
let points = vec![
Point2::new(0.0, 0.0),
Point2::new(1.0, 0.0),
Point2::new(2.0, 0.0),
Point2::new(0.0, 5.0),
Point2::new(1.0, 5.0),
Point2::new(0.0, 20.0),
];
let mut merged = merge_points_by_tol(&points, 1.1)?;
assert_eq!(merged.len(), 3);
merged.sort_by(|a, b| a.y.partial_cmp(&b.y).unwrap());
assert_relative_eq!(merged[0], Point2::new(1.0, 0.0));
assert_relative_eq!(merged[1], Point2::new(0.5, 5.0));
assert_relative_eq!(merged[2], Point2::new(0.0, 20.0));
Ok(())
}
#[test]
fn three_point_angle_right_angle_2d() {
let pc = Point2::new(0.0, 0.0);
let p1 = Point2::new(1.0, 0.0);
let p2 = Point2::new(0.0, 1.0);
let angle = three_point_angle(&pc, &p1, &p2);
assert_relative_eq!(angle, std::f64::consts::FRAC_PI_2, epsilon = 1e-12);
}
#[test]
fn triangle_area_2d() {
let a = Point2::new(0.0, 0.0);
let b = Point2::new(2.0, 0.0);
let c = Point2::new(0.0, 3.0);
let area = triangle_area(&a, &b, &c);
assert_relative_eq!(area, 3.0, epsilon = 1e-12);
}
#[test]
fn triangle_area_3d() {
let a = Point3::new(0.0, 0.0, 0.0);
let b = Point3::new(2.0, 0.0, 0.0);
let c = Point3::new(0.0, 3.0, 0.0);
let area = triangle_area(&a, &b, &c);
assert_relative_eq!(area, 3.0, epsilon = 1e-12);
}
#[test]
fn linear_interpolation_error_zero_on_line() {
let p0 = Point2::new(0.0, 0.0);
let p1 = Point2::new(2.0, 0.0);
let p_test = Point2::new(1.0, 0.0);
let error = linear_interpolation_error(&p0, &p1, &p_test);
assert_relative_eq!(error, 0.0, epsilon = 1e-12);
}
#[test]
fn linear_interpolation_error_one_on_line() {
let p0 = Point2::new(0.0, 0.0);
let p1 = Point2::new(2.0, 0.0);
let p_test = Point2::new(1.0, 1.0);
let error = linear_interpolation_error(&p0, &p1, &p_test);
assert_relative_eq!(error, 1.0, epsilon = 1e-12);
}
#[test]
fn mean_point_weighted_equal_weights_matches_unweighted() {
let points = vec![
Point3::new(0.0, 0.0, 0.0),
Point3::new(2.0, 4.0, 6.0),
Point3::new(-1.0, 3.0, 5.0),
Point3::new(4.0, -2.0, 1.0),
];
let weights = vec![2.5; points.len()];
let weighted = mean_point_weighted(&points, &weights);
let unweighted = mean_point(&points);
assert_relative_eq!(weighted, unweighted, epsilon = 1e-12);
}
#[test]
fn mean_point_weighted_known_value() {
let points = vec![Point2::new(0.0, 0.0), Point2::new(4.0, 8.0)];
let weights = vec![1.0, 3.0];
let mean = mean_point_weighted(&points, &weights);
assert_relative_eq!(mean, Point2::new(3.0, 6.0), epsilon = 1e-12);
}
#[test]
fn mean_point_weighted_zero_weights_are_ignored() {
let points = vec![
Point2::new(1.0, 1.0),
Point2::new(100.0, 100.0),
Point2::new(3.0, 3.0),
];
let weights = vec![1.0, 0.0, 1.0];
let mean = mean_point_weighted(&points, &weights);
assert_relative_eq!(mean, Point2::new(2.0, 2.0), epsilon = 1e-12);
}
#[test]
fn mean_point_weighted_scale_invariant() {
let points = vec![
Point3::new(1.0, 2.0, 3.0),
Point3::new(-4.0, 0.5, 7.0),
Point3::new(2.0, -1.0, 0.0),
];
let weights = vec![0.5, 2.0, 1.5];
let scaled: Vec<f64> = weights.iter().map(|w| w * 10.0).collect();
let a = mean_point_weighted(&points, &weights);
let b = mean_point_weighted(&points, &scaled);
assert_relative_eq!(a, b, epsilon = 1e-12);
}
}