use super::points::{mean_point, mean_point_weighted};
use crate::common::PCoords;
use crate::geom3::{Iso3, SvdBasis3, UnitVec3};
use crate::{Iso2, SvdBasis2};
use parry2d_f64::na::{Matrix2, Rotation2, Translation2, UnitComplex};
use parry3d_f64::na::{DMatrix, Matrix3, Point, SVector, Translation3, Unit, UnitQuaternion};
use std::f64::consts::FRAC_PI_2;
#[derive(Debug)]
pub struct SvdBasis<const D: usize> {
pub basis: [SVector<f64, D>; D],
pub sv: [f64; D],
pub center: Point<f64, D>,
pub n: usize,
}
impl<const D: usize> SvdBasis<D> {
pub fn largest(&self) -> Unit<SVector<f64, D>> {
Unit::new_unchecked(self.basis[0])
}
pub fn smallest(&self) -> Unit<SVector<f64, D>> {
Unit::new_unchecked(self.basis[D - 1])
}
pub fn basis_variances(&self) -> [f64; D] {
let mut result = [0.0; D];
for (r, s) in result.iter_mut().zip(self.sv.iter()) {
*r = s.powi(2) / (self.n as f64);
}
result
}
pub fn basis_stdevs(&self) -> [f64; D] {
let mut result = self.basis_variances();
for r in result.iter_mut() {
*r = r.sqrt();
}
result
}
pub fn from_points(points: &[impl PCoords<D>], weights: Option<&[f64]>) -> Option<Self> {
if points.is_empty() {
return None;
}
if let Some(w) = weights {
let center = mean_point_weighted(points, w);
let vectors = points
.iter()
.zip(w)
.map(|(p, w)| (p.coords() - center.coords) * w.max(0.0).sqrt())
.collect::<Vec<_>>();
svd_from_vectors(&vectors, Some(center))
} else {
let center = mean_point(points);
let vectors = points
.iter()
.map(|p| p.coords() - center.coords)
.collect::<Vec<_>>();
svd_from_vectors(&vectors, Some(center))
}
}
pub fn rank(&self, tol: f64) -> usize {
let mut rank = 0;
for s in self.sv.iter() {
if *s > tol {
rank += 1;
}
}
rank
}
pub fn point_to_basis(&self, point: &Point<f64, D>) -> Point<f64, D> {
let mut result = Point::<f64, D>::origin();
for i in 0..D {
let as_vec = point - self.center;
result[i] = self.basis[i].dot(&as_vec);
}
result
}
pub fn vec_to_basis(&self, vector: &SVector<f64, D>) -> SVector<f64, D> {
let mut result = SVector::<f64, D>::zeros();
for i in 0..D {
result[i] = self.basis[i].dot(vector);
}
result
}
pub fn point_from_basis(&self, point: &Point<f64, D>) -> Point<f64, D> {
let mut result = Point::<f64, D>::origin();
for i in 0..D {
result += self.basis[i] * point[i];
}
result + self.center.coords
}
}
fn svd_from_vectors<const D: usize>(
vecs: &[SVector<f64, D>],
center: Option<Point<f64, D>>,
) -> Option<SvdBasis<D>> {
let n = vecs.len();
let mut matrix = DMatrix::zeros(n, D);
for (i, p) in vecs.iter().enumerate() {
for j in 0..D {
matrix[(i, j)] = p[j];
}
}
let result = matrix.svd(false, true);
let v_t = result.v_t?;
if v_t.nrows() != D || v_t.ncols() != D {
return None;
}
let mut basis = [SVector::<f64, D>::zeros(); D];
let mut scales = [0.0; D];
for i in 0..D {
for j in 0..D {
basis[i][j] = v_t[(i, j)];
}
scales[i] = result.singular_values[i];
}
Some(SvdBasis {
basis,
sv: scales,
center: center.unwrap_or(Point::<f64, D>::origin()),
n,
})
}
pub fn iso3_from_basis(basis: &[SVector<f64, 3>; 3], origin: &Point<f64, 3>) -> Iso3 {
let b0 = basis[0].normalize();
let b1 = basis[1].normalize();
let b2 = b0.cross(&b1).normalize();
let rot_m = Matrix3::from_columns(&[b0, b1, b2]);
let r = UnitQuaternion::from_matrix(&rot_m);
let t = Translation3::from(origin.coords);
Iso3::from_parts(t, r).inverse()
}
pub fn iso3_from_xyo(x0: &UnitVec3, y: &UnitVec3, origin: &Point<f64, 3>) -> Iso3 {
let y0 = Unit::new_normalize(y.into_inner() - x0.into_inner() * x0.dot(y));
let z0 = x0.cross(&y0).normalize();
let rot_m = Matrix3::from_columns(&[x0.into_inner(), y0.into_inner(), z0]);
let r = UnitQuaternion::from_matrix(&rot_m);
let t = Translation3::from(origin.coords);
Iso3::from_parts(t, r).inverse()
}
pub fn iso2_from_basis(basis: &[SVector<f64, 2>; 2], origin: &Point<f64, 2>) -> Iso2 {
let b0 = basis[0].normalize();
let b1 = Rotation2::new(FRAC_PI_2) * b0;
let rot_m = Matrix2::from_columns(&[b0, b1]);
let r = UnitComplex::from_matrix(&rot_m);
let t = Translation2::from(origin.coords);
Iso2::from_parts(t, r).inverse()
}
impl From<&SvdBasis3> for Iso3 {
fn from(value: &SvdBasis3) -> Self {
iso3_from_basis(&value.basis, &value.center)
}
}
impl From<&SvdBasis2> for Iso2 {
fn from(value: &SvdBasis2) -> Self {
iso2_from_basis(&value.basis, &value.center)
}
}
impl From<SvdBasis3> for Iso3 {
fn from(val: SvdBasis3) -> Self {
iso3_from_basis(&val.basis, &val.center)
}
}
impl From<SvdBasis2> for Iso2 {
fn from(val: SvdBasis2) -> Self {
iso2_from_basis(&val.basis, &val.center)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom2::{Point2, Vector2};
use crate::geom3::{Point3, Vector3};
use approx::assert_relative_eq;
use std::f64::consts::{FRAC_PI_4, PI, SQRT_2};
#[test]
fn from_points_perfect() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let result = SvdBasis3::from_points(&points, None).unwrap();
assert_relative_eq!(result.center, Point3::origin());
assert_relative_eq!(result.basis[0], Vector3::x_axis());
assert_relative_eq!(result.basis[1], Vector3::y_axis());
assert_relative_eq!(result.basis[2], Vector3::z_axis());
assert_eq!(result.n, 4);
}
fn rot_z() -> [Vector3; 3] {
let iso = Iso3::from_parts(
Translation3::identity(),
UnitQuaternion::from_euler_angles(0.0, 0.0, PI / 4.0),
);
[
iso * Vector3::new(1.0, 0.0, 0.0),
iso * Vector3::new(0.0, 1.0, 0.0),
iso * Vector3::new(0.0, 0.0, 1.0),
]
}
#[test]
fn test_forward_no_shift() {
let basis = rot_z();
let origin = Point3::origin();
let iso = iso3_from_basis(&basis, &origin);
let test_point = Point3::new(1.0, 1.0, 0.0);
let result = iso * test_point;
assert_relative_eq!(result, Point3::new(SQRT_2, 0.0, 0.0), epsilon = 1e-12);
}
#[test]
fn test_forward_with_shift_0() {
let basis = rot_z();
let origin = Point3::new(1.0, 1.0, 0.0);
let iso = iso3_from_basis(&basis, &origin);
let test_point = Point3::new(2.0, 2.0, 0.0);
let result = iso * test_point;
assert_relative_eq!(result, Point3::new(SQRT_2, 0.0, 0.0), epsilon = 1e-12);
}
#[test]
fn test_forward_with_shift_1() {
let basis = rot_z();
let origin = Point3::new(-1.0, -1.0, 0.0);
let iso = iso3_from_basis(&basis, &origin);
let test_point = Point3::new(1.0, 1.0, 0.0);
let result = iso * test_point;
assert_relative_eq!(result, Point3::new(SQRT_2 * 2.0, 0.0, 0.0), epsilon = 1e-12);
}
#[test]
fn largest_returns_first_basis_vector() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let largest = basis.largest();
assert_relative_eq!(
largest.into_inner().abs(),
Vector3::x_axis().into_inner().abs(),
epsilon = 1e-10
);
}
#[test]
fn smallest_returns_last_basis_vector() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let smallest = basis.smallest();
assert_relative_eq!(
smallest.into_inner().abs(),
Vector3::z_axis().into_inner().abs(),
epsilon = 1e-10
);
}
#[test]
fn basis_variances_match_formula() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let variances = basis.basis_variances();
for (i, &var) in variances.iter().enumerate() {
assert_relative_eq!(var, basis.sv[i].powi(2) / basis.n as f64, epsilon = 1e-10);
}
}
#[test]
fn basis_stdevs_are_sqrt_of_variances() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let variances = basis.basis_variances();
let stdevs = basis.basis_stdevs();
for (&var, &std) in variances.iter().zip(stdevs.iter()) {
assert_relative_eq!(std, var.sqrt(), epsilon = 1e-10);
}
}
#[test]
fn rank_collinear_points_is_1() {
let points: Vec<Point3> = (0..5).map(|i| Point3::new(i as f64, 0.0, 0.0)).collect();
let basis = SvdBasis3::from_points(&points, None).unwrap();
assert_eq!(basis.rank(1e-10), 1);
}
#[test]
fn rank_planar_points_is_2() {
let points = vec![
Point3::new(1.0, 0.0, 0.0),
Point3::new(-1.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
assert_eq!(basis.rank(1e-10), 2);
}
#[test]
fn rank_3d_points_is_3() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
Point3::new(0.0, 0.0, 0.5),
Point3::new(0.0, 0.0, -0.5),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
assert_eq!(basis.rank(1e-10), 3);
}
#[test]
fn rank_with_high_tolerance_reduces_rank() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
Point3::new(0.0, 0.0, 0.5),
Point3::new(0.0, 0.0, -0.5),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let tol = basis.sv[2] + 0.1;
assert_eq!(basis.rank(tol), 2);
}
#[test]
fn point_to_basis_and_back_round_trips() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let p = Point3::new(1.5, 0.5, 0.0);
let in_basis = basis.point_to_basis(&p);
let back = basis.point_from_basis(&in_basis);
assert_relative_eq!(back, p, epsilon = 1e-10);
}
#[test]
fn point_to_basis_axis_aligned_preserves_coords() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let p = Point3::new(1.0, 0.5, 0.0);
let in_basis = basis.point_to_basis(&p);
assert_relative_eq!(in_basis.x, 1.0, epsilon = 1e-10);
assert_relative_eq!(in_basis.y, 0.5, epsilon = 1e-10);
assert_relative_eq!(in_basis.z, 0.0, epsilon = 1e-10);
}
#[test]
fn vec_to_basis_axis_aligned() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let v = Vector3::new(1.0, 0.0, 0.0);
let vb = basis.vec_to_basis(&v);
assert_relative_eq!(vb[0], 1.0, epsilon = 1e-10);
assert_relative_eq!(vb[1], 0.0, epsilon = 1e-10);
assert_relative_eq!(vb[2], 0.0, epsilon = 1e-10);
}
#[test]
fn from_points_2d_axis_aligned() {
let points = vec![
Point2::new(-2.0, 0.0),
Point2::new(2.0, 0.0),
Point2::new(0.0, 1.0),
Point2::new(0.0, -1.0),
];
let basis = SvdBasis2::from_points(&points, None).unwrap();
assert_relative_eq!(basis.center, Point2::origin());
assert_relative_eq!(
basis.basis[0].abs(),
Vector2::x_axis().into_inner().abs(),
epsilon = 1e-10
);
assert_eq!(basis.n, 4);
}
#[test]
fn from_points_2d_point_round_trip() {
let points = vec![
Point2::new(-2.0, 0.0),
Point2::new(2.0, 0.0),
Point2::new(0.0, 1.0),
Point2::new(0.0, -1.0),
];
let basis = SvdBasis2::from_points(&points, None).unwrap();
let p = Point2::new(1.0, 0.5);
let back = basis.point_from_basis(&basis.point_to_basis(&p));
assert_relative_eq!(back, p, epsilon = 1e-10);
}
#[test]
fn from_points_with_equal_weights_returns_some() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let weights = vec![1.0f64; 4];
let result = SvdBasis3::from_points(&points, Some(&weights));
assert!(result.is_some());
assert_eq!(result.unwrap().n, 4);
}
#[test]
fn iso3_from_xyo_identity_axes() {
let x0 = UnitVec3::new_normalize(Vector3::new(1.0, 0.0, 0.0));
let y = UnitVec3::new_normalize(Vector3::new(0.0, 1.0, 0.0));
let iso = iso3_from_xyo(&x0, &y, &Point3::origin());
let p = Point3::new(1.0, 2.0, 3.0);
let result = iso * p;
assert_relative_eq!(result, p, epsilon = 1e-10);
}
#[test]
fn iso3_from_xyo_orthogonalizes_y() {
let x0 = UnitVec3::new_normalize(Vector3::new(1.0, 0.0, 0.0));
let y = UnitVec3::new_normalize(Vector3::new(1.0, 1.0, 0.0));
let iso = iso3_from_xyo(&x0, &y, &Point3::origin());
let p = Point3::new(0.0, 1.0, 0.0);
let result = iso * p;
assert_relative_eq!(result.x, 0.0, epsilon = 1e-10);
assert_relative_eq!(result.y, 1.0, epsilon = 1e-10);
assert_relative_eq!(result.z, 0.0, epsilon = 1e-10);
}
#[test]
fn iso3_from_xyo_with_origin_offset() {
let x0 = UnitVec3::new_normalize(Vector3::new(1.0, 0.0, 0.0));
let y = UnitVec3::new_normalize(Vector3::new(0.0, 1.0, 0.0));
let origin = Point3::new(1.0, 1.0, 0.0);
let iso = iso3_from_xyo(&x0, &y, &origin);
let result = iso * origin;
assert_relative_eq!(result, Point3::origin(), epsilon = 1e-10);
}
#[test]
fn iso2_from_basis_identity() {
let basis = [Vector2::new(1.0, 0.0), Vector2::new(0.0, 1.0)];
let iso = iso2_from_basis(&basis, &Point2::origin());
let p = Point2::new(1.0, 2.0);
assert_relative_eq!(iso * p, p, epsilon = 1e-10);
}
#[test]
fn iso2_from_basis_rotated_45_degrees() {
let c = FRAC_PI_4.cos();
let s = FRAC_PI_4.sin();
let basis = [Vector2::new(c, s), Vector2::new(-s, c)];
let iso = iso2_from_basis(&basis, &Point2::origin());
let p = Point2::new(c, s);
let result = iso * p;
assert_relative_eq!(result.x, 1.0, epsilon = 1e-10);
assert_relative_eq!(result.y, 0.0, epsilon = 1e-10);
}
#[test]
fn iso2_from_basis_with_origin_offset() {
let basis = [Vector2::new(1.0, 0.0), Vector2::new(0.0, 1.0)];
let origin = Point2::new(3.0, 4.0);
let iso = iso2_from_basis(&basis, &origin);
let result = iso * origin;
assert_relative_eq!(result, Point2::origin(), epsilon = 1e-10);
}
#[test]
fn from_ref_svd_basis3_for_iso3_maps_center_to_origin() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let iso = Iso3::from(&basis);
assert_relative_eq!(iso * basis.center, Point3::origin(), epsilon = 1e-10);
}
#[test]
fn from_owned_svd_basis3_for_iso3_maps_center_to_origin() {
let points = vec![
Point3::new(-2.0, 0.0, 0.0),
Point3::new(2.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(0.0, -1.0, 0.0),
];
let basis = SvdBasis3::from_points(&points, None).unwrap();
let center = basis.center;
let iso = Iso3::from(basis);
assert_relative_eq!(iso * center, Point3::origin(), epsilon = 1e-10);
}
#[test]
fn from_ref_svd_basis2_for_iso2_maps_center_to_origin() {
let points = vec![
Point2::new(-2.0, 0.0),
Point2::new(2.0, 0.0),
Point2::new(0.0, 1.0),
Point2::new(0.0, -1.0),
];
let basis = SvdBasis2::from_points(&points, None).unwrap();
let iso = Iso2::from(&basis);
assert_relative_eq!(iso * basis.center, Point2::origin(), epsilon = 1e-10);
}
#[test]
fn from_owned_svd_basis2_for_iso2_maps_center_to_origin() {
let points = vec![
Point2::new(-2.0, 0.0),
Point2::new(2.0, 0.0),
Point2::new(0.0, 1.0),
Point2::new(0.0, -1.0),
];
let basis = SvdBasis2::from_points(&points, None).unwrap();
let center = basis.center;
let iso = Iso2::from(basis);
assert_relative_eq!(iso * center, Point2::origin(), epsilon = 1e-10);
}
}