use super::*;
use crate::common::DistMode;
use crate::geom3::align3::jacobian::{copy_jacobian, point_plane_jacobian, point_point_jacobian};
use crate::geom3::mesh::Mesh;
use crate::geom3::{Align3, Point3, SurfacePoint3};
use faer::prelude::default;
use crate::Result;
use crate::common::points::{dist, mean_point};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
use parry3d_f64::na::{Dyn, Matrix, Owned, U1, U6, Vector};
use rayon::prelude::*;
pub fn points_to_mesh(
points: &[Point3],
mesh: &Mesh,
initial: &Iso3,
mode: DistMode,
) -> Result<Align3> {
let problem = PointsToMesh::new(points, mesh, initial, mode);
let (result, report) = LevenbergMarquardt::new().minimize(problem);
if report.termination.was_successful() {
let residuals = result.residuals().unwrap().as_slice().to_vec();
Ok(Align3::new(result.current_transform(), residuals))
} else {
Err("Failed to align points to mesh".into())
}
}
struct PointsToMesh<'a> {
points: &'a [Point3],
mesh: &'a Mesh,
params: RcParams3,
moved: Vec<Point3>,
closest: Vec<SurfacePoint3>,
mode: DistMode,
}
impl<'a> PointsToMesh<'a> {
fn new(points: &'a [Point3], mesh: &'a Mesh, initial: &Iso3, mode: DistMode) -> Self {
let mean_point = mean_point(points);
let params = RcParams3::from_initial(initial, &mean_point);
let count = points.len();
let mut item = Self {
points,
mesh,
params,
moved: vec![Point3::origin(); count],
closest: vec![default(); count],
mode,
};
item.move_points();
item
}
fn move_points(&mut self) {
let t = self.current_transform();
let indices = (0..self.points.len()).collect::<Vec<_>>();
let collected = indices
.par_iter()
.map(|&i| {
let m = t * self.points[i];
let c = self.mesh.surf_closest_to(&m);
(i, m, c)
})
.collect::<Vec<_>>();
for (i, m, c) in collected {
self.moved[i] = m;
self.closest[i] = c.sp;
}
}
fn current_transform(&self) -> Iso3 {
*self.params.transform()
}
}
impl LeastSquaresProblem<f64, Dyn, U6> for PointsToMesh<'_> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, U6>;
type ParameterStorage = Owned<f64, U6>;
fn set_params(&mut self, x: &Vector<f64, U6, Self::ParameterStorage>) {
self.params.set(x);
self.move_points();
}
fn params(&self) -> Vector<f64, U6, Self::ParameterStorage> {
self.params.x
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
let mut res = Matrix::<f64, Dyn, U1, Self::ResidualStorage>::zeros(self.points.len());
for (i, (p, c)) in self.moved.iter().zip(self.closest.iter()).enumerate() {
res[i] = match self.mode {
DistMode::ToPoint => dist(p, &c.point),
DistMode::ToPlane => c.scalar_projection(p).abs(),
};
}
Some(res)
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, U6, Self::JacobianStorage>> {
let _center = self.params.transform() * self.params.rc;
let mut jac = Matrix::<f64, Dyn, U6, Self::JacobianStorage>::zeros(self.points.len());
for (i, (p, c)) in self.moved.iter().zip(self.closest.iter()).enumerate() {
let values = match self.mode {
DistMode::ToPoint => point_point_jacobian(p, &c.point, &self.params),
DistMode::ToPlane => point_plane_jacobian(p, c, &self.params),
};
copy_jacobian(&values, &mut jac, i);
}
Some(jac)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::points::transform_points;
use crate::tests::engine_blade;
use crate::{SelectOp, Selection};
use approx::assert_relative_eq;
use std::f64::consts::PI;
#[test]
fn test_initial_round_trip() {
let box_mesh = Mesh::create_box(10.0, 5.0, 2.0, false);
let points = box_mesh
.sample_uniform(1000)
.into_iter()
.map(|p| p.point)
.collect::<Vec<_>>();
let initial = iso3_from_param(&T3Storage::new(1.0, 2.0, 3.0, 0.1, 0.2, 0.3));
let problem = PointsToMesh::new(&points, &box_mesh, &initial, DistMode::ToPlane);
let result = problem.current_transform();
assert_relative_eq!(result.to_matrix(), initial.to_matrix(), epsilon = 1e-8);
}
#[test]
fn blade_example() -> Result<()> {
let mesh = engine_blade();
let mask = mesh
.face_select(Selection::None)
.facing(&Vector3::y(), PI / 4.0, SelectOp::Add)
.take_mask();
let expected_points = mesh
.sample_poisson(2.0, Some(&mask))
.iter()
.map(|mp| mp.point())
.collect::<Vec<_>>();
let disturb = Iso3::from_parts(
Translation3::new(-100.0, 150.0, 0.0),
UnitQuaternion::new(Vector3::new(1.0, 1.0, 1.0).normalize() * PI / 6.0),
);
let to_align = transform_points(&expected_points, &disturb);
let result = points_to_mesh(&to_align, &mesh, &Iso3::identity(), DistMode::ToPoint)?;
let aligned = transform_points(&to_align, result.transform());
let max_deviation = aligned
.iter()
.zip(expected_points.iter())
.map(|(a, e)| (a - e).norm())
.max_by(|a, b| a.partial_cmp(b).unwrap())
.unwrap();
assert!(
max_deviation < 1e-6,
"Max deviation is too high: {}",
max_deviation
);
Ok(())
}
}