navcore_math/
angle.rs

1//! Angle normalisation.
2//!
3//! A common source of bearing bugs. Every function that produces a
4//! bearing normalises before returning, so callers never have to check
5//! whether they are holding 350 or -10.
6
7/// Wraps an angle into `0.0..360.0` -- the range every bearing, heading and
8/// course in this crate is returned in.
9///
10/// Returns `NaN` unchanged rather than looping or producing a wrong finite
11/// answer: a `NaN` bearing means an upstream sensor value was missing, and
12/// silently turning that into 0.0 (due north) is exactly the kind of quiet
13/// wrong answer this crate must not produce.
14#[must_use]
15pub fn norm_360(deg: f64) -> f64 {
16    if !deg.is_finite() {
17        return deg;
18    }
19    let r = deg % 360.0;
20    if r < 0.0 { r + 360.0 } else { r }
21}
22
23/// Wraps an angle into `-180.0..=180.0`, positive to starboard -- the range
24/// every *relative* angle in this crate is returned in (wind angles, course
25/// differences).
26#[must_use]
27pub fn norm_180(deg: f64) -> f64 {
28    if !deg.is_finite() {
29        return deg;
30    }
31    let r = norm_360(deg);
32    if r > 180.0 { r - 360.0 } else { r }
33}
34
35/// Signed difference `from - to`, wrapped to `-180.0..=180.0`.
36///
37/// The sign is the direction you would turn: positive means `from` lies to
38/// starboard of `to`.
39#[must_use]
40pub fn diff(from_deg: f64, to_deg: f64) -> f64 {
41    norm_180(from_deg - to_deg)
42}
43
44/// The opposite bearing.
45#[must_use]
46pub fn reciprocal(deg: f64) -> f64 {
47    norm_360(deg + 180.0)
48}
49
50#[cfg(test)]
51mod tests {
52    use super::*;
53
54    #[test]
55    fn norm_360_wraps_both_directions() {
56        assert_eq!(norm_360(0.0), 0.0);
57        assert_eq!(norm_360(359.9), 359.9);
58        assert_eq!(norm_360(360.0), 0.0);
59        assert_eq!(norm_360(370.0), 10.0);
60        assert_eq!(norm_360(-10.0), 350.0);
61        assert_eq!(norm_360(-370.0), 350.0);
62        assert_eq!(norm_360(-720.0), 0.0);
63    }
64
65    #[test]
66    fn norm_180_keeps_180_positive() {
67        assert_eq!(norm_180(180.0), 180.0);
68        assert_eq!(norm_180(-180.0), 180.0);
69        assert_eq!(norm_180(190.0), -170.0);
70        assert_eq!(norm_180(-190.0), 170.0);
71        assert_eq!(norm_180(0.0), 0.0);
72    }
73
74    #[test]
75    fn diff_signs_point_to_starboard() {
76        assert_eq!(diff(10.0, 350.0), 20.0);
77        assert_eq!(diff(350.0, 10.0), -20.0);
78        assert_eq!(diff(90.0, 90.0), 0.0);
79    }
80
81    #[test]
82    fn reciprocal_round_trips() {
83        assert_eq!(reciprocal(0.0), 180.0);
84        assert_eq!(reciprocal(180.0), 0.0);
85        // Approximate, not exact: two wraps through 360 cost a bit of
86        // mantissa, and demanding bit-equality here would be testing IEEE
87        //754 rather than the navigation.
88        assert!((reciprocal(reciprocal(123.4)) - 123.4).abs() < 1e-9);
89    }
90
91    #[test]
92    fn nan_survives_rather_than_becoming_north() {
93        assert!(norm_360(f64::NAN).is_nan());
94        assert!(norm_180(f64::NAN).is_nan());
95    }
96}
navcore_math/angle.rs

navcore_math/
angle.rs
