1use xingo_sparse::{DenseBlock, Real};
76
77use crate::StatlaError;
78use crate::real::{is_exact_zero, le_exact, lt};
79
80const BIG: f64 = 3.273_390_607_896_142e150;
84const SMALL: f64 = 3.054_936_363_499_605e-151;
85
86fn pow2_floor(mu: f64) -> f64 {
89 let bits = mu.to_bits() & 0x7ff0_0000_0000_0000;
90 if bits == 0 || bits == 0x7ff0_0000_0000_0000 {
91 1.0
92 } else {
93 f64::from_bits(bits)
94 }
95}
96
97pub(crate) fn norm2<T: Real>(x: &[T]) -> T {
99 let mut mu = T::ZERO;
101 for &v in x {
102 mu = mu.max(v.abs());
103 }
104 if is_exact_zero(mu) {
105 return T::ZERO;
106 }
107 let m = mu.to_f64();
108 if le_exact(SMALL, m) && le_exact(m, BIG) {
110 return x.iter().map(|&v| v * v).sum::<T>().sqrt();
111 }
112 let sigma = pow2_floor(m);
114 let inv = T::from_f64(1.0 / sigma);
115 let s = T::from_f64(sigma);
116 s * x
117 .iter()
118 .map(|&v| {
119 let w = v * inv;
120 w * w
121 })
122 .sum::<T>()
123 .sqrt()
124}
125
126#[inline]
128pub(crate) fn dot<T: Real>(x: &[T], y: &[T]) -> T {
129 x.iter().zip(y).map(|(&a, &b)| a * b).sum()
130}
131
132#[inline]
134pub(crate) fn axpy_sub<T: Real>(alpha: T, x: &[T], y: &mut [T]) {
135 for (yi, &xi) in y.iter_mut().zip(x) {
136 *yi -= alpha * xi;
137 }
138}
139
140pub(crate) fn check_finite<T: Real>(what: &'static str, x: &[T]) -> Result<(), StatlaError> {
142 match x.iter().position(|v| !v.is_finite()) {
143 Some(index) => Err(StatlaError::NonFinite { what, index }),
144 None => Ok(()),
145 }
146}
147
148pub(crate) fn check_len(
150 what: &'static str,
151 expected: usize,
152 found: usize,
153) -> Result<(), StatlaError> {
154 if expected == found {
155 Ok(())
156 } else {
157 Err(StatlaError::Dimension {
158 what,
159 expected,
160 found,
161 })
162 }
163}
164
165#[derive(Clone, Copy, Debug)]
167pub(crate) struct Rot<T> {
168 pub(crate) c: T,
169 pub(crate) s: T,
170}
171
172impl<T: Real> Rot<T> {
173 pub(crate) const IDENTITY: Self = Self {
175 c: T::ONE,
176 s: T::ZERO,
177 };
178
179 #[inline]
181 pub(crate) fn apply(self, x: T, y: T) -> (T, T) {
182 (self.c * x + self.s * y, self.c * y - self.s * x)
183 }
184
185 pub(crate) fn rotate_cols(self, data: &mut [T], rows: usize, j1: usize, j2: usize) {
190 debug_assert!(j1 < j2);
191 let (left, right) = data.split_at_mut(j2 * rows);
192 let a = &mut left[j1 * rows..(j1 + 1) * rows];
193 let b = &mut right[..rows];
194 for (x, y) in a.iter_mut().zip(b.iter_mut()) {
195 let (nx, ny) = self.apply(*x, *y);
196 *x = nx;
197 *y = ny;
198 }
199 }
200
201 pub(crate) fn rotate_rows(self, r: &mut DenseBlock<T>, i1: usize, i2: usize, from: usize) {
203 let rows = r.rows;
204 for j in from..r.cols {
205 let (nx, ny) = self.apply(r.data[i1 + j * rows], r.data[i2 + j * rows]);
206 r.data[i1 + j * rows] = nx;
207 r.data[i2 + j * rows] = ny;
208 }
209 }
210}
211
212pub(crate) fn givens<T: Real>(a: T, b: T) -> (Rot<T>, T) {
214 if is_exact_zero(b) {
216 return if lt(a, T::ZERO) {
217 (
218 Rot {
219 c: -T::ONE,
220 s: T::ZERO,
221 },
222 -a,
223 )
224 } else {
225 (Rot::IDENTITY, a)
226 };
227 }
228 let mu = a.abs() + b.abs();
230 let (aa, bb) = (a / mu, b / mu);
231 let r = mu * (aa * aa + bb * bb).sqrt();
232 let g = Rot { c: a / r, s: b / r };
233 debug_assert!(
234 !a.is_finite() || !b.is_finite() || (r.is_finite() && le_exact(T::ZERO, r)),
235 "(S1) Givens: r must be finite and nonnegative"
236 );
237 (g, r)
238}
239
240pub(crate) fn householder<T: Real>(x: &mut [T]) -> T {
243 debug_assert_ne!(x.len(), 0, "Algorithm H needs p >= 1");
244 let sigma = norm2(x);
246 if is_exact_zero(sigma) {
247 return T::ZERO;
248 }
249 let x0 = x[0];
251 let alpha = if lt(x0, T::ZERO) { sigma } else { -sigma };
252 let d = x0 - alpha;
254 for v in &mut x[1..] {
255 *v = *v / d;
256 }
257 x[0] = alpha;
258 (alpha - x0) / alpha
259}
260
261pub(crate) fn apply_householder<T: Real>(tau: T, v_tail: &[T], y: &mut [T]) {
264 if is_exact_zero(tau) {
265 return;
266 }
267 let w = tau * (y[0] + dot(v_tail, &y[1..]));
268 y[0] -= w;
269 axpy_sub(w, v_tail, &mut y[1..]);
270}
271
272pub(crate) fn orthogonalize<T: Real>(
276 q: &DenseBlock<T>,
277 x: &mut [T],
278 h: &mut [T],
279 eta: T,
280) -> (T, u8) {
281 debug_assert_eq!(q.rows, x.len());
282 debug_assert_eq!(q.cols, h.len());
283 let m = q.rows;
284 let nu0 = norm2(x);
286 let mut s = vec![T::ZERO; q.cols];
287 let mut pass = |x: &mut [T], h: &mut [T]| {
288 for (j, sj) in s.iter_mut().enumerate() {
290 *sj = dot(&q.data[j * m..(j + 1) * m], x);
291 }
292 for (j, &sj) in s.iter().enumerate() {
293 axpy_sub(sj, &q.data[j * m..(j + 1) * m], x);
294 h[j] += sj;
295 }
296 norm2(x)
297 };
298 let mut nu = pass(x, h);
299 let mut passes = 1;
300 if lt(nu, eta * nu0) {
302 nu = pass(x, h);
303 passes = 2;
304 }
305 (nu, passes)
307}
308
309pub(crate) fn householder_qr<T: Real>(mut w: DenseBlock<T>) -> (DenseBlock<T>, DenseBlock<T>) {
311 let (m, l) = (w.rows, w.cols);
312 debug_assert!(m >= l, "Algorithm Q needs m >= l");
313 let mut taus = vec![T::ZERO; l];
314 for k in 0..l {
315 let (left, right) = w.data.split_at_mut((k + 1) * m);
316 let x = &mut left[k * m + k..];
317 taus[k] = householder(x);
318 let v = &x[1..];
319 for j in 0..l - k - 1 {
320 apply_householder(taus[k], v, &mut right[j * m + k..(j + 1) * m]);
321 }
322 }
323 let mut r = DenseBlock::zeros(l, l);
324 for j in 0..l {
325 for i in 0..=j {
326 r.data[i + j * l] = w.data[i + j * m];
327 }
328 }
329 let mut q = DenseBlock::zeros(m, l);
330 for k in 0..l {
331 q.data[k + k * m] = T::ONE;
332 }
333 for k in (0..l).rev() {
334 let v = &w.data[k * m + k + 1..(k + 1) * m];
335 for j in k..l {
336 apply_householder(taus[k], v, &mut q.data[j * m + k..(j + 1) * m]);
337 }
338 }
339 for k in 0..l {
340 if lt(r.data[k + k * l], T::ZERO) {
341 for j in k..l {
342 r.data[k + j * l] = -r.data[k + j * l];
343 }
344 for v in &mut q.data[k * m..(k + 1) * m] {
345 *v = -*v;
346 }
347 }
348 }
349 (q, r)
350}
351
352pub(crate) fn orthogonalize_against<T: Real>(
357 data: &[T],
358 rows: usize,
359 ncols: usize,
360 x: &mut [T],
361 eta: T,
362) -> (T, u8) {
363 orthogonalize_prefix(data, rows, ncols, x, eta, None)
364}
365
366pub(crate) fn orthogonalize_prefix<T: Real>(
370 data: &[T],
371 rows: usize,
372 ncols: usize,
373 x: &mut [T],
374 eta: T,
375 mut h: Option<&mut [T]>,
376) -> (T, u8) {
377 debug_assert_eq!(rows, x.len());
378 let nu0 = norm2(x);
379 let mut s = vec![T::ZERO; ncols];
380 let mut pass = |x: &mut [T], h: &mut Option<&mut [T]>| {
381 for (j, sj) in s.iter_mut().enumerate() {
382 *sj = dot(&data[j * rows..(j + 1) * rows], x);
383 }
384 for (j, &sj) in s.iter().enumerate() {
385 axpy_sub(sj, &data[j * rows..(j + 1) * rows], x);
386 }
387 if let Some(h) = h.as_deref_mut() {
388 for (hj, &sj) in h.iter_mut().zip(&s) {
389 *hj += sj;
390 }
391 }
392 norm2(x)
393 };
394 let mut nu = pass(x, &mut h);
395 let mut passes = 1;
396 if lt(nu, eta * nu0) {
397 nu = pass(x, &mut h);
398 passes = 2;
399 }
400 (nu, passes)
401}
402
403#[cfg(test)]
404#[allow(clippy::float_cmp)] mod tests {
406 use super::*;
407
408 #[test]
409 fn norm_scales_without_changing_rounding() {
410 let x = [3.0, 4.0];
411 assert_eq!(norm2(&x), 5.0);
412 let big = [3e300, 4e300];
413 assert!((norm2(&big) / 5e300 - 1.0).abs() < 4.0 * f64::EPSILON);
414 let tiny = [3e-300, 4e-300];
415 assert!((norm2(&tiny) / 5e-300 - 1.0).abs() < 4.0 * f64::EPSILON);
416 let y = [0.1_f64, 0.7, -2.3];
418 let k = 2f64.powi(600);
419 let scaled: Vec<f64> = y.iter().map(|v| v * k).collect();
420 assert_eq!(norm2(&scaled), norm2(&y) * k);
421 assert_eq!(norm2::<f64>(&[]), 0.0);
422 assert_eq!(norm2(&[0.0, -0.0]), 0.0);
423 }
424
425 #[test]
426 fn givens_annihilates_and_keeps_r_nonnegative() {
427 for &(a, b) in &[
428 (3.0, 4.0),
429 (-3.0, 4.0),
430 (3.0, -4.0),
431 (0.0, -2.0),
432 (-5.0, 0.0),
433 (0.0, 0.0),
434 (1e300, 1e300),
435 ] {
436 let (g, r) = givens(a, b);
437 let (x, y) = g.apply(a, b);
438 assert!(r >= 0.0);
439 assert!((x - r).abs() <= 4.0 * f64::EPSILON * r.max(1.0), "{a} {b}");
440 assert!(y.abs() <= 4.0 * f64::EPSILON * r.max(1.0), "{a} {b}");
441 assert!((g.c * g.c + g.s * g.s - 1.0).abs() <= 4.0 * f64::EPSILON);
442 }
443 }
444
445 #[test]
446 fn householder_maps_to_a_multiple_of_e0() {
447 let x0 = [1.0, -2.0, 2.0, 0.5];
448 for sign in [1.0, -1.0] {
449 let mut x: Vec<f64> = x0.iter().map(|v| v * sign).collect();
450 let tau = householder(&mut x);
451 let mut y: Vec<f64> = x0.iter().map(|v| v * sign).collect();
452 apply_householder(tau, &x[1..], &mut y);
453 assert!((y[0] - x[0]).abs() < 1e-15 && (x[0].abs() - norm2(&x0)).abs() < 1e-15);
454 assert!(y[1..].iter().all(|v| v.abs() < 1e-15));
455 }
456 let mut z = [0.0, 0.0];
457 assert_eq!(householder(&mut z), 0.0);
458 }
459
460 #[test]
461 fn dgks_reorthogonalizes_a_nearly_dependent_vector() {
462 let q = DenseBlock {
464 rows: 3,
465 cols: 2,
466 data: vec![1.0, 0.0, 0.0, 0.0, 1.0, 0.0],
467 };
468 let mut x = [1.0, 1.0, 1e-9];
469 let mut h = [0.0; 2];
470 let (nu, passes) = orthogonalize(&q, &mut x, &mut h, std::f64::consts::FRAC_1_SQRT_2);
471 assert_eq!(passes, 2);
472 assert_eq!((nu, h), (1e-9, [1.0, 1.0]));
473 let mut x = [0.0, 0.0, 1.0];
474 let (_, passes) = orthogonalize(&q, &mut x, &mut [0.0; 2], std::f64::consts::FRAC_1_SQRT_2);
475 assert_eq!(passes, 1);
476 assert_eq!(
477 check_finite("x", &[1.0, f64::NAN]),
478 Err(StatlaError::NonFinite {
479 what: "x",
480 index: 1
481 })
482 );
483 assert!(check_len("x", 2, 3).is_err() && check_len("x", 2, 2).is_ok());
484 let mut y = [1.0, 1.0, 1e-9];
486 let (nu2, p2) =
487 orthogonalize_against(&q.data, 3, 2, &mut y, std::f64::consts::FRAC_1_SQRT_2);
488 assert_eq!((nu2, p2, y), (1e-9, 2, [0.0, 0.0, 1e-9]));
489 }
490
491 #[test]
492 fn householder_qr_accepts_rank_deficiency() {
493 let a = DenseBlock {
495 rows: 4,
496 cols: 3,
497 data: vec![1.0, 2.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0, 2.0, 0.0, 1.0],
498 };
499 let (q, r) = householder_qr(a.clone());
500 for i in 0..3 {
501 for j in 0..3 {
502 let d = dot(&q.data[i * 4..i * 4 + 4], &q.data[j * 4..j * 4 + 4]);
503 let e = if i == j { 1.0 } else { 0.0 };
504 assert!((d - e).abs() < 1e-15);
505 }
506 assert!(r.get(i, i) >= 0.0);
507 }
508 for j in 0..3 {
509 for i in 0..4 {
510 let qr: f64 = (0..3).map(|k| q.get(i, k) * r.get(k, j)).sum();
511 assert!((qr - a.get(i, j)).abs() < 1e-15);
512 }
513 }
514 assert!(r.get(2, 2) < 1e-15);
515 }
516}