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[..]),
);
}
}