nalgebra/base/
norm.rs

1#[cfg(all(feature = "alloc", not(feature = "std")))]
2use alloc::vec::Vec;
3
4use num::Zero;
5use std::ops::Neg;
6
7use crate::allocator::Allocator;
8use crate::base::{DefaultAllocator, Dim, DimName, Matrix, Normed, OMatrix, OVector};
9use crate::constraint::{SameNumberOfColumns, SameNumberOfRows, ShapeConstraint};
10use crate::storage::{Storage, StorageMut};
11use crate::{ComplexField, Scalar, SimdComplexField, Unit};
12use simba::scalar::ClosedNeg;
13use simba::simd::{SimdOption, SimdPartialOrd, SimdValue};
14
15// TODO: this should be be a trait on alga?
16/// A trait for abstract matrix norms.
17///
18/// This may be moved to the alga crate in the future.
19pub trait Norm<T: SimdComplexField> {
20    /// Apply this norm to the given matrix.
21    fn norm<R, C, S>(&self, m: &Matrix<T, R, C, S>) -> T::SimdRealField
22    where
23        R: Dim,
24        C: Dim,
25        S: Storage<T, R, C>;
26    /// Use the metric induced by this norm to compute the metric distance between the two given matrices.
27    fn metric_distance<R1, C1, S1, R2, C2, S2>(
28        &self,
29        m1: &Matrix<T, R1, C1, S1>,
30        m2: &Matrix<T, R2, C2, S2>,
31    ) -> T::SimdRealField
32    where
33        R1: Dim,
34        C1: Dim,
35        S1: Storage<T, R1, C1>,
36        R2: Dim,
37        C2: Dim,
38        S2: Storage<T, R2, C2>,
39        ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2>;
40}
41
42/// Euclidean norm.
43#[derive(Copy, Clone, Debug)]
44pub struct EuclideanNorm;
45/// Lp norm.
46#[derive(Copy, Clone, Debug)]
47pub struct LpNorm(pub i32);
48/// L-infinite norm aka. Chebytchev norm aka. uniform norm aka. suppremum norm.
49#[derive(Copy, Clone, Debug)]
50pub struct UniformNorm;
51
52impl<T: SimdComplexField> Norm<T> for EuclideanNorm {
53    #[inline]
54    fn norm<R, C, S>(&self, m: &Matrix<T, R, C, S>) -> T::SimdRealField
55    where
56        R: Dim,
57        C: Dim,
58        S: Storage<T, R, C>,
59    {
60        m.norm_squared().simd_sqrt()
61    }
62
63    #[inline]
64    fn metric_distance<R1, C1, S1, R2, C2, S2>(
65        &self,
66        m1: &Matrix<T, R1, C1, S1>,
67        m2: &Matrix<T, R2, C2, S2>,
68    ) -> T::SimdRealField
69    where
70        R1: Dim,
71        C1: Dim,
72        S1: Storage<T, R1, C1>,
73        R2: Dim,
74        C2: Dim,
75        S2: Storage<T, R2, C2>,
76        ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2>,
77    {
78        m1.zip_fold(m2, T::SimdRealField::zero(), |acc, a, b| {
79            let diff = a - b;
80            acc + diff.simd_modulus_squared()
81        })
82        .simd_sqrt()
83    }
84}
85
86impl<T: SimdComplexField> Norm<T> for LpNorm {
87    #[inline]
88    fn norm<R, C, S>(&self, m: &Matrix<T, R, C, S>) -> T::SimdRealField
89    where
90        R: Dim,
91        C: Dim,
92        S: Storage<T, R, C>,
93    {
94        m.fold(T::SimdRealField::zero(), |a, b| {
95            a + b.simd_modulus().simd_powi(self.0)
96        })
97        .simd_powf(crate::convert(1.0 / (self.0 as f64)))
98    }
99
100    #[inline]
101    fn metric_distance<R1, C1, S1, R2, C2, S2>(
102        &self,
103        m1: &Matrix<T, R1, C1, S1>,
104        m2: &Matrix<T, R2, C2, S2>,
105    ) -> T::SimdRealField
106    where
107        R1: Dim,
108        C1: Dim,
109        S1: Storage<T, R1, C1>,
110        R2: Dim,
111        C2: Dim,
112        S2: Storage<T, R2, C2>,
113        ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2>,
114    {
115        m1.zip_fold(m2, T::SimdRealField::zero(), |acc, a, b| {
116            let diff = a - b;
117            acc + diff.simd_modulus().simd_powi(self.0)
118        })
119        .simd_powf(crate::convert(1.0 / (self.0 as f64)))
120    }
121}
122
123impl<T: SimdComplexField> Norm<T> for UniformNorm {
124    #[inline]
125    fn norm<R, C, S>(&self, m: &Matrix<T, R, C, S>) -> T::SimdRealField
126    where
127        R: Dim,
128        C: Dim,
129        S: Storage<T, R, C>,
130    {
131        // NOTE: we don't use `m.amax()` here because for the complex
132        // numbers this will return the max norm1 instead of the modulus.
133        m.fold(T::SimdRealField::zero(), |acc, a| {
134            acc.simd_max(a.simd_modulus())
135        })
136    }
137
138    #[inline]
139    fn metric_distance<R1, C1, S1, R2, C2, S2>(
140        &self,
141        m1: &Matrix<T, R1, C1, S1>,
142        m2: &Matrix<T, R2, C2, S2>,
143    ) -> T::SimdRealField
144    where
145        R1: Dim,
146        C1: Dim,
147        S1: Storage<T, R1, C1>,
148        R2: Dim,
149        C2: Dim,
150        S2: Storage<T, R2, C2>,
151        ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2>,
152    {
153        m1.zip_fold(m2, T::SimdRealField::zero(), |acc, a, b| {
154            let val = (a - b).simd_modulus();
155            acc.simd_max(val)
156        })
157    }
158}
159
160/// # Magnitude and norms
161impl<T: Scalar, R: Dim, C: Dim, S: Storage<T, R, C>> Matrix<T, R, C, S> {
162    /// The squared L2 norm of this vector.
163    #[inline]
164    #[must_use]
165    pub fn norm_squared(&self) -> T::SimdRealField
166    where
167        T: SimdComplexField,
168    {
169        let mut res = T::SimdRealField::zero();
170
171        for i in 0..self.ncols() {
172            let col = self.column(i);
173            res += col.dotc(&col).simd_real()
174        }
175
176        res
177    }
178
179    /// The L2 norm of this matrix.
180    ///
181    /// Use `.apply_norm` to apply a custom norm.
182    #[inline]
183    #[must_use]
184    pub fn norm(&self) -> T::SimdRealField
185    where
186        T: SimdComplexField,
187    {
188        self.norm_squared().simd_sqrt()
189    }
190
191    /// Compute the distance between `self` and `rhs` using the metric induced by the euclidean norm.
192    ///
193    /// Use `.apply_metric_distance` to apply a custom norm.
194    #[inline]
195    #[must_use]
196    pub fn metric_distance<R2, C2, S2>(&self, rhs: &Matrix<T, R2, C2, S2>) -> T::SimdRealField
197    where
198        T: SimdComplexField,
199        R2: Dim,
200        C2: Dim,
201        S2: Storage<T, R2, C2>,
202        ShapeConstraint: SameNumberOfRows<R, R2> + SameNumberOfColumns<C, C2>,
203    {
204        self.apply_metric_distance(rhs, &EuclideanNorm)
205    }
206
207    /// Uses the given `norm` to compute the norm of `self`.
208    ///
209    /// # Example
210    ///
211    /// ```
212    /// # use nalgebra::{Vector3, UniformNorm, LpNorm, EuclideanNorm};
213    ///
214    /// let v = Vector3::new(1.0, 2.0, 3.0);
215    /// assert_eq!(v.apply_norm(&UniformNorm), 3.0);
216    /// assert_eq!(v.apply_norm(&LpNorm(1)), 6.0);
217    /// assert_eq!(v.apply_norm(&EuclideanNorm), v.norm());
218    /// ```
219    #[inline]
220    #[must_use]
221    pub fn apply_norm(&self, norm: &impl Norm<T>) -> T::SimdRealField
222    where
223        T: SimdComplexField,
224    {
225        norm.norm(self)
226    }
227
228    /// Uses the metric induced by the given `norm` to compute the metric distance between `self` and `rhs`.
229    ///
230    /// # Example
231    ///
232    /// ```
233    /// # use nalgebra::{Vector3, UniformNorm, LpNorm, EuclideanNorm};
234    ///
235    /// let v1 = Vector3::new(1.0, 2.0, 3.0);
236    /// let v2 = Vector3::new(10.0, 20.0, 30.0);
237    ///
238    /// assert_eq!(v1.apply_metric_distance(&v2, &UniformNorm), 27.0);
239    /// assert_eq!(v1.apply_metric_distance(&v2, &LpNorm(1)), 27.0 + 18.0 + 9.0);
240    /// assert_eq!(v1.apply_metric_distance(&v2, &EuclideanNorm), (v1 - v2).norm());
241    /// ```
242    #[inline]
243    #[must_use]
244    pub fn apply_metric_distance<R2, C2, S2>(
245        &self,
246        rhs: &Matrix<T, R2, C2, S2>,
247        norm: &impl Norm<T>,
248    ) -> T::SimdRealField
249    where
250        T: SimdComplexField,
251        R2: Dim,
252        C2: Dim,
253        S2: Storage<T, R2, C2>,
254        ShapeConstraint: SameNumberOfRows<R, R2> + SameNumberOfColumns<C, C2>,
255    {
256        norm.metric_distance(self, rhs)
257    }
258
259    /// A synonym for the norm of this matrix.
260    ///
261    /// Aka the length.
262    ///
263    /// This function is simply implemented as a call to `norm()`
264    #[inline]
265    #[must_use]
266    pub fn magnitude(&self) -> T::SimdRealField
267    where
268        T: SimdComplexField,
269    {
270        self.norm()
271    }
272
273    /// A synonym for the squared norm of this matrix.
274    ///
275    /// Aka the squared length.
276    ///
277    /// This function is simply implemented as a call to `norm_squared()`
278    #[inline]
279    #[must_use]
280    pub fn magnitude_squared(&self) -> T::SimdRealField
281    where
282        T: SimdComplexField,
283    {
284        self.norm_squared()
285    }
286
287    /// Sets the magnitude of this vector.
288    #[inline]
289    pub fn set_magnitude(&mut self, magnitude: T::SimdRealField)
290    where
291        T: SimdComplexField,
292        S: StorageMut<T, R, C>,
293    {
294        let n = self.norm();
295        self.scale_mut(magnitude / n)
296    }
297
298    /// Returns a normalized version of this matrix.
299    #[inline]
300    #[must_use = "Did you mean to use normalize_mut()?"]
301    pub fn normalize(&self) -> OMatrix<T, R, C>
302    where
303        T: SimdComplexField,
304        DefaultAllocator: Allocator<R, C>,
305    {
306        self.unscale(self.norm())
307    }
308
309    /// The Lp norm of this matrix.
310    #[inline]
311    #[must_use]
312    pub fn lp_norm(&self, p: i32) -> T::SimdRealField
313    where
314        T: SimdComplexField,
315    {
316        self.apply_norm(&LpNorm(p))
317    }
318
319    /// Attempts to normalize `self`.
320    ///
321    /// The components of this matrix can be SIMD types.
322    #[inline]
323    #[must_use = "Did you mean to use simd_try_normalize_mut()?"]
324    pub fn simd_try_normalize(&self, min_norm: T::SimdRealField) -> SimdOption<OMatrix<T, R, C>>
325    where
326        T: SimdComplexField,
327        T::Element: Scalar,
328        DefaultAllocator: Allocator<R, C>,
329    {
330        let n = self.norm();
331        let le = n.clone().simd_le(min_norm);
332        let val = self.unscale(n);
333        SimdOption::new(val, le)
334    }
335
336    /// Sets the magnitude of this vector unless it is smaller than `min_magnitude`.
337    ///
338    /// If `self.magnitude()` is smaller than `min_magnitude`, it will be left unchanged.
339    /// Otherwise this is equivalent to: `*self = self.normalize() * magnitude`.
340    #[inline]
341    pub fn try_set_magnitude(&mut self, magnitude: T::RealField, min_magnitude: T::RealField)
342    where
343        T: ComplexField,
344        S: StorageMut<T, R, C>,
345    {
346        let n = self.norm();
347
348        if n > min_magnitude {
349            self.scale_mut(magnitude / n)
350        }
351    }
352
353    /// Returns a new vector with the same magnitude as `self` clamped between `0.0` and `max`.
354    #[inline]
355    #[must_use]
356    pub fn cap_magnitude(&self, max: T::RealField) -> OMatrix<T, R, C>
357    where
358        T: ComplexField,
359        DefaultAllocator: Allocator<R, C>,
360    {
361        let n = self.norm();
362
363        if n > max {
364            self.scale(max / n)
365        } else {
366            self.clone_owned()
367        }
368    }
369
370    /// Returns a new vector with the same magnitude as `self` clamped between `0.0` and `max`.
371    #[inline]
372    #[must_use]
373    pub fn simd_cap_magnitude(&self, max: T::SimdRealField) -> OMatrix<T, R, C>
374    where
375        T: SimdComplexField,
376        T::Element: Scalar,
377        DefaultAllocator: Allocator<R, C>,
378    {
379        let n = self.norm();
380        let scaled = self.scale(max.clone() / n.clone());
381        let use_scaled = n.simd_gt(max);
382        scaled.select(use_scaled, self.clone_owned())
383    }
384
385    /// Returns a normalized version of this matrix unless its norm as smaller or equal to `eps`.
386    ///
387    /// The components of this matrix cannot be SIMD types (see `simd_try_normalize`) instead.
388    #[inline]
389    #[must_use = "Did you mean to use try_normalize_mut()?"]
390    pub fn try_normalize(&self, min_norm: T::RealField) -> Option<OMatrix<T, R, C>>
391    where
392        T: ComplexField,
393        DefaultAllocator: Allocator<R, C>,
394    {
395        let n = self.norm();
396
397        if n <= min_norm {
398            None
399        } else {
400            Some(self.unscale(n))
401        }
402    }
403}
404
405/// # In-place normalization
406impl<T: Scalar, R: Dim, C: Dim, S: StorageMut<T, R, C>> Matrix<T, R, C, S> {
407    /// Normalizes this matrix in-place and returns its norm.
408    ///
409    /// The components of the matrix cannot be SIMD types (see `simd_try_normalize_mut` instead).
410    #[inline]
411    pub fn normalize_mut(&mut self) -> T::SimdRealField
412    where
413        T: SimdComplexField,
414    {
415        let n = self.norm();
416        self.unscale_mut(n.clone());
417
418        n
419    }
420
421    /// Normalizes this matrix in-place and return its norm.
422    ///
423    /// The components of the matrix can be SIMD types.
424    #[inline]
425    #[must_use = "Did you mean to use simd_try_normalize_mut()?"]
426    pub fn simd_try_normalize_mut(
427        &mut self,
428        min_norm: T::SimdRealField,
429    ) -> SimdOption<T::SimdRealField>
430    where
431        T: SimdComplexField,
432        T::Element: Scalar,
433        DefaultAllocator: Allocator<R, C>,
434    {
435        let n = self.norm();
436        let le = n.clone().simd_le(min_norm);
437        self.apply(|e| *e = e.clone().simd_unscale(n.clone()).select(le, e.clone()));
438        SimdOption::new(n, le)
439    }
440
441    /// Normalizes this matrix in-place or does nothing if its norm is smaller or equal to `eps`.
442    ///
443    /// If the normalization succeeded, returns the old norm of this matrix.
444    #[inline]
445    pub fn try_normalize_mut(&mut self, min_norm: T::RealField) -> Option<T::RealField>
446    where
447        T: ComplexField,
448    {
449        let n = self.norm();
450
451        if n <= min_norm {
452            None
453        } else {
454            self.unscale_mut(n.clone());
455            Some(n)
456        }
457    }
458}
459
460impl<T: SimdComplexField, R: Dim, C: Dim> Normed for OMatrix<T, R, C>
461where
462    DefaultAllocator: Allocator<R, C>,
463{
464    type Norm = T::SimdRealField;
465
466    #[inline]
467    fn norm(&self) -> T::SimdRealField {
468        self.norm()
469    }
470
471    #[inline]
472    fn norm_squared(&self) -> T::SimdRealField {
473        self.norm_squared()
474    }
475
476    #[inline]
477    fn scale_mut(&mut self, n: Self::Norm) {
478        self.scale_mut(n)
479    }
480
481    #[inline]
482    fn unscale_mut(&mut self, n: Self::Norm) {
483        self.unscale_mut(n)
484    }
485}
486
487impl<T: Scalar + ClosedNeg, R: Dim, C: Dim> Neg for Unit<OMatrix<T, R, C>>
488where
489    DefaultAllocator: Allocator<R, C>,
490{
491    type Output = Unit<OMatrix<T, R, C>>;
492
493    #[inline]
494    fn neg(self) -> Self::Output {
495        Unit::new_unchecked(-self.value)
496    }
497}
498
499// TODO: specialization will greatly simplify this implementation in the future.
500// In particular:
501//   − use `x()` instead of `::canonical_basis_element`
502//   − use `::new(x, y, z)` instead of `::from_slice`
503/// # Basis and orthogonalization
504impl<T: ComplexField, D: DimName> OVector<T, D>
505where
506    DefaultAllocator: Allocator<D>,
507{
508    /// The i-the canonical basis element.
509    #[inline]
510    fn canonical_basis_element(i: usize) -> Self {
511        let mut res = Self::zero();
512        res[i] = T::one();
513        res
514    }
515
516    /// Orthonormalizes the given family of vectors. The largest free family of vectors is moved at
517    /// the beginning of the array and its size is returned. Vectors at an indices larger or equal to
518    /// this length can be modified to an arbitrary value.
519    #[inline]
520    pub fn orthonormalize(vs: &mut [Self]) -> usize {
521        let mut nbasis_elements = 0;
522
523        for i in 0..vs.len() {
524            {
525                let (elt, basis) = vs[..i + 1].split_last_mut().unwrap();
526
527                for basis_element in &basis[..nbasis_elements] {
528                    *elt -= basis_element * elt.dot(basis_element)
529                }
530            }
531
532            if vs[i].try_normalize_mut(T::RealField::zero()).is_some() {
533                // TODO: this will be efficient on dynamically-allocated vectors but for
534                // statically-allocated ones, `.clone_from` would be better.
535                vs.swap(nbasis_elements, i);
536                nbasis_elements += 1;
537
538                // All the other vectors will be dependent.
539                if nbasis_elements == D::dim() {
540                    break;
541                }
542            }
543        }
544
545        nbasis_elements
546    }
547
548    /// Applies the given closure to each element of the orthonormal basis of the subspace
549    /// orthogonal to free family of vectors `vs`. If `vs` is not a free family, the result is
550    /// unspecified.
551    // TODO: return an iterator instead when `-> impl Iterator` will be supported by Rust.
552    #[inline]
553    pub fn orthonormal_subspace_basis<F>(vs: &[Self], mut f: F)
554    where
555        F: FnMut(&Self) -> bool,
556    {
557        // TODO: is this necessary?
558        assert!(
559            vs.len() <= D::dim(),
560            "The given set of vectors has no chance of being a free family."
561        );
562
563        match D::dim() {
564            1 => {
565                if vs.is_empty() {
566                    let _ = f(&Self::canonical_basis_element(0));
567                }
568            }
569            2 => {
570                if vs.is_empty() {
571                    let _ = f(&Self::canonical_basis_element(0))
572                        && f(&Self::canonical_basis_element(1));
573                } else if vs.len() == 1 {
574                    let v = &vs[0];
575                    let res = Self::from_column_slice(&[-v[1].clone(), v[0].clone()]);
576
577                    let _ = f(&res.normalize());
578                }
579
580                // Otherwise, nothing.
581            }
582            3 => {
583                if vs.is_empty() {
584                    let _ = f(&Self::canonical_basis_element(0))
585                        && f(&Self::canonical_basis_element(1))
586                        && f(&Self::canonical_basis_element(2));
587                } else if vs.len() == 1 {
588                    let v = &vs[0];
589                    let mut a;
590
591                    if v[0].clone().norm1() > v[1].clone().norm1() {
592                        a = Self::from_column_slice(&[v[2].clone(), T::zero(), -v[0].clone()]);
593                    } else {
594                        a = Self::from_column_slice(&[T::zero(), -v[2].clone(), v[1].clone()]);
595                    };
596
597                    let _ = a.normalize_mut();
598
599                    if f(&a.cross(v)) {
600                        let _ = f(&a);
601                    }
602                } else if vs.len() == 2 {
603                    let _ = f(&vs[0].cross(&vs[1]).normalize());
604                }
605            }
606            _ => {
607                #[cfg(any(feature = "std", feature = "alloc"))]
608                {
609                    // XXX: use a GenericArray instead.
610                    let mut known_basis = Vec::new();
611
612                    for v in vs.iter() {
613                        known_basis.push(v.normalize())
614                    }
615
616                    for i in 0..D::dim() - vs.len() {
617                        let mut elt = Self::canonical_basis_element(i);
618
619                        for v in &known_basis {
620                            elt -= v * elt.dot(v)
621                        }
622
623                        if let Some(subsp_elt) = elt.try_normalize(T::RealField::zero()) {
624                            if !f(&subsp_elt) {
625                                return;
626                            };
627
628                            known_basis.push(subsp_elt);
629                        }
630                    }
631                }
632                #[cfg(all(not(feature = "std"), not(feature = "alloc")))]
633                {
634                    panic!("Cannot compute the orthogonal subspace basis of a vector with a dimension greater than 3 \
635                            if #![no_std] is enabled and the 'alloc' feature is not enabled.")
636                }
637            }
638        }
639    }
640}
nalgebra/base/norm.rs

nalgebra/base/
norm.rs
