From 81bb9e94f8e110063286b99f106692ad290a7ca0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 24 Mar 2020 19:06:05 +0100 Subject: [PATCH] Re-add orthogonalization and subspace basis computation. --- src/base/norm.rs | 146 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 145 insertions(+), 1 deletion(-) diff --git a/src/base/norm.rs b/src/base/norm.rs index a1f5dcad..bc5acce5 100644 --- a/src/base/norm.rs +++ b/src/base/norm.rs @@ -2,7 +2,7 @@ use num::Zero; use std::ops::Neg; use crate::allocator::Allocator; -use crate::base::{DefaultAllocator, Dim, Matrix, MatrixMN, Normed}; +use crate::base::{DefaultAllocator, Dim, DimName, Matrix, MatrixMN, Normed, VectorN}; use crate::constraint::{SameNumberOfColumns, SameNumberOfRows, ShapeConstraint}; use crate::storage::{Storage, StorageMut}; use crate::{ComplexField, Scalar, SimdComplexField, Unit}; @@ -399,3 +399,147 @@ where DefaultAllocator: Allocator Unit::new_unchecked(-self.value) } } + +// FIXME: specialization will greatly simplify this implementation in the future. +// In particular: +// − use `x()` instead of `::canonical_basis_element` +// − use `::new(x, y, z)` instead of `::from_slice` +impl VectorN +where DefaultAllocator: Allocator +{ + /// The i-the canonical basis element. + #[inline] + fn canonical_basis_element(i: usize) -> Self { + assert!(i < D::dim(), "Index out of bound."); + + let mut res = Self::zero(); + unsafe { + *res.data.get_unchecked_linear_mut(i) = N::one(); + } + + res + } + + /// Orthonormalizes the given family of vectors. The largest free family of vectors is moved at + /// the beginning of the array and its size is returned. Vectors at an indices larger or equal to + /// this length can be modified to an arbitrary value. + #[inline] + pub fn orthonormalize(vs: &mut [Self]) -> usize { + let mut nbasis_elements = 0; + + for i in 0..vs.len() { + { + let (elt, basis) = vs[..i + 1].split_last_mut().unwrap(); + + for basis_element in &basis[..nbasis_elements] { + *elt -= &*basis_element * elt.dot(basis_element) + } + } + + if vs[i].try_normalize_mut(N::RealField::zero()).is_some() { + // FIXME: this will be efficient on dynamically-allocated vectors but for + // statically-allocated ones, `.clone_from` would be better. + vs.swap(nbasis_elements, i); + nbasis_elements += 1; + + // All the other vectors will be dependent. + if nbasis_elements == D::dim() { + break; + } + } + } + + nbasis_elements + } + + /// Applies the given closure to each element of the orthonormal basis of the subspace + /// orthogonal to free family of vectors `vs`. If `vs` is not a free family, the result is + /// unspecified. + // FIXME: return an iterator instead when `-> impl Iterator` will be supported by Rust. + #[inline] + pub fn orthonormal_subspace_basis(vs: &[Self], mut f: F) + where F: FnMut(&Self) -> bool { + // FIXME: is this necessary? + assert!( + vs.len() <= D::dim(), + "The given set of vectors has no chance of being a free family." + ); + + match D::dim() { + 1 => { + if vs.len() == 0 { + let _ = f(&Self::canonical_basis_element(0)); + } + } + 2 => { + if vs.len() == 0 { + let _ = f(&Self::canonical_basis_element(0)) + && f(&Self::canonical_basis_element(1)); + } else if vs.len() == 1 { + let v = &vs[0]; + let res = Self::from_column_slice(&[-v[1], v[0]]); + + let _ = f(&res.normalize()); + } + + // Otherwise, nothing. + } + 3 => { + if vs.len() == 0 { + let _ = f(&Self::canonical_basis_element(0)) + && f(&Self::canonical_basis_element(1)) + && f(&Self::canonical_basis_element(2)); + } else if vs.len() == 1 { + let v = &vs[0]; + let mut a; + + if v[0].norm1() > v[1].norm1() { + a = Self::from_column_slice(&[v[2], N::zero(), -v[0]]); + } else { + a = Self::from_column_slice(&[N::zero(), -v[2], v[1]]); + }; + + let _ = a.normalize_mut(); + + if f(&a.cross(v)) { + let _ = f(&a); + } + } else if vs.len() == 2 { + let _ = f(&vs[0].cross(&vs[1]).normalize()); + } + } + _ => { + #[cfg(any(feature = "std", feature = "alloc"))] + { + // XXX: use a GenericArray instead. + let mut known_basis = Vec::new(); + + for v in vs.iter() { + known_basis.push(v.normalize()) + } + + for i in 0..D::dim() - vs.len() { + let mut elt = Self::canonical_basis_element(i); + + for v in &known_basis { + elt -= v * elt.dot(v) + } + + if let Some(subsp_elt) = elt.try_normalize(N::RealField::zero()) { + if !f(&subsp_elt) { + return; + }; + + known_basis.push(subsp_elt); + } + } + } + #[cfg(all(not(feature = "std"), not(feature = "alloc")))] + { + panic!("Cannot compute the orthogonal subspace basis of a vector with a dimension greater than 3 \ + if #![no_std] is enabled and the 'alloc' feature is not enabled.") + } + } + } + } +}