use alloc::vec;
use alloc::vec::Vec;
use deep_causality_algebra::RealField;
use deep_causality_num::FromPrimitive;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum CgFailure<R> {
NotConverged { iterations: usize, residual: R },
NotPositiveDefinite { iteration: usize },
LengthMismatch { expected: usize, found: usize },
}
impl<R: core::fmt::Display> core::fmt::Display for CgFailure<R> {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
CgFailure::NotConverged {
iterations,
residual,
} => write!(
f,
"conjugate gradient did not converge in {iterations} iterations (final residual {residual})"
),
CgFailure::NotPositiveDefinite { iteration } => write!(
f,
"conjugate gradient broke down at iteration {iteration}: the operator is not positive definite, so a search direction had non-positive curvature"
),
CgFailure::LengthMismatch { expected, found } => write!(
f,
"length mismatch: the operator produced {found} entries against a right-hand side of {expected}"
),
}
}
}
impl<R: core::fmt::Debug + core::fmt::Display> core::error::Error for CgFailure<R> {}
pub fn cg_solve<R, Apply>(
apply: Apply,
b: &[R],
tolerance: R,
max_iterations: usize,
) -> Result<Vec<R>, CgFailure<R>>
where
R: RealField + FromPrimitive,
Apply: Fn(&[R]) -> Vec<R>,
{
let zero_start = vec![R::zero(); b.len()];
cg_core(
apply,
b,
CgSettings {
diag_a: None,
initial: &zero_start,
max_iterations,
tolerance,
},
)
}
struct CgSettings<'a, R> {
diag_a: Option<&'a [R]>,
initial: &'a [R],
max_iterations: usize,
tolerance: R,
}
fn cg_core<R, Apply>(
apply: Apply,
b: &[R],
settings: CgSettings<'_, R>,
) -> Result<Vec<R>, CgFailure<R>>
where
R: RealField + FromPrimitive,
Apply: Fn(&[R]) -> Vec<R>,
{
let CgSettings {
diag_a,
initial,
max_iterations,
tolerance,
} = settings;
let n = b.len();
if initial.len() != n {
return Err(CgFailure::LengthMismatch {
expected: n,
found: initial.len(),
});
}
if let Some(d) = diag_a
&& d.len() != n
{
return Err(CgFailure::LengthMismatch {
expected: n,
found: d.len(),
});
}
let precondition = |v: &[R]| -> Vec<R> {
match diag_a {
Some(d) => v
.iter()
.zip(d)
.map(|(&x, &di)| if di > R::zero() { x / di } else { x })
.collect(),
None => v.to_vec(),
}
};
let dot = |u: &[R], v: &[R]| -> R {
let mut acc = R::zero();
for (a, c) in u.iter().zip(v) {
acc += *a * *c;
}
acc
};
let b_norm = dot(b, b).sqrt();
let abs_tol = if b_norm == R::zero() {
tolerance
} else {
tolerance * b_norm
};
let mut x = initial.to_vec();
let ax = apply(&x);
if ax.len() != n {
return Err(CgFailure::LengthMismatch {
expected: n,
found: ax.len(),
});
}
let mut r: Vec<R> = b.iter().zip(&ax).map(|(bb, a)| *bb - *a).collect();
let mut z = precondition(&r);
let mut p = z.clone();
let mut rz = dot(&r, &z);
for iteration in 0..max_iterations {
let residual = dot(&r, &r).sqrt();
if residual <= abs_tol {
return Ok(x);
}
let ap = apply(&p);
if ap.len() != n {
return Err(CgFailure::LengthMismatch {
expected: n,
found: ap.len(),
});
}
let denominator = dot(&p, &ap);
if denominator <= R::zero() {
return Err(CgFailure::NotPositiveDefinite { iteration });
}
let alpha = rz / denominator;
for i in 0..n {
x[i] += alpha * p[i];
r[i] -= alpha * ap[i];
}
z = precondition(&r);
let rz_next = dot(&r, &z);
let beta = rz_next / rz;
for i in 0..n {
p[i] = z[i] + beta * p[i];
}
rz = rz_next;
}
let residual = dot(&r, &r).sqrt();
if residual <= abs_tol {
return Ok(x);
}
Err(CgFailure::NotConverged {
iterations: max_iterations,
residual,
})
}
pub fn cg_solve_preconditioned<R, Apply>(
apply: Apply,
diag_a: &[R],
b: &[R],
tolerance: R,
max_iterations: usize,
) -> Result<Vec<R>, CgFailure<R>>
where
R: RealField + FromPrimitive,
Apply: Fn(&[R]) -> Vec<R>,
{
let zero_start = vec![R::zero(); b.len()];
cg_core(
apply,
b,
CgSettings {
diag_a: Some(diag_a),
initial: &zero_start,
max_iterations,
tolerance,
},
)
}
pub fn cg_solve_preconditioned_from<R, Apply>(
apply: Apply,
diag_a: &[R],
b: &[R],
x0: &[R],
tolerance: R,
max_iterations: usize,
) -> Result<Vec<R>, CgFailure<R>>
where
R: RealField + FromPrimitive,
Apply: Fn(&[R]) -> Vec<R>,
{
cg_core(
apply,
b,
CgSettings {
diag_a: Some(diag_a),
initial: x0,
max_iterations,
tolerance,
},
)
}