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