use crate::common::points::dist;
use crate::geom2::Boundary2;
use crate::na::{DVector, Dyn, Matrix, Owned, U1, Vector};
use crate::{Point2, Result, SurfacePoint2, VecDot};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
pub type BndBuildFn = Box<dyn Fn(&DVector<f64>) -> Result<Boundary2>>;
const DELTA: f64 = 1e-6;
pub fn fit_boundary_to_points(
points: &[Point2],
builder: &BndBuildFn,
initial: DVector<f64>,
ignore_ends: bool,
) -> Result<DVector<f64>> {
let fitting = BoundaryToPoints::new(points, ignore_ends);
let problem = BoundaryFit::try_new(&fitting, builder, initial)?;
let (result, report) = LevenbergMarquardt::new().minimize(problem);
if report.termination.was_successful() {
Ok(result.params)
} else {
Err(format!("Fitting failed: {:?}", report.termination).into())
}
}
struct BoundaryToPoints<'a> {
points: &'a [Point2],
ignore_ends: bool,
}
impl<'a> BoundaryToPoints<'a> {
fn new(points: &'a [Point2], ignore_ends: bool) -> Self {
BoundaryToPoints {
points,
ignore_ends,
}
}
}
impl BoundaryFittable for BoundaryToPoints<'_> {
fn residuals_and_weights(&self, boundary: &Boundary2) -> (DVector<f64>, DVector<f64>) {
let bounds = f64::EPSILON..(boundary.length() - f64::EPSILON);
let mut res = DVector::zeros(self.points.len());
let mut weights = DVector::zeros(self.points.len());
weights.fill(1.0);
for i in 0..self.points.len() {
let (_, m) = boundary.at_closest_to_point(&self.points[i]);
if self.ignore_ends {
weights[i] = if bounds.contains(&m.l) { 1.0 } else { 0.0 };
}
res[i] = dist(&m, &self.points[i]);
}
(res, weights)
}
fn residual_only(&self, sample_i: usize, boundary: &Boundary2) -> f64 {
let (_, m) = boundary.at_closest_to_point(&self.points[sample_i]);
dist(&m, &self.points[sample_i])
}
}
pub fn fit_boundary_to_surface_points(
points: &[SurfacePoint2],
builder: &BndBuildFn,
initial: DVector<f64>,
weight_mode: VecDot,
ignore_ends: bool,
) -> Result<DVector<f64>> {
let fitting = BoundaryToSurfacePoints::new(points, weight_mode, ignore_ends);
let problem = BoundaryFit::try_new(&fitting, builder, initial)?;
let (result, report) = LevenbergMarquardt::new().minimize(problem);
if report.termination.was_successful() {
Ok(result.params)
} else {
Err(format!("Fitting failed: {:?}", report.termination).into())
}
}
struct BoundaryToSurfacePoints<'a> {
points: &'a [SurfacePoint2],
weight_mode: VecDot,
ignore_ends: bool,
}
impl<'a> BoundaryToSurfacePoints<'a> {
fn new(points: &'a [SurfacePoint2], weight_mode: VecDot, ignore_ends: bool) -> Self {
BoundaryToSurfacePoints {
points,
weight_mode,
ignore_ends,
}
}
}
impl BoundaryFittable for BoundaryToSurfacePoints<'_> {
fn residuals_and_weights(&self, boundary: &Boundary2) -> (DVector<f64>, DVector<f64>) {
let bounds = f64::EPSILON..(boundary.length() - f64::EPSILON);
let mut res = DVector::zeros(self.points.len());
let mut weights = DVector::zeros(self.points.len());
weights.fill(1.0);
for i in 0..self.points.len() {
let (_, m) = boundary.at_closest_to_point(&self.points[i]);
let dot = self.points[i].normal.dot(&m.normal);
weights[i] = match self.weight_mode {
VecDot::AsIs => dot,
VecDot::Abs => dot.abs(),
VecDot::ClampPos => dot.max(0.0),
};
if self.ignore_ends && !bounds.contains(&m.l) {
weights[i] = 0.0;
}
res[i] = dist(&m, &self.points[i]);
}
(res, weights)
}
fn residual_only(&self, sample_i: usize, boundary: &Boundary2) -> f64 {
let (_, m) = boundary.at_closest_to_point(&self.points[sample_i]);
dist(&m, &self.points[sample_i])
}
}
pub trait BoundaryFittable {
fn residuals_and_weights(&self, boundary: &Boundary2) -> (DVector<f64>, DVector<f64>);
fn residual_only(&self, sample_i: usize, boundary: &Boundary2) -> f64;
}
struct BoundaryFit<'a> {
fitting: &'a dyn BoundaryFittable,
params: DVector<f64>,
builder: &'a BndBuildFn,
current: Option<Boundary2>,
residuals: Option<DVector<f64>>,
weights: Option<DVector<f64>>,
}
impl<'a> BoundaryFit<'a> {
fn try_new(
fitting: &'a dyn BoundaryFittable,
builder: &'a BndBuildFn,
initial: DVector<f64>,
) -> Result<Self> {
let _ = builder(&initial)?;
let mut problem = BoundaryFit {
fitting,
params: DVector::zeros(initial.len()),
builder,
current: None,
residuals: None,
weights: None,
};
problem.set_params(&initial);
Ok(problem)
}
}
impl LeastSquaresProblem<f64, Dyn, Dyn> for BoundaryFit<'_> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, Dyn>;
type ParameterStorage = Owned<f64, Dyn>;
fn set_params(&mut self, x: &Vector<f64, Dyn, Self::ParameterStorage>) {
self.params = x.clone();
if let Ok(boundary) = (self.builder)(&self.params) {
let (residuals, weights) = self.fitting.residuals_and_weights(&boundary);
self.residuals = Some(residuals);
self.weights = Some(weights);
self.current = Some(boundary);
} else {
self.residuals = None;
self.current = None;
}
}
fn params(&self) -> Vector<f64, Dyn, Self::ParameterStorage> {
self.params.clone()
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
let Some(residuals) = &self.residuals else {
return None;
};
let Some(weights) = &self.weights else {
return None;
};
let mut res = DVector::zeros(residuals.len());
for i in 0..residuals.len() {
res[i] = residuals[i] * weights[i];
}
Some(res)
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, Dyn, Self::JacobianStorage>> {
let Some(residuals) = &self.residuals else {
return None;
};
let Some(weights) = &self.weights else {
return None;
};
let mut jac = Matrix::<f64, Dyn, Dyn, Self::JacobianStorage>::zeros(
residuals.len(),
self.params.len(),
);
for k in 0..self.params.len() {
let mut params = self.params.clone();
params[k] += DELTA;
let Ok(disturbed) = (self.builder)(¶ms) else {
continue;
};
for i in 0..residuals.len() {
let d = self.fitting.residual_only(i, &disturbed);
jac[(i, k)] = weights[i] * (d - residuals[i]) / DELTA;
}
}
Some(jac)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::Vector2;
use crate::common::linear_space;
use crate::common::points::{fill_gaps, to_points};
use crate::geom2::{BoundaryData2, BoundaryEditor};
use approx::assert_relative_eq;
#[test]
fn simple_triangle() {
let corners = to_points(&[[1.0, 1.0], [3.0, 2.0], [2.0, 4.0], [1.0, 1.0]]);
let points = fill_gaps(&corners, 0.1);
let builder: BndBuildFn = Box::new(|params: &DVector<f64>| {
let mut bdata = BoundaryData2::new_open(Point2::new(params[0], params[1]));
let mut cursor = bdata.get_cursor(None);
cursor.add_seg_xy(params[2], params[3]);
cursor.add_seg_xy(params[4], params[5]);
cursor.add_seg_xy(params[0], params[1]);
bdata.try_to_boundary()
});
let initial = DVector::from(vec![0.0, 0.0, 4.0, 0.0, 1.0, 7.0]);
let result = fit_boundary_to_points(&points, &builder, initial, false).unwrap();
let expected = DVector::from(vec![1.0, 1.0, 3.0, 2.0, 2.0, 4.0]);
assert_relative_eq!(result, expected, epsilon = 1.0e-6);
}
#[test]
fn surface_normal_line() {
let good = linear_space(0.0, 10.0, 10)
.iter()
.map(|x| SurfacePoint2::new(Point2::new(*x, 0.0), Vector2::y_axis()))
.collect::<Vec<_>>();
let bad = linear_space(0.0, 10.0, 10)
.iter()
.map(|x| SurfacePoint2::new(Point2::new(*x, 1.0), Vector2::x_axis()))
.collect::<Vec<_>>();
let samples = [good, bad].concat();
let builder: BndBuildFn = Box::new(|params: &DVector<f64>| {
let mut bdata = BoundaryData2::new_open(Point2::new(0.0, params[0]));
let mut cursor = bdata.get_cursor(None);
cursor.add_seg_xy(10.0, params[1]);
bdata.try_to_boundary()
});
let initial = DVector::from(vec![1.0, 1.0]);
let result =
fit_boundary_to_surface_points(&samples, &builder, initial, VecDot::Abs, false)
.unwrap();
let expected = DVector::from(vec![0.0, 0.0]);
assert_relative_eq!(result, expected, epsilon = 1.0e-6);
}
}