use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
use ogeom_geom::{Surface, SurfaceGeometry};
use ogeom_math::{Point, Vector, solve};
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Marching {
pub chord: f64,
pub grid: usize,
pub max_points: usize,
}
impl Default for Marching {
fn default() -> Self {
Self {
chord: 1e-4,
grid: 24,
max_points: 20_000,
}
}
}
impl Marching {
pub fn validate(&self) -> OgeomResult<()> {
if !self.chord.is_finite() || self.chord <= 0.0 {
ogeom_bail!(Construction, "a chord of {} is not a distance", self.chord);
}
if self.grid < 2 {
ogeom_bail!(Construction, "a sampling grid needs at least two steps");
}
if self.max_points < 2 {
ogeom_bail!(Construction, "a branch needs at least two points");
}
Ok(())
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Contact {
pub on_a: (f64, f64),
pub on_b: (f64, f64),
pub point: Point,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Stopped {
Closed,
LeftTheDomain,
Stalled,
RanOut,
}
#[derive(Debug, Clone, PartialEq)]
pub struct Traced {
pub points: Vec<Point>,
pub on_a: Vec<(f64, f64)>,
pub on_b: Vec<(f64, f64)>,
pub stopped: Stopped,
}
impl Traced {
#[must_use]
pub const fn complete(&self) -> bool {
!matches!(self.stopped, Stopped::RanOut)
}
#[must_use]
pub const fn closed(&self) -> bool {
matches!(self.stopped, Stopped::Closed)
}
}
pub fn seeds(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
options: Marching,
tol: Tolerances,
) -> OgeomResult<Vec<Contact>> {
options.validate()?;
let (mesh_a, mesh_b) = (sample(a, options.grid, tol), sample(b, options.grid, tol));
let apart = span(a).min(span(b)) / f64::from(u32::try_from(options.grid).unwrap_or(1));
let near = CellBins::over(&mesh_b, options.chord);
let mut found: Vec<Contact> = Vec::new();
let mut candidates: Vec<usize> = Vec::new();
for cell_a in &mesh_a {
near.candidates(cell_a, &mut candidates);
for &j in &candidates {
let cell_b = &mesh_b[j];
if !overlap(cell_a, cell_b, options.chord) {
continue;
}
let Some(guess) = triangles_cross(cell_a, cell_b) else {
continue;
};
let start = [cell_a.at.0, cell_a.at.1, cell_b.at.0, cell_b.at.1];
let Some(contact) = correct(a, b, start, guess, None, tol) else {
continue;
};
if found
.iter()
.any(|c| c.point.distance(contact.point) <= apart)
{
continue;
}
found.push(contact);
}
}
for (from_a, border_of, other) in [(true, a, b), (false, b, a)] {
for (border, at) in spline_borders(border_of, tol) {
let Ok(met) = crate::intersect_curve_surface(
&border,
other,
crate::CurveSurfaceOptions::default(),
tol,
) else {
continue;
};
for piercing in met.crossings {
let on_border = at(piercing.on_curve);
let start = if from_a {
[
on_border.0,
on_border.1,
piercing.on_surface.0,
piercing.on_surface.1,
]
} else {
[
piercing.on_surface.0,
piercing.on_surface.1,
on_border.0,
on_border.1,
]
};
let Some(contact) = correct(a, b, start, piercing.point, None, tol) else {
continue;
};
if found
.iter()
.any(|c| c.point.distance(contact.point) <= apart)
{
continue;
}
found.push(contact);
}
}
}
Ok(found)
}
type Border = (ogeom_geom::Curve, Box<dyn Fn(f64) -> (f64, f64)>);
fn spline_borders(surface: &SurfaceGeometry, tol: Tolerances) -> Vec<Border> {
let SurfaceGeometry::BSpline(spline) = surface else {
return Vec::new();
};
let ((u0, u1), (v0, v1)) = surface.domain();
let mut out: Vec<Border> = Vec::new();
if !surface.is_closed_u(tol) {
for u in [u0, u1] {
if let Ok(c) = spline.iso_u_curve(u, tol) {
out.push((ogeom_geom::Curve::BSpline(c), Box::new(move |t| (u, t))));
}
}
}
if !surface.is_closed_v(tol) {
for v in [v0, v1] {
if let Ok(c) = spline.iso_v_curve(v, tol) {
out.push((ogeom_geom::Curve::BSpline(c), Box::new(move |t| (t, v))));
}
}
}
out
}
pub fn branches(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
options: Marching,
tol: Tolerances,
) -> OgeomResult<Vec<Traced>> {
let found = seeds(a, b, options, tol)?;
let mut out: Vec<Traced> = Vec::new();
for seed in found {
let reach = options.chord.max(tol.confusion()) * 8.0;
if out
.iter()
.any(|branch| passes_near(branch, seed.point, reach))
{
continue;
}
if let Ok(branch) = trace(a, b, seed, options, tol)
&& branch.points.len() >= 2
&& !is_fragment(&branch, options)
{
let middle = branch.points[branch.points.len() / 2];
if out.iter().any(|other| passes_near(other, middle, reach)) {
continue;
}
out.push(branch);
}
}
let stitched = stitch_stalled(out, a, b, options, tol);
Ok(split_at_touches(stitched, a, b, options, tol))
}
pub(crate) const BRANCH_POINT_SINE: f64 = 0.05;
fn crossing_sine(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
on_a: (f64, f64),
on_b: (f64, f64),
tol: Tolerances,
) -> f64 {
let Ok(na) = a.normal_at(on_a.0, on_a.1, tol) else {
return 0.0;
};
let Ok(nb) = b.normal_at(on_b.0, on_b.1, tol) else {
return 0.0;
};
na.vector().cross(nb.vector()).magnitude()
}
fn is_fragment(branch: &Traced, options: Marching) -> bool {
if branch.stopped != Stopped::Stalled {
return false;
}
let length: f64 = branch
.points
.windows(2)
.map(|pair| pair[0].distance(pair[1]))
.sum();
length < options.chord * 10.0
}
fn passes_near(branch: &Traced, p: Point, reach: f64) -> bool {
branch
.points
.windows(2)
.any(|pair| distance_to_segment(p, pair[0], pair[1]) <= reach)
}
fn distance_to_segment(p: Point, a: Point, b: Point) -> f64 {
let along = b - a;
let length = along.square_magnitude();
if length <= f64::MIN_POSITIVE {
return p.distance(a);
}
let t = ((p - a).dot(along) / length).clamp(0.0, 1.0);
p.distance(a + along * t)
}
pub fn trace(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
from: Contact,
options: Marching,
tol: Tolerances,
) -> OgeomResult<Traced> {
options.validate()?;
if tangent_at(a, b, from, tol).is_none() {
ogeom_bail!(
NotDone,
"the surfaces are tangent here, so the intersection has no single \
direction to follow; that is a branch point and needs the seed \
moved off it"
);
}
let ahead = walk(a, b, from, 1.0, options, tol)?;
if ahead.stopped == Stopped::Closed {
return Ok(ahead);
}
let behind = walk(a, b, from, -1.0, options, tol)?;
let last_step = |walked: &[Point]| -> f64 {
walked
.windows(2)
.last()
.map_or(0.0, |w| w[0].distance(w[1]))
};
let steps = last_step(&ahead.points).max(last_step(&behind.points));
let mut points = behind.points;
let mut on_a = behind.on_a;
let mut on_b = behind.on_b;
points.reverse();
on_a.reverse();
on_b.reverse();
points.pop();
on_a.pop();
on_b.pop();
points.extend(ahead.points);
on_a.extend(ahead.on_a);
on_b.extend(ahead.on_b);
let mut stopped = if ahead.stopped == Stopped::RanOut || behind.stopped == Stopped::RanOut {
Stopped::RanOut
} else if ahead.stopped == Stopped::Stalled || behind.stopped == Stopped::Stalled {
Stopped::Stalled
} else {
Stopped::LeftTheDomain
};
if stopped == Stopped::LeftTheDomain && points.len() > 3 {
let gap = points[0].distance(points[points.len() - 1]);
if gap <= (steps * 2.0).max(tol.confusion() * 10.0) {
if gap <= tol.confusion() {
points.pop();
on_a.pop();
on_b.pop();
}
points.push(points[0]);
on_a.push(on_a[0]);
on_b.push(on_b[0]);
stopped = Stopped::Closed;
}
}
Ok(Traced {
points,
on_a,
on_b,
stopped,
})
}
struct SurfacePair<'s> {
a: &'s SurfaceGeometry,
b: &'s SurfaceGeometry,
}
impl crate::walk::Condition for SurfacePair<'_> {
fn unknowns(&self) -> usize {
4
}
fn position(&self, x: &[f64], tol: Tolerances) -> Option<Point> {
self.a.point_at(x[0], x[1], tol).ok()
}
fn position_gradient(&self, x: &[f64], tol: Tolerances) -> Option<Vec<Vector>> {
let (au, av) = self.a.d1_at(x[0], x[1], tol).ok()?;
Some(vec![au, av, Vector::ZERO, Vector::ZERO])
}
fn system(&self, x: &[f64], tol: Tolerances) -> Option<(Vec<f64>, Vec<Vec<f64>>)> {
Some(self.system_at(x, tol)?.0)
}
fn system_at(
&self,
x: &[f64],
tol: Tolerances,
) -> Option<((Vec<f64>, Vec<Vec<f64>>), Point, Vec<Vector>)> {
let pa = self.a.point_at(x[0], x[1], tol).ok()?;
let pb = self.b.point_at(x[2], x[3], tol).ok()?;
let (au, av) = self.a.d1_at(x[0], x[1], tol).ok()?;
let (bu, bv) = self.b.d1_at(x[2], x[3], tol).ok()?;
let gap = pa - pb;
Some((
(
vec![gap.x, gap.y, gap.z],
vec![
vec![au.x, av.x, -bu.x, -bv.x],
vec![au.y, av.y, -bu.y, -bv.y],
vec![au.z, av.z, -bu.z, -bv.z],
],
),
pa,
vec![au, av, Vector::ZERO, Vector::ZERO],
))
}
fn clamp(&self, x: &mut [f64]) {
let (ua, va) = clamp(self.a, x[0], x[1]);
let (ub, vb) = clamp(self.b, x[2], x[3]);
x[0] = ua;
x[1] = va;
x[2] = ub;
x[3] = vb;
}
fn outside(&self, x: &[f64], tol: Tolerances) -> bool {
outside(self.a, (x[0], x[1]), tol) || outside(self.b, (x[2], x[3]), tol)
}
fn near_edge(&self, x: &[f64]) -> bool {
near_edge(self.a, (x[0], x[1])) || near_edge(self.b, (x[2], x[3]))
}
fn extent(&self) -> f64 {
span(self.a).max(span(self.b))
}
fn tangent_is_oriented(&self) -> bool {
true
}
fn tangent(&self, x: &[f64], tol: Tolerances) -> Option<Vector> {
tangent_at(
self.a,
self.b,
Contact {
on_a: (x[0], x[1]),
on_b: (x[2], x[3]),
point: Point::ORIGIN,
},
tol,
)
}
}
fn walk(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
from: Contact,
sense: f64,
options: Marching,
tol: Tolerances,
) -> OgeomResult<Traced> {
let pair = SurfacePair { a, b };
let start = [from.on_a.0, from.on_a.1, from.on_b.0, from.on_b.1];
let mut walked = crate::walk::walk_one_way(&pair, &start, sense, options, tol)?;
if walked.stopped == Stopped::LeftTheDomain {
land_on_edge(&pair, &mut walked, tol);
}
Ok(Traced {
on_a: walked.states.iter().map(|x| (x[0], x[1])).collect(),
on_b: walked.states.iter().map(|x| (x[2], x[3])).collect(),
points: walked.points,
stopped: walked.stopped,
})
}
fn land_on_edge(pair: &SurfacePair<'_>, walked: &mut crate::walk::Walked, tol: Tolerances) {
use crate::walk::Condition as _;
let n = walked.states.len();
if n < 2 {
return;
}
let (last, before) = (&walked.states[n - 1], &walked.states[n - 2]);
let step = walked.points[n - 1].distance(walked.points[n - 2]);
let heading = walked.points[n - 1] - walked.points[n - 2];
let mut bounds: Vec<(f64, usize, f64)> = Vec::new();
for (surface, first) in [(pair.a, 0_usize), (pair.b, 2)] {
let ((ua, ub), (va, vb)) = surface.domain();
for (k, (lo, hi), periodic) in [
(first, (ua, ub), surface.is_periodic_u()),
(first + 1, (va, vb), surface.is_periodic_v()),
] {
let span = hi - lo;
if periodic || !span.is_finite() || span <= 0.0 {
continue;
}
let moving = last[k] - before[k];
let across = if k == first {
surface.domain().1
} else {
surface.domain().0
};
let collapses = |bound: f64| {
let at = |t: f64| {
if k == first {
surface.point_at(bound, t, tol)
} else {
surface.point_at(t, bound, tol)
}
};
match (at(across.0), at(f64::midpoint(across.0, across.1))) {
(Ok(p), Ok(q)) => p.distance(q) <= tol.confusion(),
_ => true,
}
};
for (bound, toward) in [(lo, moving < 0.0), (hi, moving > 0.0)] {
if (toward || (last[k] - bound).abs() <= span * 1e-4) && !collapses(bound) {
bounds.push(((last[k] - bound).abs() / span, k, bound));
}
}
}
}
bounds.sort_by(|x, y| x.0.total_cmp(&y.0));
for (_, k, bound) in bounds {
let system = |x: &[f64]| {
let Some(((mut residual, mut jacobian), _, _)) = pair.system_at(x, tol) else {
return (vec![f64::INFINITY; 4], vec![vec![0.0; 4]; 4]);
};
residual.push(x[k] - bound);
let mut row = vec![0.0; 4];
row[k] = 1.0;
jacobian.push(row);
(residual, jacobian)
};
let criteria = solve::Criteria {
residual: tol.confusion() * 0.01,
step: tol.parametric(),
max_iterations: 40,
};
let Ok(found) = solve::newton_system(system, last, criteria) else {
continue;
};
if found.residual > tol.confusion() {
continue;
}
let at = found.value;
let within = [(pair.a, 0_usize), (pair.b, 2)]
.iter()
.all(|(surface, first)| {
let ((ua, ub), (va, vb)) = surface.domain();
[
(*first, ua, ub, surface.is_periodic_u()),
(first + 1, va, vb, surface.is_periodic_v()),
]
.iter()
.all(|&(i, lo, hi, periodic)| {
periodic || (at[i] >= lo - tol.parametric() && at[i] <= hi + tol.parametric())
})
});
if !within {
continue;
}
let Ok(point) = pair.a.point_at(at[0], at[1], tol) else {
continue;
};
let ahead = point - walked.points[n - 1];
if ahead.magnitude() > (step * 2.0).max(tol.confusion())
|| ahead.dot(heading) < -tol.confusion() * step
{
continue;
}
if ahead.magnitude() <= tol.confusion() {
walked.states[n - 1] = at;
walked.points[n - 1] = point;
} else {
walked.states.push(at);
walked.points.push(point);
}
return;
}
}
const SHALLOWEST: f64 = 1e-6;
fn tangent_at(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
at: Contact,
tol: Tolerances,
) -> Option<Vector> {
let na = normal_at(a, at.on_a, tol)?;
let nb = normal_at(b, at.on_b, tol)?;
let cross = na.cross(nb);
let length = cross.magnitude();
let floor = tol.angular().max(SHALLOWEST);
let widen = |value: f64| ogeom_math::Interval::about(value, tol.confusion());
let (ax, ay, az) = (widen(na.x), widen(na.y), widen(na.z));
let (bx, by, bz) = (widen(nb.x), widen(nb.y), widen(nb.z));
let cx = ay.mul(&bz).sub(&az.mul(&by));
let cy = az.mul(&bx).sub(&ax.mul(&bz));
let cz = ax.mul(&by).sub(&ay.mul(&bx));
let magnitude2 = cx.square().add(&cy.square()).add(&cz.square());
let above = magnitude2.sub(&ogeom_math::Interval::point(floor * floor));
if above.certain_sign() != Some(ogeom_core::Sign::Positive) || length <= f64::MIN_POSITIVE {
return None;
}
Some(cross * (1.0 / length))
}
fn normal_at(surface: &SurfaceGeometry, at: (f64, f64), tol: Tolerances) -> Option<Vector> {
let (du, dv) = surface.d1_at(at.0, at.1, tol).ok()?;
let cross = du.cross(dv);
let length = cross.magnitude();
if length <= tol.confusion() {
return None;
}
Some(cross * (1.0 / length))
}
fn correct(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
start: [f64; 4],
guess: Point,
constraint: Option<(Point, Vector, f64)>,
tol: Tolerances,
) -> Option<Contact> {
let (anchor, along, reach) = match constraint {
Some(given) => given,
None => {
let at = Contact {
on_a: (start[0], start[1]),
on_b: (start[2], start[3]),
point: guess,
};
(guess, tangent_at(a, b, at, tol).unwrap_or(Vector::X), 0.0)
}
};
let system = |x: &[f64; 4]| {
let (ua, va) = clamp(a, x[0], x[1]);
let (ub, vb) = clamp(b, x[2], x[3]);
let (Ok(pa), Ok(pb), Ok((au, av)), Ok((bu, bv))) = (
a.point_at(ua, va, tol),
b.point_at(ub, vb, tol),
a.d1_at(ua, va, tol),
b.d1_at(ub, vb, tol),
) else {
return ([f64::INFINITY; 4], [[0.0; 4]; 4]);
};
let gap = pa - pb;
let residual = [gap.x, gap.y, gap.z, (pa - anchor).dot(along) - reach];
let jacobian = [
[au.x, av.x, -bu.x, -bv.x],
[au.y, av.y, -bu.y, -bv.y],
[au.z, av.z, -bu.z, -bv.z],
[au.dot(along), av.dot(along), 0.0, 0.0],
];
(residual, jacobian)
};
let criteria = solve::Criteria {
residual: tol.confusion() * 0.01,
step: tol.parametric(),
max_iterations: 40,
};
let found = solve::newton_system_fixed(system, start, criteria).ok()?;
if found.1 > tol.confusion() {
return None;
}
let (ua, va) = clamp(a, found.0[0], found.0[1]);
let (ub, vb) = clamp(b, found.0[2], found.0[3]);
Some(Contact {
on_a: (ua, va),
on_b: (ub, vb),
point: a.point_at(ua, va, tol).ok()?,
})
}
pub(crate) fn clamp(surface: &SurfaceGeometry, u: f64, v: f64) -> (f64, f64) {
let ((ua, ub), (va, vb)) = surface.domain();
let fold = |x: f64, lo: f64, hi: f64, periodic: bool| {
if !periodic {
return x.clamp(lo, hi);
}
let span = hi - lo;
if span <= 0.0 {
return x;
}
lo + (x - lo).rem_euclid(span)
};
(
fold(u, ua, ub, surface.is_periodic_u()),
fold(v, va, vb, surface.is_periodic_v()),
)
}
fn near_edge(surface: &SurfaceGeometry, at: (f64, f64)) -> bool {
let ((ua, ub), (va, vb)) = surface.domain();
let close = |x: f64, lo: f64, hi: f64, periodic: bool| {
!periodic && {
let band = (hi - lo).abs() * 1e-4;
x <= lo + band || x >= hi - band
}
};
close(at.0, ua, ub, surface.is_periodic_u()) || close(at.1, va, vb, surface.is_periodic_v())
}
fn outside(surface: &SurfaceGeometry, at: (f64, f64), tol: Tolerances) -> bool {
let ((ua, ub), (va, vb)) = surface.domain();
let past = |x: f64, lo: f64, hi: f64, periodic: bool| {
!periodic && (x <= lo + tol.parametric() || x >= hi - tol.parametric())
};
past(at.0, ua, ub, surface.is_periodic_u()) || past(at.1, va, vb, surface.is_periodic_v())
}
pub(crate) fn span(surface: &SurfaceGeometry) -> f64 {
let ((ua, ub), (va, vb)) = surface.domain();
let tol = Tolerances::millimetres();
let corners = [(ua, va), (ub, va), (ua, vb), (ub, vb)];
let mut low = Point::new(f64::MAX, f64::MAX, f64::MAX);
let mut high = Point::new(f64::MIN, f64::MIN, f64::MIN);
for (u, v) in corners {
if let Ok(p) = surface.point_at(u, v, tol) {
low = Point::new(low.x.min(p.x), low.y.min(p.y), low.z.min(p.z));
high = Point::new(high.x.max(p.x), high.y.max(p.y), high.z.max(p.z));
}
}
let size = (high - low).magnitude();
if size.is_finite() && size > 0.0 {
size
} else {
1.0
}
}
pub(crate) struct Cell {
pub(crate) corners: [Point; 3],
pub(crate) at: (f64, f64),
pub(crate) low: Point,
pub(crate) high: Point,
pub(crate) sag: f64,
pub(crate) params: [(f64, f64); 3],
}
pub(crate) fn sample(surface: &SurfaceGeometry, grid: usize, tol: Tolerances) -> Vec<Cell> {
sample_by(surface, (grid, grid), tol)
}
pub(crate) fn sample_by(
surface: &SurfaceGeometry,
counts: (usize, usize),
tol: Tolerances,
) -> Vec<Cell> {
let ((ua, ub), (va, vb)) = surface.domain();
let limit = 1.0e6;
let (ua, ub) = (ua.max(-limit), ub.min(limit));
let (va, vb) = (va.max(-limit), vb.min(limit));
let mut out = Vec::new();
#[allow(clippy::cast_precision_loss)]
let (nu, nv) = (counts.0 as f64, counts.1 as f64);
for i in 0..counts.0 {
for j in 0..counts.1 {
#[allow(clippy::cast_precision_loss)]
let (s0, s1) = (i as f64 / nu, (i + 1) as f64 / nu);
#[allow(clippy::cast_precision_loss)]
let (t0, t1) = (j as f64 / nv, (j + 1) as f64 / nv);
let at = |s: f64, t: f64| {
let (u, v) = (ua + (ub - ua) * s, va + (vb - va) * t);
surface.point_at(u, v, tol).map(|p| ((u, v), p))
};
let (Ok((p00, a00)), Ok((p10, a10)), Ok((p01, a01)), Ok((p11, a11))) =
(at(s0, t0), at(s1, t0), at(s0, t1), at(s1, t1))
else {
continue;
};
let sag = at(f64::midpoint(s0, s1), f64::midpoint(t0, t1))
.map_or(0.0, |(_, middle)| middle.distance(a00.midpoint(a11)));
for (corners, params) in [
([a00, a10, a11], [p00, p10, p11]),
([a00, a11, a01], [p00, p11, p01]),
] {
let low = Point::new(
corners.iter().map(|p| p.x).fold(f64::MAX, f64::min),
corners.iter().map(|p| p.y).fold(f64::MAX, f64::min),
corners.iter().map(|p| p.z).fold(f64::MAX, f64::min),
);
let high = Point::new(
corners.iter().map(|p| p.x).fold(f64::MIN, f64::max),
corners.iter().map(|p| p.y).fold(f64::MIN, f64::max),
corners.iter().map(|p| p.z).fold(f64::MIN, f64::max),
);
out.push(Cell {
corners,
at: p00,
low,
high,
sag,
params,
});
}
}
}
out
}
struct CellBins {
low: Point,
size: f64,
counts: [usize; 3],
bins: Vec<Vec<usize>>,
margin: f64,
}
impl CellBins {
const MOST: usize = 48;
fn over(cells: &[Cell], margin: f64) -> Self {
let mut low = Point::new(f64::INFINITY, f64::INFINITY, f64::INFINITY);
let mut high = Point::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY);
for c in cells {
low = Point::new(low.x.min(c.low.x), low.y.min(c.low.y), low.z.min(c.low.z));
high = Point::new(
high.x.max(c.high.x),
high.y.max(c.high.y),
high.z.max(c.high.z),
);
}
let extent = (high - low).magnitude();
#[allow(clippy::cast_precision_loss)]
let size = if extent.is_finite() && extent > 0.0 {
(extent / Self::MOST as f64).max(margin)
} else {
1.0
};
let count = |lo: f64, hi: f64| -> usize {
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
let n = ((hi - lo) / size).floor() as usize + 1;
n.clamp(1, Self::MOST + 1)
};
let counts = if extent.is_finite() {
[
count(low.x, high.x),
count(low.y, high.y),
count(low.z, high.z),
]
} else {
[1, 1, 1]
};
let mut bins = vec![Vec::new(); counts[0] * counts[1] * counts[2]];
let mut this = Self {
low,
size,
counts,
bins: Vec::new(),
margin,
};
for (i, c) in cells.iter().enumerate() {
this.each_bin(c.low, c.high, |k| bins[k].push(i));
}
this.bins = bins;
this
}
fn each_bin(&self, low: Point, high: Point, mut visit: impl FnMut(usize)) {
let index = |x: f64, lo: f64, n: usize| -> usize {
if !x.is_finite() {
return if x > 0.0 { n - 1 } else { 0 };
}
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
let k = ((x - lo) / self.size).floor().max(0.0) as usize;
k.min(n - 1)
};
let m = self.margin;
let [nx, ny, nz] = self.counts;
let (x0, x1) = (
index(low.x - m, self.low.x, nx),
index(high.x + m, self.low.x, nx),
);
let (y0, y1) = (
index(low.y - m, self.low.y, ny),
index(high.y + m, self.low.y, ny),
);
let (z0, z1) = (
index(low.z - m, self.low.z, nz),
index(high.z + m, self.low.z, nz),
);
for x in x0..=x1 {
for y in y0..=y1 {
for z in z0..=z1 {
visit((x * ny + y) * nz + z);
}
}
}
}
fn candidates(&self, cell: &Cell, out: &mut Vec<usize>) {
out.clear();
self.each_bin(cell.low, cell.high, |k| {
out.extend_from_slice(&self.bins[k])
});
out.sort_unstable();
out.dedup();
}
}
fn overlap(a: &Cell, b: &Cell, margin: f64) -> bool {
a.low.x <= b.high.x + margin
&& b.low.x <= a.high.x + margin
&& a.low.y <= b.high.y + margin
&& b.low.y <= a.high.y + margin
&& a.low.z <= b.high.z + margin
&& b.low.z <= a.high.z + margin
}
fn triangles_cross(a: &Cell, b: &Cell) -> Option<Point> {
for (edges, target) in [(a, b), (b, a)] {
for k in 0..3 {
let (from, to) = (edges.corners[k], edges.corners[(k + 1) % 3]);
if let Some(hit) = segment_meets_triangle(from, to, target.corners) {
return Some(hit);
}
}
}
None
}
pub(crate) fn segment_meets_triangle(from: Point, to: Point, t: [Point; 3]) -> Option<Point> {
let direction = to - from;
let (e1, e2) = (t[1] - t[0], t[2] - t[0]);
let h = direction.cross(e2);
let determinant = e1.dot(h);
if determinant.abs() <= f64::MIN_POSITIVE {
return None;
}
let inverse = 1.0 / determinant;
let s = from - t[0];
let u = inverse * s.dot(h);
if !(0.0..=1.0).contains(&u) {
return None;
}
let q = s.cross(e1);
let v = inverse * direction.dot(q);
if v < 0.0 || u + v > 1.0 {
return None;
}
let along = inverse * e2.dot(q);
if !(0.0..=1.0).contains(&along) {
return None;
}
Some(from + direction * along)
}
struct Arc {
points: Vec<Point>,
on_a: Vec<(f64, f64)>,
on_b: Vec<(f64, f64)>,
head_bp: Option<usize>,
tail_bp: Option<usize>,
}
impl Arc {
fn length(&self) -> f64 {
self.points
.windows(2)
.map(|pair| pair[0].distance(pair[1]))
.sum()
}
fn outgoing(&self, tail: bool) -> Option<Vector> {
let n = self.points.len();
if n < 2 {
return None;
}
let window = (n - 1).min(24);
let (at, back) = if tail {
(n - 1, n - 1 - window)
} else {
(0, window)
};
let out = self.points[at] - self.points[back];
let m = out.magnitude();
(m > f64::MIN_POSITIVE).then(|| out / m)
}
}
fn interior_is_transversal(
branch: &Traced,
a: &SurfaceGeometry,
b: &SurfaceGeometry,
tol: Tolerances,
) -> bool {
let n = branch.points.len();
if n < 5 {
return false;
}
[n / 4, n / 2, 3 * n / 4]
.into_iter()
.any(|i| crossing_sine(a, b, branch.on_a[i], branch.on_b[i], tol) > BRANCH_POINT_SINE)
}
fn stitch_stalled(
found: Vec<Traced>,
a: &SurfaceGeometry,
b: &SurfaceGeometry,
options: Marching,
tol: Tolerances,
) -> Vec<Traced> {
let reach = options.chord.max(tol.confusion()) * 60.0;
const CONTINUES: f64 = 0.5;
let (candidates, mut out): (Vec<Traced>, Vec<Traced>) = found.into_iter().partition(|branch| {
branch.stopped == Stopped::Stalled && interior_is_transversal(branch, a, b, tol)
});
if candidates.is_empty() {
return out;
}
let mut bps: Vec<Point> = Vec::new();
for branch in &candidates {
let n = branch.points.len();
for at in [0, n - 1] {
if crossing_sine(a, b, branch.on_a[at], branch.on_b[at], tol) < BRANCH_POINT_SINE {
let p = branch.points[at];
if !bps.iter().any(|held| held.distance(p) <= reach) {
bps.push(p);
}
}
}
}
if bps.is_empty() {
out.extend(candidates);
return out;
}
let bp_of =
|p: Point| -> Option<usize> { bps.iter().position(|held| held.distance(p) <= reach) };
let mut arcs: Vec<Arc> = Vec::new();
for branch in &candidates {
let n = branch.points.len();
let mut run_start: Option<usize> = None;
for i in 0..=n {
let near = i < n && bp_of(branch.points[i]).is_some();
match (run_start, near, i == n) {
(None, false, false) => run_start = Some(i),
(Some(s), true, _) | (Some(s), _, true) => {
let e = i;
if e > s + 1 {
let head_bp = if s > 0 {
bp_of(branch.points[s - 1])
} else {
None
};
let tail_bp = if e < n { bp_of(branch.points[e]) } else { None };
arcs.push(Arc {
points: branch.points[s..e].to_vec(),
on_a: branch.on_a[s..e].to_vec(),
on_b: branch.on_b[s..e].to_vec(),
head_bp,
tail_bp,
});
}
run_start = None;
}
_ => {}
}
}
}
arcs.retain(|arc| arc.length() > options.chord * 10.0 && arc.points.len() >= 4);
arcs.sort_by(|x, y| {
y.length()
.partial_cmp(&x.length())
.unwrap_or(core::cmp::Ordering::Equal)
});
let mut kept: Vec<Arc> = Vec::new();
'candidate: for arc in arcs {
let n = arc.points.len();
for probe in [n / 4, n / 2, 3 * n / 4] {
let p = arc.points[probe];
if kept.iter().any(|held| {
held.points
.windows(2)
.any(|pair| distance_to_segment(p, pair[0], pair[1]) <= reach)
}) {
continue 'candidate;
}
}
kept.push(arc);
}
let ends: Vec<(usize, bool, usize, Vector)> = kept
.iter()
.enumerate()
.flat_map(|(i, arc)| {
[(false, arc.head_bp), (true, arc.tail_bp)]
.into_iter()
.filter_map(move |(tail, bp)| Some((i, tail, bp?, arc.outgoing(tail)?)))
})
.collect();
let mut partner: Vec<Option<usize>> = vec![None; ends.len()];
for bp in 0..bps.len() {
loop {
let mut best: Option<(usize, usize, f64)> = None;
for x in 0..ends.len() {
if partner[x].is_some() || ends[x].2 != bp {
continue;
}
for y in (x + 1)..ends.len() {
if partner[y].is_some() || ends[y].2 != bp {
continue;
}
let score = -ends[x].3.dot(ends[y].3);
if score > CONTINUES && best.is_none_or(|(_, _, held)| score > held) {
best = Some((x, y, score));
}
}
}
let Some((x, y, _)) = best else { break };
partner[x] = Some(y);
partner[y] = Some(x);
}
}
let end_index = |arc: usize, tail: bool| -> Option<usize> {
ends.iter().position(|e| e.0 == arc && e.1 == tail)
};
let mut used = vec![false; kept.len()];
for start in 0..kept.len() {
if used[start] {
continue;
}
let mut first = start;
let mut first_reversed = false;
let mut seen_back = vec![false; kept.len()];
loop {
seen_back[first] = true;
let Some(entry) = end_index(first, first_reversed) else {
break;
};
let Some(p) = partner[entry] else { break };
let (prev, prev_tail, _, _) = ends[p];
if seen_back[prev] {
break; }
first = prev;
first_reversed = !prev_tail;
}
let mut points: Vec<Point> = Vec::new();
let mut on_a: Vec<(f64, f64)> = Vec::new();
let mut on_b: Vec<(f64, f64)> = Vec::new();
let mut current = first;
let mut reversed = first_reversed;
let mut closed = false;
loop {
used[current] = true;
let arc = &kept[current];
type Run = (Vec<Point>, Vec<(f64, f64)>, Vec<(f64, f64)>);
let (pts, pa, pb): Run = if reversed {
(
arc.points.iter().rev().copied().collect(),
arc.on_a.iter().rev().copied().collect(),
arc.on_b.iter().rev().copied().collect(),
)
} else {
(arc.points.clone(), arc.on_a.clone(), arc.on_b.clone())
};
if !points.is_empty() {
let joint_bp = if reversed { arc.tail_bp } else { arc.head_bp };
if let Some(bp) = joint_bp {
points.push(bps[bp]);
on_a.push(pa[0]);
on_b.push(pb[0]);
}
}
points.extend(pts);
on_a.extend(pa);
on_b.extend(pb);
let leaving = end_index(current, !reversed);
let Some(l) = leaving else { break };
let Some(p) = partner[l] else { break };
let (next, next_tail, _, _) = ends[p];
if used[next] {
closed = next == first;
break;
}
current = next;
reversed = next_tail;
}
if closed && points.len() > 3 {
let bridge = points[0];
let ba = on_a[0];
let bb = on_b[0];
points.push(bridge);
on_a.push(ba);
on_b.push(bb);
}
out.push(Traced {
points,
on_a,
on_b,
stopped: if closed {
Stopped::Closed
} else {
Stopped::Stalled
},
});
}
out
}
#[derive(Debug, Clone, Copy)]
struct Touch {
point: Point,
on_a: (f64, f64),
on_b: (f64, f64),
}
fn touching_point(
a: &SurfaceGeometry,
b: &SurfaceGeometry,
on_a: (f64, f64),
on_b: (f64, f64),
tol: Tolerances,
) -> Option<Touch> {
let eval = |x: &[f64]| -> Option<[f64; 5]> {
let (ua, va) = clamp(a, x[0], x[1]);
let (ub, vb) = clamp(b, x[2], x[3]);
let pa = a.point_at(ua, va, tol).ok()?;
let pb = b.point_at(ub, vb, tol).ok()?;
let na = normal_at(a, (ua, va), tol)?;
let (bu, bv) = b.d1_at(ub, vb, tol).ok()?;
let (lu, lv) = (bu.magnitude(), bv.magnitude());
if lu <= tol.confusion() || lv <= tol.confusion() {
return None;
}
let gap = pa + na * x[4] - pb;
Some([gap.x, gap.y, gap.z, na.dot(bu) / lu, na.dot(bv) / lv])
};
const STEP: f64 = 1e-7;
let system = |x: &[f64]| {
let failed = || (vec![f64::INFINITY; 5], vec![vec![0.0; 5]; 5]);
let Some(here) = eval(x) else {
return failed();
};
let mut jacobian = vec![vec![0.0; 5]; 5];
for k in 0..5 {
let mut moved = x.to_vec();
moved[k] += STEP;
let Some(there) = eval(&moved) else {
return failed();
};
for (row, (t, h)) in jacobian.iter_mut().zip(there.iter().zip(here.iter())) {
row[k] = (t - h) / STEP;
}
}
(here.to_vec(), jacobian)
};
let criteria = solve::Criteria {
residual: tol.confusion() * 1e-3,
step: tol.parametric() * 1e-3,
max_iterations: 50,
};
let start = [on_a.0, on_a.1, on_b.0, on_b.1, 0.0];
let found = solve::newton_system(system, &start, criteria).ok()?;
if found.residual > tol.confusion() || found.value[4].abs() > tol.confusion() {
return None;
}
let x = &found.value;
let (on_a, on_b) = (clamp(a, x[0], x[1]), clamp(b, x[2], x[3]));
let point = a.point_at(on_a.0, on_a.1, tol).ok()?;
if b.point_at(on_b.0, on_b.1, tol).ok()?.distance(point) > tol.confusion() {
return None;
}
Some(Touch { point, on_a, on_b })
}
fn beside(surface: &SurfaceGeometry, at: (f64, f64), near: (f64, f64)) -> (f64, f64) {
let ((ua, ub), (va, vb)) = surface.domain();
let shift = |x: f64, to: f64, span: f64, periodic: bool| {
if !periodic || !span.is_finite() || span <= 0.0 {
return x;
}
x + ((to - x) / span).round() * span
};
(
shift(at.0, near.0, ub - ua, surface.is_periodic_u()),
shift(at.1, near.1, vb - va, surface.is_periodic_v()),
)
}
fn split_at_touches(
found: Vec<Traced>,
a: &SurfaceGeometry,
b: &SurfaceGeometry,
options: Marching,
tol: Tolerances,
) -> Vec<Traced> {
let reach = options.chord.max(tol.confusion()) * 60.0;
let carry = reach * 40.0;
let mut touches: Vec<Touch> = Vec::new();
for branch in &found {
let n = branch.points.len();
if n < 5 || !interior_is_transversal(branch, a, b, tol) {
continue;
}
let sines: Vec<f64> = (0..n)
.map(|i| crossing_sine(a, b, branch.on_a[i], branch.on_b[i], tol))
.collect();
for i in 0..n {
let low = sines[i] < BRANCH_POINT_SINE
&& (i == 0 || sines[i] <= sines[i - 1])
&& (i + 1 == n || sines[i] <= sines[i + 1]);
if !low
|| touches
.iter()
.any(|t| t.point.distance(branch.points[i]) <= reach)
{
continue;
}
let Some(touch) = touching_point(a, b, branch.on_a[i], branch.on_b[i], tol) else {
continue;
};
if touch.point.distance(branch.points[i]) <= carry
&& !touches
.iter()
.any(|t| t.point.distance(touch.point) <= reach)
{
touches.push(touch);
}
}
}
if touches.is_empty() {
return found;
}
let touch_near = |p: Point, within: f64| -> Option<usize> {
touches.iter().position(|t| t.point.distance(p) <= within)
};
let mut out = Vec::with_capacity(found.len());
for branch in found {
let n = branch.points.len();
let mut visits: Vec<Option<usize>> = branch
.points
.iter()
.map(|p| touch_near(*p, reach))
.collect();
for i in 1..n {
let (p, q) = (branch.points[i - 1], branch.points[i]);
if let Some(t) = touches
.iter()
.position(|t| distance_to_segment(t.point, p, q) <= reach)
{
visits[i - 1].get_or_insert(t);
visits[i].get_or_insert(t);
}
}
let stall_end = |at: usize| {
branch.stopped == Stopped::Stalled && touch_near(branch.points[at], carry).is_some()
};
let touched =
visits.iter().any(Option::is_some) || (n > 0 && (stall_end(0) || stall_end(n - 1)));
if !touched || !interior_is_transversal(&branch, a, b, tol) {
out.push(branch);
continue;
}
let closed = branch.closed();
let order: Vec<usize> = if closed {
let last = if branch.points[0].distance(branch.points[n - 1]) <= tol.confusion() {
n - 1
} else {
n
};
let Some(first) =
(0..last).find(|&i| visits[i].is_some() && visits[(i + last - 1) % last].is_none())
else {
out.push(branch);
continue;
};
(0..last).map(|k| (first + k) % last).collect()
} else {
(0..n).collect()
};
let mut runs: Vec<(Vec<usize>, Option<usize>, Option<usize>)> = Vec::new();
let mut run: Vec<usize> = Vec::new();
let mut head: Option<usize> = None;
for (k, &i) in order.iter().enumerate() {
if let Some(t) = visits[i] {
if !run.is_empty() {
runs.push((core::mem::take(&mut run), head, Some(t)));
}
head = Some(t);
continue;
}
if run.is_empty() && k == 0 && !closed && stall_end(i) {
head = touch_near(branch.points[i], carry);
}
run.push(i);
}
if !run.is_empty() {
let last = *run.last().unwrap_or(&0);
let tail = if closed {
visits[order[0]]
} else if stall_end(last) && last == n - 1 {
touch_near(branch.points[last], carry)
} else {
None
};
runs.push((run, head, tail));
}
let unit = |v: Vector| {
let m = v.magnitude();
(m > tol.confusion()).then(|| v / m)
};
let pass = |before: &[usize], after: &[usize], t: usize| -> (usize, bool) {
let (Some(&i), Some(&o)) = (before.last(), after.first()) else {
return (t, false);
};
let into = unit(touches[t].point - branch.points[i]);
let onward = unit(branch.points[o] - touches[t].point);
(
t,
matches!((into, onward), (Some(x), Some(y)) if x.dot(y) > 0.5),
)
};
let mut passes: Vec<(usize, bool)> = runs
.windows(2)
.filter_map(|w| Some(pass(&w[0].0, &w[1].0, w[0].2?)))
.collect();
if closed
&& let (Some(last), Some(first)) = (runs.last(), runs.first())
&& let Some(t) = last.2
{
passes.push(pass(&last.0, &first.0, t));
}
let mut met: Vec<usize> = passes.iter().map(|(t, _)| *t).collect();
met.sort_unstable();
let repeated = met.windows(2).any(|w| w[0] == w[1]);
let straight = passes.iter().all(|(_, s)| *s);
let stopped_short = !closed && (stall_end(0) || stall_end(n - 1));
if !repeated && straight && !stopped_short {
out.push(branch);
continue;
}
let mut arcs: Vec<Traced> = Vec::new();
for (run, head, tail) in runs {
let mut arc = Traced {
points: run.iter().map(|&i| branch.points[i]).collect(),
on_a: run.iter().map(|&i| branch.on_a[i]).collect(),
on_b: run.iter().map(|&i| branch.on_b[i]).collect(),
stopped: Stopped::Stalled,
};
if arc.points.len() < 2 {
continue;
}
for (end, at_head) in [(tail, false), (head, true)] {
if let Some(t) = end {
carry_onto(&mut arc, &touches[t], at_head, a, b, reach, tol);
}
}
let length: f64 = arc.points.windows(2).map(|w| w[0].distance(w[1])).sum();
if length <= options.chord * 10.0 || arc.points.len() < 4 {
continue;
}
let m = arc.points.len();
if head.is_some() && head == tail && arc.points[0].distance(arc.points[m - 1]) <= reach
{
let middle = m * 382 / 1000;
let part = |r: core::ops::RangeInclusive<usize>| Traced {
points: arc.points[r.clone()].to_vec(),
on_a: arc.on_a[r.clone()].to_vec(),
on_b: arc.on_b[r].to_vec(),
stopped: Stopped::Stalled,
};
arcs.push(part(0..=middle));
arcs.push(part(middle..=m - 1));
} else {
arcs.push(arc);
}
}
out.extend(arcs);
}
out
}
fn carry_onto(
arc: &mut Traced,
touch: &Touch,
at_head: bool,
a: &SurfaceGeometry,
b: &SurfaceGeometry,
reach: f64,
tol: Tolerances,
) {
let n = arc.points.len();
let (end, inner) = if at_head { (0, 1) } else { (n - 1, n - 2) };
let from = arc.points[end];
let gap = touch.point - from;
let distance = gap.magnitude();
let mut added: Vec<Contact> = Vec::new();
if distance > tol.confusion() {
let along = gap * (1.0 / distance);
let heading = from - arc.points[inner];
if distance > reach && heading.dot(along) < 0.5 * heading.magnitude() {
return;
}
let pieces = (distance / reach).ceil().max(1.0);
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
let count = pieces as usize;
#[allow(clippy::cast_precision_loss)]
let mut short: Vec<f64> = (1..count)
.map(|k| distance - distance * k as f64 / pieces)
.collect();
let mut close = short.last().copied().unwrap_or(distance).min(reach);
for _ in 0..3 {
close *= 0.5;
short.push(close);
}
let (mut on_a, mut on_b) = (arc.on_a[end], arc.on_b[end]);
for remaining in short {
let s = distance - remaining;
let guess = from + along * s;
let start = [on_a.0, on_a.1, on_b.0, on_b.1];
let settled = correct(a, b, start, guess, Some((from, along, s)), tol)
.filter(|c| c.point.distance(guess) <= remaining * 0.5);
let Some(c) = settled else {
if remaining > reach {
return;
}
continue;
};
on_a = beside(a, c.on_a, on_a);
on_b = beside(b, c.on_b, on_b);
added.push(Contact {
on_a,
on_b,
point: c.point,
});
}
added.push(Contact {
on_a: beside(a, touch.on_a, on_a),
on_b: beside(b, touch.on_b, on_b),
point: touch.point,
});
} else {
let (on_a, on_b) = (arc.on_a[end], arc.on_b[end]);
arc.points[end] = touch.point;
arc.on_a[end] = beside(a, touch.on_a, on_a);
arc.on_b[end] = beside(b, touch.on_b, on_b);
return;
}
if at_head {
added.reverse();
arc.points.splice(0..0, added.iter().map(|c| c.point));
arc.on_a.splice(0..0, added.iter().map(|c| c.on_a));
arc.on_b.splice(0..0, added.iter().map(|c| c.on_b));
} else {
arc.points.extend(added.iter().map(|c| c.point));
arc.on_a.extend(added.iter().map(|c| c.on_a));
arc.on_b.extend(added.iter().map(|c| c.on_b));
}
}
pub(crate) fn nearest_on(
surface: &SurfaceGeometry,
seed: (f64, f64),
target: Point,
tol: Tolerances,
) -> Option<((f64, f64), Point)> {
let (mut u, mut v) = seed;
for _ in 0..16 {
let (u_ok, v_ok) = surface.normalize_parameters(u, v, tol).ok()?;
u = u_ok;
v = v_ok;
let p = surface.point_at(u, v, tol).ok()?;
let (su, sv) = surface.d1_at(u, v, tol).ok()?;
let r = p - target;
let (a11, a12, a22) = (su.dot(su), su.dot(sv), sv.dot(sv));
let det = a11.mul_add(a22, -(a12 * a12));
if det.abs() <= f64::MIN_POSITIVE {
break;
}
let (b1, b2) = (-su.dot(r), -sv.dot(r));
let du = b1.mul_add(a22, -(b2 * a12)) / det;
let dv = a11.mul_add(b2, -(a12 * b1)) / det;
u += du;
v += dv;
if du.hypot(dv) < 1e-14 {
break;
}
}
let (u, v) = surface.normalize_parameters(u, v, tol).ok()?;
Some(((u, v), surface.point_at(u, v, tol).ok()?))
}
#[cfg(test)]
#[allow(clippy::unwrap_used, clippy::print_stdout)]
mod tests {
use super::*;
use ogeom_geom::{CylinderSurface, PlaneSurface, SphereSurface};
use ogeom_math::{Cylinder, Direction, Frame, Plane, Sphere};
const T: Tolerances = Tolerances::millimetres();
fn cylinder(origin: Point, axis: Vector, radius: f64, height: (f64, f64)) -> SurfaceGeometry {
let frame = Frame::new(
origin,
Direction::new(axis, T).unwrap(),
Direction::from_cross(axis, Vector::new(0.3, 0.5, 0.9), T).unwrap(),
T,
)
.unwrap();
CylinderSurface::new(Cylinder::new(frame, radius, T).unwrap(), height)
.unwrap()
.into()
}
fn sphere(centre: Point, radius: f64) -> SurfaceGeometry {
SphereSurface::new(Sphere::centred(centre, radius, T).unwrap()).into()
}
fn plane(origin: Point, normal: Vector) -> SurfaceGeometry {
PlaneSurface::over(
Plane::through(origin, Direction::new(normal, T).unwrap()),
(-8.0, 8.0),
(-8.0, 8.0),
)
.unwrap()
.into()
}
fn off(surface: &SurfaceGeometry, p: Point) -> f64 {
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,
}
}
fn deviation(a: &SurfaceGeometry, b: &SurfaceGeometry, traced: &Traced) -> f64 {
traced
.points
.iter()
.map(|p| off(a, *p).abs().max(off(b, *p).abs()))
.fold(0.0_f64, f64::max)
}
#[test]
fn a_plane_through_a_bent_strip_seeds_both_branches() {
use ogeom_geom::BSplineSurface;
use ogeom_math::{ControlGrid, KnotVector};
let mut points = Vec::new();
for i in 0..7 {
let a = core::f64::consts::PI * f64::from(i) / 6.0;
for j in 0..2 {
points.push(Point::new(2.0 * a.cos(), f64::from(j), 2.0 * a.sin()));
}
}
let grid = ControlGrid::new(points, 7, 2).unwrap();
let strip: SurfaceGeometry = BSplineSurface::new(
KnotVector::clamped_uniform(3, 7).unwrap(),
KnotVector::clamped_uniform(1, 2).unwrap(),
&grid,
T,
)
.unwrap()
.into();
let level: SurfaceGeometry = PlaneSurface::over(
Plane::through(Point::new(0.0, 0.0, 1.0), Direction::Z),
(-1.0e9, 1.0e9),
(-1.0e9, 1.0e9),
)
.unwrap()
.into();
let options = Marching {
chord: 1e-5,
..Marching::default()
};
let found = branches(&strip, &level, options, T).unwrap();
assert_eq!(
found.len(),
2,
"the arch crosses the level twice: {}",
found.len()
);
for branch in &found {
assert!(!branch.closed());
for p in &branch.points {
assert!((p.z - 1.0).abs() < 1e-4, "on the level: {p:?}");
}
}
}
#[test]
fn two_crossed_cylinders_are_traced_onto_both_of_them() {
let a = cylinder(Point::ORIGIN, Vector::Z, 1.0, (-4.0, 4.0));
let b = cylinder(Point::ORIGIN, Vector::X, 1.0, (-4.0, 4.0));
let options = Marching {
chord: 1e-5,
..Marching::default()
};
let found = branches(&a, &b, options, T).unwrap();
assert_eq!(found.len(), 4, "the two ellipses' halves");
let touches = [Point::new(0.0, -1.0, 0.0), Point::new(0.0, 1.0, 0.0)];
let mut worst = 0.0_f64;
for branch in &found {
let ends = [branch.points[0], branch.points[branch.points.len() - 1]];
for touch in touches {
assert!(
ends.iter().any(|end| end.distance(touch) < 1e-9),
"an arc from {:?} to {:?} misses the touch {touch:?}",
ends[0],
ends[1]
);
}
assert!(
branch.points.len() > 100,
"a branch of only {} points",
branch.points.len()
);
worst = worst.max(deviation(&a, &b, branch));
}
println!(
"crossed cylinders: {} branches, worst deviation {worst:e}",
found.len()
);
assert!(worst < 1e-7, "traced off the surfaces by {worst:e}");
}
#[test]
fn a_figure_eight_is_cut_at_its_double_point() {
let radius = 4.353_623_591_855_474;
let torus: SurfaceGeometry = ogeom_geom::TorusSurface::new(
ogeom_math::Torus::new(Frame::WORLD, 10.0, 3.0, T).unwrap(),
)
.into();
let frame = Frame::new(
Point::new(13.0 - radius, 0.0, -10.0),
Direction::Z,
Direction::X,
T,
)
.unwrap();
let drill: SurfaceGeometry =
CylinderSurface::new(Cylinder::new(frame, radius, T).unwrap(), (0.0, 20.0))
.unwrap()
.into();
let options = Marching {
chord: 6e-6,
..Marching::default()
};
let steps = 200_000;
let mut expected = 0.0;
let at = |t: f64| {
let (x, y) = (13.0 - radius + radius * t.cos(), radius * t.sin());
let off = x.hypot(y) - 10.0;
((9.0 - off * off).max(0.0).sqrt(), x, y)
};
for k in 0..steps {
let (t0, t1) = (
core::f64::consts::TAU * f64::from(k) / f64::from(steps),
core::f64::consts::TAU * f64::from(k + 1) / f64::from(steps),
);
let ((z0, x0, y0), (z1, x1, y1)) = (at(t0), at(t1));
if z0 > 0.0 || z1 > 0.0 {
expected += 2.0 * Point::new(x0, y0, z0).distance(Point::new(x1, y1, z1));
}
}
let touch = Point::new(13.0, 0.0, 0.0);
for (a, b) in [(&torus, &drill), (&drill, &torus)] {
let found = branches(a, b, options, T).unwrap();
assert_eq!(found.len(), 4, "the figure eight's four halves");
let mut length = 0.0;
for branch in &found {
let ends = [branch.points[0], branch.points[branch.points.len() - 1]];
assert!(
ends.iter().filter(|end| end.distance(touch) < 1e-9).count() == 1,
"a half from {:?} to {:?} does not end once on the touch",
ends[0],
ends[1]
);
length += branch
.points
.windows(2)
.map(|w| w[0].distance(w[1]))
.sum::<f64>();
assert!(deviation(a, b, branch) < 1e-6, "traced off the surfaces");
}
assert!(
(length - expected).abs() < 1e-3,
"the halves run {length} against the section's {expected}"
);
}
}
#[test]
fn unequal_crossed_cylinders_meet_in_two_curves_as_well() {
let a = cylinder(Point::ORIGIN, Vector::Z, 1.0, (-4.0, 4.0));
let b = cylinder(Point::ORIGIN, Vector::X, 1.6, (-4.0, 4.0));
let options = Marching {
chord: 1e-5,
..Marching::default()
};
let found = branches(&a, &b, options, T).unwrap();
assert_eq!(found.len(), 2);
for branch in &found {
assert!(branch.closed());
assert!(deviation(&a, &b, branch) < 1e-7);
}
}
#[test]
fn a_traced_circle_agrees_with_the_circle_it_should_be() {
let s = sphere(Point::ORIGIN, 3.0);
let cut = plane(Point::ORIGIN, Vector::Z);
let options = Marching {
chord: 1e-6,
..Marching::default()
};
let found = seeds(&s, &cut, options, T).unwrap();
assert!(!found.is_empty());
let branch = trace(&s, &cut, found[0], options, T).unwrap();
assert!(
branch.closed(),
"a plane through a sphere gives a closed loop"
);
for p in &branch.points {
let radius = (p.x * p.x + p.y * p.y).sqrt();
assert!(
(radius - 3.0).abs() < 1e-7,
"a point at radius {radius} on a circle of 3"
);
assert!(p.z.abs() < 1e-7, "off the cutting plane by {}", p.z);
}
}
#[test]
fn a_branch_that_leaves_the_surface_says_so_rather_than_stopping_quietly() {
let s = sphere(Point::ORIGIN, 3.0);
let cut = plane(Point::new(0.0, 0.0, 0.0), Vector::Z);
let options = Marching {
chord: 1e-4,
max_points: 8,
..Marching::default()
};
let found = seeds(&s, &cut, options, T).unwrap();
let branch = trace(&s, &cut, found[0], options, T).unwrap();
assert_eq!(branch.stopped, Stopped::RanOut);
assert!(!branch.complete(), "a truncated branch is not complete");
}
#[test]
fn tangent_surfaces_are_refused_rather_than_followed_onto_a_guess() {
let s = sphere(Point::new(0.0, 0.0, 3.0), 3.0);
let ground = plane(Point::ORIGIN, Vector::Z);
let touch = Contact {
on_a: (0.0, -core::f64::consts::FRAC_PI_2),
on_b: (0.0, 0.0),
point: Point::ORIGIN,
};
let err = trace(&s, &ground, touch, Marching::default(), T).unwrap_err();
assert!(err.to_string().contains("tangent"), "unexpected: {err}");
}
#[test]
fn the_number_of_branches_is_the_number_there_are() {
let options = Marching {
chord: 1e-5,
..Marching::default()
};
let one = branches(
&sphere(Point::ORIGIN, 3.0),
&plane(Point::new(0.0, 0.0, 1.0), Vector::Z),
options,
T,
)
.unwrap();
assert_eq!(one.len(), 1, "one plane through a sphere cuts one circle");
assert!(one[0].closed());
let two = branches(
&sphere(Point::ORIGIN, 3.0),
&cylinder(Point::ORIGIN, Vector::Z, 1.5, (-4.0, 4.0)),
options,
T,
)
.unwrap();
assert_eq!(two.len(), 2, "a coaxial cylinder cuts a sphere twice");
for branch in &two {
assert!(branch.closed(), "each is a closed circle");
}
let heights: Vec<f64> = two.iter().map(|b| b.points[0].z).collect();
assert!(
heights[0] * heights[1] < 0.0,
"both branches came back on the same side: {heights:?}"
);
}
#[test]
fn a_branch_thinner_than_the_sampling_is_missed_and_the_knob_finds_it() {
let a = sphere(Point::ORIGIN, 3.0);
let b = sphere(Point::new(5.98, 0.0, 0.0), 3.0);
let coarse = seeds(
&a,
&b,
Marching {
grid: 6,
..Marching::default()
},
T,
)
.unwrap();
let fine = seeds(
&a,
&b,
Marching {
grid: 120,
..Marching::default()
},
T,
)
.unwrap();
assert!(
coarse.len() < fine.len(),
"a finer grid should find what a coarse one steps over: {} against {}",
coarse.len(),
fine.len()
);
assert!(!fine.is_empty(), "the branch is there to be found");
}
#[test]
fn settings_that_could_not_work_are_refused() {
let a = sphere(Point::ORIGIN, 1.0);
let b = plane(Point::ORIGIN, Vector::Z);
for options in [
Marching {
chord: 0.0,
..Marching::default()
},
Marching {
grid: 1,
..Marching::default()
},
Marching {
max_points: 1,
..Marching::default()
},
] {
assert!(seeds(&a, &b, options, T).is_err());
}
}
}