marg_orientation/
num_traits.rs

1pub trait GimbalLockZenithNadir<T> {
2    /// The value for the zenith, i.e. π/2;
3    const ZENITH: T;
4
5    /// The value for the nadir, i.e. -π/2;
6    const NADIR: T;
7}
8
9pub trait DetectGimbalLock<T>: GimbalLockZenithNadir<T> {
10    /// Determines whether a Gimbal Lock situation is about to occur
11    /// because the angle (provided in radians) is close to π/2 or -π/2.
12    ///
13    /// ## Arguments
14    /// * `tolerance` - The tolerance in radians, e.g. 0.01 rad.
15    fn close_to_zenith_or_nadir(&self, tolerance: T) -> bool;
16}
17
18pub trait IsNaN {
19    /// Determines whether this value represents "not a number" (NaN).
20    fn is_nan(&self) -> bool;
21}
22
23pub trait NormalizeAngle<T = Self> {
24    type Output;
25
26    /// Normalizes the angle into the range -π to π.
27    fn normalize_angle(self) -> Self::Output;
28}
29
30pub trait ArcTan<T = Self> {
31    type Output;
32
33    fn atan2(self, rhs: T) -> Self::Output;
34}
35
36pub trait ArcSin<T = Self> {
37    type Output;
38
39    fn arcsin(self) -> Self::Output;
40}
41
42pub trait Abs<T = Self> {
43    type Output;
44
45    fn abs(self) -> Self::Output;
46}
47
48impl IsNaN for f32 {
49    #[inline(always)]
50    fn is_nan(&self) -> bool {
51        (*self).is_nan()
52    }
53}
54
55impl IsNaN for f64 {
56    #[inline(always)]
57    fn is_nan(&self) -> bool {
58        (*self).is_nan()
59    }
60}
61
62#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
63#[cfg(feature = "std")]
64impl NormalizeAngle<f32> for f32 {
65    type Output = f32;
66
67    fn normalize_angle(self) -> Self::Output {
68        let mut normalized = self;
69        while normalized > std::f32::consts::PI {
70            normalized -= std::f32::consts::TAU;
71        }
72        while normalized < -std::f32::consts::PI {
73            normalized += std::f32::consts::TAU;
74        }
75        normalized
76    }
77}
78
79#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
80#[cfg(feature = "std")]
81impl NormalizeAngle<f64> for f64 {
82    type Output = f64;
83
84    fn normalize_angle(self) -> Self::Output {
85        let mut normalized = self;
86        while normalized > std::f64::consts::PI {
87            normalized -= std::f64::consts::TAU;
88        }
89        while normalized < -std::f64::consts::PI {
90            normalized += std::f64::consts::TAU;
91        }
92        normalized
93    }
94}
95
96#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
97#[cfg(feature = "std")]
98impl Abs<f32> for f32 {
99    type Output = f32;
100
101    #[inline(always)]
102    fn abs(self) -> Self::Output {
103        f32::abs(self)
104    }
105}
106
107#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
108#[cfg(feature = "std")]
109impl ArcTan<f32> for f32 {
110    type Output = f32;
111
112    #[inline(always)]
113    fn atan2(self, other: f32) -> Self::Output {
114        f32::atan2(self, other)
115    }
116}
117
118#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
119#[cfg(feature = "std")]
120impl ArcTan<f64> for f64 {
121    type Output = f64;
122
123    #[inline(always)]
124    fn atan2(self, other: f64) -> Self::Output {
125        f64::atan2(self, other)
126    }
127}
128
129#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
130#[cfg(feature = "std")]
131impl ArcSin<f32> for f32 {
132    type Output = f32;
133
134    #[inline(always)]
135    fn arcsin(self) -> Self::Output {
136        f32::asin(self)
137    }
138}
139
140#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
141#[cfg(feature = "std")]
142impl ArcSin<f64> for f64 {
143    type Output = f64;
144
145    #[inline(always)]
146    fn arcsin(self) -> Self::Output {
147        f64::asin(self)
148    }
149}
150
151impl GimbalLockZenithNadir<f32> for f32 {
152    const ZENITH: f32 = core::f32::consts::FRAC_PI_2;
153    const NADIR: f32 = -core::f32::consts::FRAC_PI_2;
154}
155
156impl GimbalLockZenithNadir<f64> for f64 {
157    const ZENITH: f64 = core::f64::consts::FRAC_PI_2;
158    const NADIR: f64 = -core::f64::consts::FRAC_PI_2;
159}
160
161#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
162#[cfg(feature = "std")]
163impl<T> DetectGimbalLock<T> for T
164where
165    T: Copy + num_traits::Float + GimbalLockZenithNadir<T>,
166{
167    #[inline]
168    fn close_to_zenith_or_nadir(&self, tolerance: T) -> bool {
169        (*self - T::ZENITH).abs() <= tolerance || (*self - T::NADIR).abs() <= tolerance
170    }
171}
172
173#[cfg(test)]
174#[cfg(feature = "std")]
175mod tests {
176    use super::*;
177
178    #[test]
179    fn test_gimbal_lock_f32() {
180        // The detection tolerance in radians.
181        const TOLERANCE: f32 = 0.1;
182
183        // The value to use for testing the tolerance. We use a value less than
184        // the tolerance here to account for floating-point rounding issues.
185        const TOLERANCE_TEST: f32 = TOLERANCE * 0.99;
186
187        assert!(!0.0_f32.close_to_zenith_or_nadir(TOLERANCE));
188        assert!(!1.0_f32.close_to_zenith_or_nadir(TOLERANCE));
189        assert!(!(-1.0_f32).close_to_zenith_or_nadir(TOLERANCE));
190
191        assert!(core::f32::consts::FRAC_PI_2.close_to_zenith_or_nadir(TOLERANCE));
192        assert!((-core::f32::consts::FRAC_PI_2).close_to_zenith_or_nadir(TOLERANCE));
193
194        assert!((core::f32::consts::FRAC_PI_2 + TOLERANCE_TEST).close_to_zenith_or_nadir(TOLERANCE));
195        assert!((core::f32::consts::FRAC_PI_2 - TOLERANCE_TEST).close_to_zenith_or_nadir(TOLERANCE));
196        assert!(
197            (-core::f32::consts::FRAC_PI_2 + TOLERANCE_TEST).close_to_zenith_or_nadir(TOLERANCE)
198        );
199        assert!(
200            (-core::f32::consts::FRAC_PI_2 - TOLERANCE_TEST).close_to_zenith_or_nadir(TOLERANCE)
201        );
202    }
203
204    #[test]
205    fn test_gimbal_lock_f64() {
206        // The detection tolerance in radians.
207        const TOLERANCE: f64 = 0.1;
208
209        // The value to use for testing the tolerance. We use a value less than
210        // the tolerance here to account for floating-point rounding issues.
211        const TOLERANCE_TEST: f64 = TOLERANCE * 0.99;
212
213        assert!(!0.0_f64.close_to_zenith_or_nadir(TOLERANCE));
214        assert!(!1.0_f64.close_to_zenith_or_nadir(TOLERANCE));
215        assert!(!(-1.0_f64).close_to_zenith_or_nadir(TOLERANCE));
216
217        assert!(core::f64::consts::FRAC_PI_2.close_to_zenith_or_nadir(TOLERANCE));
218        assert!((-core::f64::consts::FRAC_PI_2).close_to_zenith_or_nadir(TOLERANCE));
219
220        assert!((core::f64::consts::FRAC_PI_2 + TOLERANCE_TEST).close_to_zenith_or_nadir(TOLERANCE));
221        assert!((core::f64::consts::FRAC_PI_2 - TOLERANCE_TEST).close_to_zenith_or_nadir(TOLERANCE));
222        assert!(
223            (-core::f64::consts::FRAC_PI_2 + TOLERANCE_TEST).close_to_zenith_or_nadir(TOLERANCE)
224        );
225        assert!(
226            (-core::f64::consts::FRAC_PI_2 - TOLERANCE_TEST).close_to_zenith_or_nadir(TOLERANCE)
227        );
228    }
229}
marg_orientation/num_traits.rs

marg_orientation/
num_traits.rs
