rust_robotics_core/
angle.rs1use core::f64::consts::{PI, TAU};
4
5pub 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 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}