#[cfg(feature = "serde-serialize")]
use serde::{Deserialize, Serialize};
use num::One;
use simba::scalar::ComplexField;
use simba::simd::SimdComplexField;
use crate::allocator::Allocator;
use crate::base::{DefaultAllocator, Matrix, MatrixMN, MatrixN, Vector};
use crate::constraint::{SameNumberOfRows, ShapeConstraint};
use crate::dimension::{Dim, DimAdd, DimDiff, DimSub, DimSum, U1};
use crate::storage::{Storage, StorageMut};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[cfg_attr(
feature = "serde-serialize",
serde(bound(serialize = "DefaultAllocator: Allocator<N, D>,
MatrixN<N, D>: Serialize"))
)]
#[cfg_attr(
feature = "serde-serialize",
serde(bound(deserialize = "DefaultAllocator: Allocator<N, D>,
MatrixN<N, D>: Deserialize<'de>"))
)]
#[derive(Clone, Debug)]
pub struct Cholesky<N: SimdComplexField, D: Dim>
where
DefaultAllocator: Allocator<N, D, D>,
{
chol: MatrixN<N, D>,
}
impl<N: SimdComplexField, D: Dim> Copy for Cholesky<N, D>
where
DefaultAllocator: Allocator<N, D, D>,
MatrixN<N, D>: Copy,
{
}
impl<N: SimdComplexField, D: Dim> Cholesky<N, D>
where
DefaultAllocator: Allocator<N, D, D>,
{
pub fn new_unchecked(mut matrix: MatrixN<N, D>) -> Self {
assert!(matrix.is_square(), "The input matrix must be square.");
let n = matrix.nrows();
for j in 0..n {
for k in 0..j {
let factor = unsafe { -*matrix.get_unchecked((j, k)) };
let (mut col_j, col_k) = matrix.columns_range_pair_mut(j, k);
let mut col_j = col_j.rows_range_mut(j..);
let col_k = col_k.rows_range(j..);
col_j.axpy(factor.simd_conjugate(), &col_k, N::one());
}
let diag = unsafe { *matrix.get_unchecked((j, j)) };
let denom = diag.simd_sqrt();
unsafe {
*matrix.get_unchecked_mut((j, j)) = denom;
}
let mut col = matrix.slice_range_mut(j + 1.., j);
col /= denom;
}
Cholesky { chol: matrix }
}
pub fn unpack(mut self) -> MatrixN<N, D> {
self.chol.fill_upper_triangle(N::zero(), 1);
self.chol
}
pub fn unpack_dirty(self) -> MatrixN<N, D> {
self.chol
}
pub fn l(&self) -> MatrixN<N, D> {
self.chol.lower_triangle()
}
pub fn l_dirty(&self) -> &MatrixN<N, D> {
&self.chol
}
pub fn solve_mut<R2: Dim, C2: Dim, S2>(&self, b: &mut Matrix<N, R2, C2, S2>)
where
S2: StorageMut<N, R2, C2>,
ShapeConstraint: SameNumberOfRows<R2, D>,
{
self.chol.solve_lower_triangular_unchecked_mut(b);
self.chol.ad_solve_lower_triangular_unchecked_mut(b);
}
pub fn solve<R2: Dim, C2: Dim, S2>(&self, b: &Matrix<N, R2, C2, S2>) -> MatrixMN<N, R2, C2>
where
S2: Storage<N, R2, C2>,
DefaultAllocator: Allocator<N, R2, C2>,
ShapeConstraint: SameNumberOfRows<R2, D>,
{
let mut res = b.clone_owned();
self.solve_mut(&mut res);
res
}
pub fn inverse(&self) -> MatrixN<N, D> {
let shape = self.chol.data.shape();
let mut res = MatrixN::identity_generic(shape.0, shape.1);
self.solve_mut(&mut res);
res
}
}
impl<N: ComplexField, D: Dim> Cholesky<N, D>
where
DefaultAllocator: Allocator<N, D, D>,
{
pub fn new(mut matrix: MatrixN<N, D>) -> Option<Self> {
assert!(matrix.is_square(), "The input matrix must be square.");
let n = matrix.nrows();
for j in 0..n {
for k in 0..j {
let factor = unsafe { -*matrix.get_unchecked((j, k)) };
let (mut col_j, col_k) = matrix.columns_range_pair_mut(j, k);
let mut col_j = col_j.rows_range_mut(j..);
let col_k = col_k.rows_range(j..);
col_j.axpy(factor.conjugate(), &col_k, N::one());
}
let diag = unsafe { *matrix.get_unchecked((j, j)) };
if !diag.is_zero() {
if let Some(denom) = diag.try_sqrt() {
unsafe {
*matrix.get_unchecked_mut((j, j)) = denom;
}
let mut col = matrix.slice_range_mut(j + 1.., j);
col /= denom;
continue;
}
}
return None;
}
Some(Cholesky { chol: matrix })
}
#[inline]
pub fn rank_one_update<R2: Dim, S2>(&mut self, x: &Vector<N, R2, S2>, sigma: N::RealField)
where
S2: Storage<N, R2, U1>,
DefaultAllocator: Allocator<N, R2, U1>,
ShapeConstraint: SameNumberOfRows<R2, D>,
{
Self::xx_rank_one_update(&mut self.chol, &mut x.clone_owned(), sigma)
}
pub fn insert_column<R2, S2>(
&self,
j: usize,
col: Vector<N, R2, S2>,
) -> Cholesky<N, DimSum<D, U1>>
where
D: DimAdd<U1>,
R2: Dim,
S2: Storage<N, R2, U1>,
DefaultAllocator: Allocator<N, DimSum<D, U1>, DimSum<D, U1>> + Allocator<N, R2>,
ShapeConstraint: SameNumberOfRows<R2, DimSum<D, U1>>,
{
let mut col = col.into_owned();
let n = col.nrows();
assert_eq!(
n,
self.chol.nrows() + 1,
"The new column must have the size of the factored matrix plus one."
);
assert!(j < n, "j needs to be within the bound of the new matrix.");
let mut chol = unsafe {
Matrix::new_uninitialized_generic(
self.chol.data.shape().0.add(U1),
self.chol.data.shape().1.add(U1),
)
};
chol.slice_range_mut(..j, ..j)
.copy_from(&self.chol.slice_range(..j, ..j));
chol.slice_range_mut(..j, j + 1..)
.copy_from(&self.chol.slice_range(..j, j..));
chol.slice_range_mut(j + 1.., ..j)
.copy_from(&self.chol.slice_range(j.., ..j));
chol.slice_range_mut(j + 1.., j + 1..)
.copy_from(&self.chol.slice_range(j.., j..));
let top_left_corner = self.chol.slice_range(..j, ..j);
let col_j = col[j];
let (mut new_rowj_adjoint, mut new_colj) = col.rows_range_pair_mut(..j, j + 1..);
assert!(
top_left_corner.solve_lower_triangular_mut(&mut new_rowj_adjoint),
"Cholesky::insert_column : Unable to solve lower triangular system!"
);
new_rowj_adjoint.adjoint_to(&mut chol.slice_range_mut(j, ..j));
let center_element = N::sqrt(col_j - N::from_real(new_rowj_adjoint.norm_squared()));
chol[(j, j)] = center_element;
let bottom_left_corner = self.chol.slice_range(j.., ..j);
new_colj.gemm(
-N::one() / center_element,
&bottom_left_corner,
&new_rowj_adjoint,
N::one() / center_element,
);
chol.slice_range_mut(j + 1.., j).copy_from(&new_colj);
let mut bottom_right_corner = chol.slice_range_mut(j + 1.., j + 1..);
Self::xx_rank_one_update(
&mut bottom_right_corner,
&mut new_colj,
-N::RealField::one(),
);
Cholesky { chol }
}
pub fn remove_column(&self, j: usize) -> Cholesky<N, DimDiff<D, U1>>
where
D: DimSub<U1>,
DefaultAllocator: Allocator<N, DimDiff<D, U1>, DimDiff<D, U1>> + Allocator<N, D>,
{
let n = self.chol.nrows();
assert!(n > 0, "The matrix needs at least one column.");
assert!(j < n, "j needs to be within the bound of the matrix.");
let mut chol = unsafe {
Matrix::new_uninitialized_generic(
self.chol.data.shape().0.sub(U1),
self.chol.data.shape().1.sub(U1),
)
};
chol.slice_range_mut(..j, ..j)
.copy_from(&self.chol.slice_range(..j, ..j));
chol.slice_range_mut(..j, j..)
.copy_from(&self.chol.slice_range(..j, j + 1..));
chol.slice_range_mut(j.., ..j)
.copy_from(&self.chol.slice_range(j + 1.., ..j));
chol.slice_range_mut(j.., j..)
.copy_from(&self.chol.slice_range(j + 1.., j + 1..));
let mut bottom_right_corner = chol.slice_range_mut(j.., j..);
let mut workspace = self.chol.column(j).clone_owned();
let mut old_colj = workspace.rows_range_mut(j + 1..);
Self::xx_rank_one_update(&mut bottom_right_corner, &mut old_colj, N::RealField::one());
Cholesky { chol }
}
fn xx_rank_one_update<Dm, Sm, Rx, Sx>(
chol: &mut Matrix<N, Dm, Dm, Sm>,
x: &mut Vector<N, Rx, Sx>,
sigma: N::RealField,
) where
Dm: Dim,
Rx: Dim,
Sm: StorageMut<N, Dm, Dm>,
Sx: StorageMut<N, Rx, U1>,
{
let n = x.nrows();
assert_eq!(
n,
chol.nrows(),
"The input vector must be of the same size as the factorized matrix."
);
let mut beta = crate::one::<N::RealField>();
for j in 0..n {
let diag = N::real(unsafe { *chol.get_unchecked((j, j)) });
let diag2 = diag * diag;
let xj = unsafe { *x.get_unchecked(j) };
let sigma_xj2 = sigma * N::modulus_squared(xj);
let gamma = diag2 * beta + sigma_xj2;
let new_diag = (diag2 + sigma_xj2 / beta).sqrt();
unsafe { *chol.get_unchecked_mut((j, j)) = N::from_real(new_diag) };
beta += sigma_xj2 / diag2;
let mut xjplus = x.rows_range_mut(j + 1..);
let mut col_j = chol.slice_range_mut(j + 1.., j);
xjplus.axpy(-xj / N::from_real(diag), &col_j, N::one());
if gamma != crate::zero::<N::RealField>() {
col_j.axpy(
N::from_real(new_diag * sigma / gamma) * N::conjugate(xj),
&xjplus,
N::from_real(new_diag / diag),
);
}
}
}
}