use crate::common::IndexMask;
use crate::common::kd_tree::KdTreeSearch;
use crate::common::points::{dist, mean_point};
use crate::common::vec_f64::mean_and_stdev;
use crate::geom3::align3::{AlignSurfMatch3, SurfaceTarget3};
use crate::geom3::mesh::MeshSurfPoint;
use crate::{Iso3, KdTree3, Mesh3, Point3, Result, SelectOp, Selection, SvdBasis3, To2D};
use parry2d_f64::transformation::convex_hull;
use std::f64::consts::PI;
impl SurfaceTarget3 for Mesh3 {
fn find_align_match(&self, p: &Point3) -> AlignSurfMatch3 {
let m = self.surface_closest_to(p);
let match_ = AlignSurfMatch3::new(m.point(), m.normal(), true, 1.0);
match self.point_stdev() {
Some(stdev) => match_.with_sigma(interpolated_stdev(self, &m, stdev)),
None => match_,
}
}
}
pub(crate) fn interpolated_stdev(mesh: &Mesh3, m: &MeshSurfPoint, stdev: &[f64]) -> f64 {
let face = mesh.faces()[m.face_index as usize];
m.bc.iter()
.zip(face.iter())
.map(|(w, &v)| w * stdev[v as usize])
.sum()
}
pub struct AlignMesh<'a> {
pub mesh: &'a Mesh3,
pub uncertainty: Option<&'a [f64]>,
pub initial: Option<&'a Iso3>,
pub weights: Option<&'a [Box<dyn MeshWeight + Sync>]>,
}
impl<'a> AlignMesh<'a> {
pub fn new(
mesh: &'a Mesh3,
uncertainty: Option<&'a [f64]>,
initial: Option<&'a Iso3>,
weights: Option<&'a [Box<dyn MeshWeight + Sync>]>,
) -> Self {
Self {
mesh,
uncertainty,
initial,
weights,
}
}
pub fn transform(&self) -> Iso3 {
*self.initial.unwrap_or(&Iso3::identity())
}
}
pub trait MeshWeight {
fn weight(&self, point: &MeshSurfPoint) -> f64;
}
#[derive(Clone)]
pub struct FaceIndexWeight {
mask: IndexMask,
weight: f64,
}
impl FaceIndexWeight {
pub fn new(mask: IndexMask, weight: f64) -> Self {
Self { mask, weight }
}
pub fn to_boxed_trait(self) -> Box<dyn MeshWeight + Sync> {
Box::new(self)
}
}
impl MeshWeight for FaceIndexWeight {
fn weight(&self, point: &MeshSurfPoint) -> f64 {
if self.mask.get(point.face_index as usize) {
self.weight
} else {
1.0
}
}
}
#[derive(Clone)]
pub struct NearMeshWeight {
mesh: Mesh3,
weight: f64,
max_dist: f64,
max_angle: f64,
}
impl NearMeshWeight {
pub fn new(mesh: Mesh3, weight: f64, max_dist: f64, max_angle: f64) -> Self {
Self {
mesh,
weight,
max_dist,
max_angle,
}
}
pub fn to_boxed_trait(self) -> Box<dyn MeshWeight + Sync> {
Box::new(self)
}
}
impl MeshWeight for NearMeshWeight {
fn weight(&self, point: &MeshSurfPoint) -> f64 {
let nearest = self.mesh.surface_closest_to(&point.sp.point);
let dist = dist(&nearest.sp, &point.sp);
if dist > self.max_dist {
return 1.0;
}
if nearest.sp.normal.angle(&point.sp.normal) > self.max_angle {
return 1.0;
}
self.weight
}
}
#[derive(Debug, Clone, Copy)]
pub struct GAPParams {
pub sample_spacing: f64,
pub max_neighbor_angle: f64,
pub out_of_plane_ratio: f64,
pub centroid_ratio: f64,
pub filter_distances: Option<f64>,
}
impl GAPParams {
pub fn new(
sample_spacing: f64,
max_neighbor_angle: f64,
out_of_plane_ratio: f64,
centroid_ratio: f64,
filter_distances: Option<f64>,
) -> Self {
Self {
sample_spacing,
max_neighbor_angle,
out_of_plane_ratio,
centroid_ratio,
filter_distances,
}
}
pub fn defaults(sample_spacing: f64) -> Self {
Self {
sample_spacing,
max_neighbor_angle: PI / 3.0,
out_of_plane_ratio: 0.05,
centroid_ratio: 1.0,
filter_distances: Some(3.0),
}
}
}
pub fn simple_alignment_points(
test_mesh: &Mesh3,
ref_mesh: &Mesh3,
spacing: f64,
) -> Result<Vec<MeshSurfPoint>> {
let overlap = test_mesh
.face_select(Selection::None)
.faces_overlap(ref_mesh, PI / 4.0, 2.0, SelectOp::Add)
.take_mask();
if overlap.count_true() == 0 {
Ok(Vec::new())
} else {
test_mesh.sample_surface_poisson(spacing, Some(&overlap))
}
}
pub fn generate_alignment_points(
test_mesh: &Mesh3,
ref_mesh: &Mesh3,
iso: &Iso3,
params: &GAPParams,
) -> Result<Vec<MeshSurfPoint>> {
let all_points = test_mesh.sample_surface_poisson(params.sample_spacing, None)?;
let tree = KdTree3::try_new(&all_points).expect("KD tree build failed");
let mut candidates = Vec::new();
for (i, pnt) in all_points.iter().enumerate() {
let nearest = tree.nearest(pnt, 7);
let mut neighbors = Vec::new();
for (idx, _) in nearest.iter() {
if *idx != i {
neighbors.push(all_points[*idx])
}
}
let (ok, d) = smpl_check(pnt, &neighbors, ref_mesh, iso, params);
if ok {
candidates.push((d, pnt))
}
}
if let Some(sigma) = params.filter_distances
&& candidates.len() > 10
{
let distances = candidates.iter().map(|(d, _)| *d).collect::<Vec<_>>();
if let Some((mean, st_dev)) = mean_and_stdev(&distances) {
let threshold = mean + sigma * st_dev;
candidates.retain(|(d, _)| *d < threshold);
}
}
Ok(candidates.into_iter().map(|(_, p)| *p).collect())
}
fn smpl_check(
check: &MeshSurfPoint,
neighbors: &[MeshSurfPoint],
reference: &Mesh3,
iso: &Iso3,
params: &GAPParams,
) -> (bool, f64) {
if !sac_check(check, neighbors, params) {
return (false, f64::MAX);
}
let moved = iso * check.sp;
let check_ref = reference.surface_closest_to(&moved.point);
if check_ref.sp.normal.dot(&moved.normal) < 0.0 {
return (false, f64::MAX);
}
let neighbors_ref = neighbors
.iter()
.map(|sp| reference.surface_closest_to(&(iso * sp.sp.point)))
.collect::<Vec<_>>();
for sp in &neighbors_ref {
if dist(sp, &check_ref) < params.sample_spacing {
return (false, f64::MAX);
}
}
(
sac_check(&check_ref, &neighbors_ref, params),
dist(&moved, &check_ref),
)
}
pub fn sac_check(
check_point: &MeshSurfPoint,
neighbors: &[MeshSurfPoint],
params: &GAPParams,
) -> bool {
if neighbors.len() < 5 {
return false;
}
for n in neighbors.iter() {
if dist(n, check_point) > params.sample_spacing * 2.0
|| n.sp.normal.angle(&check_point.sp.normal) > params.max_neighbor_angle
|| check_point.sp.scalar_projection(&n.sp.point).abs() > params.sample_spacing
{
return false;
}
}
let mut points = neighbors.iter().map(|n| n.sp.point).collect::<Vec<_>>();
points.push(check_point.sp.point);
let Some(basis) = SvdBasis3::from_points(&points, None) else {
return false;
};
let iso = Iso3::from(&basis);
let points = points.iter().map(|p| iso * p).collect::<Vec<_>>();
if points
.iter()
.map(|p| p.z.abs())
.any(|z| z > params.out_of_plane_ratio * params.sample_spacing)
{
return false;
}
let check2 = (iso * check_point.sp.point).to_2d();
let points2 = points.to_2d();
let centroid = mean_point(&convex_hull(&points2));
if dist(¢roid, &check2) > params.sample_spacing * params.centroid_ratio {
return false;
}
true
}