use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
use ogeom_geom::{BSpline2d, BSplineCurve, Surface, SurfaceGeometry};
use ogeom_math::Point2;
use crate::march::Traced;
#[derive(Debug, Clone, PartialEq)]
pub struct IntersectionCurve {
pub curve: BSplineCurve,
pub on_a: BSpline2d,
pub on_b: BSpline2d,
pub fit_error: f64,
pub met: bool,
pub closed: bool,
}
pub fn approximate_branch(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
branch: &Traced,
tolerance: f64,
tol: Tolerances,
) -> OgeomResult<IntersectionCurve> {
if branch.points.len() < 2 {
ogeom_bail!(
Construction,
"a branch of {} points is not a curve",
branch.points.len()
);
}
let mut points: Vec<ogeom_math::Point> = Vec::with_capacity(branch.points.len());
let mut kept_a = Vec::with_capacity(branch.on_a.len());
let mut kept_b = Vec::with_capacity(branch.on_b.len());
let agrees = |i: usize, p: &ogeom_math::Point| -> bool {
let limit = tolerance.max(tol.confusion());
let (ua, va) = branch.on_a[i];
let (ub, vb) = branch.on_b[i];
a.point_at(ua, va, tol)
.is_ok_and(|q| q.distance(*p) <= limit)
&& b.point_at(ub, vb, tol)
.is_ok_and(|q| q.distance(*p) <= limit)
};
for (i, p) in branch.points.iter().enumerate() {
let end = i == 0 || i + 1 == branch.points.len();
if let Some(last) = points.last()
&& last.distance(*p) <= tol.confusion() * 10.0
&& i + 1 != branch.points.len()
{
continue;
}
if !end && !agrees(i, p) {
continue;
}
points.push(*p);
kept_a.push(branch.on_a[i]);
kept_b.push(branch.on_b[i]);
}
if points.len() < 2 {
ogeom_bail!(Construction, "a branch of coincident points is not a curve");
}
let unwrapped_a = unwrap_periodic(a, &kept_a, tol);
let unwrapped_b = unwrap_periodic(b, &kept_b, tol);
let winds_periodically = |surface: &SurfaceGeometry, image: &[Point2]| {
let (first, last) = (image[0], image[image.len() - 1]);
((last.x - first.x).abs() <= tol.parametric() || surface.is_periodic_u())
&& ((last.y - first.y).abs() <= tol.parametric() || surface.is_periodic_v())
};
let (space, on_a, on_b) = if branch.closed() {
let closed = ogeom_geom::fit::fit_points_joint_closed(
&points,
&unwrapped_a,
&unwrapped_b,
3,
tolerance,
tol,
)?;
if !closed.0.met
&& winds_periodically(a, &unwrapped_a)
&& winds_periodically(b, &unwrapped_b)
{
let winding = ogeom_geom::fit::fit_points_joint_winding(
&points,
&unwrapped_a,
&unwrapped_b,
3,
tolerance,
tol,
)?;
if winding.0.error < closed.0.error {
winding
} else {
closed
}
} else {
closed
}
} else {
ogeom_geom::fit::fit_points_joint(&points, &unwrapped_a, &unwrapped_b, 3, tolerance, tol)?
};
let lifted = lift_error(a, b, &on_a, &on_b, &space.curve, tol);
let traced = trace_error(&space.curve, &points, space.error, tol);
let charted =
space_error(a, &on_a, space.error, tol).max(space_error(b, &on_b, space.error, tol));
let fit_error = traced
.max(lifted.along)
.max(lifted.across.min(charted.max(space.error)));
Ok(IntersectionCurve {
fit_error,
met: space.met,
curve: space.curve,
on_a,
on_b,
closed: branch.closed(),
})
}
struct Lifted {
along: f64,
across: f64,
}
fn lift_error(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
on_a: &BSpline2d,
on_b: &BSpline2d,
curve: &BSplineCurve,
tol: Tolerances,
) -> Lifted {
use ogeom_geom::{Curve2d as _, Curve3d as _};
let (lo, hi) = curve.knots().domain();
let spans = curve.knots().distinct().len().saturating_sub(1).max(1);
let stations = (4 * spans).max(200);
let mut out = Lifted {
along: 0.0,
across: 0.0,
};
for k in 0..=stations {
#[allow(clippy::cast_precision_loss)]
let t = lo + (hi - lo) * k as f64 / stations as f64;
let Ok(on) = curve.point_at(t, tol) else {
continue;
};
let mut gap = 0.0_f64;
let mut normals = Vec::with_capacity(2);
for (surface, pcurve) in [(a, on_a), (b, on_b)] {
let Ok(at) = pcurve.point_at(t, tol) else {
continue;
};
let Ok(lifted) = surface.point_at(at.x, at.y, tol) else {
continue;
};
gap = gap.max(lifted.distance(on));
if let Ok(normal) = surface.normal_at(at.x, at.y, tol) {
normals.push(normal.vector());
}
}
out.along = out.along.max(gap);
if let [na, nb] = normals[..] {
let sine = na.cross(nb).magnitude().max(tol.angular());
out.across = out.across.max(gap / sine);
}
}
out
}
fn trace_error(
curve: &BSplineCurve,
samples: &[ogeom_math::Point],
bound: f64,
tol: Tolerances,
) -> f64 {
use ogeom_geom::Curve3d as _;
let (lo, hi) = curve.knots().domain();
let spans = curve.knots().distinct().len().saturating_sub(1).max(1);
let count = (4 * spans).max(2 * samples.len()).max(200);
#[allow(clippy::cast_precision_loss)]
let at = |k: usize| lo + (hi - lo) * k as f64 / count as f64;
let mut stations: Option<Vec<(f64, ogeom_math::Point)>> = None;
let foot = |p: ogeom_math::Point, mut t: f64| -> (f64, f64) {
let mut best = (t, f64::INFINITY);
for _ in 0..8 {
let (Ok(q), Ok(d)) = (curve.point_at(t, tol), curve.d1_at(t, tol)) else {
break;
};
let gap = q.distance(p);
if gap < best.1 {
best = (t, gap);
}
let speed = d.dot(d);
if speed <= f64::MIN_POSITIVE {
break;
}
let next = (t + (p - q).dot(d) / speed).clamp(lo, hi);
if (next - t).abs() <= (hi - lo) * 1e-12 {
break;
}
t = next;
}
if let Ok(q) = curve.point_at(t, tol)
&& q.distance(p) < best.1
{
best = (t, q.distance(p));
}
best
};
let mut t = lo;
let mut worst = 0.0_f64;
for p in samples {
let mut found = foot(*p, t);
if found.1 > bound {
let stations = stations.get_or_insert_with(|| {
(0..=count)
.filter_map(|k| curve.point_at(at(k), tol).ok().map(|q| (at(k), q)))
.collect()
});
if let Some(k) = (0..stations.len()).min_by(|&x, &y| {
stations[x]
.1
.distance(*p)
.total_cmp(&stations[y].1.distance(*p))
}) {
let gap = |u: f64| {
curve
.point_at(u, tol)
.map_or(f64::INFINITY, |q| q.distance(*p))
};
let (from, to) = (
stations[k.saturating_sub(2)].0,
stations[(k + 2).min(stations.len() - 1)].0,
);
const FINE: u32 = 256;
let h = (to - from) / f64::from(FINE);
let start = (0..=FINE)
.map(|j| from + h * f64::from(j))
.min_by(|&x, &y| gap(x).total_cmp(&gap(y)))
.unwrap_or(from);
let (mut a, mut b) = ((start - h).max(lo), (start + h).min(hi));
let ratio = 0.5 * (5.0_f64.sqrt() - 1.0);
for _ in 0..60 {
let (x, y) = (b - ratio * (b - a), a + ratio * (b - a));
if gap(x) <= gap(y) {
b = y;
} else {
a = x;
}
}
let again = foot(*p, 0.5 * (a + b));
if again.1 < found.1 {
found = again;
}
}
}
t = found.0;
worst = worst.max(found.1.min(bound));
}
worst
}
fn space_error(
surface: &SurfaceGeometry,
pcurve: &BSpline2d,
parameter_error: f64,
tol: Tolerances,
) -> f64 {
use ogeom_geom::Curve2d;
let (lo, hi) = pcurve.domain();
let mut worst = 0.0_f64;
for i in 0..=16 {
#[allow(clippy::cast_precision_loss)]
let u = lo + (hi - lo) * f64::from(i) / 16.0;
let Ok(at) = pcurve.point_at(u, tol) else {
continue;
};
let Ok((du, dv)) = surface.d1_at(at.x, at.y, tol) else {
continue;
};
let stretch = du.magnitude().max(dv.magnitude());
worst = worst.max(parameter_error * stretch);
}
worst
}
fn unwrap_periodic(
surface: &SurfaceGeometry,
samples: &[(f64, f64)],
tol: Tolerances,
) -> Vec<Point2> {
let ((ua, ub), (va, vb)) = surface.domain();
let u_period = if surface.is_periodic_u() || surface.is_closed_u(tol) {
Some(ub - ua)
} else {
None
};
let v_period = if surface.is_periodic_v() || surface.is_closed_v(tol) {
Some(vb - va)
} else {
None
};
let fold = |previous: f64, next: f64, period: Option<f64>| match period {
None => next,
Some(period) => {
let mut candidate = next;
while candidate - previous > period * 0.5 {
candidate -= period;
}
while previous - candidate > period * 0.5 {
candidate += period;
}
candidate
}
};
let mut out = Vec::with_capacity(samples.len());
let mut at = Point2::new(samples[0].0, samples[0].1);
out.push(at);
for sample in &samples[1..] {
at = Point2::new(
fold(at.x, sample.0, u_period),
fold(at.y, sample.1, v_period),
);
out.push(at);
}
out
}
#[cfg(test)]
#[allow(clippy::unwrap_used)]
mod tests {
use super::*;
use crate::march::{Marching, branches};
use ogeom_geom::{Curve2d, Curve3d, CylinderSurface, PlaneSurface, SphereSurface};
use ogeom_math::{Cylinder, Direction, Frame, Plane, Point, Sphere, Vector};
const T: Tolerances = Tolerances::millimetres();
fn sphere(radius: f64) -> SurfaceGeometry {
SphereSurface::new(Sphere::centred(Point::ORIGIN, radius, T).unwrap()).into()
}
fn cylinder(radius: f64) -> SurfaceGeometry {
CylinderSurface::new(Cylinder::new(Frame::WORLD, radius, T).unwrap(), (-4.0, 4.0))
.unwrap()
.into()
}
fn plane(origin: Point, normal: Vector) -> SurfaceGeometry {
PlaneSurface::over(
Plane::through(origin, Direction::new(normal, T).unwrap()),
(-6.0, 6.0),
(-6.0, 6.0),
)
.unwrap()
.into()
}
fn options() -> Marching {
Marching {
chord: 1e-5,
..Marching::default()
}
}
fn fitted_deviation(a: &SurfaceGeometry, b: &SurfaceGeometry, curve: &BSplineCurve) -> f64 {
let off = |surface: &SurfaceGeometry, p: Point| match surface {
SurfaceGeometry::Plane(x) => x.plane().distance_to(p),
SurfaceGeometry::Sphere(x) => x.sphere().distance_to(p),
SurfaceGeometry::Cylinder(x) => x.cylinder().distance_to(p),
_ => 0.0,
};
let (lo, hi) = curve.knots().domain();
let mut worst = 0.0_f64;
for i in 0..=800 {
#[allow(clippy::cast_precision_loss)]
let u = lo + (hi - lo) * f64::from(i) / 800.0;
if let Ok(p) = curve.point_at(u, T) {
worst = worst.max(off(a, p).abs().max(off(b, p).abs()));
}
}
worst
}
#[test]
fn a_fitted_branch_lies_on_both_surfaces_to_the_stated_total() {
let a = sphere(3.0);
let b = cylinder(1.5);
let found = branches(&a, &b, options(), T).unwrap();
assert_eq!(found.len(), 2);
for branch in &found {
let fitted = approximate_branch(&a, &b, branch, 1e-4, T).unwrap();
assert!(fitted.met, "fit error {:e}", fitted.fit_error);
assert!(fitted.closed);
let off = fitted_deviation(&a, &b, &fitted.curve);
assert!(
off <= 1e-4 + 1e-5,
"the fitted curve is {off:e} off the surfaces"
);
assert!(
fitted.curve.control_points().len() * 4 < branch.points.len(),
"{} control points for {} samples",
fitted.curve.control_points().len(),
branch.points.len()
);
}
}
#[test]
fn the_pcurves_lift_back_onto_the_curve() {
let a = sphere(3.0);
let b = cylinder(1.5);
let found = branches(&a, &b, options(), T).unwrap();
let branch = &found[0];
let fitted = approximate_branch(&a, &b, branch, 1e-4, T).unwrap();
for (surface, pcurve) in [(&a, &fitted.on_a), (&b, &fitted.on_b)] {
let (lo, hi) = pcurve.domain();
for i in 0..=200 {
#[allow(clippy::cast_precision_loss)]
let u = lo + (hi - lo) * f64::from(i) / 200.0;
let at = pcurve.point_at(u, T).unwrap();
let lifted = surface.point_at(at.x, at.y, T).unwrap();
let off = match (surface as &SurfaceGeometry, &a, &b) {
_ if core::ptr::eq(surface, &a) => match &b {
SurfaceGeometry::Cylinder(c) => c.cylinder().distance_to(lifted),
_ => 0.0,
},
_ => match &a {
SurfaceGeometry::Sphere(s) => s.sphere().distance_to(lifted),
_ => 0.0,
},
};
assert!(
off.abs() < 5e-4,
"a lifted pcurve point is {off:e} off the intersection"
);
}
}
}
#[test]
fn a_branch_across_the_seam_gets_a_continuous_pcurve() {
let a = cylinder(2.0);
let b = plane(Point::ORIGIN, Vector::new(0.0, 0.4, 1.0));
let found = branches(&a, &b, options(), T).unwrap();
assert_eq!(found.len(), 1, "an oblique plane cuts one ellipse");
let fitted = approximate_branch(&a, &b, &found[0], 1e-4, T).unwrap();
let (lo, hi) = fitted.on_a.domain();
let mut previous = fitted.on_a.point_at(lo, T).unwrap();
for i in 1..=400 {
#[allow(clippy::cast_precision_loss)]
let u = lo + (hi - lo) * f64::from(i) / 400.0;
let at = fitted.on_a.point_at(u, T).unwrap();
assert!(
(at.x - previous.x).abs() < 1.0,
"the pcurve tears at the seam: {} to {}",
previous.x,
at.x
);
previous = at;
}
}
#[test]
fn a_loop_cut_at_a_converted_drum_s_seam_is_closed() {
let drum: SurfaceGeometry = cylinder(2.0).to_bspline(T).unwrap().into();
assert!(matches!(drum, SurfaceGeometry::BSpline(_)));
let cut = plane(Point::new(0.0, 0.0, 1.0), Vector::new(0.0, 0.2, 1.0));
let found = branches(&drum, &cut, options(), T).unwrap();
assert_eq!(found.len(), 1, "an oblique plane cuts one loop");
assert!(found[0].closed(), "the loop closes on the seam");
let fitted = approximate_branch(&drum, &cut, &found[0], 1e-4, T).unwrap();
assert!(fitted.closed);
assert!(
fitted.fit_error < 1e-3,
"the loop fits as one: {}",
fitted.fit_error
);
let (lo, hi) = fitted.on_a.domain();
let mut previous = fitted.on_a.point_at(lo, T).unwrap();
for i in 1..=400 {
let u = lo + (hi - lo) * f64::from(i) / 400.0;
let at = fitted.on_a.point_at(u, T).unwrap();
assert!(
(at.x - previous.x).abs() < 0.5,
"the chart image tears at the seam: {} to {}",
previous.x,
at.x
);
previous = at;
}
}
#[test]
fn a_section_across_a_wide_drum_states_its_measured_error() {
let radius = 23.6;
let wall = CylinderSurface::new(
Cylinder::new(Frame::WORLD, radius, T).unwrap(),
(-30.0, 30.0),
)
.unwrap();
let drum: SurfaceGeometry = SurfaceGeometry::from(wall).to_bspline(T).unwrap().into();
let bore = Cylinder::new(
Frame::new(
Point::new(0.0, 0.0, 3.0),
Direction::new(Vector::X, T).unwrap(),
Direction::new(Vector::Y, T).unwrap(),
T,
)
.unwrap(),
9.0,
T,
)
.unwrap();
let drill: SurfaceGeometry = CylinderSurface::new(bore, (-40.0, 40.0)).unwrap().into();
let marching = Marching {
chord: 1e-4,
..Marching::default()
};
let found = branches(&drum, &drill, marching, T).unwrap();
assert!(!found.is_empty());
for branch in &found {
let fitted = approximate_branch(&drum, &drill, branch, 1e-4, T).unwrap();
let (lo, hi) = fitted.curve.knots().domain();
let mut off = 0.0_f64;
for i in 0..=2000 {
let t = lo + (hi - lo) * f64::from(i) / 2000.0;
let p = fitted.curve.point_at(t, T).unwrap();
off = off.max(
wall.cylinder()
.distance_to(p)
.abs()
.max(bore.distance_to(p).abs()),
);
}
assert!(
fitted.fit_error + marching.chord >= off,
"states {:e} for a curve {off:e} off its surfaces",
fitted.fit_error
);
assert!(
fitted.fit_error <= 10.0 * off.max(marching.chord),
"states {:e} for a curve {off:e} off its surfaces",
fitted.fit_error
);
}
}
#[test]
fn what_cannot_be_fitted_is_refused() {
let a = sphere(1.0);
let b = plane(Point::ORIGIN, Vector::Z);
let found = branches(&a, &b, options(), T).unwrap();
assert!(approximate_branch(&a, &b, &found[0], 0.0, T).is_err());
assert!(approximate_branch(&a, &b, &found[0], -1.0, T).is_err());
let empty = Traced {
points: vec![],
on_a: vec![],
on_b: vec![],
stopped: crate::march::Stopped::Stalled,
};
assert!(approximate_branch(&a, &b, &empty, 1e-4, T).is_err());
}
}