use super::coefficients::RadauIia5;
use super::config::RadauMatrixLayout;
use super::error::{RadauConfigError, RadauError, RadauStage, RadauUnsupportedRoute};
use super::telemetry::{RadauLinearStage, RadauTelemetry};
use crate::somelinalg::banded::lapack_style_banded::LapackStyleBandedLuFaithful;
use crate::somelinalg::banded::solver_traits::{DirectLinearSolver, FaerSparseLuSolver};
use faer::sparse::Triplet;
#[derive(Debug, Default)]
pub(crate) struct DenseLinearWorkspace {
pub(crate) real: Vec<f64>,
pub(crate) complex_real: Vec<f64>,
pub(crate) complex_imag: Vec<f64>,
pub(crate) real_pivots: Vec<usize>,
pub(crate) complex_pivots: Vec<usize>,
pub(crate) real_rhs: Vec<f64>,
pub(crate) complex_rhs_real: Vec<f64>,
pub(crate) complex_rhs_imag: Vec<f64>,
}
impl DenseLinearWorkspace {
pub(crate) fn resize_for_dimension(&mut self, dimension: usize) -> Result<(), RadauError> {
let matrix_len = checked_matrix_len(dimension)?;
self.real.resize(matrix_len, 0.0);
self.complex_real.resize(matrix_len, 0.0);
self.complex_imag.resize(matrix_len, 0.0);
self.real_pivots.resize(dimension, 0);
self.complex_pivots.resize(dimension, 0);
self.real_rhs.resize(dimension, 0.0);
self.complex_rhs_real.resize(dimension, 0.0);
self.complex_rhs_imag.resize(dimension, 0.0);
Ok(())
}
}
#[derive(Debug)]
pub(crate) struct BandedLinearWorkspace {
pub(crate) dimension: usize,
pub(crate) lower: usize,
pub(crate) upper: usize,
pub(crate) jacobian: Vec<f64>,
pub(crate) real: Vec<f64>,
pub(crate) complex_real: Vec<f64>,
pub(crate) complex_imag: Vec<f64>,
pub(crate) complex_block: Vec<f64>,
pub(crate) real_rhs: Vec<f64>,
pub(crate) complex_rhs_real: Vec<f64>,
pub(crate) complex_rhs_imag: Vec<f64>,
pub(crate) complex_block_rhs: Vec<f64>,
pub(crate) real_lu: Option<LapackStyleBandedLuFaithful>,
pub(crate) complex_block_lu: Option<LapackStyleBandedLuFaithful>,
}
impl BandedLinearWorkspace {
pub(crate) fn new(dimension: usize, lower: usize, upper: usize) -> Result<Self, RadauError> {
let slots = lower
.checked_add(upper)
.and_then(|value| value.checked_add(1))
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
let storage_len = slots
.checked_mul(dimension)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
let block_dimension = dimension
.checked_mul(2)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
let block_lower = lower
.checked_mul(2)
.and_then(|value| value.checked_add(1))
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
let block_upper = upper
.checked_mul(2)
.and_then(|value| value.checked_add(1))
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
let block_storage_len = block_lower
.checked_add(block_upper)
.and_then(|value| value.checked_add(1))
.and_then(|value| value.checked_mul(block_dimension))
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
Ok(Self {
dimension,
lower,
upper,
jacobian: vec![0.0; storage_len],
real: vec![0.0; storage_len],
complex_real: vec![0.0; storage_len],
complex_imag: vec![0.0; storage_len],
complex_block: vec![0.0; block_storage_len],
real_rhs: vec![0.0; dimension],
complex_rhs_real: vec![0.0; dimension],
complex_rhs_imag: vec![0.0; dimension],
complex_block_rhs: vec![
0.0;
dimension
.checked_mul(2)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?
],
real_lu: None,
complex_block_lu: None,
})
}
pub(crate) fn slot(&self, row: usize, column: usize) -> Option<usize> {
if row >= self.dimension || column >= self.dimension {
return None;
}
let row_with_upper = row.checked_add(self.upper)?;
let column_with_lower = column.checked_add(self.lower)?;
(row_with_upper >= column && column_with_lower >= row)
.then(|| (self.upper + row - column) * self.dimension + column)
}
}
#[derive(Debug)]
pub(crate) struct SparseLinearWorkspace {
pub(crate) dimension: usize,
pub(crate) column_offsets: Vec<usize>,
pub(crate) row_indices: Vec<usize>,
pub(crate) jacobian: Vec<f64>,
pub(crate) real: Vec<f64>,
pub(crate) complex_real: Vec<f64>,
pub(crate) complex_imag: Vec<f64>,
pub(crate) real_rhs: Vec<f64>,
pub(crate) complex_rhs_real: Vec<f64>,
pub(crate) complex_rhs_imag: Vec<f64>,
pub(crate) complex_block_rhs: Vec<f64>,
pub(crate) real_triplets: Vec<Triplet<usize, usize, f64>>,
pub(crate) complex_triplets: Vec<Triplet<usize, usize, f64>>,
pub(crate) real_lu: Option<FaerSparseLuSolver>,
pub(crate) complex_block_lu: Option<FaerSparseLuSolver>,
}
impl SparseLinearWorkspace {
pub(crate) fn from_pattern(
dimension: usize,
entries: &[(usize, usize)],
) -> Result<Self, RadauError> {
let mut pattern = entries.to_vec();
if pattern
.iter()
.any(|&(row, column)| row >= dimension || column >= dimension)
{
return Err(RadauConfigError::SparsePatternOutOfBounds.into());
}
for index in 0..dimension {
pattern.push((index, index));
}
pattern.sort_unstable_by_key(|&(row, column)| (column, row));
pattern.dedup();
let offset_len = dimension
.checked_add(1)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
let mut column_offsets = vec![0usize; offset_len];
for &(_, column) in &pattern {
column_offsets[column + 1] += 1;
}
for column in 0..dimension {
column_offsets[column + 1] += column_offsets[column];
}
let value_len = pattern.len();
let row_indices = pattern.into_iter().map(|(row, _)| row).collect::<Vec<_>>();
Ok(Self {
dimension,
column_offsets,
row_indices,
jacobian: vec![0.0; value_len],
real: vec![0.0; value_len],
complex_real: vec![0.0; value_len],
complex_imag: vec![0.0; value_len],
real_rhs: vec![0.0; dimension],
complex_rhs_real: vec![0.0; dimension],
complex_rhs_imag: vec![0.0; dimension],
complex_block_rhs: vec![
0.0;
dimension
.checked_mul(2)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?
],
real_triplets: Vec::with_capacity(value_len),
complex_triplets: Vec::with_capacity(value_len * 4),
real_lu: None,
complex_block_lu: None,
})
}
pub(crate) fn value_index(&self, row: usize, column: usize) -> Option<usize> {
let start = *self.column_offsets.get(column)?;
let end = *self.column_offsets.get(column + 1)?;
self.row_indices[start..end]
.binary_search(&row)
.ok()
.map(|offset| start + offset)
}
}
#[derive(Debug)]
pub(crate) enum RadauLinearWorkspace {
Dense(DenseLinearWorkspace),
Banded(BandedLinearWorkspace),
Sparse(SparseLinearWorkspace),
}
pub(crate) trait LinearSystemBackend {
type Workspace;
fn layout(&self) -> RadauMatrixLayout;
fn create_workspace(&self, dimension: usize) -> Result<Self::Workspace, RadauError>;
fn assemble_shifted(
&self,
jacobian: JacobianValues<'_>,
h: f64,
coefficients: &RadauIia5,
workspace: &mut Self::Workspace,
) -> Result<(), RadauError>;
fn factor(&self, workspace: &mut Self::Workspace) -> Result<(), RadauError>;
fn solve_real(
&self,
workspace: &mut Self::Workspace,
rhs: &mut [f64],
) -> Result<(), RadauError>;
fn solve_complex(
&self,
workspace: &mut Self::Workspace,
rhs_real: &mut [f64],
rhs_imag: &mut [f64],
) -> Result<(), RadauError>;
fn invalidate(&self, workspace: &mut Self::Workspace);
}
#[derive(Debug, Clone, Copy)]
pub(crate) enum JacobianValues<'a> {
Dense {
values: &'a [f64],
},
Banded {
values: &'a [f64],
lower: usize,
upper: usize,
},
Sparse {
values: &'a [f64],
entries: &'a [(usize, usize)],
},
}
#[derive(Debug, Clone, Copy, Default)]
pub(crate) struct DenseBackend;
#[derive(Debug, Clone, Copy)]
pub(crate) struct BandedBackend {
pub(crate) lower: usize,
pub(crate) upper: usize,
}
#[derive(Debug, Clone)]
pub(crate) struct SparseBackend {
pub(crate) pattern: Vec<(usize, usize)>,
}
#[derive(Debug, Clone)]
pub(crate) enum PreparedLinearBackend {
Dense(DenseBackend),
Banded(BandedBackend),
Sparse(SparseBackend),
}
impl PreparedLinearBackend {
pub(crate) fn from_layout(layout: RadauMatrixLayout) -> Self {
Self::from_layout_and_pattern(layout, &[])
}
pub(crate) fn from_layout_and_pattern(
layout: RadauMatrixLayout,
pattern: &[(usize, usize)],
) -> Self {
match layout {
RadauMatrixLayout::Dense => Self::Dense(DenseBackend),
RadauMatrixLayout::Banded { lower, upper } => {
Self::Banded(BandedBackend { lower, upper })
}
RadauMatrixLayout::Sparse => Self::Sparse(SparseBackend {
pattern: pattern.to_vec(),
}),
}
}
pub(crate) fn layout(&self) -> RadauMatrixLayout {
match self {
Self::Dense(backend) => backend.layout(),
Self::Banded(backend) => backend.layout(),
Self::Sparse(backend) => backend.layout(),
}
}
pub(crate) fn create_workspace(
&self,
dimension: usize,
) -> Result<RadauLinearWorkspace, RadauError> {
match self {
Self::Dense(backend) => Ok(RadauLinearWorkspace::Dense(
backend.create_workspace(dimension)?,
)),
Self::Banded(backend) => Ok(RadauLinearWorkspace::Banded(
backend.create_workspace(dimension)?,
)),
Self::Sparse(backend) => Ok(RadauLinearWorkspace::Sparse(
backend.create_workspace(dimension)?,
)),
}
}
pub(crate) fn assemble_shifted_into(
&self,
jacobian: JacobianValues<'_>,
h: f64,
coefficients: &RadauIia5,
workspace: &mut RadauLinearWorkspace,
telemetry: &mut RadauTelemetry,
) -> Result<(), RadauError> {
telemetry.measure_linear(RadauLinearStage::JacobianAssembly, || {
match (self, workspace) {
(Self::Dense(backend), RadauLinearWorkspace::Dense(workspace)) => {
backend.assemble_shifted(jacobian, h, coefficients, workspace)
}
(Self::Banded(backend), RadauLinearWorkspace::Banded(workspace)) => {
backend.assemble_shifted(jacobian, h, coefficients, workspace)
}
(Self::Sparse(backend), RadauLinearWorkspace::Sparse(workspace)) => {
backend.assemble_shifted(jacobian, h, coefficients, workspace)
}
_ => Err(RadauConfigError::LinearWorkspaceMismatch.into()),
}
})
}
pub(crate) fn factor_into(
&self,
workspace: &mut RadauLinearWorkspace,
telemetry: &mut RadauTelemetry,
) -> Result<(), RadauError> {
telemetry.measure_linear(RadauLinearStage::Factorization, || {
match (self, workspace) {
(Self::Dense(backend), RadauLinearWorkspace::Dense(workspace)) => {
backend.factor(workspace)
}
(Self::Banded(backend), RadauLinearWorkspace::Banded(workspace)) => {
backend.factor(workspace)
}
(Self::Sparse(backend), RadauLinearWorkspace::Sparse(workspace)) => {
backend.factor(workspace)
}
_ => Err(RadauConfigError::LinearWorkspaceMismatch.into()),
}
})
}
pub(crate) fn solve_real_into(
&self,
workspace: &mut RadauLinearWorkspace,
rhs: &mut [f64],
telemetry: &mut RadauTelemetry,
) -> Result<(), RadauError> {
telemetry.measure_linear(RadauLinearStage::RealSolve, || match (self, workspace) {
(Self::Dense(backend), RadauLinearWorkspace::Dense(workspace)) => {
backend.solve_real(workspace, rhs)
}
(Self::Banded(backend), RadauLinearWorkspace::Banded(workspace)) => {
backend.solve_real(workspace, rhs)
}
(Self::Sparse(backend), RadauLinearWorkspace::Sparse(workspace)) => {
backend.solve_real(workspace, rhs)
}
_ => Err(RadauConfigError::LinearWorkspaceMismatch.into()),
})
}
pub(crate) fn solve_complex_into(
&self,
workspace: &mut RadauLinearWorkspace,
rhs_real: &mut [f64],
rhs_imag: &mut [f64],
telemetry: &mut RadauTelemetry,
) -> Result<(), RadauError> {
telemetry.measure_linear(RadauLinearStage::ComplexSolve, || match (self, workspace) {
(Self::Dense(backend), RadauLinearWorkspace::Dense(workspace)) => {
backend.solve_complex(workspace, rhs_real, rhs_imag)
}
(Self::Banded(backend), RadauLinearWorkspace::Banded(workspace)) => {
backend.solve_complex(workspace, rhs_real, rhs_imag)
}
(Self::Sparse(backend), RadauLinearWorkspace::Sparse(workspace)) => {
backend.solve_complex(workspace, rhs_real, rhs_imag)
}
_ => Err(RadauConfigError::LinearWorkspaceMismatch.into()),
})
}
pub(crate) fn invalidate_into(
&self,
workspace: &mut RadauLinearWorkspace,
telemetry: &mut RadauTelemetry,
) -> Result<(), RadauError> {
telemetry.measure_linear(RadauLinearStage::Invalidate, || match (self, workspace) {
(Self::Dense(backend), RadauLinearWorkspace::Dense(workspace)) => {
backend.invalidate(workspace);
Ok(())
}
(Self::Banded(backend), RadauLinearWorkspace::Banded(workspace)) => {
backend.invalidate(workspace);
Ok(())
}
(Self::Sparse(backend), RadauLinearWorkspace::Sparse(workspace)) => {
backend.invalidate(workspace);
Ok(())
}
_ => Err(RadauConfigError::LinearWorkspaceMismatch.into()),
})
}
}
impl LinearSystemBackend for DenseBackend {
type Workspace = DenseLinearWorkspace;
fn layout(&self) -> RadauMatrixLayout {
RadauMatrixLayout::Dense
}
fn create_workspace(&self, dimension: usize) -> Result<Self::Workspace, RadauError> {
let mut workspace = DenseLinearWorkspace::default();
workspace.resize_for_dimension(dimension)?;
Ok(workspace)
}
fn assemble_shifted(
&self,
jacobian: JacobianValues<'_>,
h: f64,
coefficients: &RadauIia5,
workspace: &mut Self::Workspace,
) -> Result<(), RadauError> {
let values = match jacobian {
JacobianValues::Dense { values } => values,
_ => return Err(RadauUnsupportedRoute::DenseValuesRequired.into()),
};
let dimension = workspace.real_rhs.len();
let matrix_len = checked_matrix_len(dimension)?;
if values.len() != matrix_len {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::Jacobian,
expected: matrix_len,
actual: values.len(),
});
}
let real_shift = coefficients.mu_real / h;
let complex_shift_real = coefficients.mu_complex.0 / h;
let complex_shift_imag = coefficients.mu_complex.1 / h;
for (index, value) in values.iter().copied().enumerate() {
let diagonal = index / dimension == index % dimension;
workspace.real[index] = if diagonal { real_shift - value } else { -value };
workspace.complex_real[index] = if diagonal {
complex_shift_real - value
} else {
-value
};
workspace.complex_imag[index] = if diagonal { complex_shift_imag } else { 0.0 };
}
Ok(())
}
fn factor(&self, workspace: &mut Self::Workspace) -> Result<(), RadauError> {
let dimension = workspace.real_rhs.len();
factor_dense_in_place(&mut workspace.real, &mut workspace.real_pivots, dimension)?;
factor_complex_in_place(
&mut workspace.complex_real,
&mut workspace.complex_imag,
&mut workspace.complex_pivots,
dimension,
)
}
fn solve_real(
&self,
workspace: &mut Self::Workspace,
rhs: &mut [f64],
) -> Result<(), RadauError> {
solve_factored_dense_in_place(
&workspace.real,
&workspace.real_pivots,
rhs,
workspace.real_rhs.len(),
)
}
fn solve_complex(
&self,
workspace: &mut Self::Workspace,
rhs_real: &mut [f64],
rhs_imag: &mut [f64],
) -> Result<(), RadauError> {
solve_factored_complex_in_place(
&workspace.complex_real,
&workspace.complex_imag,
&workspace.complex_pivots,
rhs_real,
rhs_imag,
workspace.real_rhs.len(),
)
}
fn invalidate(&self, workspace: &mut Self::Workspace) {
workspace.real.fill(0.0);
workspace.complex_real.fill(0.0);
workspace.complex_imag.fill(0.0);
workspace.real_pivots.fill(0);
workspace.complex_pivots.fill(0);
}
}
impl LinearSystemBackend for BandedBackend {
type Workspace = BandedLinearWorkspace;
fn layout(&self) -> RadauMatrixLayout {
RadauMatrixLayout::Banded {
lower: self.lower,
upper: self.upper,
}
}
fn create_workspace(&self, dimension: usize) -> Result<Self::Workspace, RadauError> {
BandedLinearWorkspace::new(dimension, self.lower, self.upper)
}
fn assemble_shifted(
&self,
jacobian: JacobianValues<'_>,
h: f64,
coefficients: &RadauIia5,
workspace: &mut Self::Workspace,
) -> Result<(), RadauError> {
let (values, lower, upper) = match jacobian {
JacobianValues::Banded {
values,
lower,
upper,
} => (values, lower, upper),
_ => return Err(RadauUnsupportedRoute::BandedValuesRequired.into()),
};
if lower != self.lower || upper != self.upper {
return Err(RadauConfigError::BandedBandwidthMismatch.into());
}
let slots = lower
.checked_add(upper)
.and_then(|value| value.checked_add(1))
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: workspace.dimension,
})?;
let expected =
slots
.checked_mul(workspace.dimension)
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: workspace.dimension,
})?;
if values.len() != expected {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::Jacobian,
expected,
actual: values.len(),
});
}
workspace.real.fill(0.0);
workspace.complex_real.fill(0.0);
workspace.complex_imag.fill(0.0);
let real_shift = coefficients.mu_real / h;
let complex_shift_real = coefficients.mu_complex.0 / h;
let complex_shift_imag = coefficients.mu_complex.1 / h;
if workspace.dimension == 0 {
return Ok(());
}
for column in 0..workspace.dimension {
let row_start = column.saturating_sub(self.upper);
let row_end = column
.saturating_add(self.lower)
.min(workspace.dimension - 1)
+ 1;
for row in row_start..row_end {
let slot = workspace
.slot(row, column)
.ok_or(RadauConfigError::InvalidBandedSlot)?;
let value = values[slot];
let diagonal = row == column;
workspace.real[slot] = if diagonal { real_shift - value } else { -value };
workspace.complex_real[slot] = if diagonal {
complex_shift_real - value
} else {
-value
};
workspace.complex_imag[slot] = if diagonal { complex_shift_imag } else { 0.0 };
}
}
Ok(())
}
fn factor(&self, workspace: &mut Self::Workspace) -> Result<(), RadauError> {
let mut real_lu =
LapackStyleBandedLuFaithful::new(workspace.dimension, workspace.lower, workspace.upper)
.map_err(|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
})?;
real_lu.factor_from_compact(&workspace.real).map_err(|_| {
RadauError::LinearSolveFailure {
dimension: workspace.dimension,
}
})?;
let block_dimension =
workspace
.dimension
.checked_mul(2)
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: workspace.dimension,
})?;
let block_lower = workspace
.lower
.checked_mul(2)
.and_then(|value| value.checked_add(1))
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: workspace.dimension,
})?;
let block_upper = workspace
.upper
.checked_mul(2)
.and_then(|value| value.checked_add(1))
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: workspace.dimension,
})?;
complex_banded_block_into(
&mut workspace.complex_block,
workspace.dimension,
workspace.lower,
workspace.upper,
&workspace.complex_real,
&workspace.complex_imag,
)?;
let mut complex_block_lu =
LapackStyleBandedLuFaithful::new(block_dimension, block_lower, block_upper).map_err(
|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
},
)?;
complex_block_lu
.factor_from_compact(&workspace.complex_block)
.map_err(|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
})?;
workspace.real_lu = Some(real_lu);
workspace.complex_block_lu = Some(complex_block_lu);
Ok(())
}
fn solve_real(
&self,
workspace: &mut Self::Workspace,
rhs: &mut [f64],
) -> Result<(), RadauError> {
if rhs.len() != workspace.dimension {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: workspace.dimension,
actual: rhs.len(),
});
}
let Some(lu) = workspace.real_lu.as_ref() else {
return Err(RadauError::LinearSolveFailure {
dimension: workspace.dimension,
});
};
lu.solve_in_place(rhs)
.map_err(|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
})
}
fn solve_complex(
&self,
workspace: &mut Self::Workspace,
rhs_real: &mut [f64],
rhs_imag: &mut [f64],
) -> Result<(), RadauError> {
if rhs_real.len() != workspace.dimension || rhs_imag.len() != workspace.dimension {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: workspace.dimension,
actual: rhs_real.len().min(rhs_imag.len()),
});
}
let Some(lu) = workspace.complex_block_lu.as_ref() else {
return Err(RadauError::LinearSolveFailure {
dimension: workspace.dimension,
});
};
let block_rhs = &mut workspace.complex_block_rhs;
for index in 0..workspace.dimension {
block_rhs[2 * index] = rhs_real[index];
block_rhs[2 * index + 1] = rhs_imag[index];
}
lu.solve_in_place(block_rhs)
.map_err(|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
})?;
for index in 0..workspace.dimension {
rhs_real[index] = block_rhs[2 * index];
rhs_imag[index] = block_rhs[2 * index + 1];
}
Ok(())
}
fn invalidate(&self, workspace: &mut Self::Workspace) {
workspace.jacobian.fill(0.0);
workspace.real.fill(0.0);
workspace.complex_real.fill(0.0);
workspace.complex_imag.fill(0.0);
workspace.complex_block.fill(0.0);
workspace.real_lu = None;
workspace.complex_block_lu = None;
}
}
impl LinearSystemBackend for SparseBackend {
type Workspace = SparseLinearWorkspace;
fn layout(&self) -> RadauMatrixLayout {
RadauMatrixLayout::Sparse
}
fn create_workspace(&self, dimension: usize) -> Result<Self::Workspace, RadauError> {
SparseLinearWorkspace::from_pattern(dimension, &self.pattern)
}
fn assemble_shifted(
&self,
jacobian: JacobianValues<'_>,
h: f64,
coefficients: &RadauIia5,
workspace: &mut Self::Workspace,
) -> Result<(), RadauError> {
let (values, entries) = match jacobian {
JacobianValues::Sparse { values, entries } => (values, entries),
_ => return Err(RadauUnsupportedRoute::SparseValuesRequired.into()),
};
if values.len() != entries.len() {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::Jacobian,
expected: entries.len(),
actual: values.len(),
});
}
workspace.real.fill(0.0);
workspace.complex_real.fill(0.0);
workspace.complex_imag.fill(0.0);
for (&(row, column), &value) in entries.iter().zip(values) {
let Some(index) = workspace.value_index(row, column) else {
return Err(RadauConfigError::SparseEntryMissing.into());
};
workspace.real[index] = -value;
workspace.complex_real[index] = -value;
}
let real_shift = coefficients.mu_real / h;
let complex_shift_real = coefficients.mu_complex.0 / h;
let complex_shift_imag = coefficients.mu_complex.1 / h;
for index in 0..workspace.dimension {
let Some(diagonal) = workspace.value_index(index, index) else {
return Err(RadauConfigError::SparseDiagonalMissing.into());
};
workspace.real[diagonal] += real_shift;
workspace.complex_real[diagonal] += complex_shift_real;
workspace.complex_imag[diagonal] = complex_shift_imag;
}
Ok(())
}
fn factor(&self, workspace: &mut Self::Workspace) -> Result<(), RadauError> {
sparse_triplets_into(
&mut workspace.real_triplets,
workspace.dimension,
&workspace.column_offsets,
&workspace.row_indices,
&workspace.real,
);
let real_lu =
FaerSparseLuSolver::from_triplets(workspace.dimension, &workspace.real_triplets)
.map_err(|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
})?;
sparse_complex_block_triplets_into(
&mut workspace.complex_triplets,
workspace.dimension,
&workspace.column_offsets,
&workspace.row_indices,
&workspace.complex_real,
&workspace.complex_imag,
);
let complex_lu = FaerSparseLuSolver::from_triplets(
workspace
.dimension
.checked_mul(2)
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: workspace.dimension,
})?,
&workspace.complex_triplets,
)
.map_err(|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
})?;
workspace.real_lu = Some(real_lu);
workspace.complex_block_lu = Some(complex_lu);
Ok(())
}
fn solve_real(
&self,
workspace: &mut Self::Workspace,
rhs: &mut [f64],
) -> Result<(), RadauError> {
if rhs.len() != workspace.dimension {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: workspace.dimension,
actual: rhs.len(),
});
}
let Some(lu) = workspace.real_lu.as_ref() else {
return Err(RadauError::LinearSolveFailure {
dimension: workspace.dimension,
});
};
lu.solve_in_place(rhs)
.map_err(|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
})
}
fn solve_complex(
&self,
workspace: &mut Self::Workspace,
rhs_real: &mut [f64],
rhs_imag: &mut [f64],
) -> Result<(), RadauError> {
if rhs_real.len() != workspace.dimension || rhs_imag.len() != workspace.dimension {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: workspace.dimension,
actual: rhs_real.len().min(rhs_imag.len()),
});
}
let Some(lu) = workspace.complex_block_lu.as_ref() else {
return Err(RadauError::LinearSolveFailure {
dimension: workspace.dimension,
});
};
let block_rhs = &mut workspace.complex_block_rhs;
for index in 0..workspace.dimension {
block_rhs[2 * index] = rhs_real[index];
block_rhs[2 * index + 1] = rhs_imag[index];
}
lu.solve_in_place(block_rhs)
.map_err(|_| RadauError::LinearSolveFailure {
dimension: workspace.dimension,
})?;
for index in 0..workspace.dimension {
rhs_real[index] = block_rhs[2 * index];
rhs_imag[index] = block_rhs[2 * index + 1];
}
Ok(())
}
fn invalidate(&self, workspace: &mut Self::Workspace) {
workspace.jacobian.fill(0.0);
workspace.real.fill(0.0);
workspace.complex_real.fill(0.0);
workspace.complex_imag.fill(0.0);
workspace.real_lu = None;
workspace.complex_block_lu = None;
}
}
fn complex_banded_block_into(
output: &mut [f64],
source_dimension: usize,
source_lower: usize,
source_upper: usize,
complex_real: &[f64],
complex_imag: &[f64],
) -> Result<(), RadauError> {
let dimension = source_dimension
.checked_mul(2)
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: source_dimension,
})?;
let lower = source_lower
.checked_mul(2)
.and_then(|value| value.checked_add(1))
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: source_dimension,
})?;
let upper = source_upper
.checked_mul(2)
.and_then(|value| value.checked_add(1))
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: source_dimension,
})?;
let source_slots = source_lower
.checked_add(source_upper)
.and_then(|value| value.checked_add(1))
.and_then(|value| value.checked_mul(source_dimension))
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: source_dimension,
})?;
let output_slots = lower
.checked_add(upper)
.and_then(|value| value.checked_add(1))
.and_then(|value| value.checked_mul(dimension))
.ok_or(RadauError::WorkspaceSizeOverflow {
dimension: source_dimension,
})?;
if complex_real.len() != source_slots || complex_imag.len() != source_slots {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: source_slots,
actual: complex_real.len().min(complex_imag.len()),
});
}
if output.len() != output_slots {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: output_slots,
actual: output.len(),
});
}
output.fill(0.0);
for column in 0..source_dimension {
let row_start = column.saturating_sub(source_upper);
let row_end = column
.saturating_add(source_lower)
.min(source_dimension - 1)
+ 1;
for row in row_start..row_end {
let source_slot = (source_upper + row - column) * source_dimension + column;
let real = complex_real[source_slot];
let imag = complex_imag[source_slot];
let set = |output: &mut [f64], row: usize, column: usize, value: f64| {
output[(upper + row - column) * dimension + column] = value;
};
set(output, 2 * row, 2 * column, real);
set(output, 2 * row, 2 * column + 1, -imag);
set(output, 2 * row + 1, 2 * column, imag);
set(output, 2 * row + 1, 2 * column + 1, real);
}
}
Ok(())
}
fn sparse_triplets_into(
triplets: &mut Vec<Triplet<usize, usize, f64>>,
dimension: usize,
column_offsets: &[usize],
row_indices: &[usize],
values: &[f64],
) {
triplets.clear();
for column in 0..dimension {
let start = column_offsets[column];
let end = column_offsets[column + 1];
for index in start..end {
triplets.push(Triplet::new(row_indices[index], column, values[index]));
}
}
}
fn sparse_complex_block_triplets_into(
triplets: &mut Vec<Triplet<usize, usize, f64>>,
dimension: usize,
column_offsets: &[usize],
row_indices: &[usize],
real: &[f64],
imag: &[f64],
) {
triplets.clear();
for column in 0..dimension {
let start = column_offsets[column];
let end = column_offsets[column + 1];
for index in start..end {
let row = row_indices[index];
let real_value = real[index];
let imag_value = imag[index];
let block_row = 2 * row;
let block_column = 2 * column;
triplets.push(Triplet::new(block_row, block_column, real_value));
triplets.push(Triplet::new(block_row, block_column + 1, -imag_value));
triplets.push(Triplet::new(block_row + 1, block_column, imag_value));
triplets.push(Triplet::new(block_row + 1, block_column + 1, real_value));
}
}
}
impl Default for RadauLinearWorkspace {
fn default() -> Self {
Self::Dense(DenseLinearWorkspace::default())
}
}
fn checked_matrix_len(dimension: usize) -> Result<usize, RadauError> {
dimension
.checked_mul(dimension)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })
}
pub(crate) fn factor_dense_in_place(
matrix: &mut [f64],
pivots: &mut [usize],
dimension: usize,
) -> Result<(), RadauError> {
let matrix_len = dimension
.checked_mul(dimension)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
if matrix.len() != matrix_len || pivots.len() != dimension {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: matrix_len,
actual: matrix.len(),
});
}
for column in 0..dimension {
let mut pivot = column;
let mut pivot_abs = matrix[column * dimension + column].abs();
for row in column + 1..dimension {
let candidate = matrix[row * dimension + column].abs();
if candidate > pivot_abs {
pivot = row;
pivot_abs = candidate;
}
}
if !pivot_abs.is_finite() || pivot_abs <= f64::EPSILON {
return Err(RadauError::LinearSolveFailure { dimension });
}
pivots[column] = pivot;
if pivot != column {
for index in 0..dimension {
matrix.swap(column * dimension + index, pivot * dimension + index);
}
}
for row in column + 1..dimension {
let factor = matrix[row * dimension + column] / matrix[column * dimension + column];
matrix[row * dimension + column] = factor;
for index in column + 1..dimension {
matrix[row * dimension + index] -= factor * matrix[column * dimension + index];
}
}
}
Ok(())
}
pub(crate) fn solve_factored_dense_in_place(
matrix: &[f64],
pivots: &[usize],
rhs: &mut [f64],
dimension: usize,
) -> Result<(), RadauError> {
let matrix_len = dimension
.checked_mul(dimension)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
if matrix.len() != matrix_len || pivots.len() != dimension || rhs.len() != dimension {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: dimension,
actual: rhs.len(),
});
}
for (column, &pivot) in pivots.iter().enumerate() {
if pivot != column {
rhs.swap(column, pivot);
}
}
for row in 0..dimension {
for column in 0..row {
rhs[row] -= matrix[row * dimension + column] * rhs[column];
}
}
for row in (0..dimension).rev() {
for column in row + 1..dimension {
rhs[row] -= matrix[row * dimension + column] * rhs[column];
}
let diagonal = matrix[row * dimension + row];
if !diagonal.is_finite() || diagonal.abs() <= f64::EPSILON {
return Err(RadauError::LinearSolveFailure { dimension });
}
rhs[row] /= diagonal;
}
Ok(())
}
pub(crate) fn factor_complex_in_place(
real: &mut [f64],
imag: &mut [f64],
pivots: &mut [usize],
dimension: usize,
) -> Result<(), RadauError> {
let matrix_len = dimension
.checked_mul(dimension)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
if real.len() != matrix_len || imag.len() != matrix_len || pivots.len() != dimension {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: matrix_len,
actual: real.len(),
});
}
for column in 0..dimension {
let mut pivot = column;
let mut pivot_abs = complex_abs_squared(
real[column * dimension + column],
imag[column * dimension + column],
);
for row in column + 1..dimension {
let candidate = complex_abs_squared(
real[row * dimension + column],
imag[row * dimension + column],
);
if candidate > pivot_abs {
pivot = row;
pivot_abs = candidate;
}
}
if !pivot_abs.is_finite() || pivot_abs <= f64::EPSILON * f64::EPSILON {
return Err(RadauError::LinearSolveFailure { dimension });
}
pivots[column] = pivot;
if pivot != column {
for index in 0..dimension {
real.swap(column * dimension + index, pivot * dimension + index);
imag.swap(column * dimension + index, pivot * dimension + index);
}
}
let pivot_real = real[column * dimension + column];
let pivot_imag = imag[column * dimension + column];
for row in column + 1..dimension {
let index = row * dimension + column;
let (factor_real, factor_imag) =
complex_divide(real[index], imag[index], pivot_real, pivot_imag);
real[index] = factor_real;
imag[index] = factor_imag;
for other in column + 1..dimension {
let target = row * dimension + other;
let pivot_row = column * dimension + other;
let product_real = factor_real * real[pivot_row] - factor_imag * imag[pivot_row];
let product_imag = factor_real * imag[pivot_row] + factor_imag * real[pivot_row];
real[target] -= product_real;
imag[target] -= product_imag;
}
}
}
Ok(())
}
pub(crate) fn solve_factored_complex_in_place(
real: &[f64],
imag: &[f64],
pivots: &[usize],
rhs_real: &mut [f64],
rhs_imag: &mut [f64],
dimension: usize,
) -> Result<(), RadauError> {
let matrix_len = dimension
.checked_mul(dimension)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
if real.len() != matrix_len
|| imag.len() != matrix_len
|| pivots.len() != dimension
|| rhs_real.len() != dimension
|| rhs_imag.len() != dimension
{
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: dimension,
actual: rhs_real.len(),
});
}
for (column, &pivot) in pivots.iter().enumerate() {
if pivot != column {
rhs_real.swap(column, pivot);
rhs_imag.swap(column, pivot);
}
}
for row in 0..dimension {
for column in 0..row {
let index = row * dimension + column;
let product_real = real[index] * rhs_real[column] - imag[index] * rhs_imag[column];
let product_imag = real[index] * rhs_imag[column] + imag[index] * rhs_real[column];
rhs_real[row] -= product_real;
rhs_imag[row] -= product_imag;
}
}
for row in (0..dimension).rev() {
for column in row + 1..dimension {
let index = row * dimension + column;
let product_real = real[index] * rhs_real[column] - imag[index] * rhs_imag[column];
let product_imag = real[index] * rhs_imag[column] + imag[index] * rhs_real[column];
rhs_real[row] -= product_real;
rhs_imag[row] -= product_imag;
}
let index = row * dimension + row;
let (value_real, value_imag) =
complex_divide(rhs_real[row], rhs_imag[row], real[index], imag[index]);
if !value_real.is_finite() || !value_imag.is_finite() {
return Err(RadauError::LinearSolveFailure { dimension });
}
rhs_real[row] = value_real;
rhs_imag[row] = value_imag;
}
Ok(())
}
fn complex_abs_squared(real: f64, imag: f64) -> f64 {
real.mul_add(real, imag * imag)
}
fn complex_divide(lhs_real: f64, lhs_imag: f64, rhs_real: f64, rhs_imag: f64) -> (f64, f64) {
let denominator = complex_abs_squared(rhs_real, rhs_imag);
(
(lhs_real * rhs_real + lhs_imag * rhs_imag) / denominator,
(lhs_imag * rhs_real - lhs_real * rhs_imag) / denominator,
)
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub(crate) enum RadauShift {
Real { mu_over_h: f64 },
Complex { real: f64, imag: f64 },
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub(crate) struct RadauFactorKey {
pub jacobian_generation: u64,
pub step_generation: u64,
pub shift: RadauShift,
pub matrix_layout: RadauMatrixLayout,
}
impl RadauFactorKey {
pub(crate) const fn new(
jacobian_generation: u64,
step_generation: u64,
shift: RadauShift,
matrix_layout: RadauMatrixLayout,
) -> Self {
Self {
jacobian_generation,
step_generation,
shift,
matrix_layout,
}
}
pub(crate) fn remains_valid_for(&self, current: Self) -> bool {
*self == current
}
}
#[derive(Debug, Default)]
pub(crate) struct RadauDenseShiftedMatrix {
dimension: usize,
values: Vec<f64>,
}
impl RadauDenseShiftedMatrix {
pub(crate) fn new(dimension: usize) -> Result<Self, RadauError> {
let matrix_len = dimension
.checked_mul(dimension)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
Ok(Self {
dimension,
values: vec![0.0; matrix_len],
})
}
pub(crate) fn load_real_shifted(
&mut self,
jacobian: &[f64],
mu_over_h: f64,
) -> Result<(), RadauError> {
if jacobian.len() != self.values.len() {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: self.values.len(),
actual: jacobian.len(),
});
}
for (index, value) in jacobian.iter().copied().enumerate() {
let diagonal = index / self.dimension == index % self.dimension;
self.values[index] = if diagonal { mu_over_h - value } else { -value };
}
Ok(())
}
pub(crate) fn dimension(&self) -> usize {
self.dimension
}
pub(crate) fn values(&self) -> &[f64] {
&self.values
}
pub(crate) fn values_mut(&mut self) -> &mut [f64] {
&mut self.values
}
pub(crate) fn into_values(self) -> Vec<f64> {
self.values
}
}
#[derive(Debug, Default)]
pub(crate) struct RadauDenseComplexShiftedMatrix {
dimension: usize,
real: Vec<f64>,
imag: Vec<f64>,
}
impl RadauDenseComplexShiftedMatrix {
pub(crate) fn new(dimension: usize) -> Result<Self, RadauError> {
let matrix_len = dimension
.checked_mul(dimension)
.ok_or(RadauError::WorkspaceSizeOverflow { dimension })?;
Ok(Self {
dimension,
real: vec![0.0; matrix_len],
imag: vec![0.0; matrix_len],
})
}
pub(crate) fn load_shifted(
&mut self,
jacobian: &[f64],
real_shift: f64,
imag_shift: f64,
) -> Result<(), RadauError> {
if jacobian.len() != self.real.len() {
return Err(RadauError::ShapeMismatch {
stage: RadauStage::LinearSolve,
expected: self.real.len(),
actual: jacobian.len(),
});
}
for (index, value) in jacobian.iter().copied().enumerate() {
let diagonal = index / self.dimension == index % self.dimension;
self.real[index] = if diagonal { real_shift - value } else { -value };
self.imag[index] = if diagonal { imag_shift } else { 0.0 };
}
Ok(())
}
pub(crate) fn real(&self) -> &[f64] {
&self.real
}
pub(crate) fn imag(&self) -> &[f64] {
&self.imag
}
}