use num::{One, Zero};
use simba::scalar::ComplexField;
use crate::base::constraint::{DimEq, ShapeConstraint};
use crate::base::dimension::{Dim, U2};
use crate::base::storage::{Storage, StorageMut};
use crate::base::{Matrix, Vector};
#[derive(Debug, Clone, Copy)]
pub struct GivensRotation<T: ComplexField> {
c: T::RealField,
s: T,
}
impl<T: ComplexField> GivensRotation<T> {
pub fn identity() -> Self {
Self {
c: T::RealField::one(),
s: T::zero(),
}
}
pub fn new_unchecked(c: T::RealField, s: T) -> Self {
Self { c, s }
}
pub fn new(c: T, s: T) -> (Self, T) {
Self::try_new(c, s, T::RealField::zero())
.unwrap_or_else(|| (GivensRotation::identity(), T::zero()))
}
pub fn try_new(c: T, s: T, eps: T::RealField) -> Option<(Self, T)> {
let (mod0, sign0) = c.to_exp();
let denom = (mod0.clone() * mod0.clone() + s.clone().modulus_squared()).sqrt();
if denom > eps {
let norm = sign0.scale(denom.clone());
let c = mod0 / denom;
let s = s / norm.clone();
Some((Self { c, s }, norm))
} else {
None
}
}
pub fn cancel_y<S: Storage<T, U2>>(v: &Vector<T, U2, S>) -> Option<(Self, T)> {
if !v[1].is_zero() {
let (mod0, sign0) = v[0].clone().to_exp();
let denom = (mod0.clone() * mod0.clone() + v[1].clone().modulus_squared()).sqrt();
let c = mod0 / denom.clone();
let s = -v[1].clone() / sign0.clone().scale(denom.clone());
let r = sign0.scale(denom);
Some((Self { c, s }, r))
} else {
None
}
}
pub fn cancel_x<S: Storage<T, U2>>(v: &Vector<T, U2, S>) -> Option<(Self, T)> {
if !v[0].is_zero() {
let (mod1, sign1) = v[1].clone().to_exp();
let denom = (mod1.clone() * mod1.clone() + v[0].clone().modulus_squared()).sqrt();
let c = mod1 / denom.clone();
let s = (v[0].clone().conjugate() * sign1.clone()).unscale(denom.clone());
let r = sign1.scale(denom);
Some((Self { c, s }, r))
} else {
None
}
}
#[must_use]
pub fn c(&self) -> T::RealField {
self.c.clone()
}
#[must_use]
pub fn s(&self) -> T {
self.s.clone()
}
#[must_use = "This function does not mutate self."]
pub fn inverse(&self) -> Self {
Self {
c: self.c.clone(),
s: -self.s.clone(),
}
}
pub fn rotate<R2: Dim, C2: Dim, S2: StorageMut<T, R2, C2>>(
&self,
rhs: &mut Matrix<T, R2, C2, S2>,
) where
ShapeConstraint: DimEq<R2, U2>,
{
assert_eq!(
rhs.nrows(),
2,
"Unit complex rotation: the input matrix must have exactly two rows."
);
let s = self.s.clone();
let c = self.c.clone();
for j in 0..rhs.ncols() {
unsafe {
let a = rhs.get_unchecked((0, j)).clone();
let b = rhs.get_unchecked((1, j)).clone();
*rhs.get_unchecked_mut((0, j)) =
a.clone().scale(c.clone()) - s.clone().conjugate() * b.clone();
*rhs.get_unchecked_mut((1, j)) = s.clone() * a + b.scale(c.clone());
}
}
}
pub fn rotate_rows<R2: Dim, C2: Dim, S2: StorageMut<T, R2, C2>>(
&self,
lhs: &mut Matrix<T, R2, C2, S2>,
) where
ShapeConstraint: DimEq<C2, U2>,
{
assert_eq!(
lhs.ncols(),
2,
"Unit complex rotation: the input matrix must have exactly two columns."
);
let s = self.s.clone();
let c = self.c.clone();
for j in 0..lhs.nrows() {
unsafe {
let a = lhs.get_unchecked((j, 0)).clone();
let b = lhs.get_unchecked((j, 1)).clone();
*lhs.get_unchecked_mut((j, 0)) = a.clone().scale(c.clone()) + s.clone() * b.clone();
*lhs.get_unchecked_mut((j, 1)) = -s.clone().conjugate() * a + b.scale(c.clone());
}
}
}
}