Skip to content

Commit 7f82a6a

Browse files
authored
Add a few more uses of nalgebra (#132)
This PR updates the Reference frame module to use the `nalgebra` crate for more of the math.
1 parent 5e5f2ec commit 7f82a6a

File tree

3 files changed

+269
-560
lines changed

3 files changed

+269
-560
lines changed

swiftnav/src/math.rs

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ pub(crate) const fn compile_time_max_u16(a: u16, b: u16) -> u16 {
2929
///
3030
/// # Panics
3131
///
32+
/// - This function will panic if the given number is NOT between 0.0 and 1.0.
3233
/// - This function will panic if the computation does not converge within 100 iterations.
3334
///
3435
/// # Notes
@@ -37,6 +38,11 @@ pub(crate) const fn compile_time_max_u16(a: u16, b: u16) -> u16 {
3738
/// - The algorithm iteratively refines the approximation of the square root until the result stabilizes.
3839
#[expect(clippy::many_single_char_names, reason = "It's math, whatyagonnado?")]
3940
pub(crate) const fn compile_time_sqrt(s: f64) -> f64 {
41+
assert!(
42+
s >= 0.0 && s <= 1.0,
43+
"Can only compute square root of numbers between 0 and 1"
44+
);
45+
4046
let mut x = s;
4147
let mut y = 0.0_f64;
4248
let mut z;
@@ -71,3 +77,22 @@ pub(crate) fn ecef2ned_matrix(llh: crate::coords::LLHRadians) -> nalgebra::Matri
7177
-sin_lat,
7278
)
7379
}
80+
81+
#[cfg(test)]
82+
mod tests {
83+
use super::*;
84+
use float_eq::assert_float_eq;
85+
use proptest::prelude::*;
86+
87+
proptest! {
88+
#![proptest_config(ProptestConfig::with_cases(1000))]
89+
90+
/// Property: Converting LLH->ECEF->LLH should always result in the original value
91+
#[test]
92+
fn newton_sqrt(x in 0.0..=1.0) {
93+
let newton_approx = compile_time_sqrt(x);
94+
let sqrt = x.sqrt();
95+
assert_float_eq!(sqrt, newton_approx, ulps <= 1, "Newton approximation of square root doesn't match IEEE sqrt! (Newton: {}, IEEE: {})", newton_approx, sqrt);
96+
}
97+
}
98+
}

0 commit comments

Comments
 (0)