astrodynamics 0.14.0

Numerical astrodynamics engine for orbit propagation, force models, and flight-dynamics primitives
Documentation
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
//! Deterministic small linear-algebra kernels.
//!
//! These routines keep scalar operation order explicit for parity-sensitive
//! GNSS callers. When pivot tie-breaking or accumulation order matters, the
//! variant name states the policy instead of hiding it in a local copy.

use crate::tolerances::PIVOT_EPSILON;

#[derive(Debug, Default, Clone)]
pub struct FlatLinearScratch {
    rows: Vec<f64>,
    x: Vec<f64>,
}

#[derive(Debug, Default, Clone)]
pub struct FlatNormalSolveScratch {
    a: Vec<f64>,
    b: Vec<f64>,
    x: Vec<f64>,
}

#[allow(clippy::needless_range_loop)]
pub fn solve_linear_first_tie(a: &[Vec<f64>], b: &[f64]) -> Option<Vec<f64>> {
    let n = b.len();
    let mut rows: Vec<Vec<f64>> = a
        .iter()
        .zip(b)
        .map(|(row, &bi)| {
            let mut r = row.clone();
            r.push(bi);
            r
        })
        .collect();

    for col in 0..n {
        let mut pivot_row = col;
        let mut pivot_abs = rows[col][col].abs();
        for idx in (col + 1)..n {
            let v = rows[idx][col].abs();
            if v > pivot_abs {
                pivot_abs = v;
                pivot_row = idx;
            }
        }
        if pivot_abs <= PIVOT_EPSILON {
            return None;
        }
        rows.swap(col, pivot_row);

        let pivot = rows[col].clone();
        let pivot_value = pivot[col];
        for idx in (col + 1)..n {
            let factor = rows[idx][col] / pivot_value;
            for j in 0..=n {
                rows[idx][j] -= factor * pivot[j];
            }
        }
    }

    let mut x = vec![0.0; n];
    for i in (0..n).rev() {
        let mut known = 0.0;
        for j in (i + 1)..n {
            known += rows[i][j] * x[j];
        }
        x[i] = (rows[i][n] - known) / rows[i][i];
    }
    Some(x)
}

#[allow(clippy::needless_range_loop)]
pub fn solve_linear_last_tie(mut a: Vec<Vec<f64>>, b: Vec<f64>) -> Option<Vec<f64>> {
    let n = b.len();
    for (row, bi) in a.iter_mut().zip(b) {
        row.push(bi);
    }
    for col in 0..n {
        let (pivot_row, pivot_abs) = (col..n)
            .map(|idx| (idx, a[idx][col].abs()))
            .max_by(|lhs, rhs| lhs.1.total_cmp(&rhs.1))
            .unwrap();
        if pivot_abs <= PIVOT_EPSILON {
            return None;
        }
        a.swap(col, pivot_row);
        let pivot = a[col].clone();
        let pivot_value = pivot[col];
        for row in a.iter_mut().take(n).skip(col + 1) {
            let factor = row[col] / pivot_value;
            for j in col..=n {
                row[j] -= factor * pivot[j];
            }
        }
    }
    let mut x = vec![0.0; n];
    for i in (0..n).rev() {
        let tail_sum: f64 = ((i + 1)..n).map(|j| a[i][j] * x[j]).sum();
        x[i] = (a[i][n] - tail_sum) / a[i][i];
    }
    Some(x)
}

pub fn invert_matrix_first_tie(a: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let n = a.len();
    let mut columns: Vec<Vec<f64>> = Vec::with_capacity(n);
    for col in 0..n {
        let mut e = vec![0.0; n];
        e[col] = 1.0;
        columns.push(solve_linear_first_tie(a, &e)?);
    }
    Some(
        (0..n)
            .map(|i| (0..n).map(|j| columns[j][i]).collect())
            .collect(),
    )
}

pub fn invert_matrix_last_tie(a: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let n = a.len();
    let mut columns = Vec::with_capacity(n);
    for col in 0..n {
        let unit = (0..n)
            .map(|idx| if idx == col { 1.0 } else { 0.0 })
            .collect();
        columns.push(solve_linear_last_tie(a.to_vec(), unit)?);
    }
    Some(transpose(&columns))
}

pub fn solve_matrix_last_tie(a: &[Vec<f64>], b: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let columns = transpose(b);
    let mut solved_columns = Vec::with_capacity(columns.len());
    for col in columns {
        solved_columns.push(solve_linear_last_tie(a.to_vec(), col)?);
    }
    Some(transpose(&solved_columns))
}

pub fn normal_equations_weighted<'a, I>(rows: I, n: usize) -> (Vec<Vec<f64>>, Vec<f64>)
where
    I: IntoIterator<Item = (&'a [f64], f64, f64)>,
{
    let mut ata = vec![vec![0.0; n]; n];
    let mut aty = vec![0.0; n];
    for (row_h, row_y, row_weight) in rows {
        let h: Vec<f64> = row_h.iter().map(|v| v * row_weight).collect();
        let y = row_y * row_weight;
        for i in 0..n {
            aty[i] += h[i] * y;
            for j in 0..n {
                ata[i][j] += h[i] * h[j];
            }
        }
    }
    (ata, aty)
}

pub fn matrix_sub(a: &[Vec<f64>], b: &[Vec<f64>]) -> Vec<Vec<f64>> {
    a.iter()
        .zip(b)
        .map(|(row_a, row_b)| row_a.iter().zip(row_b).map(|(x, y)| x - y).collect())
        .collect()
}

pub fn matmul(a: &[Vec<f64>], b: &[Vec<f64>]) -> Vec<Vec<f64>> {
    let b_t = transpose(b);
    a.iter()
        .map(|row| {
            b_t.iter()
                .map(|col| row.iter().zip(col).fold(0.0, |acc, (x, y)| acc + x * y))
                .collect()
        })
        .collect()
}

pub fn transpose(matrix: &[Vec<f64>]) -> Vec<Vec<f64>> {
    if matrix.is_empty() {
        return Vec::new();
    }
    (0..matrix[0].len())
        .map(|col| matrix.iter().map(|row| row[col]).collect())
        .collect()
}

pub fn invert_flat_first_tie_into(
    a: &[f64],
    n: usize,
    out: &mut Vec<f64>,
    scratch: &mut FlatLinearScratch,
) -> Option<()> {
    out.resize(n * n, 0.0);
    scratch.rows.resize(n * (n + 1), 0.0);
    scratch.x.resize(n, 0.0);

    for col in 0..n {
        for i in 0..n {
            let src = i * n;
            let dst = i * (n + 1);
            scratch.rows[dst..(dst + n)].copy_from_slice(&a[src..(src + n)]);
            scratch.rows[dst + n] = if i == col { 1.0 } else { 0.0 };
        }
        solve_augmented_flat_first_tie_in_place(&mut scratch.rows, n, &mut scratch.x)?;
        for i in 0..n {
            out[i * n + col] = scratch.x[i];
        }
    }

    Some(())
}

pub fn solve_matrix_flat_first_tie_into(
    a: &[f64],
    n: usize,
    b: &[f64],
    cols: usize,
    out: &mut Vec<f64>,
    scratch: &mut FlatLinearScratch,
) -> Option<()> {
    out.resize(n * cols, 0.0);
    scratch.rows.resize(n * (n + 1), 0.0);
    scratch.x.resize(n, 0.0);

    for col in 0..cols {
        for i in 0..n {
            let src = i * n;
            let dst = i * (n + 1);
            scratch.rows[dst..(dst + n)].copy_from_slice(&a[src..(src + n)]);
            scratch.rows[dst + n] = b[i * cols + col];
        }
        solve_augmented_flat_first_tie_in_place(&mut scratch.rows, n, &mut scratch.x)?;
        for i in 0..n {
            out[i * cols + col] = scratch.x[i];
        }
    }
    Some(())
}

#[allow(clippy::needless_range_loop)]
pub fn solve_augmented_flat_first_tie_in_place(
    rows: &mut [f64],
    n: usize,
    x: &mut [f64],
) -> Option<()> {
    let stride = n + 1;

    for col in 0..n {
        let mut pivot_row = col;
        let mut pivot_abs = rows[col * stride + col].abs();
        for idx in (col + 1)..n {
            let v = rows[idx * stride + col].abs();
            if v > pivot_abs {
                pivot_abs = v;
                pivot_row = idx;
            }
        }
        if pivot_abs <= PIVOT_EPSILON {
            return None;
        }
        if pivot_row != col {
            for j in 0..=n {
                rows.swap(col * stride + j, pivot_row * stride + j);
            }
        }

        let pivot_value = rows[col * stride + col];
        for idx in (col + 1)..n {
            let factor = rows[idx * stride + col] / pivot_value;
            for j in 0..=n {
                rows[idx * stride + j] -= factor * rows[col * stride + j];
            }
        }
    }

    for i in (0..n).rev() {
        let mut known = 0.0;
        for j in (i + 1)..n {
            known += rows[i * stride + j] * x[j];
        }
        x[i] = (rows[i * stride + n] - known) / rows[i * stride + i];
    }

    Some(())
}

pub fn solve_flat_normal_first_tie(lambda: &[f64], eta: &[f64]) -> Option<Vec<f64>> {
    let mut scratch = FlatNormalSolveScratch::default();
    solve_flat_normal_first_tie_into(lambda, eta, &mut scratch).map(|x| x.to_vec())
}

#[allow(clippy::needless_range_loop)]
pub fn solve_flat_normal_first_tie_into<'a>(
    lambda: &[f64],
    eta: &[f64],
    scratch: &'a mut FlatNormalSolveScratch,
) -> Option<&'a [f64]> {
    let n = eta.len();
    if lambda.len() != n * n {
        return None;
    }

    scratch.a.resize(n * n, 0.0);
    scratch.a.copy_from_slice(lambda);
    scratch.b.resize(n, 0.0);
    scratch.b.copy_from_slice(eta);

    for k in 0..n {
        let mut pivot = k;
        let mut pivot_abs = scratch.a[k * n + k].abs();
        for i in (k + 1)..n {
            let candidate = scratch.a[i * n + k].abs();
            if candidate > pivot_abs {
                pivot = i;
                pivot_abs = candidate;
            }
        }
        if pivot_abs <= PIVOT_EPSILON {
            return None;
        }
        if pivot != k {
            for j in 0..n {
                scratch.a.swap(k * n + j, pivot * n + j);
            }
            scratch.b.swap(k, pivot);
        }

        let diag = scratch.a[k * n + k];
        for i in (k + 1)..n {
            let factor = scratch.a[i * n + k] / diag;
            scratch.a[i * n + k] = 0.0;
            for j in (k + 1)..n {
                scratch.a[i * n + j] -= factor * scratch.a[k * n + j];
            }
            scratch.b[i] -= factor * scratch.b[k];
        }
    }

    scratch.x.resize(n, 0.0);
    for i in (0..n).rev() {
        let mut known = 0.0;
        for j in (i + 1)..n {
            known += scratch.a[i * n + j] * scratch.x[j];
        }
        scratch.x[i] = (scratch.b[i] - known) / scratch.a[i * n + i];
    }
    Some(&scratch.x)
}

/// Reusable buffers for the owned Cholesky (square-root) solve
/// ([`solve_flat_normal_square_root_into`]): the lower-triangular factor `L`
/// (row-major `n x n`), the forward-substitution vector `z`, and the solution
/// `x`. Held across solves so a steady-state iteration does not allocate.
#[derive(Debug, Default, Clone)]
pub struct FlatCholeskySolveScratch {
    l: Vec<f64>,
    z: Vec<f64>,
    x: Vec<f64>,
}

/// Solve the symmetric positive-definite information system `Λ x = η` by an owned
/// deterministic Cholesky (square-root) factorization `Λ = L Lᵀ`, then forward
/// substitution `L z = η` and back substitution `Lᵀ x = z`. `lambda` is the
/// row-major `n x n` information matrix, `eta` the length-`n` information vector.
///
/// The Cholesky factor `L` is the information-matrix square root, so this is the
/// square-root-information solve. Unlike the general first-tie Gaussian
/// elimination ([`solve_flat_normal_first_tie_into`]) it needs no pivoting: the
/// system is SPD, so the fixed `i`/`j`/`k` reduction order (identical to
/// [`invert_symmetric_pd`]) is the entire op-order and the result is
/// bit-reproducible with no pivot-dependent branching. Returns `None` if `Λ` is
/// not positive definite (a non-positive or non-finite pivot), which for a
/// weighted least-squares normal matrix means rank-deficient geometry.
#[allow(clippy::needless_range_loop)]
pub fn solve_flat_normal_square_root_into<'a>(
    lambda: &[f64],
    eta: &[f64],
    scratch: &'a mut FlatCholeskySolveScratch,
) -> Option<&'a [f64]> {
    let n = eta.len();
    if lambda.len() != n * n {
        return None;
    }
    scratch.l.resize(n * n, 0.0);
    scratch.l.fill(0.0);

    // Cholesky Λ = L Lᵀ, the same factorization order as `invert_symmetric_pd`.
    for i in 0..n {
        for j in 0..=i {
            let mut s = lambda[i * n + j];
            for k in 0..j {
                s -= scratch.l[i * n + k] * scratch.l[j * n + k];
            }
            if i == j {
                #[allow(clippy::neg_cmp_op_on_partial_ord)]
                let nonpositive_or_nan = !(s > 0.0);
                if nonpositive_or_nan || !s.is_finite() {
                    return None;
                }
                scratch.l[i * n + j] = s.sqrt();
            } else {
                scratch.l[i * n + j] = s / scratch.l[j * n + j];
            }
        }
    }

    // Forward substitution L z = η.
    scratch.z.resize(n, 0.0);
    for i in 0..n {
        let mut s = eta[i];
        for k in 0..i {
            s -= scratch.l[i * n + k] * scratch.z[k];
        }
        scratch.z[i] = s / scratch.l[i * n + i];
    }

    // Back substitution Lᵀ x = z.
    scratch.x.resize(n, 0.0);
    for i in (0..n).rev() {
        let mut s = scratch.z[i];
        for k in (i + 1)..n {
            s -= scratch.l[k * n + i] * scratch.x[k];
        }
        scratch.x[i] = s / scratch.l[i * n + i];
    }
    Some(scratch.x.as_slice())
}

#[allow(clippy::needless_range_loop)]
pub fn normal_matrix_4_weighted_column_outer(rows: &[[f64; 4]], weights: &[f64]) -> [[f64; 4]; 4] {
    let mut a = [[0.0_f64; 4]; 4];
    for i in 0..4 {
        for j in 0..4 {
            let mut s = 0.0_f64;
            for k in 0..rows.len() {
                s += rows[k][i] * weights[k] * rows[k][j];
            }
            a[i][j] = s;
        }
    }
    a
}

#[allow(clippy::needless_range_loop)]
pub fn normal_matrix_4_unweighted_row_outer(rows: &[[f64; 4]]) -> [[f64; 4]; 4] {
    let mut a = [[0.0_f64; 4]; 4];
    for row in rows {
        for i in 0..4 {
            for j in 0..4 {
                a[i][j] += row[i] * row[j];
            }
        }
    }
    a
}

pub fn mat4_vec4(m: &[[f64; 4]; 4], v: &[f64; 4]) -> [f64; 4] {
    [
        dot4(&m[0], v),
        dot4(&m[1], v),
        dot4(&m[2], v),
        dot4(&m[3], v),
    ]
}

pub fn dot4(row: &[f64; 4], v: &[f64; 4]) -> f64 {
    row[0] * v[0] + row[1] * v[1] + row[2] * v[2] + row[3] * v[3]
}

pub fn det4_cofactor(a: &[[f64; 4]; 4]) -> f64 {
    let m01 = a[2][0] * a[3][1] - a[2][1] * a[3][0];
    let m02 = a[2][0] * a[3][2] - a[2][2] * a[3][0];
    let m03 = a[2][0] * a[3][3] - a[2][3] * a[3][0];
    let m12 = a[2][1] * a[3][2] - a[2][2] * a[3][1];
    let m13 = a[2][1] * a[3][3] - a[2][3] * a[3][1];
    let m23 = a[2][2] * a[3][3] - a[2][3] * a[3][2];

    let c0 = a[1][1] * m23 - a[1][2] * m13 + a[1][3] * m12;
    let c1 = a[1][0] * m23 - a[1][2] * m03 + a[1][3] * m02;
    let c2 = a[1][0] * m13 - a[1][1] * m03 + a[1][3] * m01;
    let c3 = a[1][0] * m12 - a[1][1] * m02 + a[1][2] * m01;

    a[0][0] * c0 - a[0][1] * c1 + a[0][2] * c2 - a[0][3] * c3
}

pub fn minor3_of_4(a: &[[f64; 4]; 4], skip_r: usize, skip_c: usize) -> f64 {
    let mut rows = [0_usize; 3];
    let mut cols = [0_usize; 3];
    let mut row_idx = 0;
    let mut col_idx = 0;
    for row in 0..4 {
        if row != skip_r {
            rows[row_idx] = row;
            row_idx += 1;
        }
    }
    for col in 0..4 {
        if col != skip_c {
            cols[col_idx] = col;
            col_idx += 1;
        }
    }

    let b00 = a[rows[0]][cols[0]];
    let b01 = a[rows[0]][cols[1]];
    let b02 = a[rows[0]][cols[2]];
    let b10 = a[rows[1]][cols[0]];
    let b11 = a[rows[1]][cols[1]];
    let b12 = a[rows[1]][cols[2]];
    let b20 = a[rows[2]][cols[0]];
    let b21 = a[rows[2]][cols[1]];
    let b22 = a[rows[2]][cols[2]];

    b00 * (b11 * b22 - b12 * b21) - b01 * (b10 * b22 - b12 * b20) + b02 * (b10 * b21 - b11 * b20)
}

#[allow(clippy::needless_range_loop)]
pub fn invert_4x4_cofactor(a: &[[f64; 4]; 4]) -> Option<[[f64; 4]; 4]> {
    let det = det4_cofactor(a);
    if det == 0.0 {
        return None;
    }

    let mut inv = [[0.0_f64; 4]; 4];
    for j in 0..4 {
        for i in 0..4 {
            let sign = if (i + j) % 2 == 0 { 1.0 } else { -1.0 };
            inv[j][i] = sign * minor3_of_4(a, i, j) / det;
        }
    }
    Some(inv)
}

pub fn invert_3x3_adjugate(m: &[[f64; 3]; 3]) -> Option<[[f64; 3]; 3]> {
    let [[a, b, c], [d, e, f], [g, h, i]] = *m;
    let det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g);
    if det.abs() <= PIVOT_EPSILON {
        return None;
    }
    let inv = 1.0 / det;
    Some([
        [
            (e * i - f * h) * inv,
            (c * h - b * i) * inv,
            (b * f - c * e) * inv,
        ],
        [
            (f * g - d * i) * inv,
            (a * i - c * g) * inv,
            (c * d - a * f) * inv,
        ],
        [
            (d * h - e * g) * inv,
            (b * g - a * h) * inv,
            (a * e - b * d) * inv,
        ],
    ])
}

#[allow(clippy::needless_range_loop)]
pub fn invert_symmetric_pd(n: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let p = n.len();
    let mut l = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        for j in 0..=i {
            let mut s = n[i][j];
            for k in 0..j {
                s -= l[i][k] * l[j][k];
            }
            if i == j {
                #[allow(clippy::neg_cmp_op_on_partial_ord)]
                let nonpositive_or_nan = !(s > 0.0);
                if nonpositive_or_nan || !s.is_finite() {
                    return None;
                }
                l[i][j] = s.sqrt();
            } else {
                l[i][j] = s / l[j][j];
            }
        }
    }

    let mut li = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        li[i][i] = 1.0 / l[i][i];
        for j in 0..i {
            let mut s = 0.0_f64;
            for k in j..i {
                s -= l[i][k] * li[k][j];
            }
            li[i][j] = s / l[i][i];
        }
    }

    let mut inv = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        for j in 0..p {
            let mut s = 0.0_f64;
            for k in 0..p {
                s += li[k][i] * li[k][j];
            }
            inv[i][j] = s;
        }
    }
    Some(inv)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn first_tie_solver_inverts_known_matrix() {
        let a = vec![vec![4.0, 7.0], vec![2.0, 6.0]];
        let inv = invert_matrix_first_tie(&a).unwrap();
        assert_eq!(inv[0][0].to_bits(), 0.6000000000000001f64.to_bits());
        assert_eq!(inv[0][1].to_bits(), (-0.7000000000000001f64).to_bits());
        assert_eq!(inv[1][0].to_bits(), (-0.2f64).to_bits());
        assert_eq!(inv[1][1].to_bits(), 0.4f64.to_bits());
    }

    #[test]
    fn flat_normal_solver_reports_singular() {
        assert!(solve_flat_normal_first_tie(&[1.0, 2.0, 2.0, 4.0], &[1.0, 2.0]).is_none());
    }

    #[test]
    fn cofactor_inverse_rejects_singular_4x4() {
        let a = [[0.0; 4]; 4];
        assert!(invert_4x4_cofactor(&a).is_none());
    }

    #[test]
    fn cholesky_square_root_solves_spd_system() {
        // Λ = [[4, 12, -16], [12, 37, -43], [-16, -43, 98]] (the classic SPD
        // Cholesky example), η chosen so the exact solution is [1, 2, 3].
        let lambda = [
            4.0, 12.0, -16.0, //
            12.0, 37.0, -43.0, //
            -16.0, -43.0, 98.0,
        ];
        let eta = [
            4.0 * 1.0 + 12.0 * 2.0 - 16.0 * 3.0,
            12.0 * 1.0 + 37.0 * 2.0 - 43.0 * 3.0,
            -16.0 * 1.0 - 43.0 * 2.0 + 98.0 * 3.0,
        ];
        let mut scratch = FlatCholeskySolveScratch::default();
        let x = solve_flat_normal_square_root_into(&lambda, &eta, &mut scratch).unwrap();
        for (got, want) in x.iter().zip([1.0_f64, 2.0, 3.0]) {
            assert!((got - want).abs() < 1.0e-12, "got {got}, want {want}");
        }
    }

    #[test]
    fn cholesky_square_root_agrees_with_first_tie_to_roundoff() {
        // The square-root solve and the first-tie Gaussian solve of the same SPD
        // system must agree to roundoff: they differ only in factorization order.
        let lambda = [
            6.0, 2.0, 1.0, //
            2.0, 5.0, 2.0, //
            1.0, 2.0, 4.0,
        ];
        let eta = [9.0, 9.0, 7.0];
        let mut sqrt_scratch = FlatCholeskySolveScratch::default();
        let sqrt_x = solve_flat_normal_square_root_into(&lambda, &eta, &mut sqrt_scratch)
            .unwrap()
            .to_vec();
        let first_tie_x = solve_flat_normal_first_tie(&lambda, &eta).unwrap();
        for (s, f) in sqrt_x.iter().zip(&first_tie_x) {
            assert!((s - f).abs() < 1.0e-12, "square-root {s} vs first-tie {f}");
        }
    }

    #[test]
    fn cholesky_square_root_frozen_bits() {
        // Frozen-bits golden on an exactly-representable SPD system
        // (Λ = L Lᵀ with L = [[2,0,0],[1,2,0],[0,0,1]]), so every factor and
        // substitution step is exact in f64 and the solution bits are a portable
        // constant: f64 sqrt is IEEE-754 correctly rounded, so these bits hold
        // across platforms, not merely run-to-run on one build.
        let lambda = [
            4.0, 2.0, 0.0, //
            2.0, 5.0, 0.0, //
            0.0, 0.0, 1.0,
        ];
        // η = Λ·[2, 0.5, 3].
        let eta = [9.0, 6.5, 3.0];
        let mut scratch = FlatCholeskySolveScratch::default();
        let x = solve_flat_normal_square_root_into(&lambda, &eta, &mut scratch).unwrap();
        assert_eq!(x[0].to_bits(), 2.0f64.to_bits());
        assert_eq!(x[1].to_bits(), 0.5f64.to_bits());
        assert_eq!(x[2].to_bits(), 3.0f64.to_bits());
    }

    #[test]
    fn cholesky_square_root_rejects_non_pd() {
        // A singular (rank-deficient) matrix has a non-positive Cholesky pivot.
        assert!(solve_flat_normal_square_root_into(
            &[1.0, 2.0, 2.0, 4.0],
            &[1.0, 2.0],
            &mut Default::default()
        )
        .is_none());
    }
}