use crate::curves::{BSplineCurve3D, InterpParametrization, ParametricCurve3D};
use crate::math::{RealRoot, newton_3d, newton_min_1d, newton_min_2d, real_roots};
use crate::surfaces::ParametricSurface;
use crate::{
Circle3D, Ellipse3D, Hyperbola3D, Line3D, Parabola3D, Plane, Point2D, Point3D, Tolerance,
Vector3D,
};
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum PlanePlaneIntersection {
Line(Line3D),
Parallel,
Coincident,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum PlaneSphereIntersection {
Circle(Circle3D),
TangentPoint(Point3D),
Empty,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum SphereSphereIntersection {
Circle(Circle3D),
TangentPoint(Point3D),
Empty,
Coincident,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum PlaneCylinderIntersection {
Circle(Circle3D),
Ellipse(Ellipse3D),
TwoLines(Line3D, Line3D),
TangentLine(Line3D),
Empty,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum PlaneConeIntersection {
Circle(Circle3D),
Ellipse(Ellipse3D),
Parabola(Parabola3D),
Hyperbola(Hyperbola3D),
TwoLines(Line3D, Line3D),
TangentLine(Line3D),
ApexPoint(Point3D),
Empty,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum LinePlaneIntersection {
Point(f64, Point3D),
Parallel,
Coincident,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum LineQuadricIntersection {
TwoPoints((f64, Point3D), (f64, Point3D)),
OnePoint(f64, Point3D),
Tangent(f64, Point3D),
Empty,
Coincident,
}
#[derive(Debug, Clone, PartialEq)]
pub enum LineTorusIntersection {
Hits(Vec<LineTorusHit>),
Empty,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct LineTorusHit {
pub line_param: f64,
pub point: Point3D,
pub surface_params: (f64, f64),
pub multiplicity: u32,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum SphereCylinderIntersection {
Circle(Circle3D),
TwoCircles(Circle3D, Circle3D),
Empty,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum SphereConeIntersection {
Circle(Circle3D),
TwoCircles(Circle3D, Circle3D),
TangentCircle(Circle3D),
Empty,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum CylinderCylinderIntersection {
TwoLines(Line3D, Line3D),
TangentLine(Line3D),
Empty,
Coincident,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum ConeCylinderIntersection {
Circle(Circle3D),
ApexPoint(Point3D),
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum TorusPlaneIntersection {
TwoCircles(Circle3D, Circle3D),
TangentCircle(Circle3D),
Circle(Circle3D),
Empty,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum TorusSphereIntersection {
Circle(Circle3D),
TwoCircles(Circle3D, Circle3D),
TangentCircle(Circle3D),
Empty,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum TorusCylinderIntersection {
TwoCircles(Circle3D, Circle3D),
TangentCircle(Circle3D),
Empty,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum TorusConeIntersection {
Circle(Circle3D),
TwoCircles(Circle3D, Circle3D),
TangentCircle(Circle3D),
Empty,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum TorusTorusIntersection {
Circle(Circle3D),
TwoCircles(Circle3D, Circle3D),
TangentCircle(Circle3D),
Empty,
Coincident,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum ConeConeIntersection {
Circle(Circle3D),
ApexPoint(Point3D),
Empty,
Coincident,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum LineLine2DIntersection {
Point(f64, Point2D, f64),
Parallel,
Coincident,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum LineCircle2DIntersection {
Points((f64, Point2D), (f64, Point2D)),
Tangent(f64, Point2D),
Empty,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum CircleCircle2DIntersection {
Points(Point2D, Point2D),
Tangent(Point2D),
Empty,
Coincident,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum LineLine3DIntersection {
Point(f64, Point3D, f64),
Parallel(f64),
Coincident,
Skew {
s: f64,
t: f64,
distance: f64,
},
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum LineCircle3DIntersection {
Points((f64, Point3D), (f64, Point3D)),
Tangent(f64, Point3D),
Empty,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct EllipseEllipseHit {
pub first_param: f64,
pub point: Point3D,
pub second_param: f64,
}
#[derive(Debug, Clone, PartialEq)]
pub enum EllipseEllipse3DIntersection {
Points(Vec<EllipseEllipseHit>),
Tangent(EllipseEllipseHit),
Empty,
Coincident,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum CircleCircle3DIntersection {
Points(Point3D, Point3D),
Tangent(Point3D),
Empty,
Coincident,
NotAnalytic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct LineConicHit {
pub line_param: f64,
pub point: Point3D,
pub conic_param: f64,
}
#[derive(Debug, Clone, PartialEq)]
pub enum LineConic3DIntersection {
Points(LineConicHit, LineConicHit),
Tangent(LineConicHit),
Empty,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CircleConicHit {
pub circle_param: f64,
pub point: Point3D,
pub conic_param: f64,
}
#[derive(Debug, Clone, PartialEq)]
pub enum CircleConic3DIntersection {
Points(Vec<CircleConicHit>),
Tangent(CircleConicHit),
Coincident,
Empty,
NotAnalytic,
}
#[derive(Debug, Clone, Copy)]
pub(crate) struct ImplicitConic {
a2: f64,
b2: f64,
c2: f64,
d1: f64,
e1: f64,
f0: f64,
}
impl ImplicitConic {
pub(crate) fn ellipse(a: f64, b: f64) -> ImplicitConic {
ImplicitConic {
a2: 1.0 / (a * a),
b2: 1.0 / (b * b),
c2: 0.0,
d1: 0.0,
e1: 0.0,
f0: -1.0,
}
}
pub(crate) fn parabola(focal: f64) -> ImplicitConic {
ImplicitConic {
a2: 0.0,
b2: 1.0,
c2: 0.0,
d1: -4.0 * focal,
e1: 0.0,
f0: 0.0,
}
}
pub(crate) fn hyperbola(a: f64, b: f64) -> ImplicitConic {
ImplicitConic {
a2: 1.0 / (a * a),
b2: -1.0 / (b * b),
c2: 0.0,
d1: 0.0,
e1: 0.0,
f0: -1.0,
}
}
pub(crate) fn line_substitute(&self, ox: f64, oy: f64, dx: f64, dy: f64) -> (f64, f64, f64) {
let qa = self.a2 * dx * dx + self.b2 * dy * dy + self.c2 * dx * dy;
let qb = 2.0 * self.a2 * ox * dx
+ 2.0 * self.b2 * oy * dy
+ self.c2 * (ox * dy + oy * dx)
+ self.d1 * dx
+ self.e1 * dy;
let qc = self.a2 * ox * ox
+ self.b2 * oy * oy
+ self.c2 * ox * oy
+ self.d1 * ox
+ self.e1 * oy
+ self.f0;
(qa, qb, qc)
}
pub(crate) fn circle_substitute(&self, cx: f64, cy: f64, r: f64) -> (f64, f64, f64, f64, f64) {
let r2 = r * r;
let a2 = r2 * (self.a2 - self.b2) / 2.0;
let b2 = r2 * self.c2 / 2.0;
let c1 = r * (2.0 * self.a2 * cx + self.c2 * cy + self.d1);
let d1 = r * (2.0 * self.b2 * cy + self.c2 * cx + self.e1);
let e = self.a2 * cx * cx
+ self.b2 * cy * cy
+ self.c2 * cx * cy
+ self.d1 * cx
+ self.e1 * cy
+ self.f0
+ r2 * (self.a2 + self.b2) / 2.0;
(a2, b2, c1, d1, e)
}
}
#[derive(Debug, Clone, Copy)]
pub(crate) struct PlacedConic {
pub plane: Plane,
pub x: Vector3D,
pub y: Vector3D,
pub origin: Point3D,
pub normal: Vector3D,
pub implicit: ImplicitConic,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ConicSurfaceHit {
pub parameter: f64,
pub point: Point3D,
pub multiplicity: u32,
}
#[derive(Debug, Clone, PartialEq)]
pub enum CircleSurfaceIntersection {
Circle(Circle3D),
Hits(Vec<ConicSurfaceHit>),
Empty,
}
pub(crate) fn solve_trig_linear(
c: f64,
d: f64,
e: f64,
scale: f64,
tol: Tolerance,
) -> Vec<(f64, u32)> {
let amp = c.hypot(d);
let band = tol.confusion * scale.max(f64::MIN_POSITIVE);
if amp <= band {
return Vec::new();
}
if e.abs() > amp + band {
return Vec::new();
}
let phi = d.atan2(c);
let t0 = (-e / amp).clamp(-1.0, 1.0).acos();
if e.abs() >= amp - band {
vec![(phi + t0, 2)]
} else {
vec![(phi + t0, 1), (phi - t0, 1)]
}
}
pub(crate) fn solve_trig(
a2: f64,
b2: f64,
c1: f64,
d1: f64,
e: f64,
tol: Tolerance,
) -> Vec<(f64, u32)> {
let q = [
a2 + c1 + e,
4.0 * b2 + 2.0 * d1,
-6.0 * a2 + 2.0 * e,
-4.0 * b2 + 2.0 * d1,
a2 - c1 + e,
];
let mut out: Vec<(f64, u32)> = crate::math::real_roots(&q, tol.confusion)
.into_iter()
.map(|r| (2.0 * r.value.atan(), r.multiplicity))
.collect();
out.push((std::f64::consts::PI, 1));
out.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap());
let mut deduped: Vec<(f64, u32)> = Vec::with_capacity(out.len());
for (t, m) in out {
match deduped.last_mut() {
Some((last_t, last_m)) if (t - *last_t).abs() <= tol.parametric * (1.0 + t.abs()) => {
*last_m = (*last_m).max(m);
}
_ => deduped.push((t, m)),
}
}
deduped
}
#[derive(Debug, Clone, PartialEq)]
pub enum ConicSurfaceIntersection {
Hits(Vec<ConicSurfaceHit>),
Coincident,
Empty,
}
pub(crate) fn verify_conic_hits<C, S>(
curve: &C,
surface: &S,
candidates: Vec<(f64, u32)>,
tol: Tolerance,
) -> Vec<ConicSurfaceHit>
where
C: ParametricCurve3D,
S: ParametricSurface,
{
candidates
.into_iter()
.filter_map(|(t, multiplicity)| {
let p = curve.eval_point(t);
let coord = p.x.abs().max(p.y.abs()).max(p.z.abs());
let tol = Tolerance {
confusion: tol.confusion.max(16.0 * f64::EPSILON * coord),
..tol
};
if surface.contains(p, tol) {
Some(ConicSurfaceHit {
parameter: t,
point: p,
multiplicity,
})
} else {
None
}
})
.collect()
}
pub(crate) fn conic_lies_on<C, S>(curve: &C, surface: &S, params: [f64; 5], tol: Tolerance) -> bool
where
C: ParametricCurve3D,
S: ParametricSurface,
{
params
.iter()
.all(|&t| surface.contains(curve.eval_point(t), tol))
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub(crate) enum QuadraticSolution {
Two(f64, f64),
One(f64),
Linear(f64),
Degenerate,
Empty,
}
impl QuadraticSolution {
pub(crate) fn shifted(self, t0: f64) -> Self {
match self {
Self::Two(t1, t2) => Self::Two(t0 + t1, t0 + t2),
Self::One(t) => Self::One(t0 + t),
Self::Linear(t) => Self::Linear(t0 + t),
other => other,
}
}
pub(crate) fn touching_at(self, vertex: f64, touches: impl Fn(f64) -> bool) -> Self {
match self {
Self::Two(..) | Self::Empty if vertex.is_finite() && touches(vertex) => {
Self::One(vertex)
}
other => other,
}
}
}
pub(crate) fn roots_with_tangency(
poly: &[f64],
tol: Tolerance,
touches: impl Fn(f64) -> bool,
) -> Vec<RealRoot> {
let roots = real_roots(poly, tol.confusion);
let mut deriv = [0.0; 4];
let n = poly.len().min(5);
for (i, (d, &c)) in deriv
.iter_mut()
.zip(poly.iter().take(n).skip(1))
.enumerate()
{
*d = (i + 1) as f64 * c;
}
let crits = real_roots(&deriv[..n.saturating_sub(1)], tol.confusion);
let mut items: Vec<(f64, u32, u8)> = roots
.iter()
.map(|r| (r.value, r.multiplicity, 0))
.chain(crits.iter().map(|c| {
let kind = if touches(c.value) { 1 } else { 2 };
(c.value, c.multiplicity, kind)
}))
.collect();
if !items.iter().any(|it| it.2 == 1) {
return roots;
}
items.sort_by(|a, b| a.0.total_cmp(&b.0));
let mut out = Vec::with_capacity(roots.len() + 2);
let mut k = 0;
while k < items.len() {
if items[k].2 == 2 {
k += 1;
continue;
}
let start = k;
while k + 1 < items.len() && items[k + 1].2 != 2 && (items[k].2 == 1 || items[k + 1].2 == 1)
{
k += 1;
}
let cluster = &items[start..=k];
let touching: Vec<&(f64, u32, u8)> = cluster.iter().filter(|it| it.2 == 1).collect();
match touching.as_slice() {
[] => out.push(RealRoot {
value: cluster[0].0,
multiplicity: cluster[0].1,
}),
[one] => out.push(RealRoot {
value: one.0,
multiplicity: 1 + one.1,
}),
many => out.push(RealRoot {
value: 0.5 * (cluster[0].0 + cluster[cluster.len() - 1].0),
multiplicity: 1 + many.iter().map(|it| it.1).sum::<u32>(),
}),
}
k += 1;
}
out
}
pub(crate) fn quadratic_centre(a: f64, b: f64, tol: Tolerance) -> f64 {
let t0 = -b / (2.0 * a);
if a.abs() > tol.angular && t0.is_finite() {
t0
} else {
0.0
}
}
pub(crate) fn solve_quadratic(a: f64, b: f64, c: f64, tol: Tolerance) -> QuadraticSolution {
if a.abs() <= tol.angular {
if b.abs() <= tol.confusion {
return QuadraticSolution::Degenerate;
}
return QuadraticSolution::Linear(-c / b);
}
let disc = b * b - 4.0 * a * c;
let mid = -b / (2.0 * a);
let sep = disc.abs().sqrt() / (2.0 * a.abs());
let noise = (4.0 * f64::EPSILON * (b * b + 4.0 * (a * c).abs())).sqrt() / (2.0 * a.abs());
if sep <= tol.confusion * (1.0 + mid.abs()) + noise {
QuadraticSolution::One(mid)
} else if disc < 0.0 {
QuadraticSolution::Empty
} else {
let q = -0.5 * (b + disc.sqrt().copysign(b));
let (r1, r2) = (q / a, c / q);
QuadraticSolution::Two(r1.min(r2), r1.max(r2))
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CurveCurveExtremum {
pub first_param: f64,
pub second_param: f64,
pub distance: f64,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CurveCurveHit {
pub first_param: f64,
pub point: Point3D,
pub second_param: f64,
}
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)]
enum ParamDomain {
Periodic { start: f64, period: f64 },
Interval(f64, f64),
}
impl ParamDomain {
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,
}
}
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),
}
}
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 = curve.eval_point(u0);
let speed = curve.eval_derivative(u0, 1).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 line = Straight {
u0,
origin: curve.eval_point(u0),
dir: curve.eval_derivative(u0, 1),
};
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) -> Vec<CurveCurveExtremum>
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
let (da, db) = (ParamDomain::of(a), ParamDomain::of(b));
let mut out = match (Straight::of(a, da), Straight::of(b, db)) {
(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(),
_ => box_minima(a, da, b, db, tol),
};
out.sort_by(|x, y| x.distance.total_cmp(&y.distance));
out
}
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 = aa * bb - ab * ab;
if denom > 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 r = perp(curve.eval_point(t) - line.origin);
let (c1, c2) = (curve.eval_derivative(t, 1), curve.eval_derivative(t, 2));
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_minima<A, B>(
a: &A,
da: ParamDomain,
b: &B,
db: ParamDomain,
tol: Tolerance,
) -> Vec<CurveCurveExtremum>
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 local = |s: f64, t: f64| {
let (a1, a2) = (a.eval_derivative(s, 1), a.eval_derivative(s, 2));
let (b1, b2) = (b.eval_derivative(t, 1), b.eval_derivative(t, 2));
let r = a.eval_point(s) - b.eval_point(t);
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 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]));
}
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 r = p - b.eval_point(t);
let (b1, b2) = (b.eval_derivative(t, 1), b.eval_derivative(t, 2));
(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_derivative(u0, 1), curve.eval_derivative(u0, 2));
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) -> Vec<CurveCurveHit>
where
A: ParametricCurve3D,
B: ParametricCurve3D,
{
let hits: Vec<CurveCurveHit> = curve_curve_extrema(a, b, tol)
.into_iter()
.filter(|e| e.distance <= tol.confusion)
.map(|e| CurveCurveHit {
first_param: e.first_param,
point: a.eval_point(e.first_param),
second_param: e.second_param,
})
.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
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CurveSurfaceHit {
pub curve_param: f64,
pub surface_params: (f64, f64),
pub point: Point3D,
}
pub fn intersect_curve_surface<C, S>(curve: &C, surface: &S, tol: Tolerance) -> Vec<CurveSurfaceHit>
where
C: ParametricCurve3D,
S: ParametricSurface,
{
let (t0, t1) = curve.bounds();
const WINDOW: f64 = 10.0;
let (lo, hi) = (
if t0.is_finite() { t0 } else { -WINDOW },
if t1.is_finite() { t1 } else { WINDOW },
);
const SEEDS: usize = 128;
const KEEP: usize = 24;
let mut samples: Vec<(f64, f64)> = (0..SEEDS)
.map(|i| {
let t = lo + (hi - lo) * i as f64 / (SEEDS - 1) as f64;
(t, surface.project_point(curve.eval_point(t), tol).distance)
})
.collect();
samples.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap());
let mut hits: Vec<CurveSurfaceHit> = Vec::new();
for (t, _) in samples.into_iter().take(KEEP) {
let proj = surface.project_point(curve.eval_point(t), tol);
let f = |x: [f64; 3]| {
let d = curve.eval_point(x[0]) - surface.eval_point(x[1], x[2]);
[d.x, d.y, d.z]
};
let jac = |x: [f64; 3]| {
let ct = curve.eval_derivative(x[0], 1);
let su = surface.eval_derivative(x[1], x[2], 1, 0);
let sv = surface.eval_derivative(x[1], x[2], 0, 1);
[
[ct.x, -su.x, -sv.x],
[ct.y, -su.y, -sv.y],
[ct.z, -su.z, -sv.z],
]
};
if let Some([rt, ru, rv]) = newton_3d(f, jac, [t, proj.u, proj.v], tol.confusion, 50) {
let p = curve.eval_point(rt);
let q = surface.eval_point(ru, rv);
let merge_gap = tol.confusion.sqrt();
if p.distance(q) <= tol.confusion
&& !hits
.iter()
.any(|h: &CurveSurfaceHit| h.point.distance(p) <= merge_gap)
{
hits.push(CurveSurfaceHit {
curve_param: rt,
surface_params: (ru, rv),
point: p,
});
}
}
}
hits.sort_by(|a, b| a.curve_param.partial_cmp(&b.curve_param).unwrap());
hits
}
pub fn marching_intersection<A, B>(a: &A, b: &B, tol: Tolerance) -> Vec<BSplineCurve3D>
where
A: ParametricSurface,
B: ParametricSurface,
{
let mut seeds: Vec<(Point3D, f64)> = Vec::new();
{
let (u0, u1) = seed_window(a.u_bounds());
let (v0, v1) = seed_window(a.v_bounds());
const NU: usize = 24;
const NV: usize = 24;
for j in 0..=NV {
for i in 0..=NU {
let (u, v) = (
u0 + (u1 - u0) * i as f64 / NU as f64,
v0 + (v1 - v0) * j as f64 / NV as f64,
);
let p = a.eval_point(u, v);
let d = b.project_point(p, tol).distance;
if d <= 100.0 * tol.confusion {
seeds.push((p, d));
}
}
}
}
{
let (u0, u1) = seed_window(b.u_bounds());
let (v0, v1) = seed_window(b.v_bounds());
const NU: usize = 24;
const NV: usize = 24;
for j in 0..=NV {
for i in 0..=NU {
let (u, v) = (
u0 + (u1 - u0) * i as f64 / NU as f64,
v0 + (v1 - v0) * j as f64 / NV as f64,
);
let p = b.eval_point(u, v);
let d = a.project_point(p, tol).distance;
if d <= 100.0 * tol.confusion {
seeds.push((p, d));
}
}
}
}
let mut leaders: Vec<Point3D> = Vec::new();
if !seeds.is_empty() {
let mut lo = seeds[0].0;
let mut hi = seeds[0].0;
for &(p, _) in &seeds[1..] {
lo = Point3D::new(lo.x.min(p.x), lo.y.min(p.y), lo.z.min(p.z));
hi = Point3D::new(hi.x.max(p.x), hi.y.max(p.y), hi.z.max(p.z));
}
let radius = (0.05 * lo.distance(hi)).max(100.0 * tol.confusion);
let mut parent: Vec<usize> = (0..seeds.len()).collect();
fn find(parent: &mut [usize], mut x: usize) -> usize {
while parent[x] != x {
parent[x] = parent[parent[x]];
x = parent[x];
}
x
}
for i in 0..seeds.len() {
for j in (i + 1)..seeds.len() {
if seeds[i].0.distance(seeds[j].0) <= radius {
let (ri, rj) = (find(&mut parent, i), find(&mut parent, j));
parent[ri] = rj;
}
}
}
let mut best: std::collections::HashMap<usize, (f64, Point3D)> =
std::collections::HashMap::new();
for (i, &(p, d)) in seeds.iter().enumerate() {
let r = find(&mut parent, i);
best.entry(r)
.and_modify(|e| {
if d < e.0 {
*e = (d, p);
}
})
.or_insert((d, p));
}
let mut ordered: Vec<(f64, Point3D)> = best.into_values().collect();
ordered.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap());
leaders = ordered.into_iter().map(|(_, p)| p).collect();
}
let mut curves: Vec<BSplineCurve3D> = Vec::new();
let suppress = 1000.0 * tol.confusion;
let mut step = 0.02f64;
for seed in leaders {
if curves
.iter()
.any(|c| c.project_point(seed, tol).distance <= suppress)
{
continue;
}
if let Some(loop_pts) = trace_loop(a, b, seed, &mut step, tol)
&& loop_pts.len() >= 4
{
let mut closed = loop_pts;
closed.push(closed[0]);
let degree = 3.min(closed.len() - 1);
if let Ok(fit) =
BSplineCurve3D::interpolate(&closed, degree, InterpParametrization::Centripetal)
{
curves.push(fit);
}
}
}
curves
}
fn trace_loop<A, B>(
a: &A,
b: &B,
seed: Point3D,
step: &mut f64,
tol: Tolerance,
) -> Option<Vec<Point3D>>
where
A: ParametricSurface,
B: ParametricSurface,
{
let normal_at = |s: &dyn ParametricSurface, u: f64, v: f64| {
let su = s.eval_derivative(u, v, 1, 0);
let sv = s.eval_derivative(u, v, 0, 1);
let n = su.cross(sv);
let scale = su.magnitude() * sv.magnitude();
if n.magnitude() <= tol.angular * scale.max(f64::MIN_POSITIVE) {
None
} else {
Some(n * (1.0 / n.magnitude()))
}
};
let mut fwd: Vec<Point3D> = vec![seed];
let mut bwd: Vec<Point3D> = Vec::new();
for (chain, dir) in [(&mut fwd, 1.0), (&mut bwd, -1.0)] {
let mut p = seed;
let mut h = *step;
for _ in 0..2000 {
let qa = a.project_point(p, tol);
let qb = b.project_point(p, tol);
let (Some(na), Some(nb)) = (normal_at(a, qa.u, qa.v), normal_at(b, qb.u, qb.v)) else {
break;
};
let t = na.cross(nb);
if t.magnitude() <= tol.angular {
break;
}
let t = t * (dir / t.magnitude());
let mut accepted = false;
for _ in 0..10 {
let q = correct(a, b, p + t * h, tol);
match q {
Some(q) if b.project_point(q, tol).distance <= 10.0 * tol.confusion => {
p = q;
h = (h * 1.25).min(0.1);
accepted = true;
break;
}
_ => {
h *= 0.5;
if h < tol.confusion {
break;
}
}
}
}
if !accepted {
break;
}
if chain.len() > 10 && p.distance(chain[0]) <= 10.0 * tol.confusion {
chain.push(chain[0]);
break;
}
let ra = a.project_point(p, tol);
let rb = b.project_point(p, tol);
if off_bounds(a, ra.u, ra.v) || off_bounds(b, rb.u, rb.v) {
chain.push(p);
break;
}
chain.push(p);
}
}
*step = (*step).clamp(tol.confusion, 1.0);
bwd.reverse();
let mut pts = bwd;
pts.extend(fwd.into_iter().skip(1));
if pts.len() < 4 { None } else { Some(pts) }
}
fn correct<A, B>(a: &A, b: &B, p: Point3D, tol: Tolerance) -> Option<Point3D>
where
A: ParametricSurface,
B: ParametricSurface,
{
let mut q = p;
for _ in 0..8 {
let pa = a.project_point(q, tol);
let qa = a.eval_point(pa.u, pa.v);
let pb = b.project_point(qa, tol);
let qb = b.eval_point(pb.u, pb.v);
if qb.distance(q) <= tol.confusion {
return Some(qb);
}
q = qb;
}
let residual = a.project_point(q, tol).distance + b.project_point(q, tol).distance;
if residual <= 10.0 * tol.confusion {
Some(q)
} else {
None
}
}
fn seed_window((lo, hi): (f64, f64)) -> (f64, f64) {
const WINDOW: f64 = 10.0;
(
if lo.is_finite() { lo } else { -WINDOW },
if hi.is_finite() { hi } else { WINDOW },
)
}
fn off_bounds<S: ParametricSurface + ?Sized>(s: &S, u: f64, v: f64) -> bool {
let (u0, u1) = s.u_bounds();
let (v0, v1) = s.v_bounds();
(s.u_period().is_none() && (u < u0 || u > u1)) || (s.v_period().is_none() && (v < v0 || v > v1))
}
#[cfg(test)]
mod tests {
use super::*;
use crate::Tolerance;
use crate::{Line3D, LineCircle3DIntersection, Point3D, Sphere, Vector3D};
#[test]
fn test_solve_quadratic_two_roots_ordered() {
assert_eq!(
solve_quadratic(1.0, -5.0, 6.0, Tolerance::DEFAULT),
QuadraticSolution::Two(2.0, 3.0)
);
}
#[test]
fn test_solve_quadratic_tangent_band() {
assert_eq!(
solve_quadratic(1.0, -4.0, 4.0, Tolerance::DEFAULT),
QuadraticSolution::One(2.0)
);
assert_eq!(
solve_quadratic(1.0, 0.0, 1e-14, Tolerance::DEFAULT),
QuadraticSolution::One(0.0)
);
assert_eq!(
solve_quadratic(1.0, 0.0, 1e-9, Tolerance::DEFAULT),
QuadraticSolution::Empty
);
assert_eq!(
solve_quadratic(1.0, 0.0, 1.0, Tolerance::DEFAULT),
QuadraticSolution::Empty
);
}
#[test]
fn test_solve_quadratic_rounded_double_root_is_tangent() {
let tight = Tolerance {
confusion: 1e-12,
..Tolerance::DEFAULT
};
for (a, r) in [(7.0, 2.3), (11.0, 0.7)] {
match solve_quadratic(a, -2.0 * a * r, a * r * r, tight) {
QuadraticSolution::One(t) => assert!((t - r).abs() < 1e-14),
other => panic!("expected a double root near {r}, got {other:?}"),
}
}
match solve_quadratic(1.0, -2.0, 1.0 - 1e-12, tight) {
QuadraticSolution::Two(t1, t2) => {
assert!((t1 - (1.0 - 1e-6)).abs() < 1e-9);
assert!((t2 - (1.0 + 1e-6)).abs() < 1e-9);
}
other => panic!("expected two roots, got {other:?}"),
}
}
#[test]
fn test_solve_quadratic_widely_separated_roots_are_accurate() {
match solve_quadratic(1.0, -1e8, 1.0, Tolerance::DEFAULT) {
QuadraticSolution::Two(t1, t2) => {
assert!((t1 - 1e-8).abs() < 1e-22);
assert!((t2 - 1e8).abs() < 1e-7);
}
other => panic!("expected two roots, got {other:?}"),
}
}
#[test]
fn test_solve_quadratic_degenerate_arms() {
assert_eq!(
solve_quadratic(0.0, 2.0, 4.0, Tolerance::DEFAULT),
QuadraticSolution::Linear(-2.0)
);
assert_eq!(
solve_quadratic(0.0, 0.0, 1.0, Tolerance::DEFAULT),
QuadraticSolution::Degenerate
);
}
#[test]
fn test_generic_line_sphere_matches_analytic() {
let tol = Tolerance::DEFAULT;
let line = Line3D::new(Point3D::ORIGIN, Vector3D::X).unwrap();
let sphere = Sphere::new(Point3D::ORIGIN, 2.0).unwrap();
let hits = intersect_curve_surface(&line, &sphere, tol);
assert_eq!(hits.len(), 2);
assert!((hits[0].curve_param + 2.0).abs() < 1e-9);
assert!((hits[1].curve_param - 2.0).abs() < 1e-9);
match line.intersect_sphere(&sphere, tol) {
LineQuadricIntersection::TwoPoints((t1, _), (t2, _)) => {
assert!((hits[0].curve_param - t1).abs() < 1e-9);
assert!((hits[1].curve_param - t2).abs() < 1e-9);
}
_ => panic!("analytic expected two points"),
}
}
#[test]
fn test_generic_line_sphere_miss() {
let tol = Tolerance::DEFAULT;
let line = Line3D::new(Point3D::new(0.0, 0.0, 3.0), Vector3D::X).unwrap();
let sphere = Sphere::new(Point3D::ORIGIN, 2.0).unwrap();
assert!(intersect_curve_surface(&line, &sphere, tol).is_empty());
}
#[test]
fn test_generic_segment_plane_hit() {
use crate::{BSplineCurve3D, Plane};
let tol = Tolerance::DEFAULT;
let poles = vec![Point3D::new(0.0, 0.0, 0.0), Point3D::new(2.0, 0.0, 0.0)];
let curve = BSplineCurve3D::new(1, poles, vec![0.0, 1.0], vec![2, 2], false).unwrap();
let plane = Plane::new(Point3D::new(1.0, 0.0, 0.0), Vector3D::X).unwrap();
let hits = intersect_curve_surface(&curve, &plane, tol);
assert_eq!(hits.len(), 1);
assert!((hits[0].curve_param - 0.5).abs() < 1e-9);
assert_eq!(hits[0].point, Point3D::new(1.0, 0.0, 0.0));
}
#[test]
fn test_marching_plane_plane_line() {
use crate::Plane;
let tol = Tolerance::DEFAULT;
let xy = Plane::new(Point3D::ORIGIN, Vector3D::Z).unwrap();
let zy = Plane::new(Point3D::ORIGIN, Vector3D::X).unwrap();
let curves = marching_intersection(&xy, &zy, tol);
assert_eq!(curves.len(), 1);
for t in [0.0, 0.25, 0.5, 0.75, 1.0] {
let p = curves[0].eval_point(t);
assert!(p.x.abs() < 1e-6, "{p:?}");
assert!(p.z.abs() < 1e-6, "{p:?}");
}
}
#[test]
fn test_marching_plane_sphere_circle() {
use crate::{Plane, Sphere};
let tol = Tolerance::DEFAULT;
let plane = Plane::new(Point3D::ORIGIN, Vector3D::Z).unwrap();
let sphere = Sphere::new(Point3D::ORIGIN, 2.0).unwrap();
let curves = marching_intersection(&plane, &sphere, tol);
assert_eq!(curves.len(), 1);
for t in [0.0, 0.2, 0.4, 0.6, 0.8, 1.0] {
let p = curves[0].eval_point(t);
assert!(plane.project_point(p, tol).distance < 1e-3, "{p:?}");
assert!(sphere.project_point(p, tol).distance < 1e-3, "{p:?}");
}
}
#[test]
fn test_marching_miss_is_empty() {
use crate::{Plane, Sphere};
let tol = Tolerance::DEFAULT;
let plane = Plane::new(Point3D::new(0.0, 0.0, 5.0), Vector3D::Z).unwrap();
let sphere = Sphere::new(Point3D::ORIGIN, 2.0).unwrap();
assert!(marching_intersection(&plane, &sphere, tol).is_empty());
}
#[test]
fn test_curve_curve_extrema_skew_matches_analytic() {
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_line(&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!("analytic expected skew"),
}
}
struct Trimmed<C>(C, f64, f64);
impl<C: ParametricCurve3D> ParametricCurve3D for Trimmed<C> {
fn eval_point(&self, u: f64) -> Point3D {
self.0.eval_point(u)
}
fn eval_derivative(&self, u: f64, order: u32) -> Vector3D {
self.0.eval_derivative(u, order)
}
fn bounds(&self) -> (f64, f64) {
(self.1, self.2)
}
fn period(&self) -> Option<f64> {
None
}
}
fn segment(from: Point3D, to: Point3D) -> Trimmed<Line3D> {
let line = Line3D::new(from, to - from).unwrap();
Trimmed(line, 0.0, from.distance(to))
}
fn arc(center: Point3D, radius: f64, lo: f64, hi: f64) -> Trimmed<crate::Circle3D> {
Trimmed(
crate::Circle3D::new(center, Vector3D::Z, radius).unwrap(),
lo,
hi,
)
}
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_line(&b, tol) {
LineLine3DIntersection::Parallel(d) => assert!((ext[0].distance - d).abs() < 1e-12),
_ => panic!("analytic 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(&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!("analytic expected two points"),
}
}
}