1use xingo_sparse::{DenseBlock, Real};
145
146use crate::StatlaError;
147use crate::real::{is_exact_zero, le_exact, lt};
148use crate::small::{apply_householder, givens, householder};
149
150#[derive(Clone, Copy, Debug)]
152pub(crate) struct SchurOptions<T> {
153 pub(crate) jacobi_tol: T,
155 pub(crate) max_sweeps: usize,
157 pub(crate) qr_iters_per_row: usize,
159 pub(crate) swap_factor: T,
161}
162
163impl<T: Real> SchurOptions<T> {
164 pub(crate) fn new() -> Self {
166 Self {
167 jacobi_tol: T::EPSILON,
168 max_sweeps: 60,
169 qr_iters_per_row: 30,
170 swap_factor: T::from_f64(10.0),
171 }
172 }
173}
174
175const BIG: f64 = 3.273_390_607_896_142e150;
178
179fn pow2_floor(mu: f64) -> f64 {
181 let bits = mu.to_bits() & 0x7ff0_0000_0000_0000;
182 if bits == 0 || bits == 0x7ff0_0000_0000_0000 {
183 1.0
184 } else {
185 f64::from_bits(bits)
186 }
187}
188
189#[inline]
191fn sign1<T: Real>(x: T) -> T {
192 if lt(x, T::ZERO) { -T::ONE } else { T::ONE }
193}
194
195#[inline]
197pub(crate) fn hypot<T: Real>(x: T, y: T) -> T {
198 let (ax, ay) = (x.abs(), y.abs());
199 let m = ax.max(ay);
200 if is_exact_zero(m) {
201 return T::ZERO;
202 }
203 let (a, b) = (ax / m, ay / m);
204 m * (a * a + b * b).sqrt()
205}
206
207fn rot_cols<T: Real>(
210 data: &mut [T],
211 rows: usize,
212 i: usize,
213 j: usize,
214 r: (usize, usize),
215 c: T,
216 s: T,
217) {
218 for k in r.0..r.1 {
219 let (x, y) = (data[k + i * rows], data[k + j * rows]);
220 data[k + i * rows] = c * x + s * y;
221 data[k + j * rows] = c * y - s * x;
222 }
223}
224
225fn rot_rows<T: Real>(
227 data: &mut [T],
228 rows: usize,
229 i: usize,
230 j: usize,
231 cr: (usize, usize),
232 c: T,
233 s: T,
234) {
235 for k in cr.0..cr.1 {
236 let (x, y) = (data[i + k * rows], data[j + k * rows]);
237 data[i + k * rows] = c * x + s * y;
238 data[j + k * rows] = c * y - s * x;
239 }
240}
241
242pub(crate) fn identity<T: Real>(p: usize) -> DenseBlock<T> {
244 let mut z = DenseBlock::zeros(p, p);
245 for i in 0..p {
246 z.data[i + i * p] = T::ONE;
247 }
248 z
249}
250
251fn max_abs<T: Real>(data: &[T]) -> T {
253 data.iter().fold(T::ZERO, |m, &v| m.max(v.abs()))
254}
255
256pub(crate) fn sym_eig<T: Real>(
260 a: &DenseBlock<T>,
261 o: &SchurOptions<T>,
262) -> Result<(Vec<T>, DenseBlock<T>), StatlaError> {
263 let p = a.rows;
264 debug_assert_eq!(p, a.cols);
265 let mut z = identity::<T>(p);
266 let mu = max_abs(&a.data);
268 if is_exact_zero(mu) {
269 return Ok((vec![T::ZERO; p], z));
270 }
271 let scale = pow2_floor(mu.to_f64());
272 let inv = T::from_f64(1.0 / scale);
273 let half = T::from_f64(0.5);
274 let mut w = DenseBlock::<T>::zeros(p, p);
275 for j in 0..p {
276 for i in 0..p {
277 w.data[i + j * p] = (a.data[i + j * p] + a.data[j + i * p]) * half * inv;
278 }
279 }
280 let (one, two) = (T::ONE, T::from_f64(2.0));
281 let tol = o.jacobi_tol;
282 let floor = tol * tol;
283 let mut sweeps = 0;
285 loop {
286 if sweeps == o.max_sweeps {
287 return Err(StatlaError::NoConvergence {
288 requested: p,
289 converged: 0,
290 restarts: sweeps,
291 });
292 }
293 sweeps += 1;
294 let mut rotated = false;
295 for pp in 0..p {
296 for q in pp + 1..p {
297 let apq = w.data[pp + q * p];
298 let (app, aqq) = (w.data[pp + pp * p], w.data[q + q * p]);
299 let rel = tol * app.abs().sqrt() * aqq.abs().sqrt();
300 if !lt(rel, apq.abs()) || !lt(floor, apq.abs()) {
301 continue;
302 }
303 rotated = true;
304 let theta = (aqq - app) / (two * apq);
305 let at = theta.abs();
306 let mag = if le_exact(one, at) {
307 let r = one / theta;
308 at * (one + (one + r * r).sqrt())
309 } else {
310 at + (one + theta * theta).sqrt()
311 };
312 let t = if lt(theta, T::ZERO) {
313 -(one / mag)
314 } else {
315 one / mag
316 };
317 let c = one / (one + t * t).sqrt();
318 let s = t * c;
319 rot_cols(&mut w.data, p, pp, q, (0, p), c, -s);
321 rot_rows(&mut w.data, p, pp, q, (0, p), c, -s);
322 rot_cols(&mut z.data, p, pp, q, (0, p), c, -s);
323 w.data[pp + pp * p] = app - t * apq;
324 w.data[q + q * p] = aqq + t * apq;
325 w.data[pp + q * p] = T::ZERO;
326 w.data[q + pp * p] = T::ZERO;
327 }
328 }
329 if !rotated {
330 break;
331 }
332 }
333 let sc = T::from_f64(scale);
335 let d: Vec<T> = (0..p).map(|i| w.data[i + i * p] * sc).collect();
336 for j in 0..p {
337 let col = &mut z.data[j * p..(j + 1) * p];
338 let nz = crate::small::norm2(col);
339 for v in col.iter_mut() {
340 *v = *v / nz;
341 }
342 }
343 Ok((d, z))
344}
345
346pub(crate) fn hessenberg<T: Real>(h: &mut DenseBlock<T>, z: &mut DenseBlock<T>) {
348 let p = h.rows;
349 let zr = z.rows;
350 if p < 3 {
351 return;
352 }
353 let mut row = vec![T::ZERO; p];
354 for k in 0..p - 2 {
355 let len = p - k - 1;
356 let mut x: Vec<T> = h.data[(k + 1) + k * p..p + k * p].to_vec();
357 let tau = householder(&mut x);
358 h.data[(k + 1) + k * p] = x[0];
359 for i in k + 2..p {
360 h.data[i + k * p] = T::ZERO;
361 }
362 if is_exact_zero(tau) {
363 continue;
364 }
365 let v = &x[1..];
366 for j in k + 1..p {
368 apply_householder(tau, v, &mut h.data[(k + 1) + j * p..p + j * p]);
369 }
370 for i in 0..p {
372 for (t, r) in row[..len].iter_mut().enumerate() {
373 *r = h.data[i + (k + 1 + t) * p];
374 }
375 apply_householder(tau, v, &mut row[..len]);
376 for (t, &r) in row[..len].iter().enumerate() {
377 h.data[i + (k + 1 + t) * p] = r;
378 }
379 }
380 let mut zrow = vec![T::ZERO; len];
381 for i in 0..zr {
382 for (t, r) in zrow.iter_mut().enumerate() {
383 *r = z.data[i + (k + 1 + t) * zr];
384 }
385 apply_householder(tau, v, &mut zrow);
386 for (t, &r) in zrow.iter().enumerate() {
387 z.data[i + (k + 1 + t) * zr] = r;
388 }
389 }
390 }
391}
392
393#[allow(clippy::many_single_char_names)]
396pub(crate) fn lanv2<T: Real>(a: T, b: T, c: T, d: T) -> ([T; 4], T, T) {
397 let (mut a, mut b, mut c, mut d) = (a, b, c, d);
398 let (zero, one, half) = (T::ZERO, T::ONE, T::from_f64(0.5));
399 let four_eps = T::from_f64(4.0) * T::EPSILON;
400 let (mut cs, mut sn);
401 if is_exact_zero(c) {
402 cs = one;
403 sn = zero;
404 } else if is_exact_zero(b) {
405 cs = zero;
406 sn = one;
407 core::mem::swap(&mut a, &mut d);
408 b = -c;
409 c = zero;
410 } else if is_exact_zero(a - d) && !is_exact_zero(sign1(b) - sign1(c)) {
411 cs = one;
412 sn = zero;
413 } else {
414 let temp = a - d;
415 let p = half * temp;
416 let bcmax = b.abs().max(c.abs());
417 let bcmis = b.abs().min(c.abs()) * sign1(b) * sign1(c);
418 let scale = p.abs().max(bcmax);
419 let z = (p / scale) * p + (bcmax / scale) * bcmis;
420 if le_exact(four_eps, z) {
421 let root = scale.sqrt() * z.sqrt();
423 let z = p + if lt(p, zero) { -root } else { root };
424 a = d + z;
425 d -= (bcmax / z) * bcmis;
426 let tau = hypot(c, z);
427 cs = z / tau;
428 sn = c / tau;
429 b -= c;
430 c = zero;
431 } else {
432 let sigma = b + c;
434 let tau = hypot(sigma, temp);
435 cs = (half * (one + sigma.abs() / tau)).sqrt();
436 sn = -(p / (tau * cs)) * sign1(sigma);
437 let aa = a * cs + b * sn;
438 let bb = -a * sn + b * cs;
439 let cc = c * cs + d * sn;
440 let dd = -c * sn + d * cs;
441 a = aa * cs + cc * sn;
442 b = bb * cs + dd * sn;
443 c = -aa * sn + cc * cs;
444 d = -bb * sn + dd * cs;
445 let temp = half * (a + d);
446 a = temp;
447 d = temp;
448 if !is_exact_zero(c) {
449 if is_exact_zero(b) {
450 b = -c;
451 c = zero;
452 let t = cs;
453 cs = -sn;
454 sn = t;
455 } else if is_exact_zero(sign1(b) - sign1(c)) {
456 let sab = b.abs().sqrt();
458 let sac = c.abs().sqrt();
459 let pp = if lt(c, zero) { -(sab * sac) } else { sab * sac };
460 let tau = one / (b + c).abs().sqrt();
461 a = temp + pp;
462 d = temp - pp;
463 b -= c;
464 c = zero;
465 let cs1 = sab * tau;
466 let sn1 = sac * tau;
467 let t = cs * cs1 - sn * sn1;
468 sn = cs * sn1 + sn * cs1;
469 cs = t;
470 }
471 }
472 }
473 }
474 ([a, b, c, d], cs, sn)
475}
476
477fn standardize<T: Real>(t: &mut DenseBlock<T>, z: &mut DenseBlock<T>, k: usize) {
480 let p = t.rows;
481 let at = |i: usize, j: usize| i + j * p;
482 let (blk, cs, sn) = lanv2(
483 t.data[at(k, k)],
484 t.data[at(k, k + 1)],
485 t.data[at(k + 1, k)],
486 t.data[at(k + 1, k + 1)],
487 );
488 t.data[at(k, k)] = blk[0];
489 t.data[at(k, k + 1)] = blk[1];
490 t.data[at(k + 1, k)] = blk[2];
491 t.data[at(k + 1, k + 1)] = blk[3];
492 rot_rows(&mut t.data, p, k, k + 1, (k + 2, p), cs, sn);
493 rot_cols(&mut t.data, p, k, k + 1, (0, k), cs, sn);
494 let zr = z.rows;
495 rot_cols(&mut z.data, zr, k, k + 1, (0, zr), cs, sn);
496}
497
498fn small_reflector<T: Real>(x: &[T]) -> (T, [T; 2], T) {
501 let mut w = [T::ZERO; 3];
502 w[..x.len()].copy_from_slice(x);
503 let tau = householder(&mut w[..x.len()]);
504 let mut v = [T::ZERO; 2];
505 v[..x.len() - 1].copy_from_slice(&w[1..x.len()]);
506 (tau, v, w[0])
507}
508
509fn reflect_rows<T: Real>(
511 t: &mut DenseBlock<T>,
512 k: usize,
513 len: usize,
514 tau: T,
515 v: &[T],
516 cr: (usize, usize),
517) {
518 let p = t.rows;
519 let mut y = [T::ZERO; 3];
520 for j in cr.0..cr.1 {
521 for r in 0..len {
522 y[r] = t.data[k + r + j * p];
523 }
524 apply_householder(tau, &v[..len - 1], &mut y[..len]);
525 for r in 0..len {
526 t.data[k + r + j * p] = y[r];
527 }
528 }
529}
530
531fn reflect_cols<T: Real>(
533 t: &mut DenseBlock<T>,
534 k: usize,
535 len: usize,
536 tau: T,
537 v: &[T],
538 rr: (usize, usize),
539) {
540 let p = t.rows;
541 let mut y = [T::ZERO; 3];
542 for i in rr.0..rr.1 {
543 for r in 0..len {
544 y[r] = t.data[i + (k + r) * p];
545 }
546 apply_householder(tau, &v[..len - 1], &mut y[..len]);
547 for r in 0..len {
548 t.data[i + (k + r) * p] = y[r];
549 }
550 }
551}
552
553#[allow(clippy::too_many_lines)] pub(crate) fn real_schur<T: Real>(
557 h: &mut DenseBlock<T>,
558 z: &mut DenseBlock<T>,
559 o: &SchurOptions<T>,
560) -> Result<(), StatlaError> {
561 let p = h.rows;
562 hessenberg(h, z);
563 if p == 0 {
564 return Ok(());
565 }
566 let at = |i: usize, j: usize| i + j * p;
567 let mu = max_abs(&h.data);
568 let eps = T::EPSILON;
569 let budget = o.qr_iters_per_row * p.max(10);
570 let mut total = 0;
571 let mut its = 0;
572 let mut hi = p; while hi > 0 {
574 let i = hi - 1;
575 let mut l = 0;
577 for k in (1..=i).rev() {
578 let s = h.data[at(k - 1, k - 1)].abs() + h.data[at(k, k)].abs();
579 let s = if is_exact_zero(s) { mu } else { s };
580 if le_exact(h.data[at(k, k - 1)].abs(), eps * s) {
581 h.data[at(k, k - 1)] = T::ZERO;
582 l = k;
583 break;
584 }
585 }
586 if l == i {
587 hi -= 1;
589 its = 0;
590 continue;
591 }
592 if l + 1 == i {
593 standardize(h, z, i - 1);
595 hi -= 2;
596 its = 0;
597 continue;
598 }
599 total += 1;
601 its += 1;
602 if total > budget {
603 return Err(StatlaError::NoConvergence {
604 requested: p,
605 converged: p - hi,
606 restarts: total,
607 });
608 }
609 let (h11, h12, h21, h22) = if its == 10 || its == 20 {
610 let w = if its == 10 {
611 h.data[at(l + 1, l)].abs() + h.data[at(l + 2, l + 1)].abs()
612 } else {
613 h.data[at(i, i - 1)].abs() + h.data[at(i - 1, i - 2)].abs()
614 };
615 let base = if its == 10 {
616 h.data[at(l, l)]
617 } else {
618 h.data[at(i, i)]
619 };
620 let h11 = T::from_f64(0.75) * w + base;
621 (h11, T::from_f64(-0.4375) * w, w, h11)
622 } else {
623 (
624 h.data[at(i - 1, i - 1)],
625 h.data[at(i - 1, i)],
626 h.data[at(i, i - 1)],
627 h.data[at(i, i)],
628 )
629 };
630 let s = h11 + h22;
631 let t = h11 * h22 - h12 * h21;
632 let hll = h.data[at(l, l)];
634 let h10 = h.data[at(l + 1, l)];
635 let mut x = hll * hll + h.data[at(l, l + 1)] * h10 - s * hll + t;
636 let mut y = h10 * (hll + h.data[at(l + 1, l + 1)] - s);
637 let mut zz = h10 * h.data[at(l + 2, l + 1)];
638 let sc = x.abs() + y.abs() + zz.abs();
639 if !is_exact_zero(sc) {
640 x = x / sc;
641 y = y / sc;
642 zz = zz / sc;
643 }
644 for k in l..i {
645 let len = if k + 2 <= i { 3 } else { 2 };
646 let (tau, v, beta) = if len == 3 {
647 small_reflector(&[x, y, zz])
648 } else {
649 small_reflector(&[x, y])
650 };
651 if k > l {
652 h.data[at(k, k - 1)] = beta;
653 h.data[at(k + 1, k - 1)] = T::ZERO;
654 if len == 3 {
655 h.data[at(k + 2, k - 1)] = T::ZERO;
656 }
657 }
658 let from = if k > l { k } else { l };
660 reflect_rows(h, k, len, tau, &v, (from, p));
661 reflect_cols(h, k, len, tau, &v, (0, (k + len + 1).min(i + 1)));
662 let zr = z.rows;
663 let mut yv = [T::ZERO; 3];
664 for r in 0..zr {
665 for q in 0..len {
666 yv[q] = z.data[r + (k + q) * zr];
667 }
668 apply_householder(tau, &v[..len - 1], &mut yv[..len]);
669 for q in 0..len {
670 z.data[r + (k + q) * zr] = yv[q];
671 }
672 }
673 if k + 1 < i {
674 x = h.data[at(k + 1, k)];
675 y = h.data[at(k + 2, k)];
676 if k + 3 <= i {
677 zz = h.data[at(k + 3, k)];
678 }
679 }
680 }
681 }
682 debug_assert!(check_quasi_triangular(h), "(E2) real Schur form");
683 Ok(())
684}
685
686pub(crate) fn blocks<T: Real>(t: &DenseBlock<T>) -> Vec<(usize, usize)> {
688 let p = t.rows;
689 let mut out = Vec::new();
690 let mut i = 0;
691 while i < p {
692 if i + 1 < p && !is_exact_zero(t.data[(i + 1) + i * p]) {
693 out.push((i, 2));
694 i += 2;
695 } else {
696 out.push((i, 1));
697 i += 1;
698 }
699 }
700 out
701}
702
703pub(crate) fn block_value<T: Real>(t: &DenseBlock<T>, (q, s): (usize, usize)) -> (T, T) {
705 let p = t.rows;
706 if s == 1 {
707 (t.data[q + q * p], T::ZERO)
708 } else {
709 let b = t.data[q + (q + 1) * p];
710 let c = t.data[(q + 1) + q * p];
711 (t.data[q + q * p], b.abs().sqrt() * c.abs().sqrt())
712 }
713}
714
715pub(crate) fn check_quasi_triangular<T: Real>(t: &DenseBlock<T>) -> bool {
718 let p = t.rows;
719 for j in 0..p {
720 for i in j + 2..p {
721 if !is_exact_zero(t.data[i + j * p]) {
722 return false;
723 }
724 }
725 }
726 for (q, s) in blocks(t) {
727 if s == 2 {
728 let (a, b, c, d) = (
729 t.data[q + q * p],
730 t.data[q + (q + 1) * p],
731 t.data[(q + 1) + q * p],
732 t.data[(q + 1) + (q + 1) * p],
733 );
734 if !is_exact_zero(a - d) || !lt(b * c, T::ZERO) {
735 return false;
736 }
737 }
738 }
739 true
740}
741
742fn solve_small<T: Real>(m: &mut [[T; 4]; 4], rhs: &mut [T; 4], n: usize, smin: T) -> [T; 4] {
745 let mut perm_c = [0, 1, 2, 3];
746 for k in 0..n {
747 let (mut pi, mut pj, mut best) = (k, k, T::ZERO);
749 for i in k..n {
750 for j in k..n {
751 if lt(best, m[i][j].abs()) {
752 best = m[i][j].abs();
753 pi = i;
754 pj = j;
755 }
756 }
757 }
758 m.swap(k, pi);
759 rhs.swap(k, pi);
760 for row in m.iter_mut() {
761 row.swap(k, pj);
762 }
763 perm_c.swap(k, pj);
764 if lt(m[k][k].abs(), smin) {
765 m[k][k] = smin;
766 }
767 for i in k + 1..n {
768 let f = m[i][k] / m[k][k];
769 for j in k..n {
770 let v = m[k][j];
771 m[i][j] -= f * v;
772 }
773 let r = rhs[k];
774 rhs[i] -= f * r;
775 }
776 }
777 let mut y = [T::ZERO; 4];
778 for k in (0..n).rev() {
779 let mut s = rhs[k];
780 for j in k + 1..n {
781 s -= m[k][j] * y[j];
782 }
783 y[k] = s / m[k][k];
784 }
785 let mut x = [T::ZERO; 4];
786 for k in 0..n {
787 x[perm_c[k]] = y[k];
788 }
789 x
790}
791
792#[allow(clippy::too_many_lines)] pub(crate) fn swap_blocks<T: Real>(
796 t: &mut DenseBlock<T>,
797 z: &mut DenseBlock<T>,
798 j: usize,
799 n1: usize,
800 n2: usize,
801 o: &SchurOptions<T>,
802) -> bool {
803 let p = t.rows;
804 let zr = z.rows;
805 let at = |i: usize, c: usize| i + c * p;
806 let nd = n1 + n2;
807 if n1 == 1 && n2 == 1 {
808 let (t11, t22) = (t.data[at(j, j)], t.data[at(j + 1, j + 1)]);
809 let (g, _) = givens(t.data[at(j, j + 1)], t22 - t11);
810 rot_rows(&mut t.data, p, j, j + 1, (j + 2, p), g.c, g.s);
811 rot_cols(&mut t.data, p, j, j + 1, (0, j), g.c, g.s);
812 t.data[at(j, j)] = t22;
813 t.data[at(j + 1, j + 1)] = t11;
814 rot_cols(&mut z.data, zr, j, j + 1, (0, zr), g.c, g.s);
815 return true;
816 }
817 let mut d = [[T::ZERO; 4]; 4];
819 let mut mu = T::ZERO;
820 for (r, row) in d.iter_mut().enumerate().take(nd) {
821 for (c, v) in row.iter_mut().enumerate().take(nd) {
822 *v = t.data[at(j + r, j + c)];
823 mu = mu.max(v.abs());
824 }
825 }
826 let eps = T::EPSILON;
827 let smin = (eps * mu).max(T::from_f64(f64::MIN_POSITIVE));
828 let nk = n1 * n2;
830 let mut kmat = [[T::ZERO; 4]; 4];
831 let mut rhs = [T::ZERO; 4];
832 for c in 0..n2 {
833 for r in 0..n1 {
834 let row = r + c * n1;
835 rhs[row] = d[r][n1 + c];
836 for c2 in 0..n2 {
837 for r2 in 0..n1 {
838 let col = r2 + c2 * n1;
839 let mut v = T::ZERO;
840 if c == c2 {
841 v += d[r][r2];
842 }
843 if r == r2 {
844 v -= d[n1 + c2][n1 + c];
845 }
846 kmat[row][col] = v;
847 }
848 }
849 }
850 }
851 let xv = solve_small(&mut kmat, &mut rhs, nk, smin);
852 let mut m = [[T::ZERO; 2]; 4];
854 for c in 0..n2 {
855 for r in 0..n1 {
856 m[r][c] = -xv[r + c * n1];
857 }
858 m[n1 + c][c] = T::ONE;
859 }
860 let mut refl: Vec<(usize, T, [T; 3])> = Vec::with_capacity(n2);
861 for c in 0..n2 {
862 let len = nd - c;
863 let mut x = [T::ZERO; 4];
864 for r in 0..len {
865 x[r] = m[c + r][c];
866 }
867 let tau = householder(&mut x[..len]);
868 let mut v = [T::ZERO; 3];
869 v[..len - 1].copy_from_slice(&x[1..len]);
870 m[c][c] = x[0];
871 for r in 1..len {
872 m[c + r][c] = T::ZERO;
873 }
874 for c2 in c + 1..n2 {
875 let mut y = [T::ZERO; 4];
876 for r in 0..len {
877 y[r] = m[c + r][c2];
878 }
879 apply_householder(tau, &v[..len - 1], &mut y[..len]);
880 for r in 0..len {
881 m[c + r][c2] = y[r];
882 }
883 }
884 refl.push((c, tau, v));
885 }
886 let apply_left = |data: &mut [T], rows: usize, cols: (usize, usize)| {
888 for &(c, tau, v) in &refl {
889 let len = nd - c;
890 let mut y = [T::ZERO; 4];
891 for col in cols.0..cols.1 {
892 for r in 0..len {
893 y[r] = data[(j + c + r) + col * rows];
894 }
895 apply_householder(tau, &v[..len - 1], &mut y[..len]);
896 for r in 0..len {
897 data[(j + c + r) + col * rows] = y[r];
898 }
899 }
900 }
901 };
902 let apply_right = |data: &mut [T], rows: usize, rr: (usize, usize)| {
903 for &(c, tau, v) in &refl {
904 let len = nd - c;
905 let mut y = [T::ZERO; 4];
906 for row in rr.0..rr.1 {
907 for r in 0..len {
908 y[r] = data[row + (j + c + r) * rows];
909 }
910 apply_householder(tau, &v[..len - 1], &mut y[..len]);
911 for r in 0..len {
912 data[row + (j + c + r) * rows] = y[r];
913 }
914 }
915 }
916 };
917 {
920 let mut dd = vec![T::ZERO; p * p];
921 for r in 0..nd {
922 for c in 0..nd {
923 dd[(j + r) + (j + c) * p] = d[r][c];
924 }
925 }
926 apply_left(&mut dd, p, (j, j + nd));
927 apply_right(&mut dd, p, (j, j + nd));
928 let mut e = T::ZERO;
929 for r in n2..nd {
930 for c in 0..n2 {
931 e = e.max(dd[(j + r) + (j + c) * p].abs());
932 }
933 }
934 if lt(
935 (o.swap_factor * eps * mu).max(T::from_f64(f64::MIN_POSITIVE)),
936 e,
937 ) {
938 return false;
939 }
940 }
941 apply_left(&mut t.data, p, (j, p));
943 apply_right(&mut t.data, p, (0, (j + nd).min(p)));
944 apply_right(&mut z.data, zr, (0, zr));
945 for r in n2..nd {
946 for c in 0..n2 {
947 t.data[at(j + r, j + c)] = T::ZERO;
948 }
949 }
950 if n2 == 2 {
951 standardize(t, z, j);
952 }
953 if n1 == 2 {
954 standardize(t, z, j + n2);
955 }
956 for c in j..j + nd {
958 for r in c + 2..j + nd {
959 t.data[at(r, c)] = T::ZERO;
960 }
961 }
962 debug_assert!(check_quasi_triangular(t), "(E2) after a swap");
963 true
964}
965
966pub(crate) fn reorder<T: Real>(
971 t: &mut DenseBlock<T>,
972 z: &mut DenseBlock<T>,
973 rank: &[usize],
974 need: usize,
975 o: &SchurOptions<T>,
976) -> (Vec<(usize, usize, usize)>, bool) {
977 let mut bl: Vec<(usize, usize, usize)> = blocks(t)
978 .iter()
979 .zip(rank)
980 .map(|(&(q, s), &r)| (q, s, r))
981 .collect();
982 let mut pos_b = 0;
983 loop {
984 if pos_b >= bl.len() || bl[pos_b].0 >= need {
985 return (bl, true);
986 }
987 let mut best = pos_b;
989 for b in pos_b..bl.len() {
990 if bl[b].2 < bl[best].2 {
991 best = b;
992 }
993 }
994 let mut cur = best;
995 while cur > pos_b {
996 let (q1, s1, r1) = bl[cur - 1];
997 let (_, s2, r2) = bl[cur];
998 if !swap_blocks(t, z, q1, s1, s2, o) {
999 return (rebuild(t, &bl), false);
1000 }
1001 let mut fresh = Vec::new();
1003 let p = t.rows;
1004 let push = |q: usize, s: usize, r: usize, fresh: &mut Vec<(usize, usize, usize)>| {
1005 if s == 2 && is_exact_zero(t.data[(q + 1) + q * p]) {
1006 fresh.push((q, 1, r));
1007 fresh.push((q + 1, 1, r));
1008 } else {
1009 fresh.push((q, s, r));
1010 }
1011 };
1012 push(q1, s2, r2, &mut fresh);
1013 push(q1 + s2, s1, r1, &mut fresh);
1014 bl.splice(cur - 1..=cur, fresh);
1015 cur -= 1;
1018 }
1019 pos_b += 1;
1020 }
1021}
1022
1023fn rebuild<T: Real>(
1025 t: &DenseBlock<T>,
1026 old: &[(usize, usize, usize)],
1027) -> Vec<(usize, usize, usize)> {
1028 blocks(t)
1029 .into_iter()
1030 .map(|(q, s)| {
1031 let r = old
1032 .iter()
1033 .find(|b| b.0 <= q && q < b.0 + b.1)
1034 .map_or(usize::MAX, |b| b.2);
1035 (q, s, r)
1036 })
1037 .collect()
1038}
1039
1040#[derive(Clone, Copy, Debug, PartialEq)]
1042pub(crate) struct Cx<T> {
1043 pub(crate) re: T,
1044 pub(crate) im: T,
1045}
1046
1047impl<T: Real> Cx<T> {
1048 pub(crate) fn new(re: T, im: T) -> Self {
1049 Self { re, im }
1050 }
1051 fn zero() -> Self {
1052 Self::new(T::ZERO, T::ZERO)
1053 }
1054 fn add(self, o: Self) -> Self {
1055 Self::new(self.re + o.re, self.im + o.im)
1056 }
1057 fn sub(self, o: Self) -> Self {
1058 Self::new(self.re - o.re, self.im - o.im)
1059 }
1060 pub(crate) fn mul(self, o: Self) -> Self {
1061 Self::new(
1062 self.re * o.re - self.im * o.im,
1063 self.re * o.im + self.im * o.re,
1064 )
1065 }
1066 fn scale(self, s: T) -> Self {
1067 Self::new(self.re * s, self.im * s)
1068 }
1069 fn mag1(self) -> T {
1071 self.re.abs().max(self.im.abs())
1072 }
1073 pub(crate) fn div(self, o: Self) -> Self {
1075 if le_exact(o.im.abs(), o.re.abs()) {
1076 let r = o.im / o.re;
1077 let den = o.re + o.im * r;
1078 Self::new((self.re + self.im * r) / den, (self.im - self.re * r) / den)
1079 } else {
1080 let r = o.re / o.im;
1081 let den = o.re * r + o.im;
1082 Self::new((self.re * r + self.im) / den, (self.im * r - self.re) / den)
1083 }
1084 }
1085}
1086
1087pub(crate) fn schur_eigvec<T: Real>(
1091 t: &DenseBlock<T>,
1092 bl: &[(usize, usize)],
1093 bi: usize,
1094) -> Vec<Cx<T>> {
1095 let p = t.rows;
1096 let at = |i: usize, j: usize| t.data[i + j * p];
1097 let (q, s) = bl[bi];
1098 let mut x = vec![Cx::<T>::zero(); p];
1099 let mu = max_abs(&t.data);
1100 let smin = (T::EPSILON * mu).max(T::from_f64(f64::MIN_POSITIVE));
1101 let lam = if s == 1 {
1103 x[q] = Cx::new(T::ONE, T::ZERO);
1104 Cx::new(at(q, q), T::ZERO)
1105 } else {
1106 let (a, b, c) = (at(q, q), at(q, q + 1), at(q + 1, q));
1107 let w = b.abs().sqrt() * c.abs().sqrt();
1108 if le_exact(c.abs(), b.abs()) {
1109 x[q] = Cx::new(T::ONE, T::ZERO);
1110 x[q + 1] = Cx::new(T::ZERO, w / b);
1111 } else {
1112 x[q] = Cx::new(T::ZERO, w / c);
1113 x[q + 1] = Cx::new(T::ONE, T::ZERO);
1114 }
1115 Cx::new(a, w)
1116 };
1117 let end = q + s;
1118 let big = T::from_f64(BIG);
1119 for b in (0..bi).rev() {
1121 let (r0, rs) = bl[b];
1122 let mut rhs = [Cx::<T>::zero(); 2];
1123 for (k, rv) in rhs.iter_mut().enumerate().take(rs) {
1124 let mut acc = Cx::zero();
1125 for j in r0 + rs..end {
1126 acc = acc.add(x[j].scale(at(r0 + k, j)));
1127 }
1128 *rv = Cx::new(-acc.re, -acc.im);
1129 }
1130 if rs == 1 {
1131 let mut d = Cx::new(at(r0, r0), T::ZERO).sub(lam);
1132 if lt(d.mag1(), smin) {
1133 d = Cx::new(smin, T::ZERO);
1134 }
1135 x[r0] = rhs[0].div(d);
1136 } else {
1137 let mut m = [
1139 [
1140 Cx::new(at(r0, r0), T::ZERO).sub(lam),
1141 Cx::new(at(r0, r0 + 1), T::ZERO),
1142 ],
1143 [
1144 Cx::new(at(r0 + 1, r0), T::ZERO),
1145 Cx::new(at(r0 + 1, r0 + 1), T::ZERO).sub(lam),
1146 ],
1147 ];
1148 let mut r = rhs;
1149 if lt(m[0][0].mag1(), m[1][0].mag1()) {
1150 m.swap(0, 1);
1151 r.swap(0, 1);
1152 }
1153 if lt(m[0][0].mag1(), smin) {
1154 m[0][0] = Cx::new(smin, T::ZERO);
1155 }
1156 let f = m[1][0].div(m[0][0]);
1157 let mut m22 = m[1][1].sub(f.mul(m[0][1]));
1158 let r1 = r[1].sub(f.mul(r[0]));
1159 if lt(m22.mag1(), smin) {
1160 m22 = Cx::new(smin, T::ZERO);
1161 }
1162 let y1 = r1.div(m22);
1163 let y0 = r[0].sub(m[0][1].mul(y1)).div(m[0][0]);
1164 x[r0] = y0;
1165 x[r0 + 1] = y1;
1166 }
1167 let mut xm = T::ZERO;
1169 for v in &x[..end] {
1170 xm = xm.max(v.mag1());
1171 }
1172 if lt(big, xm) {
1173 let f = T::from_f64(1.0 / pow2_floor(xm.to_f64()));
1174 for v in &mut x[..end] {
1175 *v = v.scale(f);
1176 }
1177 }
1178 }
1179 x
1180}
1181
1182#[cfg(test)]
1183#[allow(clippy::float_cmp, clippy::cast_precision_loss)]
1184mod tests {
1185 use super::*;
1186 use crate::random::Rng;
1187 use xingo_extprec::DoubleDouble as Dd;
1188
1189 fn rand_mat<T: Real>(p: usize, seed: u64, sym: bool) -> DenseBlock<T> {
1190 let mut rng = Rng::new(seed);
1191 let mut a = DenseBlock::<T>::zeros(p, p);
1192 for v in &mut a.data {
1193 *v = T::from_f64(rng.gaussian());
1194 }
1195 if sym {
1196 for j in 0..p {
1197 for i in 0..j {
1198 a.data[i + j * p] = a.data[j + i * p];
1199 }
1200 }
1201 }
1202 a
1203 }
1204
1205 fn mul<T: Real>(a: &DenseBlock<T>, b: &DenseBlock<T>) -> DenseBlock<T> {
1206 let (m, k, n) = (a.rows, a.cols, b.cols);
1207 let mut c = DenseBlock::zeros(m, n);
1208 for j in 0..n {
1209 for l in 0..k {
1210 let f = b.data[l + j * k];
1211 for i in 0..m {
1212 c.data[i + j * m] += a.data[i + l * m] * f;
1213 }
1214 }
1215 }
1216 c
1217 }
1218
1219 fn transpose<T: Real>(a: &DenseBlock<T>) -> DenseBlock<T> {
1220 let mut t = DenseBlock::zeros(a.cols, a.rows);
1221 for j in 0..a.cols {
1222 for i in 0..a.rows {
1223 t.data[j + i * a.cols] = a.data[i + j * a.rows];
1224 }
1225 }
1226 t
1227 }
1228
1229 fn max_diff<T: Real>(a: &DenseBlock<T>, b: &DenseBlock<T>) -> f64 {
1230 a.data
1231 .iter()
1232 .zip(&b.data)
1233 .map(|(&x, &y)| (x - y).abs().to_f64())
1234 .fold(0.0, f64::max)
1235 }
1236
1237 fn orth_err<T: Real>(z: &DenseBlock<T>) -> f64 {
1238 max_diff(&mul(&transpose(z), z), &identity(z.cols))
1239 }
1240
1241 #[test]
1243 fn jacobi_diagonalizes_symmetric_matrices() {
1244 fn run<T: Real>(u: f64) -> f64 {
1245 let mut worst = 0.0f64;
1246 for (p, seed) in [(1, 1), (2, 2), (7, 3), (20, 4), (41, 5)] {
1247 let a = rand_mat::<T>(p, seed, true);
1248 let (d, z) = sym_eig(&a, &SchurOptions::new()).unwrap();
1249 let mut dz = z.clone();
1250 for j in 0..p {
1251 for i in 0..p {
1252 dz.data[i + j * p] = z.data[i + j * p] * d[j];
1253 }
1254 }
1255 let r = max_diff(&mul(&a, &z), &dz) / max_abs(&a.data).to_f64();
1256 worst = worst
1257 .max(r / (p as f64 * u))
1258 .max(orth_err(&z) / (p as f64 * u));
1259 }
1260 worst
1261 }
1262 let r64 = run::<f64>(f64::EPSILON / 2.0);
1263 let rdd = run::<Dd>(Dd::EPSILON.to_f64() / 2.0);
1264 eprintln!("(E1) / (p u): f64 {r64:.2}, dd {rdd:.2}");
1265 assert!(r64 < 8.0 && rdd < 8.0, "{r64} {rdd}");
1266 let (d, z) = sym_eig(&DenseBlock::<f64>::zeros(3, 3), &SchurOptions::new()).unwrap();
1268 assert_eq!(d, vec![0.0; 3]);
1269 assert_eq!(z.data, identity::<f64>(3).data);
1270 }
1271
1272 #[test]
1274 fn lanv2_standardizes_every_case() {
1275 let cases = [
1276 (1.0, 2.0, 0.0, 3.0),
1277 (1.0, 0.0, 2.0, 3.0),
1278 (1.0, 2.0, -3.0, 1.0),
1279 (1.0, 2.0, 3.0, 4.0),
1280 (4.0, -5.0, 1.0, 2.0),
1281 (0.0, 1.0, -1e-20, 0.0),
1282 (2.0, 1e-8, 1e-8, 2.0),
1283 (1.0, -1.0, 1.0, 1.0 + 1e-15),
1284 (3.0, 7.0, -0.5, -1.0),
1285 ];
1286 for (a, b, c, d) in cases {
1287 let ([a2, b2, c2, d2], cs, sn) = lanv2(a, b, c, d);
1288 assert!((cs * cs + sn * sn - 1.0).abs() < 4e-16);
1289 let r = [[cs, -sn], [sn, cs]];
1291 let ap = [[a2, b2], [c2, d2]];
1292 let mut out = [[0.0; 2]; 2];
1293 for i in 0..2 {
1294 for j in 0..2 {
1295 for k in 0..2 {
1296 for l in 0..2 {
1297 out[i][j] += r[i][k] * ap[k][l] * r[j][l];
1298 }
1299 }
1300 }
1301 }
1302 let scale = a.abs().max(b.abs()).max(c.abs()).max(d.abs());
1303 let err = (out[0][0] - a).abs()
1304 + (out[0][1] - b).abs()
1305 + (out[1][0] - c).abs()
1306 + (out[1][1] - d).abs();
1307 assert!(err <= 8.0 * f64::EPSILON * scale, "{a} {b} {c} {d}: {err}");
1308 assert!(
1309 c2 == 0.0 || (a2 == d2 && b2 * c2 < 0.0),
1310 "{a2} {b2} {c2} {d2}"
1311 );
1312 }
1313 }
1314
1315 fn schur_of<T: Real>(a: &DenseBlock<T>) -> (DenseBlock<T>, DenseBlock<T>) {
1316 let mut t = a.clone();
1317 let mut z = identity::<T>(a.rows);
1318 real_schur(&mut t, &mut z, &SchurOptions::new()).unwrap();
1319 (t, z)
1320 }
1321
1322 #[test]
1324 fn francis_reaches_real_schur_form() {
1325 fn run<T: Real>(u: f64) -> f64 {
1326 let mut worst = 0.0f64;
1327 for (p, seed) in [(1, 1), (2, 2), (3, 3), (6, 4), (20, 5), (45, 6)] {
1328 let a = rand_mat::<T>(p, seed, false);
1329 let (t, z) = schur_of(&a);
1330 assert!(check_quasi_triangular(&t));
1331 let r = max_diff(&mul(&a, &z), &mul(&z, &t)) / max_abs(&a.data).to_f64();
1332 worst = worst
1333 .max(r / (p as f64 * u))
1334 .max(orth_err(&z) / (p as f64 * u));
1335 }
1336 worst
1337 }
1338 let r64 = run::<f64>(f64::EPSILON / 2.0);
1339 let rdd = run::<Dd>(Dd::EPSILON.to_f64() / 2.0);
1340 eprintln!("(E2) / (p u): f64 {r64:.2}, dd {rdd:.2}");
1341 assert!(r64 < 16.0 && rdd < 16.0, "{r64} {rdd}");
1342 let mut a = DenseBlock::<f64>::zeros(2, 2);
1344 a.data = vec![0.0, 1.0, -1.0, 0.0];
1345 let (t, _) = schur_of(&a);
1346 let (re, im) = block_value(&t, (0, 2));
1347 assert!(re.abs() < 1e-15 && (im - 1.0).abs() < 1e-15);
1348 let mut j = DenseBlock::<f64>::zeros(4, 4);
1350 for i in 0..4 {
1351 j.data[i + i * 4] = 2.0;
1352 if i + 1 < 4 {
1353 j.data[i + (i + 1) * 4] = 1.0;
1354 }
1355 }
1356 let (t, _) = schur_of(&j);
1357 assert!(check_quasi_triangular(&t));
1358 let mut c = DenseBlock::<f64>::zeros(5, 5);
1361 for i in 1..5 {
1362 c.data[i + (i - 1) * 5] = 1.0;
1363 }
1364 c.data[4 * 5] = 1.0;
1365 let (t, z) = schur_of(&c);
1366 assert!(check_quasi_triangular(&t));
1367 assert!(max_diff(&mul(&c, &z), &mul(&z, &t)) < 1e-14);
1368 let mut mods: Vec<f64> = blocks(&t)
1369 .iter()
1370 .map(|&b| {
1371 let (re, im) = block_value(&t, b);
1372 hypot(re, im)
1373 })
1374 .collect();
1375 mods.sort_by(f64::total_cmp);
1376 for m in mods {
1377 assert!((m - 1.0).abs() < 1e-13, "{m}");
1378 }
1379 }
1380
1381 #[test]
1384 fn reordering_moves_wanted_blocks_first() {
1385 for (p, seed) in [(6, 11), (12, 12), (25, 13), (40, 14)] {
1386 let a = rand_mat::<f64>(p, seed, false);
1387 let (mut t, mut z) = schur_of(&a);
1388 let bl = blocks(&t);
1389 let vals: Vec<(f64, f64)> = bl.iter().map(|&b| block_value(&t, b)).collect();
1390 let mut order: Vec<usize> = (0..bl.len()).collect();
1392 order.sort_by(|&i, &j| vals[i].0.total_cmp(&vals[j].0));
1393 let mut rank = vec![0; bl.len()];
1394 for (r, &i) in order.iter().enumerate() {
1395 rank[i] = r;
1396 }
1397 let (nb, ok) = reorder(&mut t, &mut z, &rank, p, &SchurOptions::new());
1398 assert!(ok);
1399 assert!(check_quasi_triangular(&t));
1400 assert!(max_diff(&mul(&a, &z), &mul(&z, &t)) < 1e-13 * p as f64);
1401 assert!(orth_err(&z) < 1e-14 * p as f64);
1402 let res: Vec<f64> = blocks(&t).iter().map(|&b| block_value(&t, b).0).collect();
1403 for w in res.windows(2) {
1404 assert!(w[0] <= w[1] + 1e-12, "{res:?}");
1405 }
1406 for w in nb.windows(2) {
1407 assert!(w[0].2 <= w[1].2);
1408 }
1409 }
1410 let mut t = DenseBlock::<f64>::zeros(3, 3);
1412 t.data[0] = 3.0;
1413 t.data[4] = 1.0;
1414 t.data[8] = 2.0;
1415 let mut z = identity::<f64>(3);
1416 reorder(&mut t, &mut z, &[2, 0, 1], 3, &SchurOptions::new());
1417 assert_eq!((t.data[0], t.data[4], t.data[8]), (1.0, 2.0, 3.0));
1418 for v in &z.data {
1419 assert!(*v == 0.0 || v.abs() == 1.0);
1420 }
1421 }
1422
1423 #[test]
1425 fn eigenvectors_of_the_schur_form() {
1426 for (p, seed) in [(5, 21), (17, 22), (33, 23)] {
1427 let a = rand_mat::<f64>(p, seed, false);
1428 let (t, _) = schur_of(&a);
1429 let bl = blocks(&t);
1430 for bi in 0..bl.len() {
1431 let x = schur_eigvec(&t, &bl, bi);
1432 let (re, im) = block_value(&t, bl[bi]);
1433 let lam = Cx::new(re, im);
1434 let xn = x
1435 .iter()
1436 .map(|v| v.re * v.re + v.im * v.im)
1437 .sum::<f64>()
1438 .sqrt();
1439 let mut err = 0.0f64;
1440 for i in 0..p {
1441 let mut acc = Cx::new(0.0, 0.0);
1442 for j in 0..p {
1443 acc = acc.add(x[j].scale(t.data[i + j * p]));
1444 }
1445 let r = acc.sub(lam.mul(x[i]));
1446 err = err.max(r.mag1());
1447 }
1448 let tn = max_abs(&t.data);
1449 assert!(err <= 1e-14 * tn * xn * p as f64, "{p} {bi}: {err}");
1450 }
1451 }
1452 }
1453}