use super::curve_curve_hull::{CurveCurveExtrema, CurveCurveHits};
use super::{CurveCurveExtremum, CurveCurveHit, curve_surface_hull, mixed};
use crate::curves::ParametricCurve3D;
use crate::math::{newton_min_1d, newton_min_2d};
use crate::{Point3D, Tolerance, Vector3D};
const SEED_GRID: usize = 32;
const SEED_GRID_1D: usize = 256;
const MAX_SEEDS: usize = 64;
const SEED_REACH: f64 = 64.0;
#[derive(Debug, Clone, Copy)]
pub(super) enum ParamDomain {
Periodic { start: f64, period: f64 },
Interval(f64, f64),
}
impl ParamDomain {
pub(super) fn of<C: ParametricCurve3D>(curve: &C) -> ParamDomain {
let (lo, hi) = curve.bounds();
match curve.period() {
Some(period) if lo.is_finite() && hi - lo >= period * (1.0 - 1e-12) => {
ParamDomain::Periodic { start: lo, period }
}
_ => ParamDomain::Interval(lo, hi),
}
}
fn bounds(self) -> Option<(f64, f64)> {
match self {
ParamDomain::Interval(lo, hi) if lo.is_finite() || hi.is_finite() => Some((lo, hi)),
_ => None,
}
}
pub(super) fn is_periodic(self) -> bool {
matches!(self, ParamDomain::Periodic { .. })
}
fn is_unbounded(self) -> bool {
matches!(self, ParamDomain::Interval(lo, hi) if lo.is_infinite() && hi.is_infinite())
}
fn is_finite(self) -> bool {
match self {
ParamDomain::Periodic { .. } => true,
ParamDomain::Interval(lo, hi) => lo.is_finite() && hi.is_finite(),
}
}
fn finite_ends(self) -> impl Iterator<Item = f64> {
let ends = match self {
ParamDomain::Periodic { .. } => [f64::NAN; 2],
ParamDomain::Interval(lo, hi) => [lo, hi],
};
ends.into_iter().filter(|u| u.is_finite())
}
fn reference(self) -> f64 {
match self {
ParamDomain::Periodic { start, .. } => start,
ParamDomain::Interval(lo, _) if lo.is_finite() => lo,
ParamDomain::Interval(_, hi) => hi.min(0.0),
}
}
fn wrap(self, u: f64) -> f64 {
let near = |end: f64| end.is_finite() && (u - end).abs() <= 2e-14 * (1.0 + end.abs());
match self {
ParamDomain::Periodic { start, period } => start + (u - start).rem_euclid(period),
ParamDomain::Interval(lo, _) if near(lo) => lo,
ParamDomain::Interval(_, hi) if near(hi) => hi,
ParamDomain::Interval(lo, hi) => u.clamp(lo, hi),
}
}
pub(super) fn range(self) -> curve_surface_hull::Range {
match self {
ParamDomain::Periodic { start, period } => curve_surface_hull::Range {
lo: start,
hi: start + period,
periodic: true,
},
ParamDomain::Interval(lo, hi) => curve_surface_hull::Range {
lo,
hi,
periodic: false,
},
}
}
fn step(self, u: f64, v: f64) -> f64 {
match self {
ParamDomain::Periodic { period, .. } => {
let d = (v - u).rem_euclid(period);
if d > 0.5 * period { d - period } else { d }
}
ParamDomain::Interval(..) => v - u,
}
}
fn seeds<C: ParametricCurve3D>(self, curve: &C, scale: f64, count: usize) -> Vec<f64> {
match self {
ParamDomain::Periodic { start, period } => (0..count)
.map(|i| start + period * i as f64 / count as f64)
.collect(),
ParamDomain::Interval(lo, hi) if self.is_finite() => (0..count)
.map(|i| match i {
0 => lo,
_ if i == count - 1 => hi,
_ => lo + (hi - lo) * i as f64 / (count - 1) as f64,
})
.collect(),
ParamDomain::Interval(lo, hi) => {
let u0 = self.reference();
let (p0, d1) = curve.eval_d1(u0);
let speed = d1.magnitude();
let first = scale / (16.0 * speed);
let first = if first.is_finite() && first > 0.0 {
first
} else {
1.0
};
let mut seeds: Vec<f64> = self.finite_ends().collect();
seeds.push(u0);
for side in [-1.0, 1.0] {
let mut step = first;
for _ in 0..96 {
let u = u0 + side * step;
if u < lo || u > hi {
break;
}
seeds.push(u);
if curve.eval_point(u).distance(p0) >= SEED_REACH * scale {
break;
}
step *= 1.25;
}
}
seeds.sort_by(f64::total_cmp);
seeds.dedup();
seeds
}
}
}
}
#[derive(Debug, Clone, Copy)]
struct Straight {
u0: f64,
origin: Point3D,
dir: Vector3D,
}
impl Straight {
fn of<C: ParametricCurve3D>(curve: &C, dom: ParamDomain) -> Option<Straight> {
const SPREAD: [f64; 6] = [0.137, 0.382, 0.5, 0.731, 0.919, 1.0];
const REACH: [f64; 4] = [0.7, 3.1, 17.3, 101.9];
let probes: Vec<f64> = match dom {
ParamDomain::Periodic { .. } => return None,
ParamDomain::Interval(lo, hi) if dom.is_finite() => {
SPREAD.iter().map(|f| lo + (hi - lo) * f).collect()
}
ParamDomain::Interval(lo, _) if lo.is_finite() => {
REACH.iter().map(|r| lo + r).collect()
}
ParamDomain::Interval(_, hi) if hi.is_finite() => {
REACH.iter().map(|r| hi - r).collect()
}
ParamDomain::Interval(..) => REACH.iter().flat_map(|&r| [-r, r]).collect(),
};
let u0 = dom.reference();
let (origin, dir) = curve.eval_d1(u0);
let line = Straight { u0, origin, dir };
if line.dir.magnitude() <= 0.0 {
return None;
}
let size = |p: Point3D| (p - Point3D::ORIGIN).magnitude();
probes
.into_iter()
.all(|u| {
let (p, q) = (curve.eval_point(u), line.point(u));
p.distance(q) <= 1e-12 * (size(p) + size(line.origin) + p.distance(line.origin))
})
.then_some(line)
}
fn point(&self, u: f64) -> Point3D {
self.origin + self.dir * (u - self.u0)
}
fn foot(&self, p: Point3D) -> f64 {
self.u0 + (p - self.origin).dot(self.dir) / self.dir.square_magnitude()
}
}
fn curve_pair_extremum<A, B>(a: &A, b: &B, s: f64, t: f64) -> CurveCurveExtremum
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
CurveCurveExtremum {
first_param: s,
second_param: t,
distance: a.eval_point(s).distance(b.eval_point(t)),
}
}
pub fn curve_curve_extrema<A, B>(a: &A, b: &B, tol: Tolerance) -> CurveCurveExtrema
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
curve_curve_minima(a, b, tol, true)
}
pub(super) fn curve_curve_minima<A, B>(
a: &A,
b: &B,
tol: Tolerance,
pieces: bool,
) -> CurveCurveExtrema
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
let (da, db) = (ParamDomain::of(a), ParamDomain::of(b));
let straight = (Straight::of(a, da), Straight::of(b, db));
if pieces
&& !matches!(straight, (Some(_), Some(_)))
&& let Some((mut seeds, completeness)) = mixed::curve_curve_seeds(a, b, tol)
{
if !completeness.is_complete() {
seeds.extend(box_seeds(a, da, b, db));
}
let minima = refine_minima(a, da, b, db, seeds, tol);
return CurveCurveExtrema::new(minima, completeness);
}
let minima = match straight {
(Some(la), Some(lb)) => vec![straight_pair_minimum(a, la, da, b, lb, db, tol)],
(Some(la), None) if da.is_unbounded() => line_curve_minima(a, la, b, db, tol),
(None, Some(lb)) if db.is_unbounded() => line_curve_minima(b, lb, a, da, tol)
.into_iter()
.map(|e| CurveCurveExtremum {
first_param: e.second_param,
second_param: e.first_param,
distance: e.distance,
})
.collect(),
_ => refine_minima(a, da, b, db, box_seeds(a, da, b, db), tol),
};
CurveCurveExtrema::sampled(minima)
}
fn straight_pair_minimum<A, B>(
a: &A,
la: Straight,
da: ParamDomain,
b: &B,
lb: Straight,
db: ParamDomain,
tol: Tolerance,
) -> CurveCurveExtremum
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
let (d1, d2) = (la.dir, lb.dir);
let w = la.origin - lb.origin;
let (aa, ab, bb) = (d1.dot(d1), d1.dot(d2), d2.dot(d2));
let denom = d1.cross(d2).square_magnitude();
if denom > tol.angular * tol.angular * aa * bb {
let (e1, e2) = (d1.dot(w), d2.dot(w));
let s = la.u0 + (ab * e2 - bb * e1) / denom;
let t = lb.u0 + (aa * e2 - ab * e1) / denom;
if da.wrap(s) == s && db.wrap(t) == t {
return curve_pair_extremum(a, b, s, t);
}
}
let edges = da
.finite_ends()
.map(|s| (s, db.wrap(lb.foot(la.point(s)))))
.chain(db.finite_ends().map(|t| (da.wrap(la.foot(lb.point(t))), t)));
let mut best: Option<CurveCurveExtremum> = None;
for (s, t) in edges {
let e = curve_pair_extremum(a, b, s, t);
if best.is_none_or(|m| e.distance < m.distance) {
best = Some(e);
}
}
best.unwrap_or_else(|| {
let s = da.reference();
curve_pair_extremum(a, b, s, lb.foot(la.point(s)))
})
}
fn line_curve_minima<L, C>(
line_curve: &L,
line: Straight,
curve: &C,
dom: ParamDomain,
tol: Tolerance,
) -> Vec<CurveCurveExtremum>
where
L: ParametricCurve3D,
C: ParametricCurve3D,
{
let unit = line.dir * (1.0 / line.dir.magnitude());
let perp = |v: Vector3D| v - unit * v.dot(unit);
let local = |t: f64| {
let (p, c1, c2) = curve.eval_d2(t);
let r = perp(p - line.origin);
let c1p = perp(c1);
(
(0.5 * r.dot(r), r.dot(c1), c1p.dot(c1p) + r.dot(c2)),
c1.magnitude(),
)
};
let extremum =
|t: f64| curve_pair_extremum(line_curve, curve, line.foot(curve.eval_point(t)), t);
let gap = perp(curve.eval_point(dom.reference()) - line.origin).magnitude();
let scale = configuration_scale(gap, 0.0, curve_size(curve, dom));
let seeds = dom.seeds(curve, scale, SEED_GRID_1D);
let values: Vec<f64> = seeds.iter().map(|&t| local(t).0.0).collect();
let bounds = dom.bounds();
let mut found = Vec::new();
for (k, _) in grid_minima(&values, [seeds.len(), 1], [dom.is_periodic(), false]) {
let t = dom.wrap(newton_min_1d(|t| local(t).0, seeds[k], bounds, 1e-14, 100));
let ((_, g, h), speed) = local(t);
let optimal = is_box_minimum(
[t, 0.0],
[g, 0.0],
[[h, 0.0], [0.0, 0.0]],
[speed, 0.0],
[bounds, None],
tol,
);
if t.is_finite() && optimal {
found.push(extremum(t));
}
}
let distance = |t: f64| {
line_curve
.eval_point(line.foot(curve.eval_point(t)))
.distance(curve.eval_point(t))
};
let same_valley = |p: &CurveCurveExtremum, q: &CurveCurveExtremum| {
let dt = dom.step(p.second_param, q.second_param);
flat_between(
p.distance,
q.distance,
|f| distance(p.second_param + f * dt),
tol,
)
};
merge_valleys(found, same_valley, |_| extremum(dom.reference()), tol)
}
fn box_seeds<A, B>(a: &A, da: ParamDomain, b: &B, db: ParamDomain) -> Vec<[f64; 2]>
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
let gap = a
.eval_point(da.reference())
.distance(b.eval_point(db.reference()));
let scale = configuration_scale(gap, curve_size(a, da), curve_size(b, db));
let (ss, ts) = (da.seeds(a, scale, SEED_GRID), db.seeds(b, scale, SEED_GRID));
let pb: Vec<Point3D> = ts.iter().map(|&t| b.eval_point(t)).collect();
let values: Vec<f64> = ss
.iter()
.flat_map(|&s| {
let p = a.eval_point(s);
pb.iter().map(move |q| p.distance(*q))
})
.collect();
let shape = [ss.len(), ts.len()];
let mut seeds: Vec<[f64; 2]> =
grid_minima(&values, shape, [da.is_periodic(), db.is_periodic()])
.into_iter()
.map(|(i, j)| [ss[i], ts[j]])
.collect();
let edge = |p: Point3D, curve: &dyn Fn(f64) -> Point3D, us: &[f64], dom: ParamDomain| {
let values: Vec<f64> = us.iter().map(|&u| p.distance(curve(u))).collect();
grid_minima(&values, [us.len(), 1], [dom.is_periodic(), false])
.into_iter()
.map(|(k, _)| us[k])
.collect::<Vec<f64>>()
};
let (dense_s, dense_t) = (
da.seeds(a, scale, SEED_GRID_1D),
db.seeds(b, scale, SEED_GRID_1D),
);
for s in da.finite_ends() {
let on_b = edge(a.eval_point(s), &|t| b.eval_point(t), &dense_t, db);
seeds.extend(on_b.into_iter().map(|t| [s, t]));
}
for t in db.finite_ends() {
let on_a = edge(b.eval_point(t), &|s| a.eval_point(s), &dense_s, da);
seeds.extend(on_a.into_iter().map(|s| [s, t]));
}
seeds
}
pub(super) fn refine_minima<A, B>(
a: &A,
da: ParamDomain,
b: &B,
db: ParamDomain,
seeds: Vec<[f64; 2]>,
tol: Tolerance,
) -> Vec<CurveCurveExtremum>
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
let local = |s: f64, t: f64| {
let (pa, a1, a2) = a.eval_d2(s);
let (pb, b1, b2) = b.eval_d2(t);
let r = pa - pb;
let cross = -a1.dot(b1);
(
(
0.5 * r.dot(r),
[r.dot(a1), -r.dot(b1)],
[
[a1.dot(a1) + r.dot(a2), cross],
[cross, b1.dot(b1) - r.dot(b2)],
],
),
[a1.magnitude(), b1.magnitude()],
)
};
let bounds = [da.bounds(), db.bounds()];
let mut found = Vec::new();
for seed in seeds {
let [s, t] = newton_min_2d(|s, t| local(s, t).0, seed, bounds, 1e-14, 100);
let (s, t) = (da.wrap(s), db.wrap(t));
let ((_, g, h), speed) = local(s, t);
if s.is_finite() && t.is_finite() && is_box_minimum([s, t], g, h, speed, bounds, tol) {
found.push(curve_pair_extremum(a, b, s, t));
}
}
let same_valley = |p: &CurveCurveExtremum, q: &CurveCurveExtremum| {
let ds = da.step(p.first_param, q.first_param);
let dt = db.step(p.second_param, q.second_param);
flat_between(
p.distance,
q.distance,
|f| {
a.eval_point(p.first_param + f * ds)
.distance(b.eval_point(p.second_param + f * dt))
},
tol,
)
};
let representative = |e: &CurveCurveExtremum| {
let s = da.reference();
let p = a.eval_point(s);
let objective = |t: f64| {
let (q, b1, b2) = b.eval_d2(t);
let r = p - q;
(0.5 * r.dot(r), -r.dot(b1), b1.dot(b1) - r.dot(b2))
};
let t = newton_min_1d(objective, e.second_param, db.bounds(), 1e-14, 100);
curve_pair_extremum(a, b, s, db.wrap(t))
};
merge_valleys(found, same_valley, representative, tol)
}
fn configuration_scale(gap: f64, size_a: f64, size_b: f64) -> f64 {
let scale = gap + size_a + size_b;
if scale.is_finite() && scale > 0.0 {
scale
} else {
1.0
}
}
fn curve_size<C: ParametricCurve3D>(curve: &C, dom: ParamDomain) -> f64 {
let u0 = dom.reference();
if dom.is_finite() {
let p0 = curve.eval_point(u0);
dom.seeds(curve, 1.0, SEED_GRID)
.into_iter()
.map(|u| curve.eval_point(u).distance(p0))
.fold(0.0, f64::max)
} else {
let (_, d1, d2) = curve.eval_d2(u0);
let bend = d1.cross(d2).magnitude();
if bend > 0.0 {
d1.magnitude().powi(3) / bend
} else {
0.0
}
}
}
fn grid_minima(values: &[f64], shape: [usize; 2], wrap: [bool; 2]) -> Vec<(usize, usize)> {
let [n, m] = shape;
let neighbour = |i: usize, di: isize, len: usize, wrap: bool| {
let k = i as isize + di;
if (0..len as isize).contains(&k) {
Some(k as usize)
} else if wrap && len > 1 {
Some(k.rem_euclid(len as isize) as usize)
} else {
None
}
};
let mut out = Vec::new();
for i in 0..n {
for j in 0..m {
let v = values[i * m + j];
let lowest = v.is_finite()
&& (-1..=1).all(|di| {
(-1..=1).all(|dj| {
match (neighbour(i, di, n, wrap[0]), neighbour(j, dj, m, wrap[1])) {
(Some(x), Some(y)) => {
values[x * m + y] >= v || values[x * m + y].is_nan()
}
_ => true,
}
})
});
if lowest {
out.push((i, j));
}
}
}
out.sort_by(|p, q| values[p.0 * m + p.1].total_cmp(&values[q.0 * m + q.1]));
out.truncate(MAX_SEEDS);
out
}
fn is_box_minimum(
x: [f64; 2],
g: [f64; 2],
h: [[f64; 2]; 2],
speed: [f64; 2],
bounds: [Option<(f64, f64)>; 2],
tol: Tolerance,
) -> bool {
let mut stationary = [false; 2];
for i in 0..2 {
let held = match bounds[i] {
Some((lo, hi)) => (x[i] <= lo && g[i] > 0.0) || (x[i] >= hi && g[i] < 0.0),
None => false,
};
stationary[i] = g[i].abs() <= tol.confusion * speed[i];
if !stationary[i] && !held {
return false;
}
}
let slack = 1e-6 * (h[0][0].abs() + h[1][1].abs() + h[0][1].abs() + h[1][0].abs());
match stationary {
[true, true] => {
let (p, q, r) = (h[0][0], 0.5 * (h[0][1] + h[1][0]), h[1][1]);
0.5 * (p + r) - (0.25 * (p - r) * (p - r) + q * q).sqrt() >= -slack
}
[true, false] => h[0][0] >= -slack,
[false, true] => h[1][1] >= -slack,
[false, false] => true,
}
}
fn flat_between(d0: f64, d1: f64, along: impl Fn(f64) -> f64, tol: Tolerance) -> bool {
(d0 - d1).abs() <= tol.confusion
&& (1..8).all(|k| (along(k as f64 / 8.0) - d0).abs() <= tol.confusion)
}
fn merge_valleys(
mut found: Vec<CurveCurveExtremum>,
same_valley: impl Fn(&CurveCurveExtremum, &CurveCurveExtremum) -> bool,
representative: impl Fn(&CurveCurveExtremum) -> CurveCurveExtremum,
tol: Tolerance,
) -> Vec<CurveCurveExtremum> {
let apart = |u: f64, v: f64| (u - v).abs() > tol.parametric * (1.0 + u.abs());
found.sort_by(|x, y| x.distance.total_cmp(&y.distance));
let mut kept: Vec<(CurveCurveExtremum, bool)> = Vec::new();
for e in found {
match kept.iter_mut().find(|(k, _)| same_valley(k, &e)) {
Some((k, valley)) => {
*valley |=
apart(k.first_param, e.first_param) || apart(k.second_param, e.second_param);
}
None => kept.push((e, false)),
}
}
kept.into_iter()
.map(|(e, valley)| {
if valley {
let r = representative(&e);
if same_valley(&e, &r) {
return r;
}
}
e
})
.collect()
}
pub fn intersect_curve_curve<A, B>(a: &A, b: &B, tol: Tolerance) -> CurveCurveHits
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
if let Some((meetings, completeness)) = mixed::curve_curve_meetings(a, b, tol) {
let mut hits = CurveCurveHits::new(hits_at_minima(a, b, meetings, tol), completeness);
if !hits.is_complete() {
hits.add_sampled(sampled_curve_curve(a, b, tol), tol);
}
return hits;
}
let extrema = curve_curve_extrema(a, b, tol).into_minima();
CurveCurveHits::sampled(hits_at_minima(a, b, extrema, tol))
}
pub(super) fn sampled_curve_curve<A, B>(a: &A, b: &B, tol: Tolerance) -> Vec<CurveCurveHit>
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
hits_at_minima(
a,
b,
curve_curve_minima(a, b, tol, false).into_minima(),
tol,
)
}
pub(super) fn hits_at_minima<A, B>(
a: &A,
b: &B,
extrema: Vec<CurveCurveExtremum>,
tol: Tolerance,
) -> Vec<CurveCurveHit>
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
let hits: Vec<CurveCurveHit> = extrema
.into_iter()
.filter(|e| e.distance <= tol.confusion)
.map(|e| {
let ta = a.eval_derivative(e.first_param, 1);
let tb = b.eval_derivative(e.second_param, 1);
let sin = ta.cross(tb).magnitude() / (ta.magnitude() * tb.magnitude());
CurveCurveHit {
first_param: e.first_param,
point: a.eval_point(e.first_param),
second_param: e.second_param,
multiplicity: if sin <= tol.angular { 2 } else { 1 },
}
})
.collect();
let merge_gap = tol.confusion.sqrt();
let mut merged: Vec<CurveCurveHit> = Vec::new();
for h in hits {
if merged
.iter()
.all(|m: &CurveCurveHit| m.point.distance(h.point) > merge_gap)
{
merged.push(h);
}
}
merged.sort_by(|x, y| x.first_param.partial_cmp(&y.first_param).unwrap());
merged
}
#[cfg(test)]
mod tests {
use super::*;
use crate::curves::BSplineCurve3D;
use crate::{Intersect, Line3D, LineCircle3DIntersection};
#[test]
fn test_curve_curve_extrema_skew_matches_exact() {
use crate::{Line3D, LineLine3DIntersection};
let tol = Tolerance::DEFAULT;
let l1 = Line3D::new(Point3D::ORIGIN, Vector3D::X).unwrap();
let l2 = Line3D::new(Point3D::new(0.0, 0.0, 1.0), Vector3D::Y).unwrap();
let ext = curve_curve_extrema(&l1, &l2, tol);
assert_eq!(ext.len(), 1);
assert!((ext[0].distance - 1.0).abs() < 1e-9);
match l1.intersect(&l2, tol) {
LineLine3DIntersection::Skew { s, t, distance } => {
assert!((ext[0].first_param - s).abs() < 1e-9);
assert!((ext[0].second_param - t).abs() < 1e-9);
assert!((ext[0].distance - distance).abs() < 1e-12);
}
_ => panic!("exact intersection expected skew"),
}
}
fn segment(from: Point3D, to: Point3D) -> crate::TrimmedCurve3D<Line3D> {
crate::TrimmedCurve3D::segment(from, to).unwrap()
}
fn arc(
center: Point3D,
radius: f64,
lo: f64,
hi: f64,
) -> crate::TrimmedCurve3D<crate::Circle3D> {
let circle = crate::Circle3D::new(center, Vector3D::Z, radius).unwrap();
crate::TrimmedCurve3D::new(circle, lo, hi).unwrap()
}
fn grid_min<A: ParametricCurve3D, B: ParametricCurve3D>(a: &A, b: &B, n: usize) -> f64 {
let ((a0, a1), (b0, b1)) = (a.bounds(), b.bounds());
let at = |lo: f64, hi: f64, i: usize| lo + (hi - lo) * i as f64 / (n - 1) as f64;
let pb: Vec<Point3D> = (0..n).map(|j| b.eval_point(at(b0, b1, j))).collect();
(0..n)
.flat_map(|i| {
let p = a.eval_point(at(a0, a1, i));
pb.iter().map(move |q| p.distance(*q))
})
.fold(f64::INFINITY, f64::min)
}
fn assert_local_minima<A: ParametricCurve3D, B: ParametricCurve3D>(
a: &A,
b: &B,
ext: &[CurveCurveExtremum],
) {
let ((a0, a1), (b0, b1)) = (a.bounds(), b.bounds());
for e in ext {
for (ds, dt) in [(1.0, 0.0), (0.0, 1.0), (1.0, 1.0), (1.0, -1.0)] {
for sign in [-1.0, 1.0] {
let h = 1e-4 * sign;
let s = (e.first_param + h * ds).clamp(a0, a1);
let t = (e.second_param + h * dt).clamp(b0, b1);
let d = a.eval_point(s).distance(b.eval_point(t));
assert!(
d >= e.distance - 1e-12,
"{e:?} is not a minimum: {d} at ({s}, {t})"
);
}
}
}
}
#[test]
fn test_curve_curve_extrema_concentric_arcs_corner() {
let tol = Tolerance::DEFAULT;
let a = arc(Point3D::ORIGIN, 5.0, 0.0, 2.0);
let b = arc(Point3D::ORIGIN, 10.0, 2.0, 4.0);
let ext = curve_curve_extrema(&a, &b, tol);
assert!((ext[0].distance - 5.0).abs() < 1e-12, "{ext:?}");
assert_eq!((ext[0].first_param, ext[0].second_param), (2.0, 2.0));
assert_eq!(ext.len(), 2, "{ext:?}");
assert_eq!((ext[1].first_param, ext[1].second_param), (0.0, 4.0));
assert_local_minima(&a, &b, &ext);
}
#[test]
fn test_curve_curve_extrema_concentric_arcs_same_range() {
let tol = Tolerance::DEFAULT;
let a = arc(Point3D::ORIGIN, 5.0, 0.0, 2.0);
let b = arc(Point3D::ORIGIN, 10.0, 0.0, 2.0);
let ext = curve_curve_extrema(&a, &b, tol);
assert_eq!(ext.len(), 1, "{ext:?}");
assert!((ext[0].distance - 5.0).abs() < 1e-12);
assert_eq!(ext[0].first_param, 0.0);
assert!(ext[0].second_param.abs() < 1e-9, "{ext:?}");
}
#[test]
fn test_curve_curve_extrema_concentric_circles() {
let tol = Tolerance::DEFAULT;
let a = crate::Circle3D::new(Point3D::ORIGIN, Vector3D::Z, 100.0).unwrap();
let b = crate::Circle3D::new(Point3D::ORIGIN, Vector3D::Z, 50.0).unwrap();
let ext = curve_curve_extrema(&a, &b, tol);
assert_eq!(ext.len(), 1, "{ext:?}");
assert!((ext[0].distance - 50.0).abs() < 1e-12);
assert_eq!(ext[0].first_param, 0.0);
assert!(ext[0].second_param.abs() < 1e-9, "{ext:?}");
}
#[test]
fn test_curve_curve_extrema_parallel_segments() {
let tol = Tolerance::DEFAULT;
let (p0, p1) = (Point3D::ORIGIN, Point3D::new(10.0, 0.0, 0.0));
let (q0, q1) = (Point3D::new(2.0, 4.0, 2.0), Point3D::new(8.0, 4.0, 2.0));
let ext = curve_curve_extrema(&segment(p0, p1), &segment(q0, q1), tol);
assert_eq!(ext.len(), 1, "{ext:?}");
assert!((ext[0].distance - 4.47213595499958).abs() < 1e-12);
assert!((ext[0].first_param - 2.0).abs() < 1e-12);
assert_eq!(ext[0].second_param, 0.0);
let spline = |p: Point3D, q: Point3D| {
BSplineCurve3D::new(1, vec![p, q], vec![0.0, 1.0], vec![2, 2], false).unwrap()
};
let ext = curve_curve_extrema(&spline(p0, p1), &spline(q0, q1), tol);
assert_eq!(ext.len(), 1, "{ext:?}");
assert!((ext[0].distance - 20f64.sqrt()).abs() < 1e-12);
}
#[test]
fn test_curve_curve_extrema_parallel_lines() {
use crate::LineLine3DIntersection;
let tol = Tolerance::DEFAULT;
let a = Line3D::new(Point3D::new(3.0, 0.0, 0.0), Vector3D::X).unwrap();
let b = Line3D::new(Point3D::new(0.0, 9.0, 5.0), Vector3D::new(-2.0, 0.0, 0.0)).unwrap();
let ext = curve_curve_extrema(&a, &b, tol);
assert_eq!(ext.len(), 1, "{ext:?}");
assert!((ext[0].distance - 106f64.sqrt()).abs() < 1e-12);
assert_eq!(ext[0].first_param, 0.0);
let foot = b.eval_point(ext[0].second_param);
assert!(foot.distance(Point3D::new(3.0, 9.0, 5.0)) < 1e-12);
match a.intersect(&b, tol) {
LineLine3DIntersection::Parallel(d) => assert!((ext[0].distance - d).abs() < 1e-12),
_ => panic!("exact intersection expected parallel"),
}
}
#[test]
fn test_curve_curve_extrema_opposite_lines() {
let tol = Tolerance::DEFAULT;
let a = Line3D::new(Point3D::new(1.0, 0.0, 0.0), Vector3D::new(0.0, 0.0, -1.0)).unwrap();
let b = Line3D::new(Point3D::new(5.0, 0.0, 0.0), Vector3D::Z).unwrap();
let ext = curve_curve_extrema(&a, &b, tol);
assert_eq!(ext.len(), 1, "{ext:?}");
assert_eq!(ext[0].distance, 4.0);
assert_eq!((ext[0].first_param, ext[0].second_param), (0.0, 0.0));
}
#[test]
fn test_curve_curve_extrema_segment_end_to_interior() {
let tol = Tolerance::DEFAULT;
let a = segment(Point3D::ORIGIN, Point3D::new(1.0, 0.0, 0.0));
let b = segment(Point3D::new(2.5, -0.5, 1.5), Point3D::new(2.5, 0.5, 1.5));
let ext = curve_curve_extrema(&a, &b, tol);
assert_eq!(ext.len(), 1, "{ext:?}");
assert_eq!(ext[0].first_param, 1.0);
assert!((ext[0].second_param - 0.5).abs() < 1e-12);
assert!((ext[0].distance - 1.5 * 2f64.sqrt()).abs() < 1e-12);
}
#[test]
fn test_curve_curve_extrema_line_far_minima() {
let tol = Tolerance::DEFAULT;
let line = Line3D::new(Point3D::ORIGIN, Vector3D::X).unwrap();
let ellipse = crate::Ellipse3D::new(
Point3D::new(40.0, 0.0, 0.0),
Vector3D::X,
Vector3D::Y,
5.0,
3.0,
)
.unwrap();
let ext = curve_curve_extrema(&line, &ellipse, tol);
assert_eq!(ext.len(), 2, "{ext:?}");
let quarter = std::f64::consts::FRAC_PI_2;
let mut params: Vec<f64> = ext.iter().map(|e| e.second_param).collect();
params.sort_by(f64::total_cmp);
assert!((params[0] - quarter).abs() < 1e-7, "{ext:?}");
assert!((params[1] - 3.0 * quarter).abs() < 1e-7, "{ext:?}");
for e in &ext {
assert!((e.distance - 3.0).abs() < 1e-12, "{e:?}");
assert!((e.first_param - 40.0).abs() < 1e-9, "{e:?}");
}
let swapped = curve_curve_extrema(&ellipse, &line, tol);
assert_eq!(swapped.len(), 2);
assert!((swapped[0].second_param - 40.0).abs() < 1e-9);
}
#[test]
fn test_curve_curve_extrema_parabola_far_circle() {
use crate::{Circle3D, Parabola3D};
let tol = Tolerance::DEFAULT;
let parabola = Parabola3D::new(Point3D::ORIGIN, Vector3D::Z, Vector3D::X, 1.0).unwrap();
let u = 60.0;
let normal = parabola
.eval_derivative(u, 1)
.cross(Vector3D::Z)
.normalized()
.unwrap();
let center = parabola.eval_point(u) + normal * 5.0;
let circle = Circle3D::new(center, Vector3D::Z, 1.0).unwrap();
let ext = curve_curve_extrema(¶bola, &circle, tol);
assert!((ext[0].distance - 4.0).abs() < 1e-9, "{ext:?}");
assert!((ext[0].first_param - u).abs() < 1e-7, "{ext:?}");
}
#[test]
fn test_curve_curve_extrema_random_arcs_beat_grid() {
let tol = Tolerance::DEFAULT;
let mut state = 0x2545_f491_4f6c_dd1d_u64;
let mut rand = move || {
state ^= state << 13;
state ^= state >> 7;
state ^= state << 17;
(state >> 11) as f64 / (1u64 << 53) as f64
};
for case in 0..120 {
let ca = Point3D::new(10.0 * rand() - 5.0, 10.0 * rand() - 5.0, 0.0);
let cb = if case % 2 == 1 {
ca
} else {
Point3D::new(10.0 * rand() - 5.0, 10.0 * rand() - 5.0, 0.0)
};
let (ra, rb) = (1.0 + 9.0 * rand(), 1.0 + 9.0 * rand());
let (a0, b0) = (6.0 * rand(), 6.0 * rand());
let a = arc(ca, ra, a0, a0 + 0.2 + 3.0 * rand());
let b = arc(cb, rb, b0, b0 + 0.2 + 3.0 * rand());
let ext = curve_curve_extrema(&a, &b, tol);
assert!(!ext.is_empty(), "case {case}");
let grid = grid_min(&a, &b, 400);
assert!(
ext[0].distance <= grid + 1e-12,
"case {case}: {ext:?} > grid {grid}, arcs {:?} {:?} {:?} {:?} {:?} {:?}",
ca,
ra,
a.bounds(),
cb,
rb,
b.bounds()
);
assert_local_minima(&a, &b, &ext);
}
}
#[test]
fn test_intersect_curve_curve_line_circle() {
use crate::{Circle3D, Line3D};
let tol = Tolerance::DEFAULT;
let line = Line3D::new(Point3D::ORIGIN, Vector3D::X).unwrap();
let circle = Circle3D::new(Point3D::ORIGIN, Vector3D::Z, 2.0).unwrap();
let hits = intersect_curve_curve(&line, &circle, tol);
assert_eq!(hits.len(), 2);
assert!((hits[0].first_param + 2.0).abs() < 1e-9);
assert!((hits[1].first_param - 2.0).abs() < 1e-9);
match line.intersect(&circle, tol) {
LineCircle3DIntersection::Points((t1, _), (t2, _)) => {
assert!((hits[0].first_param - t1).abs() < 1e-9);
assert!((hits[1].first_param - t2).abs() < 1e-9);
}
_ => panic!("exact intersection expected two points"),
}
}
}