nalgebra/linalg/
givens.rs

1//! Construction of givens rotations.
2
3use num::{One, Zero};
4use simba::scalar::ComplexField;
5
6use crate::base::constraint::{DimEq, ShapeConstraint};
7use crate::base::dimension::{Dim, U2};
8use crate::base::storage::{Storage, StorageMut};
9use crate::base::{Matrix, Vector};
10
11/// A Givens rotation.
12#[derive(Debug, Clone, Copy)]
13pub struct GivensRotation<T: ComplexField> {
14    c: T::RealField,
15    s: T,
16}
17
18// Matrix = UnitComplex * Matrix
19impl<T: ComplexField> GivensRotation<T> {
20    /// The Givens rotation that does nothing.
21    pub fn identity() -> Self {
22        Self {
23            c: T::RealField::one(),
24            s: T::zero(),
25        }
26    }
27
28    /// Initializes a Givens rotation from its components.
29    ///
30    /// The components are copies as-is. It is not checked whether they describe
31    /// an actually valid Givens rotation.
32    pub fn new_unchecked(c: T::RealField, s: T) -> Self {
33        Self { c, s }
34    }
35
36    /// Initializes a Givens rotation from its non-normalized cosine an sine components.
37    pub fn new(c: T, s: T) -> (Self, T) {
38        Self::try_new(c, s, T::RealField::zero())
39            .unwrap_or_else(|| (GivensRotation::identity(), T::zero()))
40    }
41
42    /// Initializes a Givens rotation form its non-normalized cosine an sine components.
43    pub fn try_new(c: T, s: T, eps: T::RealField) -> Option<(Self, T)> {
44        let (mod0, sign0) = c.to_exp();
45        let denom = (mod0.clone() * mod0.clone() + s.clone().modulus_squared()).sqrt();
46
47        if denom > eps {
48            let norm = sign0.scale(denom.clone());
49            let c = mod0 / denom;
50            let s = s / norm.clone();
51            Some((Self { c, s }, norm))
52        } else {
53            None
54        }
55    }
56
57    /// Computes the rotation `R` required such that the `y` component of `R * v` is zero.
58    ///
59    /// Returns `None` if no rotation is needed (i.e. if `v.y == 0`). Otherwise, this returns the norm
60    /// of `v` and the rotation `r` such that `R * v = [ |v|, 0.0 ]^t` where `|v|` is the norm of `v`.
61    pub fn cancel_y<S: Storage<T, U2>>(v: &Vector<T, U2, S>) -> Option<(Self, T)> {
62        if !v[1].is_zero() {
63            let (mod0, sign0) = v[0].clone().to_exp();
64            let denom = (mod0.clone() * mod0.clone() + v[1].clone().modulus_squared()).sqrt();
65            let c = mod0 / denom.clone();
66            let s = -v[1].clone() / sign0.clone().scale(denom.clone());
67            let r = sign0.scale(denom);
68            Some((Self { c, s }, r))
69        } else {
70            None
71        }
72    }
73
74    /// Computes the rotation `R` required such that the `x` component of `R * v` is zero.
75    ///
76    /// Returns `None` if no rotation is needed (i.e. if `v.x == 0`). Otherwise, this returns the norm
77    /// of `v` and the rotation `r` such that `R * v = [ 0.0, |v| ]^t` where `|v|` is the norm of `v`.
78    pub fn cancel_x<S: Storage<T, U2>>(v: &Vector<T, U2, S>) -> Option<(Self, T)> {
79        if !v[0].is_zero() {
80            let (mod1, sign1) = v[1].clone().to_exp();
81            let denom = (mod1.clone() * mod1.clone() + v[0].clone().modulus_squared()).sqrt();
82            let c = mod1 / denom.clone();
83            let s = (v[0].clone().conjugate() * sign1.clone()).unscale(denom.clone());
84            let r = sign1.scale(denom);
85            Some((Self { c, s }, r))
86        } else {
87            None
88        }
89    }
90
91    /// The cos part of this rotation.
92    #[must_use]
93    pub fn c(&self) -> T::RealField {
94        self.c.clone()
95    }
96
97    /// The sin part of this rotation.
98    #[must_use]
99    pub fn s(&self) -> T {
100        self.s.clone()
101    }
102
103    /// The inverse of this givens rotation.
104    #[must_use = "This function does not mutate self."]
105    pub fn inverse(&self) -> Self {
106        Self {
107            c: self.c.clone(),
108            s: -self.s.clone(),
109        }
110    }
111
112    /// Performs the multiplication `rhs = self * rhs` in-place.
113    pub fn rotate<R2: Dim, C2: Dim, S2: StorageMut<T, R2, C2>>(
114        &self,
115        rhs: &mut Matrix<T, R2, C2, S2>,
116    ) where
117        ShapeConstraint: DimEq<R2, U2>,
118    {
119        assert_eq!(
120            rhs.nrows(),
121            2,
122            "Unit complex rotation: the input matrix must have exactly two rows."
123        );
124        let s = self.s.clone();
125        let c = self.c.clone();
126
127        for j in 0..rhs.ncols() {
128            unsafe {
129                let a = rhs.get_unchecked((0, j)).clone();
130                let b = rhs.get_unchecked((1, j)).clone();
131
132                *rhs.get_unchecked_mut((0, j)) =
133                    a.clone().scale(c.clone()) - s.clone().conjugate() * b.clone();
134                *rhs.get_unchecked_mut((1, j)) = s.clone() * a + b.scale(c.clone());
135            }
136        }
137    }
138
139    /// Performs the multiplication `lhs = lhs * self` in-place.
140    pub fn rotate_rows<R2: Dim, C2: Dim, S2: StorageMut<T, R2, C2>>(
141        &self,
142        lhs: &mut Matrix<T, R2, C2, S2>,
143    ) where
144        ShapeConstraint: DimEq<C2, U2>,
145    {
146        assert_eq!(
147            lhs.ncols(),
148            2,
149            "Unit complex rotation: the input matrix must have exactly two columns."
150        );
151        let s = self.s.clone();
152        let c = self.c.clone();
153
154        // TODO: can we optimize that to iterate on one column at a time ?
155        for j in 0..lhs.nrows() {
156            unsafe {
157                let a = lhs.get_unchecked((j, 0)).clone();
158                let b = lhs.get_unchecked((j, 1)).clone();
159
160                *lhs.get_unchecked_mut((j, 0)) = a.clone().scale(c.clone()) + s.clone() * b.clone();
161                *lhs.get_unchecked_mut((j, 1)) = -s.clone().conjugate() * a + b.scale(c.clone());
162            }
163        }
164    }
165}
nalgebra/linalg/givens.rs

nalgebra/linalg/
givens.rs
