use super::{centroid_area, Tri};
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Screw {
pub point: [f64; 3],
pub direction: [f64; 3],
pub angle: f64,
pub slide: f64,
pub residual: f64,
}
impl Screw {
pub fn is_revolute(&self, tol: f64) -> bool {
self.angle.abs() > tol && self.slide.abs() <= tol
}
pub fn is_prismatic(&self, tol: f64) -> bool {
self.angle.abs() <= tol && self.slide.abs() > tol
}
pub fn lead(&self) -> f64 {
if self.angle.abs() < 1e-12 {
f64::INFINITY
} else {
self.slide / self.angle
}
}
}
pub fn recover_screw(before: &[Tri], after: &[Tri]) -> Option<Screw> {
if before.is_empty() || before.len() != after.len() {
return None;
}
let ca = centroid_area(before);
let cb = centroid_area(after);
let pts: Vec<([f64; 3], [f64; 3])> = before
.iter()
.zip(after.iter())
.flat_map(|(a, b)| a.iter().copied().zip(b.iter().copied()))
.collect();
let mut mean = [0.0; 3];
for (p, q) in &pts {
for k in 0..3 {
mean[k] += (q[k] - p[k]) / pts.len() as f64;
}
}
let mut cov = [[0.0f64; 3]; 3];
for (p, q) in &pts {
let d = [
q[0] - p[0] - mean[0],
q[1] - p[1] - mean[1],
q[2] - p[2] - mean[2],
];
for i in 0..3 {
for j in 0..3 {
cov[i][j] += d[i] * d[j];
}
}
}
let spread = cov[0][0] + cov[1][1] + cov[2][2];
let scale = radius(before, ca).max(1e-12);
if spread <= (scale * 1e-6).powi(2) * pts.len() as f64 {
let slide = norm(mean);
if slide < 1e-12 {
return None; }
return Some(Screw {
point: ca,
direction: scaled(mean, 1.0 / slide),
angle: 0.0,
slide,
residual: 0.0,
});
}
let axis = smallest_eigenvector(cov)?;
let planar = recover_in_plane(before, after, ca, cb, axis)?;
Some(Screw {
point: planar.point,
direction: axis,
angle: planar.angle,
slide: dot(sub(cb, ca), axis),
residual: planar.residual,
})
}
pub(super) struct Planar {
pub point: [f64; 3],
pub angle: f64,
pub residual: f64,
}
pub(super) fn recover_in_plane(
before: &[Tri],
after: &[Tri],
ca: [f64; 3],
cb: [f64; 3],
axis: [f64; 3],
) -> Option<Planar> {
let (u, v) = basis(axis);
let proj = |p: [f64; 3], o: [f64; 3]| {
let d = sub(p, o);
[dot(d, u), dot(d, v)]
};
let (mut cross_sum, mut dot_sum) = (0.0, 0.0);
let mut count = 0usize;
for (ta, tb) in before.iter().zip(after.iter()) {
for (pa, pb) in ta.iter().zip(tb.iter()) {
let a = proj(*pa, ca);
let b = proj(*pb, cb);
cross_sum += a[0] * b[1] - a[1] * b[0];
dot_sum += a[0] * b[0] + a[1] * b[1];
count += 1;
}
}
if count == 0 {
return None;
}
let angle = cross_sum.atan2(dot_sum);
let (co, si) = (angle.cos(), angle.sin());
let mut residual = 0.0;
for (ta, tb) in before.iter().zip(after.iter()) {
for (pa, pb) in ta.iter().zip(tb.iter()) {
let a = proj(*pa, ca);
let b = proj(*pb, cb);
let rx = co * a[0] - si * a[1];
let ry = si * a[0] + co * a[1];
residual += (rx - b[0]).powi(2) + (ry - b[1]).powi(2);
}
}
let residual = (residual / count as f64).sqrt();
let cap = proj(ca, ca);
let cbp = proj(cb, ca);
let det = 2.0 * (1.0 - co);
if det < 1e-9 {
return None;
}
let dx = cbp[0] - (co * cap[0] - si * cap[1]);
let dy = cbp[1] - (si * cap[0] + co * cap[1]);
let cx = ((1.0 - co) * dx - si * dy) / det;
let cy = (si * dx + (1.0 - co) * dy) / det;
Some(Planar {
point: [
ca[0] + u[0] * cx + v[0] * cy,
ca[1] + u[1] * cx + v[1] * cy,
ca[2] + u[2] * cx + v[2] * cy,
],
angle,
residual,
})
}
pub(super) fn radius(tris: &[Tri], c: [f64; 3]) -> f64 {
let mut r: f64 = 0.0;
for t in tris {
for v in t {
r = r.max(norm(sub(*v, c)));
}
}
r
}
fn smallest_eigenvector(a: [[f64; 3]; 3]) -> Option<[f64; 3]> {
eigenvector(a, symmetric_eigenvalues(a)[2])
}
pub(super) fn symmetric_eigen(a: [[f64; 3]; 3]) -> ([f64; 3], [[f64; 3]; 3]) {
let values = symmetric_eigenvalues(a);
let mut vectors = [[0.0; 3]; 3];
for (i, &l) in values.iter().enumerate() {
vectors[i] = eigenvector(a, l).unwrap_or([0.0; 3]);
}
(values, vectors)
}
fn eigenvector(a: [[f64; 3]; 3], lambda: f64) -> Option<[f64; 3]> {
let shifted = [
[a[0][0] - lambda, a[0][1], a[0][2]],
[a[1][0], a[1][1] - lambda, a[1][2]],
[a[2][0], a[2][1], a[2][2] - lambda],
];
let mut best: Option<([f64; 3], f64)> = None;
for (i, j) in [(0, 1), (0, 2), (1, 2)] {
let c = cross(shifted[i], shifted[j]);
let l = norm(c);
if best.is_none_or(|(_, bl)| l > bl) {
best = Some((c, l));
}
}
let (v, l) = best?;
(l > 1e-300).then(|| scaled(v, 1.0 / l))
}
fn symmetric_eigenvalues(a: [[f64; 3]; 3]) -> [f64; 3] {
let p1 = a[0][1].powi(2) + a[0][2].powi(2) + a[1][2].powi(2);
let q = (a[0][0] + a[1][1] + a[2][2]) / 3.0;
if p1 <= 0.0 {
let mut d = [a[0][0], a[1][1], a[2][2]];
d.sort_by(|x, y| y.partial_cmp(x).unwrap_or(std::cmp::Ordering::Equal));
return d;
}
let p2 = (a[0][0] - q).powi(2) + (a[1][1] - q).powi(2) + (a[2][2] - q).powi(2) + 2.0 * p1;
let p = (p2 / 6.0).sqrt();
if p <= 0.0 {
return [q, q, q];
}
let b = [
[(a[0][0] - q) / p, a[0][1] / p, a[0][2] / p],
[a[1][0] / p, (a[1][1] - q) / p, a[1][2] / p],
[a[2][0] / p, a[2][1] / p, (a[2][2] - q) / p],
];
let det = b[0][0] * (b[1][1] * b[2][2] - b[1][2] * b[2][1])
- b[0][1] * (b[1][0] * b[2][2] - b[1][2] * b[2][0])
+ b[0][2] * (b[1][0] * b[2][1] - b[1][1] * b[2][0]);
let r = (det / 2.0).clamp(-1.0, 1.0);
let phi = r.acos() / 3.0;
let e1 = q + 2.0 * p * phi.cos();
let e3 = q + 2.0 * p * (phi + 2.0 * std::f64::consts::PI / 3.0).cos();
[e1, 3.0 * q - e1 - e3, e3]
}
fn basis(n: [f64; 3]) -> ([f64; 3], [f64; 3]) {
let t = if n[0].abs() < 0.9 {
[1.0, 0.0, 0.0]
} else {
[0.0, 1.0, 0.0]
};
let u = unit(cross(t, n));
(u, unit(cross(n, u)))
}
fn sub(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
[a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}
fn scaled(a: [f64; 3], s: f64) -> [f64; 3] {
[a[0] * s, a[1] * s, a[2] * s]
}
fn dot(a: [f64; 3], b: [f64; 3]) -> f64 {
a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
[
a[1] * b[2] - a[2] * b[1],
a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0],
]
}
fn norm(a: [f64; 3]) -> f64 {
dot(a, a).sqrt()
}
fn unit(a: [f64; 3]) -> [f64; 3] {
let l = norm(a);
if l > 0.0 {
scaled(a, 1.0 / l)
} else {
[0.0, 0.0, 1.0]
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum ParamMotion {
Fixed,
Rigid(Screw),
Mixed {
screw: Screw,
residual: f64,
},
Reshaped {
before: usize,
after: usize,
},
}
impl ParamMotion {
pub fn is_poseable(&self) -> bool {
matches!(self, ParamMotion::Fixed | ParamMotion::Rigid(_))
}
}
pub fn classify(before: &[Tri], after: &[Tri], tol: f64) -> ParamMotion {
if before.len() != after.len() {
return ParamMotion::Reshaped {
before: before.len(),
after: after.len(),
};
}
let moved = before.iter().zip(after).any(|(b, a)| {
b.iter()
.zip(a)
.any(|(p, q)| (0..3).any(|i| (p[i] - q[i]).abs() > tol))
});
if !moved {
return ParamMotion::Fixed;
}
let Some(screw) = recover_screw(before, after) else {
return ParamMotion::Mixed {
screw: Screw {
point: [0.0; 3],
direction: [0.0, 0.0, 1.0],
angle: 0.0,
slide: 0.0,
residual: f64::INFINITY,
},
residual: f64::INFINITY,
};
};
if screw.residual > tol {
return ParamMotion::Mixed {
screw,
residual: screw.residual,
};
}
ParamMotion::Rigid(screw)
}