1use axiolid_core::{Aabb, Point2, Point3, Tolerance};
41use axiolid_exact::{certify, Arith, SignExpr};
42use axiolid_guarantees::Sign;
43use axiolid_heal::self_intersections;
44use axiolid_mesh::{audit_mesh, TriangleMeshView};
45use axiolid_spatial::{Bvh, SpatialIndex, SpatialItem};
46use core::ops::ControlFlow;
47
48#[derive(Debug, Clone, Copy, PartialEq)]
50pub struct VolumeInterval {
51 pub lower: f64,
53 pub upper: f64,
55}
56
57impl VolumeInterval {
58 #[must_use]
60 pub fn contains(&self, volume: f64) -> bool {
61 self.lower <= volume && volume <= self.upper
62 }
63
64 #[must_use]
66 pub fn width(&self) -> f64 {
67 self.upper - self.lower
68 }
69}
70
71#[derive(Debug, Clone, Copy, PartialEq, Eq)]
73#[non_exhaustive]
74pub enum Operand {
75 First,
77 Second,
79}
80
81#[derive(Debug, Clone, Copy, PartialEq, Eq)]
83#[non_exhaustive]
84pub enum OverlapError {
85 NonFinite {
87 operand: Operand,
89 },
90 NotClosed {
93 operand: Operand,
95 },
96 SelfIntersecting {
99 operand: Operand,
101 triangles: [u32; 2],
103 },
104 NoVolume {
106 operand: Operand,
108 },
109}
110
111pub fn enclosed_volume<M: TriangleMeshView + ?Sized>(
119 mesh: &M,
120) -> Result<VolumeInterval, OverlapError> {
121 let solid = Solid::new(mesh, Operand::First, None)?;
122 Ok(clamp(solid.volume, 0.0, f64::INFINITY))
123}
124
125pub fn intersection_volume<A, B>(first: &A, second: &B) -> Result<VolumeInterval, OverlapError>
132where
133 A: TriangleMeshView + ?Sized,
134 B: TriangleMeshView + ?Sized,
135{
136 let (a, b) = solids(first, second)?;
137 Ok(shared(&a, &b))
138}
139
140pub fn difference_volume<A, B>(first: &A, second: &B) -> Result<VolumeInterval, OverlapError>
146where
147 A: TriangleMeshView + ?Sized,
148 B: TriangleMeshView + ?Sized,
149{
150 let (a, b) = solids(first, second)?;
151 let both = shared(&a, &b);
152 let rest = a.volume.sub(Iv::new(both.lower, both.upper));
153 Ok(clamp(rest, 0.0, a.volume.hi))
154}
155
156fn solids<A, B>(first: &A, second: &B) -> Result<(Solid, Solid), OverlapError>
157where
158 A: TriangleMeshView + ?Sized,
159 B: TriangleMeshView + ?Sized,
160{
161 let base = lowest(first).min(lowest(second));
162 let a = Solid::new(first, Operand::First, Some(base))?;
163 let b = Solid::new(second, Operand::Second, Some(base))?;
164 Ok((a, b))
165}
166
167fn shared(a: &Solid, b: &Solid) -> VolumeInterval {
169 let items: Vec<SpatialItem<u32>> = b
170 .faces
171 .iter()
172 .enumerate()
173 .map(|(i, f)| SpatialItem::new(i as u32, f.shadow()))
174 .collect();
175 let bvh = Bvh::build(items);
176 let mut total = Iv::point(0.0);
177 for f in &a.faces {
178 bvh.visit_aabb(&f.shadow(), &mut |j: &u32| {
179 let g = &b.faces[*j as usize];
180 let term = overlap(f, g, a.base);
181 total = if f.up == g.up {
182 total.add(term)
183 } else {
184 total.sub(term)
185 };
186 ControlFlow::Continue(())
187 });
188 }
189 clamp(total, 0.0, a.volume.hi.min(b.volume.hi))
190}
191
192fn clamp(v: Iv, low: f64, high: f64) -> VolumeInterval {
193 VolumeInterval {
194 lower: v.lo.max(low).min(high),
195 upper: v.hi.min(high).max(low),
196 }
197}
198
199fn lowest<M: TriangleMeshView + ?Sized>(mesh: &M) -> f64 {
200 (0..mesh.position_count())
201 .map(|i| mesh.position(i).z)
202 .fold(f64::INFINITY, f64::min)
203}
204
205#[derive(Debug, Clone, Copy)]
209struct Face {
210 p: [Point3; 3],
211 up: bool,
212}
213
214impl Face {
215 fn shadow(&self) -> Aabb {
216 let mut b = Aabb::from_point(Point3::new(self.p[0].x, self.p[0].y, 0.0));
217 for q in &self.p[1..] {
218 b.extend(Point3::new(q.x, q.y, 0.0));
219 }
220 b
221 }
222
223 fn flat(&self, i: usize) -> Point2 {
224 Point2::new(self.p[i].x, self.p[i].y)
225 }
226
227 fn lines(&self) -> [Line; 3] {
229 [0, 1, 2].map(|i| Line {
230 a: self.flat(i),
231 b: self.flat((i + 1) % 3),
232 })
233 }
234
235 fn normal(&self) -> [Iv; 3] {
237 let d = |i: usize, k: usize| Iv::point(self.p[i][k]).sub(Iv::point(self.p[0][k]));
238 let (u, v) = ([d(1, 0), d(1, 1), d(1, 2)], [d(2, 0), d(2, 1), d(2, 2)]);
239 [
240 u[1].mul(v[2]).sub(u[2].mul(v[1])),
241 u[2].mul(v[0]).sub(u[0].mul(v[2])),
242 u[0].mul(v[1]).sub(u[1].mul(v[0])),
243 ]
244 }
245
246 fn height(&self, at: [Iv; 2], base: f64) -> Iv {
248 let n = self.normal();
249 let dx = at[0].sub(Iv::point(self.p[0].x));
250 let dy = at[1].sub(Iv::point(self.p[0].y));
251 let lift = n[0].mul(dx).add(n[1].mul(dy)).div(n[2]);
252 Iv::point(self.p[0].z).sub(Iv::point(base)).sub(lift)
253 }
254}
255
256struct Solid {
257 faces: Vec<Face>,
258 volume: Iv,
259 base: f64,
260}
261
262impl Solid {
263 fn new<M: TriangleMeshView + ?Sized>(
264 mesh: &M,
265 operand: Operand,
266 base: Option<f64>,
267 ) -> Result<Self, OverlapError> {
268 if (0..mesh.position_count()).any(|i| !mesh.position(i).is_finite()) {
269 return Err(OverlapError::NonFinite { operand });
270 }
271 if !audit_mesh(mesh, Tolerance::ZERO).is_closed_two_manifold() {
272 return Err(OverlapError::NotClosed { operand });
273 }
274 if let Some(pair) = self_intersections(mesh).first() {
275 return Err(OverlapError::SelfIntersecting {
276 operand,
277 triangles: [pair.first, pair.second],
278 });
279 }
280 let base = base.unwrap_or_else(|| lowest(mesh));
281 let mut faces = Vec::new();
282 for t in 0..mesh.triangle_count() {
283 let [a, b, c] = mesh.triangle(t).map(|i| mesh.position(i as usize));
284 let flat = |p: Point3| Point2::new(p.x, p.y);
285 match sign(&Orient {
286 a: flat(a),
287 b: flat(b),
288 c: flat(c),
289 }) {
290 Sign::Positive => faces.push(Face {
291 p: [a, b, c],
292 up: true,
293 }),
294 Sign::Negative => faces.push(Face {
295 p: [a, c, b],
296 up: false,
297 }),
298 _ => {}
299 }
300 }
301 let mut volume = Iv::point(0.0);
303 for f in &faces {
304 let area = triangle_area(f.flat(0), f.flat(1), f.flat(2));
305 let rise = [0, 1, 2]
306 .map(|i| Iv::point(f.p[i].z).sub(Iv::point(base)))
307 .into_iter()
308 .fold(Iv::point(0.0), Iv::add)
309 .div(Iv::point(3.0));
310 let term = area.mul(rise);
311 volume = if f.up {
312 volume.add(term)
313 } else {
314 volume.sub(term)
315 };
316 }
317 if volume.hi < 0.0 {
319 volume = Iv::point(0.0).sub(volume);
320 for f in &mut faces {
321 f.up = !f.up;
322 }
323 } else if volume.lo <= 0.0 {
324 return Err(OverlapError::NoVolume { operand });
325 }
326 Ok(Self {
327 faces,
328 volume,
329 base,
330 })
331 }
332}
333
334fn triangle_area(a: Point2, b: Point2, c: Point2) -> Iv {
335 let (ux, uy) = (
336 Iv::point(b.x).sub(Iv::point(a.x)),
337 Iv::point(b.y).sub(Iv::point(a.y)),
338 );
339 let (vx, vy) = (
340 Iv::point(c.x).sub(Iv::point(a.x)),
341 Iv::point(c.y).sub(Iv::point(a.y)),
342 );
343 ux.mul(vy).sub(uy.mul(vx)).mul(Iv::point(0.5))
344}
345
346fn overlap(f: &Face, g: &Face, base: f64) -> Iv {
349 let mut poly: Vec<(Vertex, Line)> = (0..3)
351 .map(|i| (Vertex::Input(f.flat(i)), f.lines()[i]))
352 .collect();
353 for line in g.lines() {
354 poly = clip(&poly, line);
355 if poly.len() < 3 {
356 return Iv::point(0.0);
357 }
358 }
359 let signs: Vec<Sign> = poly
361 .iter()
362 .map(|(v, _)| {
363 sign(&Lower {
364 f: *f,
365 g: *g,
366 v: *v,
367 })
368 })
369 .collect();
370 let points: Vec<[Iv; 2]> = poly.iter().map(|(v, _)| v.enclose()).collect();
371 if signs.iter().all(|s| *s == Sign::Zero) {
372 return integral(&points, f, base);
374 }
375 let rise = |p: [Iv; 2]| f.height(p, base).sub(g.height(p, base));
376 let below = split(&points, &signs, Sign::Negative, &rise);
377 let above = split(&points, &signs, Sign::Positive, &rise);
378 integral(&below, f, base).add(integral(&above, g, base))
379}
380
381fn split(
384 points: &[[Iv; 2]],
385 signs: &[Sign],
386 keep: Sign,
387 rise: &dyn Fn([Iv; 2]) -> Iv,
388) -> Vec<[Iv; 2]> {
389 let n = points.len();
390 let mut out = Vec::new();
391 for i in 0..n {
392 let j = (i + 1) % n;
393 let (si, sj) = (signs[i], signs[j]);
394 if si == keep || si == Sign::Zero {
395 out.push(points[i]);
396 }
397 let opposite = matches!(
398 (si, sj),
399 (Sign::Negative, Sign::Positive) | (Sign::Positive, Sign::Negative)
400 );
401 if opposite {
402 let (ri, rj) = (rise(points[i]), rise(points[j]));
404 let t = ri.div(ri.sub(rj)).within(0.0, 1.0);
405 let at = |k: usize| points[i][k].add(t.mul(points[j][k].sub(points[i][k])));
406 out.push([at(0), at(1)]);
407 }
408 }
409 out
410}
411
412fn integral(points: &[[Iv; 2]], face: &Face, base: f64) -> Iv {
415 if points.len() < 3 {
416 return Iv::point(0.0);
417 }
418 let heights: Vec<Iv> = points.iter().map(|p| face.height(*p, base)).collect();
419 let mut total = Iv::point(0.0);
420 for i in 1..points.len() - 1 {
421 let (a, b, c) = (points[0], points[i], points[i + 1]);
422 let area = b[0]
423 .sub(a[0])
424 .mul(c[1].sub(a[1]))
425 .sub(b[1].sub(a[1]).mul(c[0].sub(a[0])))
426 .mul(Iv::point(0.5));
427 let mean = heights[0]
428 .add(heights[i])
429 .add(heights[i + 1])
430 .div(Iv::point(3.0));
431 total = total.add(area.mul(mean));
432 }
433 total
434}
435
436#[derive(Debug, Clone, Copy, PartialEq)]
438struct Line {
439 a: Point2,
440 b: Point2,
441}
442
443#[derive(Debug, Clone, Copy)]
446enum Vertex {
447 Input(Point2),
448 Cross(Line, Line),
449}
450
451impl Vertex {
452 fn homogeneous<T: Arith>(&self) -> [T; 3] {
456 let f = T::from_f64;
457 match *self {
458 Vertex::Input(p) => [f(p.x), f(p.y), f(1.0)],
459 Vertex::Cross(l1, l2) => {
460 let d1 = [f(l1.b.x).sub(&f(l1.a.x)), f(l1.b.y).sub(&f(l1.a.y))];
461 let d2 = [f(l2.b.x).sub(&f(l2.a.x)), f(l2.b.y).sub(&f(l2.a.y))];
462 let w = d1[0].mul(&d2[1]).sub(&d1[1].mul(&d2[0]));
463 let e = [f(l2.a.x).sub(&f(l1.a.x)), f(l2.a.y).sub(&f(l1.a.y))];
464 let n = e[0].mul(&d2[1]).sub(&e[1].mul(&d2[0]));
465 [
466 f(l1.a.x).mul(&w).add(&d1[0].mul(&n)),
467 f(l1.a.y).mul(&w).add(&d1[1].mul(&n)),
468 w,
469 ]
470 }
471 }
472 }
473
474 fn enclose(&self) -> [Iv; 2] {
475 match *self {
476 Vertex::Input(p) => [Iv::point(p.x), Iv::point(p.y)],
477 Vertex::Cross(l1, l2) => {
478 let d1 = [
479 Iv::point(l1.b.x).sub(Iv::point(l1.a.x)),
480 Iv::point(l1.b.y).sub(Iv::point(l1.a.y)),
481 ];
482 let d2 = [
483 Iv::point(l2.b.x).sub(Iv::point(l2.a.x)),
484 Iv::point(l2.b.y).sub(Iv::point(l2.a.y)),
485 ];
486 let w = d1[0].mul(d2[1]).sub(d1[1].mul(d2[0]));
487 let e = [
488 Iv::point(l2.a.x).sub(Iv::point(l1.a.x)),
489 Iv::point(l2.a.y).sub(Iv::point(l1.a.y)),
490 ];
491 let t = e[0].mul(d2[1]).sub(e[1].mul(d2[0])).div(w);
492 [
493 Iv::point(l1.a.x).add(d1[0].mul(t)),
494 Iv::point(l1.a.y).add(d1[1].mul(t)),
495 ]
496 }
497 }
498 }
499}
500
501fn clip(poly: &[(Vertex, Line)], line: Line) -> Vec<(Vertex, Line)> {
505 let n = poly.len();
506 let signs: Vec<Sign> = poly
507 .iter()
508 .map(|(v, _)| sign(&SideOf { line, v: *v }))
509 .collect();
510 let mut out = Vec::with_capacity(n + 1);
511 for i in 0..n {
512 let (v, edge) = poly[i];
513 let (si, sj) = (signs[i], signs[(i + 1) % n]);
514 if si != Sign::Negative {
515 let along = si == Sign::Zero && sj == Sign::Negative;
518 out.push((v, if along { line } else { edge }));
519 }
520 match (si, sj) {
521 (Sign::Positive, Sign::Negative) => out.push((Vertex::Cross(edge, line), line)),
522 (Sign::Negative, Sign::Positive) => out.push((Vertex::Cross(edge, line), edge)),
523 _ => {}
524 }
525 }
526 out
527}
528
529fn sign<E: SignExpr>(e: &E) -> Sign {
531 certify(e).unwrap_or(Sign::Zero)
532}
533
534struct Orient {
535 a: Point2,
536 b: Point2,
537 c: Point2,
538}
539
540impl SignExpr for Orient {
541 fn sign_in<T: Arith>(&self) -> Option<Sign> {
542 SideOf {
543 line: Line {
544 a: self.a,
545 b: self.b,
546 },
547 v: Vertex::Input(self.c),
548 }
549 .sign_in::<T>()
550 }
551}
552
553struct SideOf {
555 line: Line,
556 v: Vertex,
557}
558
559impl SignExpr for SideOf {
560 fn sign_in<T: Arith>(&self) -> Option<Sign> {
561 let f = T::from_f64;
562 let [x, y, w] = self.v.homogeneous::<T>();
563 let (a, b) = (self.line.a, self.line.b);
564 let (dx, dy) = (f(b.x).sub(&f(a.x)), f(b.y).sub(&f(a.y)));
565 let rx = x.sub(&f(a.x).mul(&w));
566 let ry = y.sub(&f(a.y).mul(&w));
567 let s = dx.mul(&ry).sub(&dy.mul(&rx)).sign()?;
568 Some(times(s, w.sign()?))
569 }
570}
571
572struct Lower {
576 f: Face,
577 g: Face,
578 v: Vertex,
579}
580
581impl SignExpr for Lower {
582 fn sign_in<T: Arith>(&self) -> Option<Sign> {
583 let f = T::from_f64;
584 let [x, y, w] = self.v.homogeneous::<T>();
585 let normal = |face: &Face| {
586 let d = |i: usize, k: usize| f(face.p[i][k]).sub(&f(face.p[0][k]));
587 let (u, v) = ([d(1, 0), d(1, 1), d(1, 2)], [d(2, 0), d(2, 1), d(2, 2)]);
588 [
589 u[1].mul(&v[2]).sub(&u[2].mul(&v[1])),
590 u[2].mul(&v[0]).sub(&u[0].mul(&v[2])),
591 u[0].mul(&v[1]).sub(&u[1].mul(&v[0])),
592 ]
593 };
594 let (nf, ng) = (normal(&self.f), normal(&self.g));
595 let lift = |face: &Face, n: &[T; 3]| {
597 let p = face.p[0];
598 w.mul(&n[2])
599 .mul(&f(p.z))
600 .sub(&n[0].mul(&x.sub(&w.mul(&f(p.x)))))
601 .sub(&n[1].mul(&y.sub(&w.mul(&f(p.y)))))
602 };
603 let difference = lift(&self.f, &nf)
604 .mul(&ng[2])
605 .sub(&lift(&self.g, &ng).mul(&nf[2]));
606 Some(times(difference.sign()?, w.sign()?))
607 }
608}
609
610fn times(a: Sign, b: Sign) -> Sign {
611 match (a, b) {
612 (Sign::Zero, _) | (_, Sign::Zero) => Sign::Zero,
613 (x, y) if x == y => Sign::Positive,
614 _ => Sign::Negative,
615 }
616}
617
618#[derive(Debug, Clone, Copy, PartialEq)]
621struct Iv {
622 lo: f64,
623 hi: f64,
624}
625
626impl Iv {
627 fn point(v: f64) -> Self {
628 Self { lo: v, hi: v }
629 }
630
631 fn new(lo: f64, hi: f64) -> Self {
632 Self { lo, hi }
633 }
634
635 fn outward(lo: f64, hi: f64) -> Self {
636 if lo.is_nan() || hi.is_nan() {
637 return Self::new(f64::NEG_INFINITY, f64::INFINITY);
638 }
639 Self::new(lo.next_down(), hi.next_up())
640 }
641
642 fn add(self, o: Self) -> Self {
643 Self::outward(self.lo + o.lo, self.hi + o.hi)
644 }
645
646 fn sub(self, o: Self) -> Self {
647 Self::outward(self.lo - o.hi, self.hi - o.lo)
648 }
649
650 fn mul(self, o: Self) -> Self {
651 let p = [
652 self.lo * o.lo,
653 self.lo * o.hi,
654 self.hi * o.lo,
655 self.hi * o.hi,
656 ];
657 let lo = p.iter().copied().fold(f64::INFINITY, f64::min);
658 let hi = p.iter().copied().fold(f64::NEG_INFINITY, f64::max);
659 Self::outward(lo, hi)
660 }
661
662 fn div(self, o: Self) -> Self {
664 if o.lo <= 0.0 && o.hi >= 0.0 {
665 return Self::new(f64::NEG_INFINITY, f64::INFINITY);
666 }
667 let q = [
668 self.lo / o.lo,
669 self.lo / o.hi,
670 self.hi / o.lo,
671 self.hi / o.hi,
672 ];
673 let lo = q.iter().copied().fold(f64::INFINITY, f64::min);
674 let hi = q.iter().copied().fold(f64::NEG_INFINITY, f64::max);
675 Self::outward(lo, hi)
676 }
677
678 fn within(self, low: f64, high: f64) -> Self {
680 Self::new(self.lo.max(low), self.hi.min(high))
681 }
682}