use crate::Vec3;
#[derive(Clone, Copy, Debug)]
pub(crate) struct OffsetLine {
pub(crate) point: Vec3,
pub(crate) direction: Vec3,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub(crate) enum OffsetPairDegeneracy {
ParallelPlanes,
SingularSystem,
CollapsedCylinder,
LineParallelToAxis,
PlaneNormalAlongAxis,
NoRealIntersection,
}
pub(crate) fn offset_cylinder_radius(
radius: f64,
outward_away: bool,
distance: f64,
) -> Result<f64, OffsetPairDegeneracy> {
let rho = if outward_away {
radius + distance
} else {
radius - distance
};
if !(rho > 0.0) {
return Err(OffsetPairDegeneracy::CollapsedCylinder);
}
Ok(rho)
}
pub(crate) fn offset_plane_pair(
anchor: Vec3,
normal_a: Vec3,
distance_a: f64,
normal_b: Vec3,
distance_b: f64,
) -> Result<OffsetLine, OffsetPairDegeneracy> {
let direction = normal_a
.cross(normal_b)
.normalized()
.map_err(|_| OffsetPairDegeneracy::ParallelPlanes)?;
let matrix = [
[normal_a.x, normal_a.y, normal_a.z],
[normal_b.x, normal_b.y, normal_b.z],
[direction.x, direction.y, direction.z],
];
let delta = crate::fit::solve_small::<3>(matrix, [distance_a, distance_b, 0.0], 3)
.map_err(|_| OffsetPairDegeneracy::SingularSystem)?;
Ok(OffsetLine {
point: anchor.add(Vec3::new(delta[0], delta[1], delta[2])),
direction,
})
}
pub(crate) fn offset_plane_triple(
anchor: Vec3,
normals: [Vec3; 3],
distances: [f64; 3],
) -> Result<Vec3, OffsetPairDegeneracy> {
let matrix = [
[normals[0].x, normals[0].y, normals[0].z],
[normals[1].x, normals[1].y, normals[1].z],
[normals[2].x, normals[2].y, normals[2].z],
];
let delta = crate::fit::solve_small::<3>(matrix, distances, 3)
.map_err(|_| OffsetPairDegeneracy::SingularSystem)?;
Ok(anchor.add(Vec3::new(delta[0], delta[1], delta[2])))
}
pub(crate) fn line_meets_offset_cylinder(
line: &OffsetLine,
axis_point: Vec3,
axis_dir: Vec3,
rho: f64,
) -> Result<[Vec3; 2], OffsetPairDegeneracy> {
let w0 = line.point.sub(axis_point);
let w0p = w0.sub(axis_dir.scale(w0.dot(axis_dir)));
let dlp = line
.direction
.sub(axis_dir.scale(line.direction.dot(axis_dir)));
let a = dlp.dot(dlp);
if a < 1e-12 {
return Err(OffsetPairDegeneracy::LineParallelToAxis);
}
let b = 2.0 * w0p.dot(dlp);
let c = w0p.dot(w0p) - rho * rho;
let disc = b * b - 4.0 * a * c;
if disc < 0.0 {
return Err(OffsetPairDegeneracy::NoRealIntersection);
}
let sq = disc.sqrt();
Ok([
line.point.add(line.direction.scale((-b - sq) / (2.0 * a))),
line.point.add(line.direction.scale((-b + sq) / (2.0 * a))),
])
}
pub(crate) fn axial_plane_meets_offset_cylinder(
anchor: Vec3,
normal: Vec3,
distance: f64,
axis_point: Vec3,
axis_dir: Vec3,
rho: f64,
) -> Result<[Vec3; 2], OffsetPairDegeneracy> {
let along = normal
.cross(axis_dir)
.normalized()
.map_err(|_| OffsetPairDegeneracy::PlaneNormalAlongAxis)?;
debug_assert!(
normal.dot(axis_dir).abs() <= 1e-6,
"axial_plane_meets_offset_cylinder: the plane must be parallel to the axis"
);
let base = anchor.add(normal.scale(distance));
let w0 = base.sub(axis_point);
let w0p = w0.sub(axis_dir.scale(w0.dot(axis_dir)));
let b = 2.0 * w0p.dot(along);
let c = w0p.dot(w0p) - rho * rho;
let disc = b * b - 4.0 * c;
if disc < 0.0 {
return Err(OffsetPairDegeneracy::NoRealIntersection);
}
let sq = disc.sqrt();
Ok([
base.add(along.scale((-b - sq) / 2.0)),
base.add(along.scale((-b + sq) / 2.0)),
])
}
pub(crate) mod diag {
use super::*;
pub(super) mod legacy {
use super::*;
pub(crate) fn plane_pair(
anchor: Vec3,
normal_a: Vec3,
distance_a: f64,
normal_b: Vec3,
distance_b: f64,
third_row: Vec3,
) -> Option<Vec3> {
let mat = [
[normal_a.x, normal_a.y, normal_a.z],
[normal_b.x, normal_b.y, normal_b.z],
[third_row.x, third_row.y, third_row.z],
];
let d = crate::fit::solve_small::<3>(mat, [distance_a, distance_b, 0.0], 3).ok()?;
Some(anchor.add(Vec3::new(d[0], d[1], d[2])))
}
pub(crate) fn plane_triple(
anchor: Vec3,
normals: [Vec3; 3],
distances: [f64; 3],
) -> Option<Vec3> {
let mat = [
[normals[0].x, normals[0].y, normals[0].z],
[normals[1].x, normals[1].y, normals[1].z],
[normals[2].x, normals[2].y, normals[2].z],
];
let d = crate::fit::solve_small::<3>(mat, distances, 3).ok()?;
Some(anchor.add(Vec3::new(d[0], d[1], d[2])))
}
pub(crate) fn line_cylinder(
p0: Vec3,
dl: Vec3,
axis_point: Vec3,
axis_dir: Vec3,
rho: f64,
) -> Option<[Vec3; 2]> {
let w0 = p0.sub(axis_point);
let w0p = w0.sub(axis_dir.scale(w0.dot(axis_dir)));
let dlp = dl.sub(axis_dir.scale(dl.dot(axis_dir)));
let a = dlp.dot(dlp);
if a < 1e-12 {
return None;
}
let b = 2.0 * w0p.dot(dlp);
let c = w0p.dot(w0p) - rho * rho;
let disc = b * b - 4.0 * a * c;
if disc < 0.0 {
return None;
}
let sq = disc.sqrt();
Some([
p0.add(dl.scale((-b - sq) / (2.0 * a))),
p0.add(dl.scale((-b + sq) / (2.0 * a))),
])
}
pub(crate) fn axial_plane_cylinder(
corner: Vec3,
na: Vec3,
radius: f64,
axis_point: Vec3,
axis_dir: Vec3,
rho_q: f64,
) -> Option<[Vec3; 2]> {
let dl = na.cross(axis_dir).normalized().ok()?;
let p0 = corner.add(na.scale(radius));
let w0 = p0.sub(axis_point);
let w0p = w0.sub(axis_dir.scale(w0.dot(axis_dir)));
let b = 2.0 * w0p.dot(dl);
let c = w0p.dot(w0p) - rho_q * rho_q;
let disc = b * b - 4.0 * c;
if disc < 0.0 {
return None;
}
let sq = disc.sqrt();
Some([p0.add(dl.scale((-b - sq) / 2.0)), p0.add(dl.scale((-b + sq) / 2.0))])
}
}
#[derive(Default, Clone, Copy)]
struct SiteStats {
calls: u64,
outcome_splits: u64,
bit_differences: u64,
worst_delta: f64,
worst_ulps: u64,
}
struct DiagnosticSink {
sites: std::collections::BTreeMap<&'static str, SiteStats>,
}
impl Drop for DiagnosticSink {
fn drop(&mut self) {
for (site, stats) in &self.sites {
eprintln!(
"offset-pair-diag site={site} calls={} split={} bitdiff={} \
worst_delta={:.3e} worst_ulps={}",
stats.calls,
stats.outcome_splits,
stats.bit_differences,
stats.worst_delta,
stats.worst_ulps
);
}
}
}
thread_local! {
static DIAGNOSTIC: std::cell::RefCell<DiagnosticSink> =
std::cell::RefCell::new(DiagnosticSink {
sites: std::collections::BTreeMap::new(),
});
}
pub(crate) fn enabled() -> bool {
static ON: std::sync::OnceLock<bool> = std::sync::OnceLock::new();
*ON.get_or_init(|| std::env::var("BREP_OFFSET_PAIR_DIAG").is_ok())
}
fn ulps(a: f64, b: f64) -> u64 {
if a == b {
return 0;
}
if !a.is_finite() || !b.is_finite() {
return u64::MAX;
}
let key = |x: f64| -> i64 {
let bits = x.to_bits() as i64;
if bits < 0 {
i64::MIN - bits
} else {
bits
}
};
key(a).abs_diff(key(b))
}
fn record(site: &'static str, shared: Option<&[Vec3]>, legacy: Option<&[Vec3]>) {
DIAGNOSTIC.with(|sink| {
let mut sink = sink.borrow_mut();
let stats = sink.sites.entry(site).or_default();
stats.calls += 1;
match (shared, legacy) {
(Some(shared), Some(legacy)) if shared.len() == legacy.len() => {
let mut differed = false;
for (a, b) in shared.iter().zip(legacy.iter()) {
let delta = a.sub(*b).length();
if delta > stats.worst_delta {
stats.worst_delta = delta;
}
for (x, y) in [(a.x, b.x), (a.y, b.y), (a.z, b.z)] {
let gap = ulps(x, y);
if gap > 0 {
differed = true;
}
if gap > stats.worst_ulps {
stats.worst_ulps = gap;
}
}
}
if differed {
stats.bit_differences += 1;
}
}
(None, None) => {}
_ => stats.outcome_splits += 1,
}
});
}
#[allow(clippy::too_many_arguments)]
pub(crate) fn plane_pair(
site: &'static str,
anchor: Vec3,
normal_a: Vec3,
distance_a: f64,
normal_b: Vec3,
distance_b: f64,
third_row: Vec3,
shared: Option<&OffsetLine>,
) {
if !enabled() {
return;
}
let legacy =
legacy::plane_pair(anchor, normal_a, distance_a, normal_b, distance_b, third_row);
let shared = shared.map(|line| [line.point]);
record(
site,
shared.as_ref().map(|p| &p[..]),
legacy.as_ref().map(std::slice::from_ref),
);
}
pub(crate) fn plane_triple(
site: &'static str,
anchor: Vec3,
normals: [Vec3; 3],
distances: [f64; 3],
shared: Option<Vec3>,
) {
if !enabled() {
return;
}
let legacy = legacy::plane_triple(anchor, normals, distances);
record(
site,
shared.as_ref().map(std::slice::from_ref),
legacy.as_ref().map(std::slice::from_ref),
);
}
pub(crate) fn line_cylinder(
site: &'static str,
line: &OffsetLine,
axis_point: Vec3,
axis_dir: Vec3,
rho: f64,
shared: Option<&[Vec3; 2]>,
) {
if !enabled() {
return;
}
let legacy = legacy::line_cylinder(line.point, line.direction, axis_point, axis_dir, rho);
record(
site,
shared.map(|roots| &roots[..]),
legacy.as_ref().map(|roots| &roots[..]),
);
}
#[allow(clippy::too_many_arguments)]
pub(crate) fn axial_plane_cylinder(
site: &'static str,
anchor: Vec3,
normal: Vec3,
distance: f64,
axis_point: Vec3,
axis_dir: Vec3,
rho: f64,
shared: Option<&[Vec3; 2]>,
) {
if !enabled() {
return;
}
let legacy = legacy::axial_plane_cylinder(anchor, normal, distance, axis_point, axis_dir, rho);
record(
site,
shared.map(|roots| &roots[..]),
legacy.as_ref().map(|roots| &roots[..]),
);
}
}
#[cfg(test)]
mod tests {
use super::*;
fn bits(p: Vec3) -> [u64; 3] {
[p.x.to_bits(), p.y.to_bits(), p.z.to_bits()]
}
#[test]
fn bit_identical_plane_pair_under_a_negated_direction_row() {
let cases = [
(Vec3::new(1.0, 0.0, 0.0), Vec3::new(0.0, 1.0, 0.0)),
(Vec3::new(0.0, 0.0, 1.0), Vec3::new(1.0, 0.0, 0.0)),
(
Vec3::new(0.7, 0.2, -0.3).normalized().unwrap(),
Vec3::new(-0.1, 0.9, 0.25).normalized().unwrap(),
),
(
Vec3::new(1.0, 0.0, 0.0),
Vec3::new(1.0, 1e-6, 0.0).normalized().unwrap(),
),
];
let anchor = Vec3::new(-3.25, 7.5, 0.125);
for (na, nb) in cases {
for radius in [0.4, 1.0, 12.5] {
for signed in [radius, -radius] {
let axis = na.cross(nb).normalized().unwrap();
let shared = offset_plane_pair(anchor, na, signed, nb, signed).unwrap();
for third in [axis, axis.scale(-1.0)] {
let legacy =
diag::legacy::plane_pair(anchor, na, signed, nb, signed, third)
.unwrap();
assert_eq!(
bits(shared.point),
bits(legacy),
"negating the direction row moved the solve at n_a={na:?} \
n_b={nb:?} d={signed}"
);
}
}
}
}
}
#[test]
fn the_plane_pair_locus_is_equidistant_from_both_offset_planes() {
let anchor = Vec3::new(2.0, -1.0, 0.5);
let na = Vec3::new(0.3, 0.9, -0.2).normalized().unwrap();
let nb = Vec3::new(-0.8, 0.1, 0.55).normalized().unwrap();
let line = offset_plane_pair(anchor, na, 1.5, nb, -0.75).unwrap();
for t in [-40.0, -1.0, 0.0, 1.0, 40.0] {
let p = line.point.add(line.direction.scale(t));
assert!((na.dot(p.sub(anchor)) - 1.5).abs() < 1e-12, "wall A at t={t}");
assert!(
(nb.dot(p.sub(anchor)) + 0.75).abs() < 1e-12,
"wall B at t={t}"
);
}
assert!(
line.direction.dot(line.point.sub(anchor)).abs() < 1e-12,
"the anchor point is not the foot of the perpendicular"
);
}
#[test]
fn parallel_and_antiparallel_walls_refuse_as_parallel_planes() {
let anchor = Vec3::default();
let n = Vec3::new(0.0, 0.0, 1.0);
for other in [n, n.scale(-1.0)] {
assert_eq!(
offset_plane_pair(anchor, n, 1.0, other, 1.0).unwrap_err(),
OffsetPairDegeneracy::ParallelPlanes
);
}
assert_eq!(
offset_plane_triple(
anchor,
[
Vec3::new(1.0, 0.0, 0.0),
Vec3::new(0.0, 1.0, 0.0),
Vec3::new(1.0, 1.0, 0.0).normalized().unwrap(),
],
[-1.0, -1.0, -1.0],
)
.unwrap_err(),
OffsetPairDegeneracy::SingularSystem
);
}
#[test]
fn the_cylinder_offset_sign_is_the_convex_concave_split() {
let (r_wall, r_ball) = (7.25, 0.4);
assert_eq!(
offset_cylinder_radius(r_wall, true, -r_ball).unwrap().to_bits(),
(r_wall - r_ball).to_bits()
);
assert_eq!(
offset_cylinder_radius(r_wall, false, -r_ball).unwrap().to_bits(),
(r_wall + r_ball).to_bits()
);
assert_eq!(
offset_cylinder_radius(r_wall, true, r_ball).unwrap().to_bits(),
(r_wall + r_ball).to_bits()
);
assert_eq!(
offset_cylinder_radius(r_wall, false, r_ball).unwrap().to_bits(),
(r_wall - r_ball).to_bits()
);
assert_eq!(
offset_cylinder_radius(1.0, false, 1.0).unwrap_err(),
OffsetPairDegeneracy::CollapsedCylinder
);
assert_eq!(
offset_cylinder_radius(1.0, false, 2.0).unwrap_err(),
OffsetPairDegeneracy::CollapsedCylinder
);
}
#[test]
fn both_quadratic_lanes_return_both_roots_on_the_offset_cylinder() {
let axis_point = Vec3::new(1.0, 2.0, -3.0);
let axis_dir = Vec3::new(0.0, 0.0, 1.0);
let rho = 5.0;
let anchor = Vec3::new(3.0, 9.0, 6.0);
let roots =
axial_plane_meets_offset_cylinder(anchor, Vec3::new(1.0, 0.0, 0.0), 1.0, axis_point, axis_dir, rho)
.unwrap();
for p in roots {
let rel = p.sub(axis_point);
let radial = rel.sub(axis_dir.scale(rel.dot(axis_dir))).length();
assert!((radial - rho).abs() < 1e-12, "root off the offset cylinder");
assert!((p.z - anchor.z).abs() < 1e-12);
}
assert!(roots[0].y > roots[1].y, "roots are not in algebraic order");
let line = OffsetLine {
point: Vec3::new(1.0, 2.0, -3.0),
direction: Vec3::new(1.0, 0.0, 1.0).normalized().unwrap(),
};
let roots = line_meets_offset_cylinder(&line, axis_point, axis_dir, rho).unwrap();
for p in roots {
let rel = p.sub(axis_point);
let radial = rel.sub(axis_dir.scale(rel.dot(axis_dir))).length();
assert!((radial - rho).abs() < 1e-12, "root off the offset cylinder");
}
assert!(roots[0].x < roots[1].x, "roots are not in algebraic order");
let tangent = axial_plane_meets_offset_cylinder(
Vec3::new(1.0, 2.0, 0.0),
Vec3::new(1.0, 0.0, 0.0),
rho,
axis_point,
axis_dir,
rho,
)
.unwrap();
assert!(tangent[0].sub(tangent[1]).length() < 1e-12, "not a double root");
}
#[test]
fn every_quadratic_degeneracy_refuses_with_its_own_variant() {
let axis_point = Vec3::default();
let axis_dir = Vec3::new(0.0, 0.0, 1.0);
let along_axis = OffsetLine {
point: Vec3::new(1.0, 0.0, 0.0),
direction: axis_dir,
};
assert_eq!(
line_meets_offset_cylinder(&along_axis, axis_point, axis_dir, 2.0).unwrap_err(),
OffsetPairDegeneracy::LineParallelToAxis
);
let far = OffsetLine {
point: Vec3::new(10.0, 0.0, 0.0),
direction: Vec3::new(0.0, 1.0, 0.0),
};
assert_eq!(
line_meets_offset_cylinder(&far, axis_point, axis_dir, 2.0).unwrap_err(),
OffsetPairDegeneracy::NoRealIntersection
);
assert_eq!(
axial_plane_meets_offset_cylinder(
Vec3::new(10.0, 0.0, 0.0),
Vec3::new(1.0, 0.0, 0.0),
0.0,
axis_point,
axis_dir,
2.0,
)
.unwrap_err(),
OffsetPairDegeneracy::NoRealIntersection
);
assert_eq!(
axial_plane_meets_offset_cylinder(
Vec3::default(),
axis_dir,
1.0,
axis_point,
axis_dir,
2.0,
)
.unwrap_err(),
OffsetPairDegeneracy::PlaneNormalAlongAxis
);
}
#[test]
fn the_general_and_unit_leading_coefficient_lanes_are_not_bitwise_interchangeable() {
let axis_dir = Vec3::new(0.3, -0.5, 0.81).normalized().unwrap();
let anchor = Vec3::new(0.3, 0.7, 0.11);
let axis_point = Vec3::new(2.5, 1.25, -4.0);
let rho = 8.0;
let mut any_bit_difference = false;
for seed in [
Vec3::new(0.17, 0.93, -0.4),
Vec3::new(-0.62, 0.21, 0.55),
Vec3::new(0.88, -0.13, 0.32),
Vec3::new(0.05, 0.71, 0.66),
] {
let normal = seed.cross(axis_dir).normalized().unwrap();
let unit =
axial_plane_meets_offset_cylinder(anchor, normal, 0.4, axis_point, axis_dir, rho)
.unwrap();
let line = OffsetLine {
point: anchor.add(normal.scale(0.4)),
direction: normal.cross(axis_dir).normalized().unwrap(),
};
let general = line_meets_offset_cylinder(&line, axis_point, axis_dir, rho).unwrap();
for (a, b) in unit.iter().zip(general.iter()) {
assert!(
a.sub(*b).length() < 1e-9,
"the two lanes disagree geometrically, not just in the last bits"
);
if bits(*a) != bits(*b) {
any_bit_difference = true;
}
}
}
assert!(
any_bit_difference,
"the two lanes were bitwise equal at every probe, so this test no longer \
pins the reason they are kept apart — re-derive it before merging them"
);
}
}