Skip to main content

rust_robotics_core/
angle.rs

1//! Angle helpers.
2
3use core::f64::consts::{PI, TAU};
4
5/// Wraps `angle` \[rad\] into `[-π, π]`.
6///
7/// Angles already in range (including both ends) come back unchanged, and
8/// any other finite angle is reduced with one exact remainder, so the result
9/// matches the classic "subtract 2π while above π" loop without its
10/// unbounded running time. Non-finite input is returned as is.
11pub fn normalize_angle(angle: f64) -> f64 {
12    if (-PI..=PI).contains(&angle) || !angle.is_finite() {
13        return angle;
14    }
15    let wrapped = angle % TAU;
16    if wrapped > PI {
17        wrapped - TAU
18    } else if wrapped < -PI {
19        wrapped + TAU
20    } else {
21        wrapped
22    }
23}
24
25#[cfg(test)]
26mod tests {
27    use super::*;
28
29    /// The loop every module used to carry.
30    fn reference(mut angle: f64) -> f64 {
31        while angle > PI {
32            angle -= TAU;
33        }
34        while angle < -PI {
35            angle += TAU;
36        }
37        angle
38    }
39
40    #[test]
41    fn matches_the_subtraction_loop() {
42        for i in -2000..=2000 {
43            let angle = i as f64 * 0.00731;
44            assert_eq!(normalize_angle(angle), reference(angle), "angle {angle}");
45        }
46        for angle in [
47            PI,
48            -PI,
49            0.0,
50            -0.0,
51            PI + 1e-12,
52            -PI - 1e-12,
53            3.0 * PI,
54            -3.0 * PI,
55        ] {
56            assert_eq!(normalize_angle(angle), reference(angle), "angle {angle}");
57        }
58    }
59
60    #[test]
61    fn large_and_non_finite_angles_return() {
62        let wrapped = normalize_angle(1.0e12);
63        assert!((-PI..=PI).contains(&wrapped));
64        assert_eq!(normalize_angle(f64::INFINITY), f64::INFINITY);
65        assert!(normalize_angle(f64::NAN).is_nan());
66    }
67}