#[cfg(all(feature = "alloc", not(feature = "std")))]
use alloc::vec::Vec;
use num::Zero;
use std::ops::Neg;
use crate::allocator::Allocator;
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};
use simba::scalar::ClosedNeg;
use simba::simd::{SimdOption, SimdPartialOrd};
pub trait Norm<N: SimdComplexField> {
fn norm<R, C, S>(&self, m: &Matrix<N, R, C, S>) -> N::SimdRealField
where
R: Dim,
C: Dim,
S: Storage<N, R, C>;
fn metric_distance<R1, C1, S1, R2, C2, S2>(
&self,
m1: &Matrix<N, R1, C1, S1>,
m2: &Matrix<N, R2, C2, S2>,
) -> N::SimdRealField
where
R1: Dim,
C1: Dim,
S1: Storage<N, R1, C1>,
R2: Dim,
C2: Dim,
S2: Storage<N, R2, C2>,
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2>;
}
pub struct EuclideanNorm;
pub struct LpNorm(pub i32);
pub struct UniformNorm;
impl<N: SimdComplexField> Norm<N> for EuclideanNorm {
#[inline]
fn norm<R, C, S>(&self, m: &Matrix<N, R, C, S>) -> N::SimdRealField
where
R: Dim,
C: Dim,
S: Storage<N, R, C>,
{
m.norm_squared().simd_sqrt()
}
#[inline]
fn metric_distance<R1, C1, S1, R2, C2, S2>(
&self,
m1: &Matrix<N, R1, C1, S1>,
m2: &Matrix<N, R2, C2, S2>,
) -> N::SimdRealField
where
R1: Dim,
C1: Dim,
S1: Storage<N, R1, C1>,
R2: Dim,
C2: Dim,
S2: Storage<N, R2, C2>,
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2>,
{
m1.zip_fold(m2, N::SimdRealField::zero(), |acc, a, b| {
let diff = a - b;
acc + diff.simd_modulus_squared()
})
.simd_sqrt()
}
}
impl<N: SimdComplexField> Norm<N> for LpNorm {
#[inline]
fn norm<R, C, S>(&self, m: &Matrix<N, R, C, S>) -> N::SimdRealField
where
R: Dim,
C: Dim,
S: Storage<N, R, C>,
{
m.fold(N::SimdRealField::zero(), |a, b| {
a + b.simd_modulus().simd_powi(self.0)
})
.simd_powf(crate::convert(1.0 / (self.0 as f64)))
}
#[inline]
fn metric_distance<R1, C1, S1, R2, C2, S2>(
&self,
m1: &Matrix<N, R1, C1, S1>,
m2: &Matrix<N, R2, C2, S2>,
) -> N::SimdRealField
where
R1: Dim,
C1: Dim,
S1: Storage<N, R1, C1>,
R2: Dim,
C2: Dim,
S2: Storage<N, R2, C2>,
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2>,
{
m1.zip_fold(m2, N::SimdRealField::zero(), |acc, a, b| {
let diff = a - b;
acc + diff.simd_modulus().simd_powi(self.0)
})
.simd_powf(crate::convert(1.0 / (self.0 as f64)))
}
}
impl<N: SimdComplexField> Norm<N> for UniformNorm {
#[inline]
fn norm<R, C, S>(&self, m: &Matrix<N, R, C, S>) -> N::SimdRealField
where
R: Dim,
C: Dim,
S: Storage<N, R, C>,
{
m.fold(N::SimdRealField::zero(), |acc, a| {
acc.simd_max(a.simd_modulus())
})
}
#[inline]
fn metric_distance<R1, C1, S1, R2, C2, S2>(
&self,
m1: &Matrix<N, R1, C1, S1>,
m2: &Matrix<N, R2, C2, S2>,
) -> N::SimdRealField
where
R1: Dim,
C1: Dim,
S1: Storage<N, R1, C1>,
R2: Dim,
C2: Dim,
S2: Storage<N, R2, C2>,
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2>,
{
m1.zip_fold(m2, N::SimdRealField::zero(), |acc, a, b| {
let val = (a - b).simd_modulus();
acc.simd_max(val)
})
}
}
impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
#[inline]
pub fn norm_squared(&self) -> N::SimdRealField
where
N: SimdComplexField,
{
let mut res = N::SimdRealField::zero();
for i in 0..self.ncols() {
let col = self.column(i);
res += col.dotc(&col).simd_real()
}
res
}
#[inline]
pub fn norm(&self) -> N::SimdRealField
where
N: SimdComplexField,
{
self.norm_squared().simd_sqrt()
}
#[inline]
pub fn metric_distance<R2, C2, S2>(&self, rhs: &Matrix<N, R2, C2, S2>) -> N::SimdRealField
where
N: SimdComplexField,
R2: Dim,
C2: Dim,
S2: Storage<N, R2, C2>,
ShapeConstraint: SameNumberOfRows<R, R2> + SameNumberOfColumns<C, C2>,
{
self.apply_metric_distance(rhs, &EuclideanNorm)
}
#[inline]
pub fn apply_norm(&self, norm: &impl Norm<N>) -> N::SimdRealField
where
N: SimdComplexField,
{
norm.norm(self)
}
#[inline]
pub fn apply_metric_distance<R2, C2, S2>(
&self,
rhs: &Matrix<N, R2, C2, S2>,
norm: &impl Norm<N>,
) -> N::SimdRealField
where
N: SimdComplexField,
R2: Dim,
C2: Dim,
S2: Storage<N, R2, C2>,
ShapeConstraint: SameNumberOfRows<R, R2> + SameNumberOfColumns<C, C2>,
{
norm.metric_distance(self, rhs)
}
#[inline]
pub fn magnitude(&self) -> N::SimdRealField
where
N: SimdComplexField,
{
self.norm()
}
#[inline]
pub fn magnitude_squared(&self) -> N::SimdRealField
where
N: SimdComplexField,
{
self.norm_squared()
}
#[inline]
pub fn set_magnitude(&mut self, magnitude: N::SimdRealField)
where
N: SimdComplexField,
S: StorageMut<N, R, C>,
{
let n = self.norm();
self.scale_mut(magnitude / n)
}
#[inline]
#[must_use = "Did you mean to use normalize_mut()?"]
pub fn normalize(&self) -> MatrixMN<N, R, C>
where
N: SimdComplexField,
DefaultAllocator: Allocator<N, R, C>,
{
self.unscale(self.norm())
}
#[inline]
pub fn lp_norm(&self, p: i32) -> N::SimdRealField
where
N: SimdComplexField,
{
self.apply_norm(&LpNorm(p))
}
#[inline]
#[must_use = "Did you mean to use simd_try_normalize_mut()?"]
pub fn simd_try_normalize(&self, min_norm: N::SimdRealField) -> SimdOption<MatrixMN<N, R, C>>
where
N: SimdComplexField,
N::Element: Scalar,
DefaultAllocator: Allocator<N, R, C> + Allocator<N::Element, R, C>,
{
let n = self.norm();
let le = n.simd_le(min_norm);
let val = self.unscale(n);
SimdOption::new(val, le)
}
#[inline]
pub fn try_set_magnitude(&mut self, magnitude: N::RealField, min_magnitude: N::RealField)
where
N: ComplexField,
S: StorageMut<N, R, C>,
{
let n = self.norm();
if n >= min_magnitude {
self.scale_mut(magnitude / n)
}
}
#[inline]
#[must_use = "Did you mean to use try_normalize_mut()?"]
pub fn try_normalize(&self, min_norm: N::RealField) -> Option<MatrixMN<N, R, C>>
where
N: ComplexField,
DefaultAllocator: Allocator<N, R, C>,
{
let n = self.norm();
if n <= min_norm {
None
} else {
Some(self.unscale(n))
}
}
}
impl<N: Scalar, R: Dim, C: Dim, S: StorageMut<N, R, C>> Matrix<N, R, C, S> {
#[inline]
pub fn normalize_mut(&mut self) -> N::SimdRealField
where
N: SimdComplexField,
{
let n = self.norm();
self.unscale_mut(n);
n
}
#[inline]
#[must_use = "Did you mean to use simd_try_normalize_mut()?"]
pub fn simd_try_normalize_mut(
&mut self,
min_norm: N::SimdRealField,
) -> SimdOption<N::SimdRealField>
where
N: SimdComplexField,
N::Element: Scalar,
DefaultAllocator: Allocator<N, R, C> + Allocator<N::Element, R, C>,
{
let n = self.norm();
let le = n.simd_le(min_norm);
self.apply(|e| e.simd_unscale(n).select(le, e));
SimdOption::new(n, le)
}
#[inline]
pub fn try_normalize_mut(&mut self, min_norm: N::RealField) -> Option<N::RealField>
where
N: ComplexField,
{
let n = self.norm();
if n <= min_norm {
None
} else {
self.unscale_mut(n);
Some(n)
}
}
}
impl<N: SimdComplexField, R: Dim, C: Dim> Normed for MatrixMN<N, R, C>
where
DefaultAllocator: Allocator<N, R, C>,
{
type Norm = N::SimdRealField;
#[inline]
fn norm(&self) -> N::SimdRealField {
self.norm()
}
#[inline]
fn norm_squared(&self) -> N::SimdRealField {
self.norm_squared()
}
#[inline]
fn scale_mut(&mut self, n: Self::Norm) {
self.scale_mut(n)
}
#[inline]
fn unscale_mut(&mut self, n: Self::Norm) {
self.unscale_mut(n)
}
}
impl<N: Scalar + ClosedNeg, R: Dim, C: Dim> Neg for Unit<MatrixMN<N, R, C>>
where
DefaultAllocator: Allocator<N, R, C>,
{
type Output = Unit<MatrixMN<N, R, C>>;
#[inline]
fn neg(self) -> Self::Output {
Unit::new_unchecked(-self.value)
}
}
impl<N: ComplexField, D: DimName> VectorN<N, D>
where
DefaultAllocator: Allocator<N, D>,
{
#[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
}
#[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() {
vs.swap(nbasis_elements, i);
nbasis_elements += 1;
if nbasis_elements == D::dim() {
break;
}
}
}
nbasis_elements
}
#[inline]
pub fn orthonormal_subspace_basis<F>(vs: &[Self], mut f: F)
where
F: FnMut(&Self) -> bool,
{
assert!(
vs.len() <= D::dim(),
"The given set of vectors has no chance of being a free family."
);
match D::dim() {
1 => {
if vs.is_empty() {
let _ = f(&Self::canonical_basis_element(0));
}
}
2 => {
if vs.is_empty() {
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());
}
}
3 => {
if vs.is_empty() {
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"))]
{
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.")
}
}
}
}
}