use super::{correspond, nearest, Tri};
pub type Body = Vec<Tri>;
pub fn align(poses: &[Vec<Body>]) -> Vec<Vec<Option<usize>>> {
if poses.is_empty() {
return Vec::new();
}
let mut out: Vec<Vec<Option<usize>>> = vec![(0..poses[0].len()).map(Some).collect()];
for p in 1..poses.len() {
let mut step = vec![None; poses[p - 1].len()];
for (i, j) in correspond(&poses[p - 1], &poses[p]) {
step[i] = Some(j);
}
let mapped = out[p - 1]
.iter()
.map(|slot| slot.and_then(|i| step.get(i).copied().flatten()))
.collect();
out.push(mapped);
}
out
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Engagement {
pub a: usize,
pub b: usize,
pub touching: usize,
pub present: usize,
pub widest: f64,
pub at_first: bool,
pub at_last: bool,
}
impl Engagement {
pub fn persistent(&self) -> bool {
self.touching == self.present && self.present > 0
}
pub fn came_apart(&self) -> bool {
self.at_first && !self.at_last
}
pub fn came_together(&self) -> bool {
!self.at_first && self.at_last
}
}
pub fn engagement(poses: &[Vec<Body>], tolerance: f64) -> Vec<Engagement> {
engagement_with(poses, &align(poses), tolerance)
}
pub fn engagement_with(
poses: &[Vec<Body>],
alignment: &[Vec<Option<usize>>],
tolerance: f64,
) -> Vec<Engagement> {
let bodies = alignment.first().map_or(0, |p| p.len());
let mut out = Vec::new();
for a in 0..bodies {
for b in (a + 1)..bodies {
let mut touching = 0;
let mut present = 0;
let mut widest: f64 = 0.0;
let mut ever = false;
let mut at_first = false;
let mut at_last = false;
let mut seen = false;
for (p, pose) in poses.iter().enumerate() {
let (Some(ia), Some(ib)) = (alignment[p][a], alignment[p][b]) else {
continue;
};
let (Some(ba), Some(bb)) = (pose.get(ia), pose.get(ib)) else {
continue;
};
present += 1;
let gap = if bounds_gap(ba, bb) > tolerance {
bounds_gap(ba, bb)
} else {
nearest(ba, bb)
};
widest = widest.max(gap);
let close = gap <= tolerance;
if close {
touching += 1;
ever = true;
}
if !seen {
at_first = close;
seen = true;
}
at_last = close;
}
if ever {
out.push(Engagement {
a,
b,
touching,
present,
widest,
at_first,
at_last,
});
}
}
}
out
}
pub fn floating(poses: &[Vec<Body>], tolerance: f64) -> Vec<usize> {
floating_with(poses, &align(poses), tolerance)
}
pub fn floating_with(
poses: &[Vec<Body>],
alignment: &[Vec<Option<usize>>],
tolerance: f64,
) -> Vec<usize> {
let bodies = alignment.first().map_or(0, |p| p.len());
let attached: std::collections::HashSet<usize> = engagement_with(poses, alignment, tolerance)
.iter()
.flat_map(|e| [e.a, e.b])
.collect();
(0..bodies).filter(|i| !attached.contains(i)).collect()
}
fn bounds_gap(a: &[Tri], b: &[Tri]) -> f64 {
let (alo, ahi) = super::aabb(a);
let (blo, bhi) = super::aabb(b);
let mut d = 0.0;
for k in 0..3 {
let gap = (alo[k] - bhi[k]).max(blo[k] - ahi[k]).max(0.0);
d += gap * gap;
}
d.sqrt()
}