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