use xingo_sparse::{DenseBlock, DenseKernel, NaiveDense, Real};
use crate::real::{is_exact_zero, le_exact, lt};
use crate::small::{
apply_householder, axpy_sub, check_finite, check_len, dot, givens, householder, norm2,
orthogonalize,
};
use crate::{CholOptions, Cholesky, MatRef, StatlaError};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct QrOptions {
pub reorth_eta: f64,
pub rank_tol: f64,
}
impl Default for QrOptions {
fn default() -> Self {
Self {
reorth_eta: core::f64::consts::FRAC_1_SQRT_2,
rank_tol: 0.0,
}
}
}
impl QrOptions {
fn validate(&self) -> Result<(), StatlaError> {
if !(self.reorth_eta.is_finite()
&& lt(0.0, self.reorth_eta)
&& le_exact(self.reorth_eta, 1.0))
{
return Err(StatlaError::InvalidArgument {
what: "QrOptions::reorth_eta must be in (0, 1]",
});
}
if !(self.rank_tol.is_finite() && le_exact(0.0, self.rank_tol)) {
return Err(StatlaError::InvalidArgument {
what: "QrOptions::rank_tol must be finite and >= 0",
});
}
Ok(())
}
fn tau<T: Real>(&self, n: usize) -> T {
if is_exact_zero(self.rank_tol) {
#[allow(clippy::cast_precision_loss)] let nf = n.max(1) as f64;
T::EPSILON * T::from_f64(nf)
} else {
T::from_f64(self.rank_tol)
}
}
}
#[derive(Clone, Debug)]
pub struct Qr<T: Real = f64, K: DenseKernel<T> = NaiveDense> {
q: DenseBlock<T>,
r: DenseBlock<T>,
kernel: K,
opts: QrOptions,
}
impl<T: Real> Qr<T, NaiveDense> {
pub fn factor(a: MatRef<'_, T>, opts: &QrOptions) -> Result<Self, StatlaError> {
Self::factor_with(a, NaiveDense, opts)
}
}
fn fix_signs<T: Real>(q: &mut DenseBlock<T>, r: &mut DenseBlock<T>, from: usize) {
let (m, n) = (q.rows, r.cols);
for k in from..n.min(r.rows) {
if lt(r.data[k + k * r.rows], T::ZERO) {
for j in k..n {
r.data[k + j * r.rows] = -r.data[k + j * r.rows];
}
for v in &mut q.data[k * m..(k + 1) * m] {
*v = -*v;
}
}
}
}
fn drop_last_row<T: Real>(r: &DenseBlock<T>) -> DenseBlock<T> {
let (rows, cols) = (r.rows - 1, r.cols);
let mut out = DenseBlock::zeros(rows, cols);
for j in 0..cols {
out.data[j * rows..(j + 1) * rows].copy_from_slice(&r.data[j * r.rows..j * r.rows + rows]);
}
out
}
impl<T: Real, K: DenseKernel<T>> Qr<T, K> {
pub fn factor_with(a: MatRef<'_, T>, kernel: K, opts: &QrOptions) -> Result<Self, StatlaError> {
opts.validate()?;
let (m, n) = (a.rows(), a.cols());
if m < n {
return Err(StatlaError::InvalidArgument {
what: "Qr::factor: fewer rows than columns",
});
}
let mut w = a.to_block();
check_finite("a", &w.data)?;
let col_norms: Vec<T> = (0..n).map(|j| norm2(&w.data[j * m..(j + 1) * m])).collect();
let mut taus = vec![T::ZERO; n];
for k in 0..n {
let (left, right) = w.data.split_at_mut((k + 1) * m);
let x = &mut left[k * m + k..];
taus[k] = householder(x);
let v = &x[1..];
for j in 0..n - k - 1 {
apply_householder(taus[k], v, &mut right[j * m + k..(j + 1) * m]);
}
}
let mut r = DenseBlock::zeros(n, n);
for j in 0..n {
for i in 0..=j {
r.data[i + j * n] = w.data[i + j * m];
}
}
let mut q = DenseBlock::zeros(m, n);
for k in 0..n {
q.data[k + k * m] = T::ONE;
}
for k in (0..n).rev() {
let v = &w.data[k * m + k + 1..(k + 1) * m];
for j in k..n {
apply_householder(taus[k], v, &mut q.data[j * m + k..(j + 1) * m]);
}
}
fix_signs(&mut q, &mut r, 0);
let tau = opts.tau::<T>(n);
for k in 0..n {
if le_exact(r.data[k + k * n], tau * col_norms[k]) {
return Err(StatlaError::RankDeficient { column: k });
}
}
let f = Self {
q,
r,
kernel,
opts: *opts,
};
debug_assert_eq!(f.check_shape(), Ok(()), "(Q2) after factor");
Ok(f)
}
#[must_use]
pub fn nrows(&self) -> usize {
self.q.rows
}
#[must_use]
pub fn ncols(&self) -> usize {
self.r.cols
}
#[must_use]
pub fn q(&self) -> MatRef<'_, T> {
MatRef::from_block(&self.q)
}
#[must_use]
pub fn r(&self) -> MatRef<'_, T> {
MatRef::from_block(&self.r)
}
pub fn insert_row(&mut self, i: usize, w: &[T]) -> Result<(), StatlaError> {
let (m, n) = (self.nrows(), self.ncols());
check_len("w", n, w.len())?;
if i > m {
return Err(StatlaError::InvalidArgument {
what: "Qr::insert_row: row index > m",
});
}
check_finite("w", w)?;
let m1 = m + 1;
let mut q = DenseBlock::zeros(m1, n);
for j in 0..n {
let src = &self.q.data[j * m..(j + 1) * m];
let dst = &mut q.data[j * m1..(j + 1) * m1];
dst[..i].copy_from_slice(&src[..i]);
dst[i + 1..].copy_from_slice(&src[i..]);
}
let mut e = vec![T::ZERO; m1];
e[i] = T::ONE;
let mut wr = w.to_vec();
for j in 0..n {
let (g, rho) = givens(self.r.data[j + j * n], wr[j]);
self.r.data[j + j * n] = rho;
wr[j] = T::ZERO;
for l in j + 1..n {
let (p, s) = g.apply(self.r.data[j + l * n], wr[l]);
self.r.data[j + l * n] = p;
wr[l] = s;
}
for (x, y) in q.data[j * m1..(j + 1) * m1].iter_mut().zip(e.iter_mut()) {
let (p, s) = g.apply(*x, *y);
*x = p;
*y = s;
}
}
self.q = q;
debug_assert_eq!(self.check_shape(), Ok(()), "(Q2) after insert_row");
Ok(())
}
pub fn delete_row(&mut self, i: usize) -> Result<(), StatlaError> {
let (m, n) = (self.nrows(), self.ncols());
if m <= n {
return Err(StatlaError::InvalidArgument {
what: "Qr::delete_row: requires m > n",
});
}
if i >= m {
return Err(StatlaError::InvalidArgument {
what: "Qr::delete_row: row index >= m",
});
}
let mut z = vec![T::ZERO; m];
z[i] = T::ONE;
let mut h = vec![T::ZERO; n];
let (nu, _) = orthogonalize(&self.q, &mut z, &mut h, T::from_f64(self.opts.reorth_eta));
if le_exact(nu, self.opts.tau::<T>(n)) {
return Err(StatlaError::RankDeficient { column: i });
}
let n1 = n + 1;
let mut mq = DenseBlock::zeros(m, n1);
mq.data[..m * n].copy_from_slice(&self.q.data);
for (d, &s) in mq.data[m * n..].iter_mut().zip(&z) {
*d = s / nu;
}
let mut rh = DenseBlock::zeros(n1, n);
for j in 0..n {
rh.data[j * n1..j * n1 + n].copy_from_slice(&self.r.data[j * n..(j + 1) * n]);
}
let mut w: Vec<T> = (0..n1).map(|k| mq.data[i + k * m]).collect();
for k in (0..n).rev() {
let (g, rho) = givens(w[k], w[k + 1]);
w[k] = rho;
w[k + 1] = T::ZERO;
g.rotate_rows(&mut rh, k, k + 1, k);
g.rotate_cols(&mut mq.data, m, k, k + 1);
}
let mut r = DenseBlock::zeros(n, n);
for j in 0..n {
for p in 0..=j {
r.data[p + j * n] = rh.data[p + 1 + j * n1];
}
}
let m0 = m - 1;
let mut q = DenseBlock::zeros(m0, n);
for j in 0..n {
let src = &mq.data[(j + 1) * m..(j + 2) * m];
let dst = &mut q.data[j * m0..(j + 1) * m0];
dst[..i].copy_from_slice(&src[..i]);
dst[i..].copy_from_slice(&src[i + 1..]);
}
fix_signs(&mut q, &mut r, 0);
self.q = q;
self.r = r;
debug_assert_eq!(self.check_shape(), Ok(()), "(Q2) after delete_row");
Ok(())
}
pub fn insert_col(&mut self, j: usize, c: &[T]) -> Result<(), StatlaError> {
let (m, n) = (self.nrows(), self.ncols());
check_len("c", m, c.len())?;
if m <= n {
return Err(StatlaError::InvalidArgument {
what: "Qr::insert_col: requires m > n",
});
}
if j > n {
return Err(StatlaError::InvalidArgument {
what: "Qr::insert_col: column index > n",
});
}
check_finite("c", c)?;
let cn = norm2(c);
let mut z = c.to_vec();
let mut h = vec![T::ZERO; n];
let (rho, _) = orthogonalize(&self.q, &mut z, &mut h, T::from_f64(self.opts.reorth_eta));
if le_exact(rho, self.opts.tau::<T>(n + 1) * cn) {
return Err(StatlaError::RankDeficient { column: j });
}
let n1 = n + 1;
let mut mq = DenseBlock::zeros(m, n1);
mq.data[..m * n].copy_from_slice(&self.q.data);
for (d, &s) in mq.data[m * n..].iter_mut().zip(&z) {
*d = s / rho;
}
let mut rh = DenseBlock::zeros(n1, n1);
for p in 0..n1 {
let dst = &mut rh.data[p * n1..p * n1 + n];
match p.cmp(&j) {
core::cmp::Ordering::Less => {
dst.copy_from_slice(&self.r.data[p * n..(p + 1) * n]);
}
core::cmp::Ordering::Equal => {
dst.copy_from_slice(&h);
rh.data[n + p * n1] = rho;
}
core::cmp::Ordering::Greater => {
dst.copy_from_slice(&self.r.data[(p - 1) * n..p * n]);
}
}
}
for k in (j + 1..=n).rev() {
let (g, top) = givens(rh.data[k - 1 + j * n1], rh.data[k + j * n1]);
rh.data[k - 1 + j * n1] = top;
rh.data[k + j * n1] = T::ZERO;
g.rotate_rows(&mut rh, k - 1, k, j + 1);
g.rotate_cols(&mut mq.data, m, k - 1, k);
}
fix_signs(&mut mq, &mut rh, j);
self.q = mq;
self.r = rh;
debug_assert_eq!(self.check_shape(), Ok(()), "(Q2) after insert_col");
Ok(())
}
pub fn delete_col(&mut self, j: usize) -> Result<(), StatlaError> {
let (m, n) = (self.nrows(), self.ncols());
if j >= n {
return Err(StatlaError::InvalidArgument {
what: "Qr::delete_col: column index >= n",
});
}
let mut rh = DenseBlock::zeros(n, n - 1);
for p in 0..n - 1 {
let src = if p < j { p } else { p + 1 };
rh.data[p * n..(p + 1) * n].copy_from_slice(&self.r.data[src * n..(src + 1) * n]);
}
for p in j..n - 1 {
let (g, top) = givens(rh.data[p + p * n], rh.data[p + 1 + p * n]);
rh.data[p + p * n] = top;
rh.data[p + 1 + p * n] = T::ZERO;
g.rotate_rows(&mut rh, p, p + 1, p + 1);
g.rotate_cols(&mut self.q.data, m, p, p + 1);
}
let r = drop_last_row(&rh);
self.q.data.truncate(m * (n - 1));
self.q.cols = n - 1;
self.r = r;
fix_signs(&mut self.q, &mut self.r, j);
debug_assert_eq!(self.check_shape(), Ok(()), "(Q2) after delete_col");
Ok(())
}
pub fn rank1_update(&mut self, u: &[T], v: &[T]) -> Result<(), StatlaError> {
let (m, n) = (self.nrows(), self.ncols());
check_len("u", m, u.len())?;
check_len("v", n, v.len())?;
check_finite("u", u)?;
check_finite("v", v)?;
let tau = self.opts.tau::<T>(n);
let un = norm2(u);
let mut z = u.to_vec();
let mut w = vec![T::ZERO; n];
let (mut rho, _) =
orthogonalize(&self.q, &mut z, &mut w, T::from_f64(self.opts.reorth_eta));
if le_exact(rho, tau * un) {
rho = T::ZERO;
}
let n1 = n + 1;
let mut mq = DenseBlock::zeros(m, n1);
mq.data[..m * n].copy_from_slice(&self.q.data);
if !is_exact_zero(rho) {
for (d, &s) in mq.data[m * n..].iter_mut().zip(&z) {
*d = s / rho;
}
}
let mut rh = DenseBlock::zeros(n1, n);
for j in 0..n {
rh.data[j * n1..j * n1 + n].copy_from_slice(&self.r.data[j * n..(j + 1) * n]);
}
let mut wh = w;
wh.push(rho);
for k in (0..n).rev() {
let (g, top) = givens(wh[k], wh[k + 1]);
wh[k] = top;
wh[k + 1] = T::ZERO;
g.rotate_rows(&mut rh, k, k + 1, k);
g.rotate_cols(&mut mq.data, m, k, k + 1);
}
let t = wh.first().copied().unwrap_or(T::ZERO);
for (j, &vj) in v.iter().enumerate() {
rh.data[j * n1] += t * vj;
}
for k in 0..n {
let (g, top) = givens(rh.data[k + k * n1], rh.data[k + 1 + k * n1]);
rh.data[k + k * n1] = top;
rh.data[k + 1 + k * n1] = T::ZERO;
g.rotate_rows(&mut rh, k, k + 1, k + 1);
g.rotate_cols(&mut mq.data, m, k, k + 1);
}
let mut r = drop_last_row(&rh);
mq.data.truncate(m * n);
mq.cols = n;
fix_signs(&mut mq, &mut r, 0);
for k in 0..n {
let cn = norm2(&self.r.data[(k * n)..=(k * n + k)]) + v[k].abs() * un;
if le_exact(r.data[k + k * n], tau * cn) {
return Err(StatlaError::RankDeficient { column: k });
}
}
self.q = mq;
self.r = r;
debug_assert_eq!(self.check_shape(), Ok(()), "(Q2) after rank1_update");
Ok(())
}
pub fn solve_least_squares(&self, b: &[T], x: &mut [T]) -> Result<T, StatlaError> {
let (m, n) = (self.nrows(), self.ncols());
check_len("b", m, b.len())?;
check_len("x", n, x.len())?;
let mut res = b.to_vec();
for j in 0..n {
let qj = &self.q.data[j * m..(j + 1) * m];
x[j] = dot(qj, b);
}
for j in 0..n {
axpy_sub(x[j], &self.q.data[j * m..(j + 1) * m], &mut res);
}
for j in (0..n).rev() {
let col = &self.r.data[(j * n)..=(j * n + j)];
let xj = x[j] / col[j];
x[j] = xj;
axpy_sub(xj, &col[..j], &mut x[..j]);
}
Ok(norm2(&res))
}
pub fn to_cholesky(&self) -> Result<Cholesky<T, K>, StatlaError>
where
K: Clone,
{
Cholesky::from_upper(self.r.clone(), self.kernel.clone(), &CholOptions::default())
}
fn check_shape(&self) -> Result<(), &'static str> {
let (m, n) = (self.q.rows, self.r.cols);
if self.r.rows != n
|| self.q.cols != n
|| self.q.data.len() != m * n
|| self.r.data.len() != n * n
{
return Err("(Q2) Q and R are not consistent blocks");
}
if m < n {
return Err("(Q2) fewer rows than columns");
}
if self.q.data.iter().any(|v| !v.is_finite()) {
return Err("(Q2) Q has a non-finite entry");
}
for j in 0..n {
for i in 0..n {
let v = self.r.data[i + j * n];
if !v.is_finite() {
return Err("(Q2) R has a non-finite entry");
}
if i > j && !is_exact_zero(v) {
return Err("(Q2) R is not upper triangular");
}
if i == j && !lt(T::ZERO, v) {
return Err("(Q2) R has a nonpositive diagonal entry");
}
}
}
Ok(())
}
pub fn check_invariants(&self, c_q: f64) -> Result<(), &'static str> {
self.check_shape()?;
let (m, n) = (self.q.rows, self.q.cols);
let mut s = T::ZERO;
for j in 0..n {
let qj = &self.q.data[j * m..(j + 1) * m];
for i in 0..n {
let mut d = dot(&self.q.data[i * m..(i + 1) * m], qj);
if i == j {
d -= T::ONE;
}
s += d * d;
}
}
#[allow(clippy::cast_precision_loss)] let bound = T::from_f64(c_q * m.max(1) as f64) * T::EPSILON * T::from_f64(0.5);
if !s.is_finite() || !le_exact(s.sqrt(), bound) {
return Err("(Q1) Q has lost orthogonality");
}
Ok(())
}
}