flight_solver/
givens.rs

1//! Givens rotation primitives for numerically stable QR updates.
2//!
3//! Shared across all solvers in this crate: used by the inverse QR-RLS for
4//! information-matrix updates, and by the WLS active-set solver for
5//! incremental QR maintenance.
6//!
7//! The implementation uses [`libm::hypotf`] for the 2-norm computation,
8//! which avoids overflow and underflow in the intermediate `a² + b²` term.
9
10use nalgebra::{Const, DimName, OMatrix};
11
12/// Compute Givens rotation parameters `(c, s)` that zero the second element.
13///
14/// Returns `(c, s)` such that the rotation matrix
15///
16/// ```text
17/// G = [ c  -s ]
18///     [ s   c ]
19/// ```
20///
21/// applied to the column vector `[a, b]ᵀ` yields `[r, 0]ᵀ`
22/// where `r = √(a² + b²)`.
23///
24/// When both `a` and `b` are zero, returns `(1, 0)` (identity rotation).
25#[inline]
26pub fn givens(a: f32, b: f32) -> (f32, f32) {
27    let h = libm::hypotf(a, b);
28    if h == 0.0 {
29        return (1.0, 0.0);
30    }
31    let inv_h = 1.0 / h;
32    (a * inv_h, -(b * inv_h))
33}
34
35/// Apply a Givens rotation from the left to two rows of a matrix.
36///
37/// For each column `k` in `0..n_cols`:
38///
39/// ```text
40/// r[row1, k] ← c · r[row1, k] − s · r[row2, k]
41/// r[row2, k] ← s · r[row1, k] + c · r[row2, k]
42/// ```
43///
44/// This is equivalent to left-multiplying by the Givens matrix `G(row1, row2, c, s)`.
45#[inline]
46pub fn givens_left_apply<const R: usize, const C: usize>(
47    r: &mut OMatrix<f32, Const<R>, Const<C>>,
48    c: f32,
49    s: f32,
50    row1: usize,
51    row2: usize,
52    n_cols: usize,
53) where
54    Const<R>: DimName,
55    Const<C>: DimName,
56{
57    for col in 0..n_cols {
58        let r1 = r[(row1, col)];
59        let r2 = r[(row2, col)];
60        r[(row1, col)] = c * r1 - s * r2;
61        r[(row2, col)] = s * r1 + c * r2;
62    }
63}
64
65/// Apply a Givens rotation transpose from the right to two columns of a matrix.
66///
67/// For each row `i` in `0..n_rows`:
68///
69/// ```text
70/// q[i, col1] ← c · q[i, col1] − s · q[i, col2]
71/// q[i, col2] ← s · q[i, col1] + c · q[i, col2]
72/// ```
73///
74/// This is equivalent to right-multiplying by `Gᵀ(col1, col2, c, s)`.
75#[inline]
76pub fn givens_right_apply_t<const R: usize, const C: usize>(
77    q: &mut OMatrix<f32, Const<R>, Const<C>>,
78    c: f32,
79    s: f32,
80    col1: usize,
81    col2: usize,
82    n_rows: usize,
83) where
84    Const<R>: DimName,
85    Const<C>: DimName,
86{
87    for i in 0..n_rows {
88        let q1 = q[(i, col1)];
89        let q2 = q[(i, col2)];
90        q[(i, col1)] = c * q1 - s * q2;
91        q[(i, col2)] = s * q1 + c * q2;
92    }
93}
flight_solver/givens.rs

flight_solver/
givens.rs
