1use crate::internal_prelude::*;
2use crate::utils;
3use equator::assert;
4use linalg::matmul::{matmul, triangular as tr};
5pub fn generalized_hessenberg_scratch<T: ComplexField>(
8 n: usize,
9 params: GeneralizedHessenbergParams,
10) -> StackReq {
11 if params.block_size <= 1 || n <= params.block_size {
12 StackReq::EMPTY
13 } else {
14 linalg::temp_mat_scratch::<T>(
15 n,
16 match params.block_size.checked_mul(6) {
17 Some(n) => n,
18 None => return StackReq::OVERFLOW,
19 },
20 )
21 }
22}
23pub(crate) fn make_givens<T: ComplexField>(f: T, g: T) -> (T::Real, T, T) {
28 if g == zero() {
29 (one(), zero(), f)
30 } else if f == zero() {
31 let c = zero::<T::Real>();
32 let d = g.abs();
33 let d_inv = d.recip();
34 if !(d.is_finite() && d_inv.is_finite()) {
35 return lartg(f, g);
37 }
38 let s = g.conj().mul_real(&d_inv);
39 let r = d.to_cplx();
40 (c, s, r)
41 } else {
42 let f1 = f.abs();
50 let g1 = g.abs();
51 let h1 = f1.hypot(g1);
52 let rtmin = sqrt_min_positive::<T::Real>();
53 let rtmax = sqrt_max_positive::<T::Real>();
54 if h1 > rtmin && h1 < rtmax {
55 let h1_inv = &h1.recip();
56 let c = f1 * h1_inv;
57 if c > rtmin {
58 let r = f.mul_real(c.recip());
59 let s = g.conj() * r.mul_real(h1_inv).mul_real(h1_inv);
60 return (c, s, r);
61 }
62 }
63 lartg(f, g)
64 }
65}
66fn abs_max<T: ComplexField>(z: &T) -> T::Real {
68 z.real().abs().fmax(z.imag().abs())
69}
70fn div_real<T: ComplexField>(z: &T, x: &T::Real) -> T {
73 z.mul_real(x.recip())
74}
75fn lartg<T: ComplexField>(f: T, g: T) -> (T::Real, T, T) {
83 let safmin = min_positive::<T::Real>();
84 let safmax = max_positive::<T::Real>();
85 let rtmin = sqrt_min_positive::<T::Real>();
86 let two = from_f64::<T::Real>(2.0);
87 let four = from_f64::<T::Real>(4.0);
88 let clamp = |x: T::Real| safmin.fmax(&x).fmin(&safmax);
89
90 if g == zero() {
91 return (one(), zero(), f);
92 }
93 if f == zero() {
94 let c = zero::<T::Real>();
95 let g1 = abs_max(&g);
96 let rtmax = (&safmax / &two).sqrt();
97 let (s, d) = if g1 > rtmin && g1 < rtmax {
98 let d = g.abs2().sqrt();
99 (div_real(&g.conj(), &d), d)
100 } else {
101 let u = clamp(g1);
102 let gs = div_real(&g, &u);
103 let d = gs.abs2().sqrt();
104 (div_real(&gs.conj(), &d), d * &u)
105 };
106 return (c, s, d.to_cplx());
107 }
108
109 let f1 = abs_max(&f);
110 let g1 = abs_max(&g);
111 let rtmax = (&safmax / &four).sqrt();
112 let (fs, gs, u, w) = if f1 > rtmin && f1 < rtmax && g1 > rtmin && g1 < rtmax
114 {
115 (f, g, one::<T::Real>(), one::<T::Real>())
116 } else {
117 let u = clamp(f1.fmax(&g1));
118 let gs = div_real(&g, &u);
119 if &f1 / &u < rtmin {
120 let v = clamp(f1);
122 let w = &v / &u;
123 (div_real(&f, &v), gs, u, w)
124 } else {
125 (div_real(&f, &u), gs, u, one::<T::Real>())
126 }
127 };
128 let f2 = fs.abs2();
129 let g2 = gs.abs2();
130 let h2 = &(&(&f2 * &w) * &w) + &g2;
131 let (c, r, s);
133 if f2 >= &h2 * &safmin {
134 c = (&f2 / &h2).sqrt();
136 r = div_real(&fs, &c);
137 if f2 > rtmin && h2 < &rtmax * &two {
138 s = gs.conj() * div_real(&fs, &(&f2 * &h2).sqrt());
140 } else {
141 s = gs.conj() * div_real(&r, &h2);
142 }
143 } else {
144 let d = (&f2 * &h2).sqrt();
148 c = &f2 / &d;
149 if c >= safmin {
150 r = div_real(&fs, &c);
151 } else {
152 r = fs.mul_real(&h2 / &d);
153 }
154 s = gs.conj() * div_real(&fs, &d);
155 }
156 (c * &w, s, r.mul_real(&u))
157}
158pub(crate) fn active_block_is_finite<T: ComplexField>(
163 A: MatRef<'_, T>,
164 B: MatRef<'_, T>,
165 istart: usize,
166 istop: usize,
167) -> bool {
168 (istart..istop + 1).all(|k| {
169 A[(k, k)].is_finite()
170 && B[(k, k)].is_finite()
171 && (k == istart || A[(k, k - 1)].is_finite())
172 })
173}
174pub(crate) fn rot<T: ComplexField>(
175 c: T::Real,
176 s: T,
177 x: RowMut<'_, T>,
178 y: RowMut<'_, T>,
179) {
180 let (c, s) = &(c, s);
181 zip!(x, y).for_each(|unzip!(x, y): Zip!(&mut _, &mut _)| {
182 (*x, *y) = (x.mul_real(c) + &*y * s, y.mul_real(c) - &*x * s.conj());
183 });
184}
185#[derive(Copy, Clone, Debug)]
187pub struct GeneralizedHessenbergParams {
188 pub block_size: usize,
190 pub blocking_threshold: usize,
192 #[doc(hidden)]
193 pub non_exhaustive: NonExhaustive,
194}
195impl<T: ComplexField> Auto<T> for GeneralizedHessenbergParams {
196 fn auto() -> Self {
197 Self {
198 block_size: 32,
199 blocking_threshold: 256 * 256,
200 non_exhaustive: NonExhaustive(()),
201 }
202 }
203}
204pub(crate) fn trot<T: ComplexField>(
205 c: T::Real,
206 s: T,
207 x: ColMut<'_, T>,
208 y: ColMut<'_, T>,
209) {
210 rot(c, -s.conj(), x.transpose_mut(), y.transpose_mut());
211}
212fn generalized_hessenberg_unblocked<T: ComplexField>(
213 A: MatMut<'_, T>,
214 B: MatMut<'_, T>,
215 Q: Option<MatMut<'_, T>>,
216 Z: Option<MatMut<'_, T>>,
217 Q_is_I: bool,
218 Z_is_I: bool,
219 par: Par,
220 stack: &mut MemStack,
221 params: GeneralizedHessenbergParams,
222) {
223 _ = params;
224 _ = stack;
225 _ = Q_is_I;
226 _ = Z_is_I;
227 _ = par;
228 let mut A = A;
229 let mut B = B;
230 let mut Q = Q;
231 let mut Z = Z;
232 let n = A.nrows();
233 let (Q_nrows, Q_ncols) =
234 Q.rb().map(|Q| (Q.nrows(), Q.ncols())).unwrap_or((n, n));
235 let (Z_nrows, Z_ncols) =
236 Z.rb().map(|Z| (Z.nrows(), Z.ncols())).unwrap_or((n, n));
237 assert!(all(
238 A.nrows() == n,
239 A.ncols() == n,
240 B.nrows() == n,
241 B.ncols() == n,
242 Q_nrows == n,
243 Q_ncols == n,
244 Z_nrows == n,
245 Z_ncols == n,
246 ));
247 if n <= 2 {
248 return;
249 }
250 for jcol in 0..n - 2 {
251 for jrow in (jcol + 2..n).rev() {
252 let (c, s, r) =
253 make_givens(A[(jrow - 1, jcol)].copy(), A[(jrow, jcol)].copy());
254 A[(jrow - 1, jcol)] = r;
255 A[(jrow, jcol)] = zero();
256 let (x, y) = A
257 .rb_mut()
258 .get_mut(.., jcol + 1..)
259 .two_rows_mut(jrow - 1, jrow);
260 rot(c.copy(), s.copy(), x, y);
261 let (x, y) = B
262 .rb_mut()
263 .get_mut(.., jrow - 1..)
264 .two_rows_mut(jrow - 1, jrow);
265 rot(c.copy(), s.copy(), x, y);
266 if let Some(mut Q) = Q.rb_mut() {
267 let (x, y) = Q.rb_mut().two_cols_mut(jrow - 1, jrow);
268 rot(c.copy(), s.conj(), x.transpose_mut(), y.transpose_mut());
269 }
270 let (c, s, r) =
271 make_givens(B[(jrow, jrow)].copy(), B[(jrow, jrow - 1)].copy());
272 B[(jrow, jrow)] = r;
273 B[(jrow, jrow - 1)] = zero();
274 let (x, y) = A.rb_mut().two_cols_mut(jrow, jrow - 1);
275 rot(c.copy(), s.copy(), x.transpose_mut(), y.transpose_mut());
276 let (x, y) =
277 B.rb_mut().get_mut(..jrow, ..).two_cols_mut(jrow, jrow - 1);
278 rot(c.copy(), s.copy(), x.transpose_mut(), y.transpose_mut());
279 if let Some(mut Z) = Z.rb_mut() {
280 let (x, y) = Z.rb_mut().two_cols_mut(jrow, jrow - 1);
281 rot(c, s, x.transpose_mut(), y.transpose_mut());
282 }
283 }
284 }
285}
286fn apply_U_from_the_right<T: ComplexField>(
287 M: MatMut<'_, T>,
288 U: MatRef<'_, T>,
289 par: Par,
290 stack: &mut MemStack,
291) {
292 let n = U.nrows() / 2;
293 let (U11, U12, U21, U22) = U.split_at(n, n);
294 let mut M = M;
295 let (mut tmp, _) = unsafe {
296 linalg::temp_mat_uninit::<T, _, _>(M.nrows(), M.ncols(), stack)
297 };
298 let mut tmp = tmp.as_mat_mut();
299 let (mut T1, mut T2) = tmp.rb_mut().split_at_col_mut(n);
300 let (M1, M2) = M.rb().split_at_col(n);
301 utils::thread::join_raw(
302 |par| {
303 matmul(T1.rb_mut(), Accum::Replace, M1.rb(), U11, one(), par);
304 tr::matmul(
305 T1.rb_mut(),
306 tr::BlockStructure::Rectangular,
307 Accum::Add,
308 M2.rb(),
309 tr::BlockStructure::Rectangular,
310 U21,
311 tr::BlockStructure::TriangularUpper,
312 one(),
313 par,
314 );
315 },
316 |par| {
317 tr::matmul(
318 T2.rb_mut(),
319 tr::BlockStructure::Rectangular,
320 Accum::Replace,
321 M1.rb(),
322 tr::BlockStructure::Rectangular,
323 U12,
324 tr::BlockStructure::TriangularLower,
325 one(),
326 par,
327 );
328 matmul(T2.rb_mut(), Accum::Add, M2.rb(), U22, one(), par);
329 },
330 par,
331 );
332 M.copy_from(tmp);
333}
334pub fn generalized_hessenberg<T: ComplexField>(
354 A: MatMut<'_, T>,
355 B: MatMut<'_, T>,
356 Q_inout: Option<MatMut<'_, T>>,
357 Z_inout: Option<MatMut<'_, T>>,
358 par: Par,
359 stack: &mut MemStack,
360 params: GeneralizedHessenbergParams,
361) {
362 let n = A.nrows();
363 let (Q_nrows, Q_ncols) = Q_inout
364 .rb()
365 .map(|Q| (Q.nrows(), Q.ncols()))
366 .unwrap_or((n, n));
367 let (Z_nrows, Z_ncols) = Z_inout
368 .rb()
369 .map(|Z| (Z.nrows(), Z.ncols()))
370 .unwrap_or((n, n));
371 let Q_is_I = Q_inout.rb().is_some_and(|Q| Q.is_identity());
372 let Z_is_I = Z_inout.rb().is_some_and(|Z| Z.is_identity());
373 assert!(all(
374 A.nrows() == n,
375 A.ncols() == n,
376 B.nrows() == n,
377 B.ncols() == n,
378 Q_nrows == n,
379 Q_ncols == n,
380 Z_nrows == n,
381 Z_ncols == n,
382 ));
383 if n <= 2 {
384 return;
385 }
386 if params.block_size <= 1 || A.nrows() <= params.block_size {
387 return generalized_hessenberg_unblocked(
388 A, B, Q_inout, Z_inout, Q_is_I, Z_is_I, par, stack, params,
389 );
390 }
391 let mut A = A;
392 let mut B = B;
393 let mut Q = Q_inout;
394 let mut Z = Z_inout;
395 let mut jcol = 0;
396 while jcol < n - 2 {
397 let nnb = params.block_size.clamp(1, n - 2 - jcol);
398 let top = if jcol < 2 { 0 } else { jcol + 1 };
399 let n2nb = ((n - 2 - jcol) / nnb).saturating_sub(1);
400 let nblst = (n - 1 - jcol) - n2nb * nnb;
401 alloca!('stack: {
402 let mut work0 = unsafe { mat![uninit::<T>, nblst, nblst] };
403 let mut work1 =
404 unsafe { mat![uninit::<T>, 2 * nnb, 2 * nnb * n2nb] };
405 });
406 work0.fill(zero());
407 work0.rb_mut().diagonal_mut().fill(one());
408 work1.fill(zero());
409 for i in 0..n2nb {
410 work1
411 .rb_mut()
412 .subcols_mut(2 * nnb * i, 2 * nnb)
413 .diagonal_mut()
414 .fill(one());
415 }
416 for j in jcol..jcol + nnb {
417 for i in (j + 2..n).rev() {
418 let (c, s, r) =
419 make_givens(A[(i - 1, j)].copy(), A[(i, j)].copy());
420 A[(i - 1, j)] = r;
421 A[(i, j)] = c.to_cplx();
422 B[(i, j)] = s;
423 }
424 let mut len = 2 + j - jcol;
425 let jrow = j + n2nb * nnb + 2;
426 for i in (jrow..n).rev() {
427 let ref c = A[(i, j)].real();
428 let ref s = B[(i, j)].copy();
429 let col = nblst - (n - i) - 1;
430 for jj in nblst - len..nblst {
431 let ref temp0 = work0[(jj, col)].copy();
432 let ref temp1 = work0[(jj, col + 1)].copy();
433 work0[(jj, col + 1)] = temp1.mul_real(c) - s * temp0;
434 work0[(jj, col)] = temp0.mul_real(c) + s.conj() * temp1;
435 }
436 len += 1;
437 }
438 let mut j0 = jrow;
439 let mut idx = 0usize;
440 while j0 > j + 2 {
441 j0 -= nnb;
442 let mut start = nnb;
443 let mut len = 2 + j - jcol;
444 let mut col = nnb + j - jcol;
445 let mut U = work1.rb_mut().submatrix_mut(
446 0,
447 idx * 2 * nnb,
448 2 * nnb,
449 2 * nnb,
450 );
451 for i in (j0..j0 + nnb).rev() {
452 let ref c = A[(i, j)].real();
453 let ref s = B[(i, j)].copy();
454 col -= 1;
455 start -= 1;
456 for jj in start..start + len {
457 let ref temp0 = U[(jj, col)].copy();
458 let ref temp1 = U[(jj, col + 1)].copy();
459 U[(jj, col + 1)] = temp1.mul_real(c) - s * temp0;
460 U[(jj, col)] = s.conj() * temp1 + temp0.mul_real(c);
461 }
462 len += 1;
463 }
464 idx += 1;
465 }
466 for jj in (j + 1..n).rev() {
467 for i in (j + 2..Ord::min(n, jj + 2)).rev() {
468 let ref c = A[(i, j)].real();
469 let ref s = B[(i, j)].copy();
470 let ref temp0 = B[(i - 1, jj)].copy();
471 let ref temp1 = B[(i, jj)].copy();
472 B[(i, jj)] = temp1.mul_real(c) - s.conj() * temp0;
473 B[(i - 1, jj)] = temp0.mul_real(c) + s * temp1;
474 }
475 if jj + 1 < n {
476 let (c, s, r) = make_givens(
477 B[(jj + 1, jj + 1)].copy(),
478 B[(jj + 1, jj)].copy(),
479 );
480 B[(jj + 1, jj + 1)] = r;
481 B[(jj + 1, jj)] = zero();
482 let (bjj, bjj1) = B
483 .rb_mut()
484 .get_mut(top..jj + 1, ..)
485 .two_cols_mut(jj, jj + 1);
486 rot(
487 c.copy(),
488 s.copy(),
489 bjj1.transpose_mut(),
490 bjj.transpose_mut(),
491 );
492 A[(jj + 1, j)] = c.to_cplx();
493 B[(jj + 1, j)] = -s.conj();
494 }
495 }
496 let jj = (n - 2 - j) % 3;
497 let mut i = n - 1 - j;
498 while i > jj + 1 {
499 i -= 3;
500 let ref c0 = A[(j + i + 1, j)].real();
501 let ref s0 = -&B[(j + i + 1, j)];
502 let ref c1 = A[(j + i + 2, j)].real();
503 let ref s1 = -&B[(j + i + 2, j)];
504 let ref c2 = A[(j + i + 3, j)].real();
505 let ref s2 = -&B[(j + i + 3, j)];
506 for k in top..n {
507 let ref temp0 = A[(k, j + i + 0)].copy();
508 let ref temp1 = A[(k, j + i + 1)].copy();
509 let ref temp2 = A[(k, j + i + 2)].copy();
510 let ref temp3 = A[(k, j + i + 3)].copy();
511 A[(k, j + i + 3)] = temp3.mul_real(c2) + temp2 * s2.conj();
512 let temp2 = temp2.mul_real(c2) - temp3 * s2;
513 A[(k, j + i + 2)] = temp2.mul_real(c1) + temp1 * s1.conj();
514 let temp1 = temp1.mul_real(c1) - temp2 * s1;
515 A[(k, j + i + 1)] = temp1.mul_real(c0) + temp0 * s0.conj();
516 let temp0 = temp0.mul_real(c0) - temp1 * s0;
517 A[(k, j + i + 0)] = temp0;
518 }
519 }
520 for i in (1..jj + 1).rev() {
521 let c = A[(j + i + 1, j)].real();
522 let s = B[(j + i + 1, j)].copy();
523 let (aj1, aj) = A
524 .rb_mut()
525 .get_mut(top.., ..)
526 .two_cols_mut(j + i + 1, j + i);
527 trot(c, s, aj1, aj);
528 }
529 if j < jcol + nnb - 1 {
530 let len = j - jcol + 1;
531 let jrow = n - nblst;
532 {
533 let (mut work2, _) = unsafe {
534 linalg::temp_mat_uninit::<T, _, _>(nblst, 1, stack)
535 };
536 let mut work2 = work2.as_mat_mut().col_mut(0);
537 matmul(
538 work2.rb_mut().get_mut(..len),
539 Accum::Replace,
540 work0.rb().get(.., ..len).adjoint(),
541 A.rb().get(jrow.., j + 1),
542 one(),
543 par,
544 );
545 tr::matmul(
546 work2.rb_mut().get_mut(len..),
547 tr::BlockStructure::Rectangular,
548 Accum::Replace,
549 work0.rb().get(..nblst - len, len..).adjoint(),
550 tr::BlockStructure::TriangularUpper,
551 A.rb().get(jrow..jrow + nblst - len, j + 1),
552 tr::BlockStructure::Rectangular,
553 one(),
554 par,
555 );
556 matmul(
557 work2.rb_mut().get_mut(len..),
558 Accum::Add,
559 work0.rb().get(nblst - len.., len..).adjoint(),
560 A.rb().get(jrow + nblst - len.., j + 1),
561 one(),
562 par,
563 );
564 for i in jrow..jrow + nblst {
565 A[(i, j + 1)] = work2[i - jrow].copy();
566 }
567 }
568 let mut j0 = jrow;
569 let mut idx = 0usize;
570 while j0 > jcol + nnb {
571 j0 -= nnb;
572 let U = work1.rb().submatrix(
573 0,
574 idx * 2 * nnb,
575 nnb + len,
576 nnb + len,
577 );
578 let (U11, U12, U21, U22) = U.split_at(nnb, len);
579 let (mut work2, _) = unsafe {
580 linalg::temp_mat_uninit::<T, _, _>(nnb + len, 1, stack)
581 };
582 let mut work2 = work2.as_mat_mut().col_mut(0);
583 matmul(
584 work2.rb_mut().get_mut(..len),
585 Accum::Replace,
586 U11.adjoint(),
587 A.rb().get(j0..j0 + nnb, j + 1),
588 one(),
589 par,
590 );
591 tr::matmul(
592 work2.rb_mut().get_mut(..len),
593 tr::BlockStructure::Rectangular,
594 Accum::Add,
595 U21.adjoint(),
596 tr::BlockStructure::TriangularLower,
597 A.rb().get(j0 + nnb..j0 + nnb + len, j + 1),
598 tr::BlockStructure::Rectangular,
599 one(),
600 par,
601 );
602 tr::matmul(
603 work2.rb_mut().get_mut(len..),
604 tr::BlockStructure::Rectangular,
605 Accum::Replace,
606 U12.adjoint(),
607 tr::BlockStructure::TriangularUpper,
608 A.rb().get(j0..j0 + nnb, j + 1),
609 tr::BlockStructure::Rectangular,
610 one(),
611 par,
612 );
613 matmul(
614 work2.rb_mut().get_mut(len..),
615 Accum::Add,
616 U22.adjoint(),
617 A.rb().get(j0 + nnb..j0 + nnb + len, j + 1),
618 one(),
619 par,
620 );
621 for i in j0..j0 + len + nnb {
622 A[(i, j + 1)] = work2[i - j0].copy();
623 }
624 idx += 1;
625 }
626 }
627 }
628 let cola = n - jcol - nnb;
629 let j = n - nblst;
630 {
631 let (mut work2, _) = unsafe {
632 linalg::temp_mat_uninit::<T, _, _>(nblst, cola, stack)
633 };
634 let mut work2 = work2.as_mat_mut();
635 matmul(
636 work2.rb_mut(),
637 Accum::Replace,
638 work0.rb().adjoint(),
639 A.rb().get(j.., jcol + nnb..),
640 one(),
641 par,
642 );
643 A.rb_mut().get_mut(j.., jcol + nnb..).copy_from(&work2);
644 }
645 let mut j0 = j;
646 let mut idx = 0usize;
647 while j0 > jcol + nnb {
648 j0 -= nnb;
649 let U = work1.rb().submatrix(0, idx * 2 * nnb, 2 * nnb, 2 * nnb);
650 let (U11, U12, U21, U22) = U.split_at(nnb, nnb);
651 let (mut work2, _) = unsafe {
652 linalg::temp_mat_uninit::<T, _, _>(2 * nnb, cola, stack)
653 };
654 let mut work2 = work2.as_mat_mut();
655 let (mut work2_top, mut work2_bot) =
656 work2.rb_mut().split_at_row_mut(nnb);
657 utils::thread::join_raw(
658 |par| {
659 matmul(
660 work2_top.rb_mut(),
661 Accum::Replace,
662 U11.adjoint(),
663 A.rb().get(j0..j0 + nnb, jcol + nnb..),
664 one(),
665 par,
666 );
667 tr::matmul(
668 work2_top.rb_mut(),
669 tr::BlockStructure::Rectangular,
670 Accum::Add,
671 U21.adjoint(),
672 tr::BlockStructure::TriangularLower,
673 A.rb().get(j0 + nnb..j0 + 2 * nnb, jcol + nnb..),
674 tr::BlockStructure::Rectangular,
675 one(),
676 par,
677 );
678 },
679 |par| {
680 tr::matmul(
681 work2_bot.rb_mut(),
682 tr::BlockStructure::Rectangular,
683 Accum::Replace,
684 U12.adjoint(),
685 tr::BlockStructure::TriangularUpper,
686 A.rb().get(j0..j0 + nnb, jcol + nnb..),
687 tr::BlockStructure::Rectangular,
688 one(),
689 par,
690 );
691 matmul(
692 work2_bot.rb_mut(),
693 Accum::Add,
694 U22.adjoint(),
695 A.rb().get(j0 + nnb..j0 + 2 * nnb, jcol + nnb..),
696 one(),
697 par,
698 );
699 },
700 par,
701 );
702 A.rb_mut()
703 .get_mut(j0..j0 + 2 * nnb, jcol + nnb..)
704 .copy_from(&work2);
705 idx += 1;
706 }
707 if let Some(mut Q) = Q.rb_mut() {
708 let topq;
709 let nh;
710 if Q_is_I {
711 topq = Ord::max(1, j - jcol);
712 nh = n - topq;
713 } else {
714 topq = 0;
715 nh = n;
716 }
717 {
718 let (mut work2, _) = unsafe {
719 linalg::temp_mat_uninit::<T, _, _>(nh, nblst, stack)
720 };
721 let mut work2 = work2.as_mat_mut();
722 matmul(
723 work2.rb_mut(),
724 Accum::Replace,
725 Q.rb().get(topq.., j..),
726 work0.rb(),
727 one(),
728 par,
729 );
730 Q.rb_mut().get_mut(topq.., j..).copy_from(&work2);
731 }
732 let mut j0 = j;
733 let mut idx = 0usize;
734 while j0 > jcol + nnb {
735 j0 -= nnb;
736 let topq;
737 if Q_is_I {
738 topq = Ord::max(1, j0 - jcol);
739 } else {
740 topq = 0;
741 }
742 let U =
743 work1.rb().submatrix(0, idx * 2 * nnb, 2 * nnb, 2 * nnb);
744 apply_U_from_the_right(
745 Q.rb_mut().get_mut(topq.., j0..j0 + 2 * nnb),
746 U,
747 par,
748 stack,
749 );
750 idx += 1;
751 }
752 }
753 if Z.is_some() || top > 0 {
754 work0.fill(zero());
755 work0.rb_mut().diagonal_mut().fill(one());
756 work1.fill(zero());
757 for i in 0..n2nb {
758 work1
759 .rb_mut()
760 .subcols_mut(2 * nnb * i, 2 * nnb)
761 .diagonal_mut()
762 .column_vector_mut()
763 .fill(one());
764 }
765 for j in jcol..jcol + nnb {
766 let mut len = 2 + j - jcol;
767 let jrow = j + n2nb * nnb + 2;
768 for i in (jrow..n).rev() {
769 let ref c = A[(i, j)].real();
770 let ref s = B[(i, j)].copy();
771 A[(i, j)] = zero();
772 B[(i, j)] = zero();
773 let col = nblst - (n - i) - 1;
774 for jj in nblst - len..nblst {
775 let ref temp0 = work0[(jj, col)].copy();
776 let ref temp1 = work0[(jj, col + 1)].copy();
777 work0[(jj, col + 1)] =
778 temp1.mul_real(c) - temp0 * s.conj();
779 work0[(jj, col)] = temp0.mul_real(c) + temp1 * s;
780 }
781 len += 1;
782 }
783 let mut j0 = jrow;
784 let mut idx = 0usize;
785 while j0 > j + 2 {
786 j0 -= nnb;
787 let mut len = 2 + j - jcol;
788 let mut col = nnb + j - jcol;
789 let mut start = nnb;
790 let mut U = work1.rb_mut().submatrix_mut(
791 0,
792 idx * 2 * nnb,
793 2 * nnb,
794 2 * nnb,
795 );
796 for i in (j0..j0 + nnb).rev() {
797 let ref c = A[(i, j)].real();
798 let ref s = B[(i, j)].copy();
799 A[(i, j)] = zero();
800 B[(i, j)] = zero();
801 col -= 1;
802 start -= 1;
803 for jj in start..start + len {
804 let ref temp0 = U[(jj, col)].copy();
805 let ref temp1 = U[(jj, col + 1)].copy();
806 U[(jj, col + 1)] =
807 temp1.mul_real(c) - temp0 * s.conj();
808 U[(jj, col)] = temp0.mul_real(c) + temp1 * s;
809 }
810 len += 1;
811 }
812 idx += 1;
813 }
814 }
815 } else {
816 for k in 0..nnb {
822 zip!(A.rb_mut().get_mut(jcol + 2 + k.., jcol + k))
823 .for_each(|unzip!(x)| *x = zero());
824 zip!(B.rb_mut().get_mut(jcol + 2 + k.., jcol + k))
825 .for_each(|unzip!(x)| *x = zero());
826 }
827 }
828 if top > 0 {
829 for mut M in [A.rb_mut(), B.rb_mut()] {
830 let j = n - nblst;
831 {
832 let (mut work2, _) = unsafe {
833 linalg::temp_mat_uninit::<T, _, _>(top, nblst, stack)
834 };
835 let mut work2 = work2.as_mat_mut();
836 matmul(
837 work2.rb_mut(),
838 Accum::Replace,
839 M.rb().get(..top, j..),
840 work0.rb(),
841 one(),
842 par,
843 );
844 M.rb_mut().get_mut(..top, j..).copy_from(&work2);
845 }
846 let mut j0 = j;
847 let mut idx = 0usize;
848 while j0 > jcol + nnb {
849 j0 -= nnb;
850 let U = work1.rb().submatrix(
851 0,
852 idx * 2 * nnb,
853 2 * nnb,
854 2 * nnb,
855 );
856 apply_U_from_the_right(
857 M.rb_mut().get_mut(..top, j0..j0 + 2 * nnb),
858 U,
859 par,
860 stack,
861 );
862 idx += 1;
863 }
864 }
865 }
866 if let Some(mut Z) = Z.rb_mut() {
867 let topq;
868 let nh;
869 if Z_is_I {
870 topq = Ord::max(1, j - jcol);
871 nh = n - topq;
872 } else {
873 topq = 0;
874 nh = n;
875 }
876 {
877 let (mut work2, _) = unsafe {
878 linalg::temp_mat_uninit::<T, _, _>(nh, nblst, stack)
879 };
880 let mut work2 = work2.as_mat_mut();
881 matmul(
882 work2.rb_mut(),
883 Accum::Replace,
884 Z.rb().get(topq.., j..),
885 work0.rb(),
886 one(),
887 par,
888 );
889 Z.rb_mut().get_mut(topq.., j..).copy_from(&work2);
890 }
891 let mut j0 = j;
892 let mut idx = 0usize;
893 while j0 > jcol + nnb {
894 j0 -= nnb;
895 let topq;
896 if Z_is_I {
897 topq = Ord::max(1, j0 - jcol);
898 } else {
899 topq = 0;
900 }
901 let U =
902 work1.rb().submatrix(0, idx * 2 * nnb, 2 * nnb, 2 * nnb);
903 apply_U_from_the_right(
904 Z.rb_mut().get_mut(topq.., j0..j0 + 2 * nnb),
905 U,
906 par,
907 stack,
908 );
909 idx += 1;
910 }
911 }
912 jcol += nnb;
913 }
914}
915#[cfg(test)]
916mod tests {
917 use super::*;
918 use crate::{linalg, stats};
919 use dyn_stack::MemBuffer;
920 use equator::assert;
921 use stats::prelude::*;
922 #[test]
923 fn test_givens() {
924 let rng = &mut StdRng::seed_from_u64(0);
925 let rand = ComplexDistribution::new(StandardUniform, StandardUniform);
926 let mut sample = || -> c32 { rand.sample(rng) };
927 let shift = c32::new(0.5, 0.5);
928 let x = sample() - shift;
929 let y = sample() - shift;
930 let (c, s, r) = make_givens(x, y);
931 assert!((x * c + y * s - r).norm() < 1e-6);
932 assert!((-x * s.conj() + y * c).norm() < 1e-6);
933 }
934 #[test]
935 fn test_hessenberg() {
936 let rng = &mut StdRng::seed_from_u64(0);
937 for n in [12, 35, 128, 255] {
938 let rand = stats::CwiseMatDistribution {
939 nrows: n,
940 ncols: n,
941 dist: ComplexDistribution::new(StandardNormal, StandardNormal),
942 };
943 let mut sample = || -> Mat<c64> { rand.rand(rng) };
944 let A = sample();
945 let mut B = sample();
946 zip!(&mut B).for_each_triangular_lower(
947 linalg::zip::Diag::Skip,
948 |unzip!(x)| {
949 *x = 0.0.into();
950 },
951 );
952 let B = B;
953 let mut mem =
954 MemBuffer::new(generalized_hessenberg_scratch::<c64>(
955 n,
956 GeneralizedHessenbergParams {
957 block_size: 32,
958 ..auto!(c64)
959 },
960 ));
961 for gen_hessenberg in [
962 |A: MatMut<'_, _>,
963 B: MatMut<'_, _>,
964 Q: Option<MatMut<'_, _>>,
965 Z: Option<MatMut<'_, _>>,
966 _,
967 _,
968 par,
969 stack: &mut MemStack,
970 params| {
971 generalized_hessenberg(A, B, Q, Z, par, stack, params)
972 },
973 generalized_hessenberg_unblocked,
974 ] {
975 for (Q_is_I, Z_is_I) in
976 [(false, false), (true, true), (true, false), (false, true)]
977 {
978 let Q0 = &if Q_is_I {
979 Mat::identity(n, n)
980 } else {
981 UnitaryMat {
982 dim: n,
983 standard_normal: ComplexDistribution::new(
984 StandardNormal,
985 StandardNormal,
986 ),
987 }
988 .rand::<Mat<c64>>(rng)
989 };
990 let Z0 = &if Z_is_I {
991 Mat::identity(n, n)
992 } else {
993 UnitaryMat {
994 dim: n,
995 standard_normal: ComplexDistribution::new(
996 StandardNormal,
997 StandardNormal,
998 ),
999 }
1000 .rand::<Mat<c64>>(rng)
1001 };
1002 let mut Q = Q0.to_owned();
1003 let mut Z = Z0.to_owned();
1004 let mut H = A.clone();
1005 let mut T = B.clone();
1006 gen_hessenberg(
1007 H.as_mut(),
1008 T.as_mut(),
1009 Some(Q.as_mut()),
1010 Some(Z.as_mut()),
1011 Q_is_I,
1012 Z_is_I,
1013 Par::Seq,
1014 MemStack::new(&mut mem),
1015 GeneralizedHessenbergParams {
1016 block_size: 32,
1017 ..auto!(c64)
1018 },
1019 );
1020 Q = Q0.adjoint() * &Q;
1021 Z = Z0.adjoint() * &Z;
1022 assert!((&Q * &H * Z.adjoint() - &A).norm_max() < 1e-13);
1023 assert!((&Q * &T * Z.adjoint() - &B).norm_max() < 1e-13);
1024 for j in 0..n {
1025 for i in j + 1..n {
1026 assert!(T[(i, j)] == c64::ZERO);
1027 }
1028 for i in j + 2..n {
1029 assert!(H[(i, j)] == c64::ZERO);
1030 }
1031 }
1032 }
1033 }
1034 }
1035 }
1036}
1037#[cfg(test)]
1038mod make_givens_tests {
1039 use super::make_givens;
1040 use crate::c64;
1041 use crate::internal_prelude::*;
1042 use std::assert;
1043
1044 const EPS: f64 = f64::EPSILON;
1045 const TINY: f64 = 5e-324;
1047 const K_EPS: f64 = 4.0;
1050 const K_TINY: f64 = 4.0;
1051
1052 const MAGS: [f64; 22] = [
1055 TINY,
1056 3.0 * TINY,
1057 1e-320,
1058 1e-310,
1059 f64::MIN_POSITIVE,
1060 1e-305,
1061 1e-300,
1062 1e-200,
1063 1e-160,
1064 1.4916681462400413e-154, 1e-150,
1066 1e-30,
1067 0.7,
1068 1.0,
1069 1e30,
1070 1e150,
1071 6.703903964971299e153, 1e160,
1073 1e200,
1074 1e300,
1075 1e307,
1076 8e307,
1077 ];
1078
1079 fn upstream<T: ComplexField>(f: T, g: T) -> Option<(T::Real, T, T)> {
1083 if g == zero() {
1084 Some((one(), zero(), f))
1085 } else if f == zero() {
1086 let c = zero::<T::Real>();
1087 let d = g.abs();
1088 let d_inv = d.recip();
1089 let s = g.conj().mul_real(&d_inv);
1090 let r = d.to_cplx();
1091 (d.is_finite() && d_inv.is_finite()).then_some((c, s, r))
1092 } else {
1093 let rtmin = sqrt_min_positive::<T::Real>();
1094 let rtmax = sqrt_max_positive::<T::Real>();
1095 let f1 = f.abs();
1096 let g1 = g.abs();
1097 let h1 = f1.hypot(g1);
1098 let h1_inv = &h1.recip();
1099 let c = f1 * h1_inv;
1100 let r = f.mul_real(c.recip());
1101 let s = g.conj() * r.mul_real(h1_inv).mul_real(h1_inv);
1102 (h1 > rtmin && h1 < rtmax && c > rtmin).then_some((c, s, r))
1103 }
1104 }
1105
1106 fn scale(mut z: c64, mut k: i32) -> c64 {
1108 while k != 0 {
1109 let step = k.clamp(-1000, 1000);
1110 z *= 2.0f64.powi(step);
1111 k -= step;
1112 }
1113 z
1114 }
1115
1116 fn exponent_shift(x: f64) -> i32 {
1118 -(x.log2().floor() as i32)
1119 }
1120
1121 fn phase(z: c64) -> c64 {
1123 let z = scale(z, exponent_shift(z.norm()));
1124 z / z.norm()
1125 }
1126
1127 #[track_caller]
1132 fn check_cplx(f: c64, g: c64) {
1133 let (c, s, r) = make_givens(f, g);
1134 let ctx =
1135 format!("f = {f:e}, g = {g:e} -> c = {c:e}, s = {s:e}, r = {r:e}");
1136 assert!(
1137 c.is_finite()
1138 && s.re.is_finite()
1139 && s.im.is_finite()
1140 && r.re.is_finite()
1141 && r.im.is_finite(),
1142 "non-finite: {ctx}"
1143 );
1144 assert!(c >= 0.0, "{ctx}");
1145 let unit = c * c + s.norm_sqr();
1146 assert!(
1147 (unit - 1.0).abs() <= K_EPS * EPS,
1148 "not unitary ({:e}): {ctx}",
1149 unit - 1.0
1150 );
1151
1152 let c0 = c64::new(0.0, 0.0);
1153 if g == c0 {
1154 assert!(c == 1.0 && s == c0 && r == f, "{ctx}");
1155 return;
1156 }
1157 let m = f.norm().max(g.norm());
1158 let k = exponent_shift(m);
1159 let (fk, gk) = (scale(f, k), scale(g, k));
1160 let h = fk.norm().hypot(gk.norm());
1161 let sgn = if f == c0 {
1162 c64::new(1.0, 0.0)
1163 } else {
1164 phase(f)
1165 };
1166 let c_ref = fk.norm() / h;
1167 let s_ref = sgn * gk.conj() / h;
1168 let r_ref = sgn * h;
1169 let floor = K_TINY * TINY;
1170 assert!((c - c_ref).abs() <= K_EPS * EPS * c_ref + floor, "c: {ctx}");
1171 assert!((s - s_ref).norm() <= K_EPS * EPS + floor, "s: {ctx}");
1172 let r_floor = scale(c64::new(floor, 0.0), k).re;
1174 assert!(
1175 (scale(r, k) - r_ref).norm() <= K_EPS * EPS * h + r_floor,
1176 "r: {ctx}"
1177 );
1178
1179 let tol = 2.0 * K_EPS * EPS * m + floor;
1181 assert!((f * c + s * g - r).norm() <= tol, "top: {ctx}");
1182 assert!((g * c - s.conj() * f).norm() <= tol, "bottom: {ctx}");
1183
1184 if let Some((cu, su, ru)) = upstream(f, g) {
1185 assert!(
1186 c.to_bits() == cu.to_bits()
1187 && s.re.to_bits() == su.re.to_bits()
1188 && s.im.to_bits() == su.im.to_bits()
1189 && r.re.to_bits() == ru.re.to_bits()
1190 && r.im.to_bits() == ru.im.to_bits(),
1191 "differs from upstream ({cu:e}, {su:e}, {ru:e}): {ctx}"
1192 );
1193 }
1194 }
1195
1196 #[track_caller]
1198 fn check_real(f: f64, g: f64) {
1199 let (c, s, r) = make_givens(f, g);
1200 let ctx =
1201 format!("f = {f:e}, g = {g:e} -> c = {c:e}, s = {s:e}, r = {r:e}");
1202 assert!(c.is_finite() && s.is_finite() && r.is_finite(), "{ctx}");
1203 assert!(c >= 0.0, "{ctx}");
1204 let unit = c * c + s * s;
1205 assert!((unit - 1.0).abs() <= K_EPS * EPS, "not unitary: {ctx}");
1206 if g == 0.0 {
1207 assert!(c == 1.0 && s == 0.0 && r == f, "{ctx}");
1208 return;
1209 }
1210 let m = f.abs().max(g.abs());
1211 let k = exponent_shift(m);
1212 let fk = scale(c64::new(f, 0.0), k).re;
1213 let gk = scale(c64::new(g, 0.0), k).re;
1214 let h = fk.hypot(gk);
1215 let sgn = if f == 0.0 { 1.0 } else { f.signum() };
1216 let floor = K_TINY * TINY;
1217 let c_ref = fk.abs() / h;
1218 assert!((c - c_ref).abs() <= K_EPS * EPS * c_ref + floor, "c: {ctx}");
1219 assert!((s - sgn * gk / h).abs() <= K_EPS * EPS + floor, "s: {ctx}");
1220 let r_floor = scale(c64::new(floor, 0.0), k).re;
1221 let rk = scale(c64::new(r, 0.0), k).re;
1222 assert!(
1223 (rk - sgn * h).abs() <= K_EPS * EPS * h + r_floor,
1224 "r: {ctx}"
1225 );
1226 let tol = 2.0 * K_EPS * EPS * m + floor;
1227 assert!((f * c + s * g - r).abs() <= tol, "top: {ctx}");
1228 assert!((g * c - s * f).abs() <= tol, "bottom: {ctx}");
1229 if let Some((cu, su, ru)) = upstream(f, g) {
1230 assert!(
1231 c.to_bits() == cu.to_bits()
1232 && s.to_bits() == su.to_bits()
1233 && r.to_bits() == ru.to_bits(),
1234 "differs from upstream: {ctx}"
1235 );
1236 }
1237 }
1238
1239 #[test]
1249 fn givens_is_finite_and_unitary_across_the_exponent_range() {
1250 let phases = [
1251 c64::new(1.0, 0.0),
1252 c64::new(0.0, 1.0),
1253 c64::new(-1.0, 0.0),
1254 c64::new(0.6, 0.8),
1255 c64::new(-0.28, -0.96),
1256 c64::new(0.3, -0.4),
1257 ];
1258 let c0 = c64::new(0.0, 0.0);
1259 for &mf in &MAGS {
1260 for &pf in &phases {
1261 let f = pf * mf;
1262 check_cplx(f, c0);
1263 check_cplx(c0, f);
1264 for &mg in &MAGS {
1265 for &pg in &phases {
1266 check_cplx(f, pg * mg);
1267 }
1268 }
1269 }
1270 for sf in [1.0, -1.0] {
1271 check_real(sf * mf, 0.0);
1272 check_real(0.0, sf * mf);
1273 for &mg in &MAGS {
1274 for sg in [1.0, -1.0] {
1275 check_real(sf * mf, sg * mg);
1276 }
1277 }
1278 }
1279 }
1280 check_cplx(c0, c0);
1281 check_real(0.0, 0.0);
1282
1283 check_cplx(c64::new(3e-301, 4e-301), c64::new(1e15, 0.0));
1285 check_cplx(c64::new(3e-301, 4e-301), c64::new(-2e19, 1e20));
1286 }
1287
1288 #[test]
1289 fn givens_terminates_on_non_finite_input() {
1290 for bad in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] {
1291 for (f, g) in [
1292 (c64::new(bad, 0.0), c64::new(1.0, 0.0)),
1293 (c64::new(1.0, 0.0), c64::new(0.0, bad)),
1294 (c64::new(0.0, 0.0), c64::new(bad, 1.0)),
1295 ] {
1296 let _ = make_givens(f, g);
1297 }
1298 let _ = make_givens(bad, 1.0);
1299 let _ = make_givens(1.0, bad);
1300 let _ = make_givens(0.0, bad);
1301 }
1302 }
1303}