use crate::common::points::dist;
use crate::common::vec_f64::mean_and_stdev;
use crate::geom2::align2::target::is_curve_endpoint;
use crate::geom2::curve2::CurveStation2;
use crate::geom2::{Curve2, CurveGroup2, Iso2, SurfacePoint2};
use std::f64::consts::PI;
#[derive(Debug, Clone, Copy)]
pub struct CurveSurfPoint {
pub member: usize,
pub index: usize,
pub fraction: f64,
pub length_along: f64,
pub sp: SurfacePoint2,
}
impl CurveSurfPoint {
pub fn from_station(station: &CurveStation2, member: usize) -> Self {
Self {
member,
index: station.index(),
fraction: station.fraction(),
length_along: station.length_along(),
sp: station.surface_point(),
}
}
}
#[derive(Debug, Clone, Copy)]
pub struct CAPParams {
pub sample_spacing: f64,
pub max_corner_angle: f64,
pub end_margin: f64,
pub filter_distances: Option<f64>,
}
impl CAPParams {
pub fn new(
sample_spacing: f64,
max_corner_angle: f64,
end_margin: f64,
filter_distances: Option<f64>,
) -> Self {
Self {
sample_spacing,
max_corner_angle,
end_margin,
filter_distances,
}
}
pub fn defaults(sample_spacing: f64) -> Self {
Self {
sample_spacing,
max_corner_angle: PI / 3.0,
end_margin: 2.0 * sample_spacing,
filter_distances: Some(3.0),
}
}
}
pub fn generate_alignment_points(
test: &CurveGroup2,
reference: &CurveGroup2,
iso: &Iso2,
params: &CAPParams,
) -> Vec<CurveSurfPoint> {
let mut candidates = Vec::new();
for (member, test_curve) in test.curves().iter().enumerate() {
for cp in sample_curve(test_curve, params.sample_spacing, member) {
if !clear_of_the_ends(test_curve, cp.length_along, params.end_margin) {
continue;
}
if !turns_gently(test_curve, &cp, params) {
continue;
}
let moved = cp.sp.transformed_by(iso);
let (ref_m, station) = reference.at_closest_to_point(&moved.point);
let ref_curve = &reference.curves()[ref_m];
if !ref_curve.is_closed() && is_curve_endpoint(&station, ref_curve) {
continue;
}
let ref_cp = CurveSurfPoint::from_station(&station, ref_m);
if ref_cp.sp.normal.dot(&moved.normal) < 0.0 {
continue;
}
if !clear_of_the_ends(ref_curve, ref_cp.length_along, params.end_margin) {
continue;
}
if !turns_gently(ref_curve, &ref_cp, params) {
continue;
}
candidates.push((dist(&moved.point, &ref_cp.sp.point), cp));
}
}
if let Some(sigma) = params.filter_distances
&& candidates.len() > 10
{
let distances = candidates.iter().map(|(d, _)| *d).collect::<Vec<_>>();
if let Some((mean, st_dev)) = mean_and_stdev(&distances)
&& st_dev > 0.0
{
let threshold = mean + sigma * st_dev;
candidates.retain(|(d, _)| *d < threshold);
}
}
candidates.into_iter().map(|(_, cp)| cp).collect()
}
fn sample_curve(curve: &Curve2, spacing: f64, member: usize) -> Vec<CurveSurfPoint> {
if !spacing.is_finite() || spacing <= 0.0 || curve.length() <= 0.0 {
return Vec::new();
}
let count = (curve.length() / spacing).floor() as usize;
(0..count)
.filter_map(|k| {
let l = (k as f64 + 0.5) * spacing;
curve
.at_length(l)
.map(|s| CurveSurfPoint::from_station(&s, member))
})
.collect()
}
fn clear_of_the_ends(curve: &Curve2, length_along: f64, margin: f64) -> bool {
curve.is_closed() || (length_along >= margin && length_along <= curve.length() - margin)
}
fn turns_gently(curve: &Curve2, cp: &CurveSurfPoint, params: &CAPParams) -> bool {
for delta in [-params.sample_spacing, params.sample_spacing] {
let raw = cp.length_along + delta;
let l = if curve.is_closed() {
raw.rem_euclid(curve.length())
} else {
raw
};
let Some(station) = curve.at_length(l) else {
continue;
};
if station.normal().angle(&cp.sp.normal) > params.max_corner_angle {
return false;
}
}
true
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom2::{Point2, Vector2};
use approx::assert_relative_eq;
use std::f64::consts::PI;
fn square(closed: bool) -> Curve2 {
let points =
[(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)].map(|(x, y)| Point2::new(x, y));
Curve2::from_points(&points, 1e-8, closed).unwrap()
}
fn sample_at(curve: &Curve2, length: f64) -> CurveSurfPoint {
CurveSurfPoint::from_station(&curve.at_length(length).unwrap(), 0)
}
fn group_of(curve: &Curve2) -> CurveGroup2 {
CurveGroup2::from(curve.clone())
}
#[test]
fn a_sample_records_where_it_came_from() {
let curve = square(false);
let cp = sample_at(&curve, 2.5);
assert_eq!(cp.member, 0);
assert_eq!(cp.index, 1);
assert_relative_eq!(cp.fraction, 0.25, epsilon = 1e-12);
assert_relative_eq!(cp.length_along, 2.5, epsilon = 1e-12);
assert_relative_eq!(cp.sp.point, Point2::new(2.0, 0.5), epsilon = 1e-12);
}
#[test]
fn a_sample_carries_the_outward_normal() {
let curve = square(true);
let cp = sample_at(&curve, 1.0);
assert_relative_eq!(
cp.sp.normal.into_inner(),
Vector2::new(0.0, -1.0),
epsilon = 1e-12
);
}
fn rect_curve() -> Curve2 {
let points =
[(-5.0, -3.0), (5.0, -3.0), (5.0, 3.0), (-5.0, 3.0)].map(|(x, y)| Point2::new(x, y));
Curve2::from_points(&points, 1e-8, true).unwrap()
}
fn open_line() -> Curve2 {
Curve2::from_points(
&[Point2::new(0.0, 0.0), Point2::new(10.0, 0.0)],
1e-8,
false,
)
.unwrap()
}
#[test]
fn samples_are_spaced_along_the_curve_and_miss_the_vertices() {
let curve = rect_curve();
let samples = sample_curve(&curve, 0.5, 0);
assert_eq!(samples.len(), 64);
assert_relative_eq!(samples[0].length_along, 0.25, epsilon = 1e-12);
assert_relative_eq!(samples[1].length_along, 0.75, epsilon = 1e-12);
for s in &samples {
for corner in [0.0, 10.0, 16.0, 26.0, 32.0] {
assert!((s.length_along - corner).abs() > 1e-9);
}
}
}
#[test]
fn a_degenerate_spacing_samples_nothing() {
let curve = rect_curve();
assert!(sample_curve(&curve, 0.0, 0).is_empty());
assert!(sample_curve(&curve, -1.0, 0).is_empty());
assert!(sample_curve(&curve, f64::NAN, 0).is_empty());
}
#[test]
fn the_corner_check_rejects_samples_beside_a_corner() {
let curve = rect_curve();
let params = CAPParams::defaults(0.5);
assert!(turns_gently(&curve, &sample_at(&curve, 5.0), ¶ms));
assert!(!turns_gently(&curve, &sample_at(&curve, 9.75), ¶ms));
assert!(!turns_gently(&curve, &sample_at(&curve, 10.25), ¶ms));
assert!(!turns_gently(&curve, &sample_at(&curve, 0.25), ¶ms));
assert!(!turns_gently(&curve, &sample_at(&curve, 31.75), ¶ms));
}
#[test]
fn the_end_margin_only_applies_to_open_curves() {
let closed = rect_curve();
let open = open_line();
assert!(clear_of_the_ends(&closed, 0.0, 2.0));
assert!(clear_of_the_ends(&closed, closed.length(), 2.0));
assert!(!clear_of_the_ends(&open, 0.5, 2.0));
assert!(!clear_of_the_ends(&open, 9.5, 2.0));
assert!(clear_of_the_ends(&open, 5.0, 2.0));
assert!(clear_of_the_ends(&open, 2.0, 2.0));
assert!(clear_of_the_ends(&open, 8.0, 2.0));
}
#[test]
fn generated_points_avoid_the_corners_of_a_closed_curve() {
let test = rect_curve();
let reference = rect_curve();
let params = CAPParams::defaults(0.5);
let points = generate_alignment_points(
&group_of(&test),
&group_of(&reference),
&Iso2::identity(),
¶ms,
);
assert_eq!(points.len(), 56);
for p in &points {
for corner in [0.0, 10.0, 16.0, 26.0, 32.0] {
assert!(
(p.length_along - corner).abs() > 0.5,
"a sample at {} is too close to the corner at {corner}",
p.length_along
);
}
}
}
#[test]
fn coincident_curves_still_produce_correspondences() {
let test = rect_curve();
let reference = rect_curve();
let params = CAPParams::defaults(0.5);
assert!(params.filter_distances.is_some());
let points = generate_alignment_points(
&group_of(&test),
&group_of(&reference),
&Iso2::identity(),
¶ms,
);
assert!(!points.is_empty());
}
#[test]
fn the_distance_filter_drops_the_furthest_candidates() {
let test = open_line();
let reference = Curve2::from_points(
&[Point2::new(0.0, 0.0), Point2::new(10.0, 2.0)],
1e-8,
false,
)
.unwrap();
let unfiltered = CAPParams::new(0.25, PI / 3.0, 0.5, None);
let filtered = CAPParams::new(0.25, PI / 3.0, 0.5, Some(1.0));
let test = group_of(&test);
let reference = group_of(&reference);
let a = generate_alignment_points(&test, &reference, &Iso2::identity(), &unfiltered);
let b = generate_alignment_points(&test, &reference, &Iso2::identity(), &filtered);
assert!(
b.len() < a.len(),
"the filter should have dropped the far samples: {} unfiltered, {} filtered",
a.len(),
b.len()
);
let worst_kept = b.iter().map(|p| p.length_along).fold(f64::MIN, f64::max);
let worst_dropped = a.iter().map(|p| p.length_along).fold(f64::MIN, f64::max);
assert!(worst_kept < worst_dropped);
}
#[test]
fn samples_whose_normals_disagree_are_rejected() {
let test = open_line();
let reference = Curve2::from_points(
&[Point2::new(10.0, 0.0), Point2::new(0.0, 0.0)],
1e-8,
false,
)
.unwrap();
let params = CAPParams::new(0.5, PI / 3.0, 1.0, None);
let points = generate_alignment_points(
&group_of(&test),
&group_of(&reference),
&Iso2::identity(),
¶ms,
);
assert!(points.is_empty());
}
#[test]
fn samples_projecting_past_an_open_end_are_rejected() {
let test = open_line();
let reference =
Curve2::from_points(&[Point2::new(0.0, 0.0), Point2::new(5.0, 0.0)], 1e-8, false)
.unwrap();
let params = CAPParams::new(0.5, PI / 3.0, 0.5, None);
let points = generate_alignment_points(
&group_of(&test),
&group_of(&reference),
&Iso2::identity(),
¶ms,
);
assert!(!points.is_empty());
for p in &points {
assert!(
p.length_along < 5.0,
"a sample at {} matched past the end of the reference",
p.length_along
);
}
}
#[test]
fn samples_carry_their_member_and_respect_each_members_ends() {
let above = Curve2::from_points(
&[Point2::new(0.0, 0.1), Point2::new(10.0, 0.1)],
1e-8,
false,
)
.unwrap();
let below = Curve2::from_points(
&[Point2::new(0.0, -0.1), Point2::new(10.0, -0.1)],
1e-8,
false,
)
.unwrap();
let test = CurveGroup2::new(vec![above, below]).unwrap();
let reference = group_of(&open_line());
let params = CAPParams::new(0.5, PI / 3.0, 0.5, None);
let points = generate_alignment_points(&test, &reference, &Iso2::identity(), ¶ms);
let from_0 = points.iter().filter(|p| p.member == 0).count();
let from_1 = points.iter().filter(|p| p.member == 1).count();
assert_eq!(from_0, 18);
assert_eq!(from_1, 18);
for p in &points {
assert!(p.length_along >= 0.5 && p.length_along <= 9.5);
}
}
#[test]
fn reference_checks_run_against_the_owning_member() {
let far_segment = Curve2::from_points(
&[Point2::new(0.0, 50.0), Point2::new(1.0, 50.0)],
1e-8,
false,
)
.unwrap();
let reference = CurveGroup2::new(vec![far_segment, rect_curve()]).unwrap();
let test = group_of(&rect_curve());
let params = CAPParams::defaults(0.5);
let points = generate_alignment_points(&test, &reference, &Iso2::identity(), ¶ms);
assert_eq!(points.len(), 56);
}
}