use axiolid_core::{Point2, Point3, Vec3};
use axiolid_exact::{certify, Arith, Dyadic, Interval, SignExpr};
use axiolid_guarantees::Sign;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[non_exhaustive]
pub enum BoundingError {
Empty,
NonFinite,
}
fn validate(points: &[Point3]) -> Result<(), BoundingError> {
if points.is_empty() {
return Err(BoundingError::Empty);
}
if !points.iter().all(|p| p.is_finite()) {
return Err(BoundingError::NonFinite);
}
Ok(())
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct EnclosingSphere {
pub centre: Point3,
pub radius: f64,
}
#[derive(Debug, Clone, PartialEq)]
#[non_exhaustive]
pub struct SphereEvidence {
pub support: Vec<usize>,
pub error: f64,
}
#[derive(Debug, Clone, PartialEq)]
#[non_exhaustive]
pub struct MinimumSphere {
pub sphere: EnclosingSphere,
pub evidence: SphereEvidence,
}
type V<T> = [T; 3];
fn diff<T: Arith>(a: Point3, b: Point3) -> V<T> {
let f = T::from_f64;
[
f(a.x).sub(&f(b.x)),
f(a.y).sub(&f(b.y)),
f(a.z).sub(&f(b.z)),
]
}
fn dot<T: Arith>(a: &V<T>, b: &V<T>) -> T {
a[0].mul(&b[0]).add(&a[1].mul(&b[1])).add(&a[2].mul(&b[2]))
}
fn cross<T: Arith>(a: &V<T>, b: &V<T>) -> V<T> {
[
a[1].mul(&b[2]).sub(&a[2].mul(&b[1])),
a[2].mul(&b[0]).sub(&a[0].mul(&b[2])),
a[0].mul(&b[1]).sub(&a[1].mul(&b[0])),
]
}
fn scale<T: Arith>(s: &T, a: &V<T>) -> V<T> {
[s.mul(&a[0]), s.mul(&a[1]), s.mul(&a[2])]
}
fn plus<T: Arith>(a: &V<T>, b: &V<T>) -> V<T> {
[a[0].add(&b[0]), a[1].add(&b[1]), a[2].add(&b[2])]
}
fn triangle_centre<T: Arith>(a: Point3, b: Point3, c: Point3) -> (V<T>, T) {
let u: V<T> = diff(b, a);
let v: V<T> = diff(c, a);
let w = cross(&u, &v);
let n = plus(
&scale(&dot(&u, &u), &cross(&v, &w)),
&scale(&dot(&v, &v), &cross(&w, &u)),
);
let ww = dot(&w, &w);
(n, ww.add(&ww))
}
fn tetrahedron_centre<T: Arith>(a: Point3, b: Point3, c: Point3, d: Point3) -> (V<T>, T) {
let u: V<T> = diff(b, a);
let v: V<T> = diff(c, a);
let w: V<T> = diff(d, a);
let m = plus(
&plus(
&scale(&dot(&u, &u), &cross(&v, &w)),
&scale(&dot(&v, &v), &cross(&w, &u)),
),
&scale(&dot(&w, &w), &cross(&u, &v)),
);
let det = dot(&u, &cross(&v, &w));
(m, det.add(&det))
}
struct Diametral {
a: Point3,
b: Point3,
p: Point3,
}
impl SignExpr for Diametral {
fn sign_in<T: Arith>(&self) -> Option<Sign> {
dot::<T>(&diff(self.p, self.a), &diff(self.p, self.b)).sign()
}
}
struct Beyond {
support: [Point3; 4],
count: usize,
p: Point3,
}
impl Beyond {
fn parts<T: Arith>(&self) -> (T, T) {
let [a, b, c, d] = self.support;
let (n, den) = if self.count == 3 {
triangle_centre::<T>(a, b, c)
} else {
tetrahedron_centre::<T>(a, b, c, d)
};
let q: V<T> = diff(self.p, a);
let lhs = dot(&q, &q).mul(&den);
let rhs = dot(&q, &n);
(lhs.sub(&rhs.add(&rhs)), den)
}
}
impl SignExpr for Beyond {
fn sign_in<T: Arith>(&self) -> Option<Sign> {
self.parts::<T>().0.sign()
}
}
struct Denominator(Beyond);
impl SignExpr for Denominator {
fn sign_in<T: Arith>(&self) -> Option<Sign> {
self.0.parts::<T>().1.sign()
}
}
fn sign<E: SignExpr>(e: &E) -> Sign {
certify(e).unwrap_or(Sign::Zero)
}
fn inside(points: &[Point3], support: &[usize], p: Point3) -> bool {
match *support {
[a] => points[a] == p,
[a, b] => {
sign(&Diametral {
a: points[a],
b: points[b],
p,
}) != Sign::Positive
}
_ => {
let mut s = [points[support[0]]; 4];
for (slot, &i) in s.iter_mut().zip(support) {
*slot = points[i];
}
let beyond = Beyond {
support: s,
count: support.len(),
p,
};
let side = sign(&beyond);
let den = sign(&Denominator(beyond));
side == Sign::Zero || den == Sign::Zero || side != den
}
}
}
fn visiting_order(n: usize) -> Vec<usize> {
let mut order: Vec<usize> = (0..n).collect();
let mut state: u64 = 0x9e37_79b9_7f4a_7c15;
let mut next = || {
state = state.wrapping_add(0x9e37_79b9_7f4a_7c15);
let mut z = state;
z = (z ^ (z >> 30)).wrapping_mul(0xbf58_476d_1ce4_e5b9);
z = (z ^ (z >> 27)).wrapping_mul(0x94d0_49bb_1331_11eb);
z ^ (z >> 31)
};
for i in (1..n).rev() {
let j = (next() % (i as u64 + 1)) as usize;
order.swap(i, j);
}
order
}
fn welzl(points: &[Point3]) -> Vec<usize> {
let order = visiting_order(points.len());
let mut support = vec![order[0]];
for i in 1..order.len() {
let pi = order[i];
if inside(points, &support, points[pi]) {
continue;
}
support = vec![pi];
for j in 0..i {
let pj = order[j];
if inside(points, &support, points[pj]) {
continue;
}
support = vec![pi, pj];
for k in 0..j {
let pk = order[k];
if inside(points, &support, points[pk]) {
continue;
}
support = vec![pi, pj, pk];
for &pl in &order[..k] {
if !inside(points, &support, points[pl]) {
support = vec![pi, pj, pk, pl];
}
}
}
}
}
support
}
fn centre_enclosure(points: &[Point3], support: &[usize]) -> [Interval; 3] {
let p = |i: usize| points[support[i]];
let a = p(0);
let at = [a.x, a.y, a.z];
let (n, den): (V<Dyadic>, Dyadic) = match support.len() {
1 => return at.map(Interval::point),
2 => {
let b = p(1);
let half = Dyadic::from_f64(0.5);
let mid = |s: f64, t: f64| {
Dyadic::from_f64(s)
.add(&Dyadic::from_f64(t))
.mul(&half)
.enclosure()
};
return [mid(a.x, b.x), mid(a.y, b.y), mid(a.z, b.z)];
}
3 => triangle_centre(a, p(1), p(2)),
_ => tetrahedron_centre(a, p(1), p(2), p(3)),
};
let den = den.enclosure();
let mut out = [Interval::point(0.0); 3];
for k in 0..3 {
out[k] = Interval::point(at[k]).add(&n[k].enclosure().quotient(den));
}
out
}
fn distance_bounds(p: Point3, centre: &[Interval; 3]) -> (f64, f64) {
let at = [p.x, p.y, p.z];
let mut squared = Interval::point(0.0);
for k in 0..3 {
let d = Interval::point(at[k]).sub(¢re[k]);
squared = squared.add(&d.mul(&d));
}
let low = squared.lo().max(0.0).sqrt().next_down().max(0.0);
(low, squared.hi().sqrt().next_up())
}
pub fn minimum_enclosing_sphere(points: &[Point3]) -> Result<MinimumSphere, BoundingError> {
validate(points)?;
let mut support = welzl(points);
support.sort_unstable();
if let [a] = *support {
return Ok(MinimumSphere {
sphere: EnclosingSphere {
centre: points[a],
radius: 0.0,
},
evidence: SphereEvidence {
support,
error: 0.0,
},
});
}
let enclosure = centre_enclosure(points, &support);
let mid = |i: Interval| i.lo() + 0.5 * (i.hi() - i.lo());
let centre = Point3::new(mid(enclosure[0]), mid(enclosure[1]), mid(enclosure[2]));
let gap = |i: Interval, m: f64| (i.hi() - m).max(m - i.lo());
let centre_error =
(gap(enclosure[0], centre.x) + gap(enclosure[1], centre.y) + gap(enclosure[2], centre.z))
.next_up()
.next_up();
let (radius_low, radius_high) = distance_bounds(points[support[0]], &enclosure);
let mut radius = (radius_high + centre_error).next_up();
let at = [centre.x, centre.y, centre.z].map(Interval::point);
for &p in points {
radius = radius.max(distance_bounds(p, &at).1);
}
Ok(MinimumSphere {
sphere: EnclosingSphere { centre, radius },
evidence: SphereEvidence {
support,
error: (radius - radius_low).next_up(),
},
})
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct OrientedBox {
pub centre: Point3,
pub axes: [Vec3; 3],
pub half_extents: [f64; 3],
}
impl OrientedBox {
#[must_use]
pub fn volume(&self) -> f64 {
8.0 * self.half_extents[0] * self.half_extents[1] * self.half_extents[2]
}
#[must_use]
pub fn corners(&self) -> [Point3; 8] {
std::array::from_fn(|i| {
let mut p = self.centre;
for k in 0..3 {
let s = if i & (1 << k) == 0 { -1.0 } else { 1.0 };
p += self.axes[k] * (s * self.half_extents[k]);
}
p
})
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub struct BoxEvidence {
pub candidates: usize,
pub axis_aligned_volume: f64,
pub orthogonality: f64,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub struct OrientedBoundingBox {
pub bounding_box: OrientedBox,
pub evidence: BoxEvidence,
}
fn unit(v: Vec3) -> Option<Vec3> {
let length = v.length();
(length > 0.0 && length.is_finite()).then(|| v / length)
}
fn frame(normal: Vec3, first: Vec3) -> Option<[Vec3; 3]> {
let n = unit(normal)?;
let a = unit(first - n * first.dot(n))?;
let b = unit(n.cross(a))?;
Some([a, b, n])
}
fn perpendicular(n: Vec3) -> Vec3 {
let helper = if n.x.abs() <= n.y.abs() && n.x.abs() <= n.z.abs() {
Vec3::X
} else if n.y.abs() <= n.z.abs() {
Vec3::Y
} else {
Vec3::Z
};
unit(n.cross(helper)).unwrap_or(Vec3::X)
}
fn flush_frame(points: &[Point3], normal: Vec3) -> Option<[Vec3; 3]> {
let n = unit(normal)?;
let e1 = perpendicular(n);
let e2 = n.cross(e1);
let projected: Vec<Point2> = points
.iter()
.map(|p| Point2::new(p.dot(e1), p.dot(e2)))
.collect();
let rectangle = axiolid_overlay::minimum_area_rectangle(&projected).ok()?;
let [r, _] = rectangle.rectangle.axes;
frame(n, e1 * r.x + e2 * r.y)
}
fn principal_axes(points: &[Point3]) -> [Vec3; 3] {
let n = points.len() as f64;
let mean = points.iter().fold(Vec3::ZERO, |s, p| s + *p) / n;
let mut c = [[0.0f64; 3]; 3];
for p in points {
let d = *p - mean;
let d = [d.x, d.y, d.z];
for i in 0..3 {
for j in 0..3 {
c[i][j] += d[i] * d[j];
}
}
}
let mut v = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
for _ in 0..32 {
let off = c[0][1].abs() + c[0][2].abs() + c[1][2].abs();
if off == 0.0 {
break;
}
for (p, q) in [(0, 1), (0, 2), (1, 2)] {
if c[p][q] == 0.0 {
continue;
}
let theta = (c[q][q] - c[p][p]) / (2.0 * c[p][q]);
let t = theta.signum() / (theta.abs() + (theta * theta + 1.0).sqrt());
let cs = 1.0 / (t * t + 1.0).sqrt();
let sn = t * cs;
for row in &mut c {
let (a, b) = (row[p], row[q]);
row[p] = cs * a - sn * b;
row[q] = sn * a + cs * b;
}
for k in 0..3 {
let (a, b) = (c[p][k], c[q][k]);
c[p][k] = cs * a - sn * b;
c[q][k] = sn * a + cs * b;
}
for row in &mut v {
let (a, b) = (row[p], row[q]);
row[p] = cs * a - sn * b;
row[q] = sn * a + cs * b;
}
}
}
std::array::from_fn(|k| Vec3::new(v[0][k], v[1][k], v[2][k]))
}
fn ranking_volume(points: &[Point3], axes: &[Vec3; 3], pad: f64) -> f64 {
let mut volume = 1.0;
for a in axes {
let (low, high) = points
.iter()
.fold((f64::INFINITY, f64::NEG_INFINITY), |(l, h), p| {
let s = p.dot(*a);
(l.min(s), h.max(s))
});
volume *= high - low + pad;
}
volume
}
fn fit(points: &[Point3], axes: [Vec3; 3]) -> OrientedBox {
let e = Dyadic::from_f64;
let project = |p: Point3, a: Vec3| {
e(p.x)
.mul(&e(a.x))
.add(&e(p.y).mul(&e(a.y)))
.add(&e(p.z).mul(&e(a.z)))
};
let is_less = |x: &Dyadic, y: &Dyadic| x.sub(y).sign() == Some(Sign::Negative);
let mut low: Vec<Dyadic> = axes.iter().map(|a| project(points[0], *a)).collect();
let mut high = low.clone();
for &p in &points[1..] {
for k in 0..3 {
let s = project(p, axes[k]);
if is_less(&s, &low[k]) {
low[k] = s;
} else if is_less(&high[k], &s) {
high[k] = s;
}
}
}
let half = e(0.5);
let mut centre = Point3::ZERO;
for k in 0..3 {
centre += axes[k] * low[k].add(&high[k]).mul(&half).to_f64();
}
let half_extents = std::array::from_fn(|k| {
let c = project(centre, axes[k]);
let (above, below) = (high[k].sub(&c), c.sub(&low[k]));
let widest = if is_less(&above, &below) {
below
} else {
above
};
round_up(&widest).max(0.0)
});
OrientedBox {
centre,
axes,
half_extents,
}
}
fn round_up(d: &Dyadic) -> f64 {
let mut x = d.to_f64();
let e = Dyadic::from_f64;
while e(x).sub(d).sign() == Some(Sign::Negative) {
x = x.next_up();
}
while e(x.next_down()).sub(d).sign() != Some(Sign::Negative) && x.next_down() >= 0.0 {
x = x.next_down();
}
x
}
fn orthogonality(axes: &[Vec3; 3]) -> f64 {
let e = Dyadic::from_f64;
let mut worst = 0.0f64;
for i in 0..3 {
for j in i..3 {
let (a, b) = (axes[i], axes[j]);
let mut d = e(a.x)
.mul(&e(b.x))
.add(&e(a.y).mul(&e(b.y)))
.add(&e(a.z).mul(&e(b.z)));
if i == j {
d = d.sub(&e(1.0));
}
if d.sign() == Some(Sign::Negative) {
d = d.neg();
}
worst = worst.max(d.enclosure().hi());
}
}
worst
}
pub fn oriented_bounding_box(points: &[Point3]) -> Result<OrientedBoundingBox, BoundingError> {
validate(points)?;
let hull = crate::hull::convex_hull(points).ok();
let extreme: &[Point3] = hull.as_ref().map_or(points, |h| &h.positions);
let world = [Vec3::X, Vec3::Y, Vec3::Z];
let principal = principal_axes(points);
let mut frames: Vec<[Vec3; 3]> = vec![world];
if let Some(f) = frame(principal[0].cross(principal[1]), principal[0]) {
frames.push(f);
}
let mut normals: Vec<Vec3> = world.to_vec();
normals.extend(principal);
match &hull {
Some(h) => {
for t in h.indices.chunks_exact(3) {
let [a, b, c] = [0, 1, 2].map(|k| h.positions[t[k] as usize]);
normals.push((b - a).cross(c - a));
}
}
None => normals.push(flat_normal(points)),
}
let mut seen: Vec<Vec3> = Vec::new();
for normal in normals {
let Some(n) = unit(normal) else { continue };
let n = if (n.x, n.y, n.z) < (0.0, 0.0, 0.0) {
-n
} else {
n
};
if seen.contains(&n) {
continue;
}
seen.push(n);
if let Some(f) = flush_frame(extreme, n) {
frames.push(f);
}
}
let axis_aligned_volume = fit(points, world).volume();
let mut best = world;
let size = world
.iter()
.map(|a| {
let along = extreme.iter().map(|p| p.dot(*a));
along.clone().fold(f64::NEG_INFINITY, f64::max) - along.fold(f64::INFINITY, f64::min)
})
.fold(0.0, f64::max);
let pad = 1e-9 * size;
let mut best_volume = ranking_volume(extreme, &world, pad);
for f in &frames[1..] {
let volume = ranking_volume(extreme, f, pad);
if volume < best_volume {
best = *f;
best_volume = volume;
}
}
let mut chosen = fit(points, best);
if chosen.volume() > axis_aligned_volume {
best = world;
chosen = fit(points, world);
}
Ok(OrientedBoundingBox {
bounding_box: chosen,
evidence: BoxEvidence {
candidates: frames.len(),
axis_aligned_volume,
orthogonality: orthogonality(&best),
},
})
}
fn flat_normal(points: &[Point3]) -> Vec3 {
let a = points[0];
let farthest = |from: &dyn Fn(Point3) -> f64| {
points
.iter()
.copied()
.max_by(|p, q| from(*p).total_cmp(&from(*q)))
.unwrap_or(a)
};
let b = farthest(&|p| (p - a).length_squared());
let d = b - a;
let c = farthest(&|p| (p - a).cross(d).length_squared());
let n = d.cross(c - a);
if unit(n).is_some() {
n
} else if let Some(d) = unit(d) {
perpendicular(d)
} else {
Vec3::Z
}
}