1use axiolid_core::{Point2, Point3, Vec3};
49use axiolid_exact::{certify, Arith, Dyadic, Interval, SignExpr};
50use axiolid_guarantees::Sign;
51
52#[derive(Debug, Clone, Copy, PartialEq, Eq)]
54#[non_exhaustive]
55pub enum BoundingError {
56 Empty,
58 NonFinite,
60}
61
62fn validate(points: &[Point3]) -> Result<(), BoundingError> {
63 if points.is_empty() {
64 return Err(BoundingError::Empty);
65 }
66 if !points.iter().all(|p| p.is_finite()) {
67 return Err(BoundingError::NonFinite);
68 }
69 Ok(())
70}
71
72#[derive(Debug, Clone, Copy, PartialEq)]
78pub struct EnclosingSphere {
79 pub centre: Point3,
81 pub radius: f64,
83}
84
85#[derive(Debug, Clone, PartialEq)]
87#[non_exhaustive]
88pub struct SphereEvidence {
89 pub support: Vec<usize>,
92 pub error: f64,
97}
98
99#[derive(Debug, Clone, PartialEq)]
101#[non_exhaustive]
102pub struct MinimumSphere {
103 pub sphere: EnclosingSphere,
105 pub evidence: SphereEvidence,
107}
108
109type V<T> = [T; 3];
110
111fn diff<T: Arith>(a: Point3, b: Point3) -> V<T> {
112 let f = T::from_f64;
113 [
114 f(a.x).sub(&f(b.x)),
115 f(a.y).sub(&f(b.y)),
116 f(a.z).sub(&f(b.z)),
117 ]
118}
119
120fn dot<T: Arith>(a: &V<T>, b: &V<T>) -> T {
121 a[0].mul(&b[0]).add(&a[1].mul(&b[1])).add(&a[2].mul(&b[2]))
122}
123
124fn cross<T: Arith>(a: &V<T>, b: &V<T>) -> V<T> {
125 [
126 a[1].mul(&b[2]).sub(&a[2].mul(&b[1])),
127 a[2].mul(&b[0]).sub(&a[0].mul(&b[2])),
128 a[0].mul(&b[1]).sub(&a[1].mul(&b[0])),
129 ]
130}
131
132fn scale<T: Arith>(s: &T, a: &V<T>) -> V<T> {
133 [s.mul(&a[0]), s.mul(&a[1]), s.mul(&a[2])]
134}
135
136fn plus<T: Arith>(a: &V<T>, b: &V<T>) -> V<T> {
137 [a[0].add(&b[0]), a[1].add(&b[1]), a[2].add(&b[2])]
138}
139
140fn triangle_centre<T: Arith>(a: Point3, b: Point3, c: Point3) -> (V<T>, T) {
142 let u: V<T> = diff(b, a);
143 let v: V<T> = diff(c, a);
144 let w = cross(&u, &v);
145 let n = plus(
146 &scale(&dot(&u, &u), &cross(&v, &w)),
147 &scale(&dot(&v, &v), &cross(&w, &u)),
148 );
149 let ww = dot(&w, &w);
150 (n, ww.add(&ww))
151}
152
153fn tetrahedron_centre<T: Arith>(a: Point3, b: Point3, c: Point3, d: Point3) -> (V<T>, T) {
155 let u: V<T> = diff(b, a);
156 let v: V<T> = diff(c, a);
157 let w: V<T> = diff(d, a);
158 let m = plus(
159 &plus(
160 &scale(&dot(&u, &u), &cross(&v, &w)),
161 &scale(&dot(&v, &v), &cross(&w, &u)),
162 ),
163 &scale(&dot(&w, &w), &cross(&u, &v)),
164 );
165 let det = dot(&u, &cross(&v, &w));
166 (m, det.add(&det))
167}
168
169struct Diametral {
171 a: Point3,
172 b: Point3,
173 p: Point3,
174}
175
176impl SignExpr for Diametral {
177 fn sign_in<T: Arith>(&self) -> Option<Sign> {
178 dot::<T>(&diff(self.p, self.a), &diff(self.p, self.b)).sign()
179 }
180}
181
182struct Beyond {
185 support: [Point3; 4],
186 count: usize,
187 p: Point3,
188}
189
190impl Beyond {
191 fn parts<T: Arith>(&self) -> (T, T) {
192 let [a, b, c, d] = self.support;
193 let (n, den) = if self.count == 3 {
194 triangle_centre::<T>(a, b, c)
195 } else {
196 tetrahedron_centre::<T>(a, b, c, d)
197 };
198 let q: V<T> = diff(self.p, a);
199 let lhs = dot(&q, &q).mul(&den);
200 let rhs = dot(&q, &n);
201 (lhs.sub(&rhs.add(&rhs)), den)
202 }
203}
204
205impl SignExpr for Beyond {
206 fn sign_in<T: Arith>(&self) -> Option<Sign> {
207 self.parts::<T>().0.sign()
208 }
209}
210
211struct Denominator(Beyond);
212
213impl SignExpr for Denominator {
214 fn sign_in<T: Arith>(&self) -> Option<Sign> {
215 self.0.parts::<T>().1.sign()
216 }
217}
218
219fn sign<E: SignExpr>(e: &E) -> Sign {
221 certify(e).unwrap_or(Sign::Zero)
222}
223
224fn inside(points: &[Point3], support: &[usize], p: Point3) -> bool {
226 match *support {
227 [a] => points[a] == p,
228 [a, b] => {
229 sign(&Diametral {
230 a: points[a],
231 b: points[b],
232 p,
233 }) != Sign::Positive
234 }
235 _ => {
236 let mut s = [points[support[0]]; 4];
237 for (slot, &i) in s.iter_mut().zip(support) {
238 *slot = points[i];
239 }
240 let beyond = Beyond {
241 support: s,
242 count: support.len(),
243 p,
244 };
245 let side = sign(&beyond);
246 let den = sign(&Denominator(beyond));
247 side == Sign::Zero || den == Sign::Zero || side != den
249 }
250 }
251}
252
253fn visiting_order(n: usize) -> Vec<usize> {
256 let mut order: Vec<usize> = (0..n).collect();
257 let mut state: u64 = 0x9e37_79b9_7f4a_7c15;
258 let mut next = || {
259 state = state.wrapping_add(0x9e37_79b9_7f4a_7c15);
260 let mut z = state;
261 z = (z ^ (z >> 30)).wrapping_mul(0xbf58_476d_1ce4_e5b9);
262 z = (z ^ (z >> 27)).wrapping_mul(0x94d0_49bb_1331_11eb);
263 z ^ (z >> 31)
264 };
265 for i in (1..n).rev() {
266 let j = (next() % (i as u64 + 1)) as usize;
267 order.swap(i, j);
268 }
269 order
270}
271
272fn welzl(points: &[Point3]) -> Vec<usize> {
274 let order = visiting_order(points.len());
275 let mut support = vec![order[0]];
276 for i in 1..order.len() {
277 let pi = order[i];
278 if inside(points, &support, points[pi]) {
279 continue;
280 }
281 support = vec![pi];
282 for j in 0..i {
283 let pj = order[j];
284 if inside(points, &support, points[pj]) {
285 continue;
286 }
287 support = vec![pi, pj];
288 for k in 0..j {
289 let pk = order[k];
290 if inside(points, &support, points[pk]) {
291 continue;
292 }
293 support = vec![pi, pj, pk];
294 for &pl in &order[..k] {
295 if !inside(points, &support, points[pl]) {
296 support = vec![pi, pj, pk, pl];
297 }
298 }
299 }
300 }
301 }
302 support
303}
304
305fn centre_enclosure(points: &[Point3], support: &[usize]) -> [Interval; 3] {
307 let p = |i: usize| points[support[i]];
308 let a = p(0);
309 let at = [a.x, a.y, a.z];
310 let (n, den): (V<Dyadic>, Dyadic) = match support.len() {
311 1 => return at.map(Interval::point),
312 2 => {
313 let b = p(1);
314 let half = Dyadic::from_f64(0.5);
315 let mid = |s: f64, t: f64| {
316 Dyadic::from_f64(s)
317 .add(&Dyadic::from_f64(t))
318 .mul(&half)
319 .enclosure()
320 };
321 return [mid(a.x, b.x), mid(a.y, b.y), mid(a.z, b.z)];
322 }
323 3 => triangle_centre(a, p(1), p(2)),
324 _ => tetrahedron_centre(a, p(1), p(2), p(3)),
325 };
326 let den = den.enclosure();
327 let mut out = [Interval::point(0.0); 3];
328 for k in 0..3 {
329 out[k] = Interval::point(at[k]).add(&n[k].enclosure().quotient(den));
330 }
331 out
332}
333
334fn distance_bounds(p: Point3, centre: &[Interval; 3]) -> (f64, f64) {
337 let at = [p.x, p.y, p.z];
338 let mut squared = Interval::point(0.0);
339 for k in 0..3 {
340 let d = Interval::point(at[k]).sub(¢re[k]);
341 squared = squared.add(&d.mul(&d));
342 }
343 let low = squared.lo().max(0.0).sqrt().next_down().max(0.0);
345 (low, squared.hi().sqrt().next_up())
346}
347
348pub fn minimum_enclosing_sphere(points: &[Point3]) -> Result<MinimumSphere, BoundingError> {
355 validate(points)?;
356 let mut support = welzl(points);
357 support.sort_unstable();
358 if let [a] = *support {
359 return Ok(MinimumSphere {
360 sphere: EnclosingSphere {
361 centre: points[a],
362 radius: 0.0,
363 },
364 evidence: SphereEvidence {
365 support,
366 error: 0.0,
367 },
368 });
369 }
370 let enclosure = centre_enclosure(points, &support);
371 let mid = |i: Interval| i.lo() + 0.5 * (i.hi() - i.lo());
372 let centre = Point3::new(mid(enclosure[0]), mid(enclosure[1]), mid(enclosure[2]));
373 let gap = |i: Interval, m: f64| (i.hi() - m).max(m - i.lo());
375 let centre_error =
376 (gap(enclosure[0], centre.x) + gap(enclosure[1], centre.y) + gap(enclosure[2], centre.z))
377 .next_up()
378 .next_up();
379 let (radius_low, radius_high) = distance_bounds(points[support[0]], &enclosure);
380 let mut radius = (radius_high + centre_error).next_up();
381 let at = [centre.x, centre.y, centre.z].map(Interval::point);
384 for &p in points {
385 radius = radius.max(distance_bounds(p, &at).1);
386 }
387 Ok(MinimumSphere {
388 sphere: EnclosingSphere { centre, radius },
389 evidence: SphereEvidence {
390 support,
391 error: (radius - radius_low).next_up(),
392 },
393 })
394}
395
396#[derive(Debug, Clone, Copy, PartialEq)]
402pub struct OrientedBox {
403 pub centre: Point3,
405 pub axes: [Vec3; 3],
408 pub half_extents: [f64; 3],
410}
411
412impl OrientedBox {
413 #[must_use]
415 pub fn volume(&self) -> f64 {
416 8.0 * self.half_extents[0] * self.half_extents[1] * self.half_extents[2]
417 }
418
419 #[must_use]
422 pub fn corners(&self) -> [Point3; 8] {
423 std::array::from_fn(|i| {
424 let mut p = self.centre;
425 for k in 0..3 {
426 let s = if i & (1 << k) == 0 { -1.0 } else { 1.0 };
427 p += self.axes[k] * (s * self.half_extents[k]);
428 }
429 p
430 })
431 }
432}
433
434#[derive(Debug, Clone, Copy, PartialEq)]
436#[non_exhaustive]
437pub struct BoxEvidence {
438 pub candidates: usize,
440 pub axis_aligned_volume: f64,
443 pub orthogonality: f64,
446}
447
448#[derive(Debug, Clone, Copy, PartialEq)]
450#[non_exhaustive]
451pub struct OrientedBoundingBox {
452 pub bounding_box: OrientedBox,
454 pub evidence: BoxEvidence,
456}
457
458fn unit(v: Vec3) -> Option<Vec3> {
459 let length = v.length();
460 (length > 0.0 && length.is_finite()).then(|| v / length)
461}
462
463fn frame(normal: Vec3, first: Vec3) -> Option<[Vec3; 3]> {
466 let n = unit(normal)?;
467 let a = unit(first - n * first.dot(n))?;
468 let b = unit(n.cross(a))?;
469 Some([a, b, n])
470}
471
472fn perpendicular(n: Vec3) -> Vec3 {
474 let helper = if n.x.abs() <= n.y.abs() && n.x.abs() <= n.z.abs() {
475 Vec3::X
476 } else if n.y.abs() <= n.z.abs() {
477 Vec3::Y
478 } else {
479 Vec3::Z
480 };
481 unit(n.cross(helper)).unwrap_or(Vec3::X)
482}
483
484fn flush_frame(points: &[Point3], normal: Vec3) -> Option<[Vec3; 3]> {
487 let n = unit(normal)?;
488 let e1 = perpendicular(n);
489 let e2 = n.cross(e1);
490 let projected: Vec<Point2> = points
491 .iter()
492 .map(|p| Point2::new(p.dot(e1), p.dot(e2)))
493 .collect();
494 let rectangle = axiolid_overlay::minimum_area_rectangle(&projected).ok()?;
495 let [r, _] = rectangle.rectangle.axes;
496 frame(n, e1 * r.x + e2 * r.y)
497}
498
499fn principal_axes(points: &[Point3]) -> [Vec3; 3] {
501 let n = points.len() as f64;
502 let mean = points.iter().fold(Vec3::ZERO, |s, p| s + *p) / n;
503 let mut c = [[0.0f64; 3]; 3];
504 for p in points {
505 let d = *p - mean;
506 let d = [d.x, d.y, d.z];
507 for i in 0..3 {
508 for j in 0..3 {
509 c[i][j] += d[i] * d[j];
510 }
511 }
512 }
513 let mut v = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
514 for _ in 0..32 {
515 let off = c[0][1].abs() + c[0][2].abs() + c[1][2].abs();
516 if off == 0.0 {
517 break;
518 }
519 for (p, q) in [(0, 1), (0, 2), (1, 2)] {
520 if c[p][q] == 0.0 {
521 continue;
522 }
523 let theta = (c[q][q] - c[p][p]) / (2.0 * c[p][q]);
524 let t = theta.signum() / (theta.abs() + (theta * theta + 1.0).sqrt());
525 let cs = 1.0 / (t * t + 1.0).sqrt();
526 let sn = t * cs;
527 for row in &mut c {
528 let (a, b) = (row[p], row[q]);
529 row[p] = cs * a - sn * b;
530 row[q] = sn * a + cs * b;
531 }
532 for k in 0..3 {
533 let (a, b) = (c[p][k], c[q][k]);
534 c[p][k] = cs * a - sn * b;
535 c[q][k] = sn * a + cs * b;
536 }
537 for row in &mut v {
538 let (a, b) = (row[p], row[q]);
539 row[p] = cs * a - sn * b;
540 row[q] = sn * a + cs * b;
541 }
542 }
543 }
544 std::array::from_fn(|k| Vec3::new(v[0][k], v[1][k], v[2][k]))
545}
546
547fn ranking_volume(points: &[Point3], axes: &[Vec3; 3], pad: f64) -> f64 {
553 let mut volume = 1.0;
554 for a in axes {
555 let (low, high) = points
556 .iter()
557 .fold((f64::INFINITY, f64::NEG_INFINITY), |(l, h), p| {
558 let s = p.dot(*a);
559 (l.min(s), h.max(s))
560 });
561 volume *= high - low + pad;
562 }
563 volume
564}
565
566fn fit(points: &[Point3], axes: [Vec3; 3]) -> OrientedBox {
572 let e = Dyadic::from_f64;
573 let project = |p: Point3, a: Vec3| {
574 e(p.x)
575 .mul(&e(a.x))
576 .add(&e(p.y).mul(&e(a.y)))
577 .add(&e(p.z).mul(&e(a.z)))
578 };
579 let is_less = |x: &Dyadic, y: &Dyadic| x.sub(y).sign() == Some(Sign::Negative);
580 let mut low: Vec<Dyadic> = axes.iter().map(|a| project(points[0], *a)).collect();
581 let mut high = low.clone();
582 for &p in &points[1..] {
583 for k in 0..3 {
584 let s = project(p, axes[k]);
585 if is_less(&s, &low[k]) {
586 low[k] = s;
587 } else if is_less(&high[k], &s) {
588 high[k] = s;
589 }
590 }
591 }
592 let half = e(0.5);
593 let mut centre = Point3::ZERO;
594 for k in 0..3 {
595 centre += axes[k] * low[k].add(&high[k]).mul(&half).to_f64();
596 }
597 let half_extents = std::array::from_fn(|k| {
598 let c = project(centre, axes[k]);
599 let (above, below) = (high[k].sub(&c), c.sub(&low[k]));
600 let widest = if is_less(&above, &below) {
601 below
602 } else {
603 above
604 };
605 round_up(&widest).max(0.0)
606 });
607 OrientedBox {
608 centre,
609 axes,
610 half_extents,
611 }
612}
613
614fn round_up(d: &Dyadic) -> f64 {
616 let mut x = d.to_f64();
617 let e = Dyadic::from_f64;
618 while e(x).sub(d).sign() == Some(Sign::Negative) {
619 x = x.next_up();
620 }
621 while e(x.next_down()).sub(d).sign() != Some(Sign::Negative) && x.next_down() >= 0.0 {
622 x = x.next_down();
623 }
624 x
625}
626
627fn orthogonality(axes: &[Vec3; 3]) -> f64 {
629 let e = Dyadic::from_f64;
630 let mut worst = 0.0f64;
631 for i in 0..3 {
632 for j in i..3 {
633 let (a, b) = (axes[i], axes[j]);
634 let mut d = e(a.x)
635 .mul(&e(b.x))
636 .add(&e(a.y).mul(&e(b.y)))
637 .add(&e(a.z).mul(&e(b.z)));
638 if i == j {
639 d = d.sub(&e(1.0));
640 }
641 if d.sign() == Some(Sign::Negative) {
642 d = d.neg();
643 }
644 worst = worst.max(d.enclosure().hi());
645 }
646 }
647 worst
648}
649
650pub fn oriented_bounding_box(points: &[Point3]) -> Result<OrientedBoundingBox, BoundingError> {
660 validate(points)?;
661 let hull = crate::hull::convex_hull(points).ok();
664 let extreme: &[Point3] = hull.as_ref().map_or(points, |h| &h.positions);
665
666 let world = [Vec3::X, Vec3::Y, Vec3::Z];
667 let principal = principal_axes(points);
668 let mut frames: Vec<[Vec3; 3]> = vec![world];
669 if let Some(f) = frame(principal[0].cross(principal[1]), principal[0]) {
670 frames.push(f);
671 }
672 let mut normals: Vec<Vec3> = world.to_vec();
673 normals.extend(principal);
674 match &hull {
675 Some(h) => {
676 for t in h.indices.chunks_exact(3) {
677 let [a, b, c] = [0, 1, 2].map(|k| h.positions[t[k] as usize]);
678 normals.push((b - a).cross(c - a));
679 }
680 }
681 None => normals.push(flat_normal(points)),
682 }
683 let mut seen: Vec<Vec3> = Vec::new();
684 for normal in normals {
685 let Some(n) = unit(normal) else { continue };
686 let n = if (n.x, n.y, n.z) < (0.0, 0.0, 0.0) {
688 -n
689 } else {
690 n
691 };
692 if seen.contains(&n) {
693 continue;
694 }
695 seen.push(n);
696 if let Some(f) = flush_frame(extreme, n) {
697 frames.push(f);
698 }
699 }
700
701 let axis_aligned_volume = fit(points, world).volume();
702 let mut best = world;
703 let size = world
705 .iter()
706 .map(|a| {
707 let along = extreme.iter().map(|p| p.dot(*a));
708 along.clone().fold(f64::NEG_INFINITY, f64::max) - along.fold(f64::INFINITY, f64::min)
709 })
710 .fold(0.0, f64::max);
711 let pad = 1e-9 * size;
712 let mut best_volume = ranking_volume(extreme, &world, pad);
713 for f in &frames[1..] {
714 let volume = ranking_volume(extreme, f, pad);
715 if volume < best_volume {
716 best = *f;
717 best_volume = volume;
718 }
719 }
720 let mut chosen = fit(points, best);
721 if chosen.volume() > axis_aligned_volume {
724 best = world;
725 chosen = fit(points, world);
726 }
727 Ok(OrientedBoundingBox {
728 bounding_box: chosen,
729 evidence: BoxEvidence {
730 candidates: frames.len(),
731 axis_aligned_volume,
732 orthogonality: orthogonality(&best),
733 },
734 })
735}
736
737fn flat_normal(points: &[Point3]) -> Vec3 {
741 let a = points[0];
742 let farthest = |from: &dyn Fn(Point3) -> f64| {
743 points
744 .iter()
745 .copied()
746 .max_by(|p, q| from(*p).total_cmp(&from(*q)))
747 .unwrap_or(a)
748 };
749 let b = farthest(&|p| (p - a).length_squared());
750 let d = b - a;
751 let c = farthest(&|p| (p - a).cross(d).length_squared());
752 let n = d.cross(c - a);
753 if unit(n).is_some() {
754 n
755 } else if let Some(d) = unit(d) {
756 perpendicular(d)
757 } else {
758 Vec3::Z
759 }
760}