1#[repr(C)]
18#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
19pub enum SensorKind {
20 Accelerometer,
23 Gyroscope,
26 Magnetometer,
29}
30
31#[repr(C)]
34#[derive(Debug, Clone, Copy, PartialEq)]
35pub struct SensorReading {
36 pub kind: SensorKind,
38 pub x: f32,
40 pub y: f32,
42 pub z: f32,
44 pub timestamp_ms: u64,
46}
47
48impl SensorReading {
49 #[allow(clippy::suboptimal_flops)] #[must_use] pub fn magnitude(&self) -> f32 {
53 (self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
54 }
55}
56
57impl_option!(
60 SensorReading,
61 OptionSensorReading,
62 [Debug, Clone, Copy, PartialEq]
63);
64
65#[cfg(test)]
66#[allow(clippy::float_cmp)] mod autotest_generated {
68 use core::{
69 cell::Cell,
70 hash::{Hash, Hasher},
71 };
72
73 use super::*;
74
75 fn reading(kind: SensorKind, x: f32, y: f32, z: f32) -> SensorReading {
79 SensorReading {
80 kind,
81 x,
82 y,
83 z,
84 timestamp_ms: 0,
85 }
86 }
87
88 fn accel(x: f32, y: f32, z: f32) -> SensorReading {
89 reading(SensorKind::Accelerometer, x, y, z)
90 }
91
92 fn approx(a: f32, b: f32, rel: f32) -> bool {
95 let scale = a.abs().max(b.abs()).max(1.0);
96 (a - b).abs() <= rel * scale
97 }
98
99 struct Fnv(u64);
101 impl Default for Fnv {
102 fn default() -> Self {
103 Self(0xcbf2_9ce4_8422_2325)
104 }
105 }
106 impl Hasher for Fnv {
107 fn finish(&self) -> u64 {
108 self.0
109 }
110 fn write(&mut self, bytes: &[u8]) {
111 for b in bytes {
112 self.0 ^= u64::from(*b);
113 self.0 = self.0.wrapping_mul(0x100_0000_01b3);
114 }
115 }
116 }
117 fn hash_of<T: Hash>(v: &T) -> u64 {
118 let mut h = Fnv::default();
119 v.hash(&mut h);
120 h.finish()
121 }
122
123 #[test]
128 fn magnitude_pythagorean_is_exact() {
129 assert_eq!(accel(3.0, 4.0, 0.0).magnitude(), 5.0);
130 assert_eq!(accel(0.0, 3.0, 4.0).magnitude(), 5.0);
131 assert_eq!(accel(1.0, 2.0, 2.0).magnitude(), 3.0);
132 assert_eq!(accel(0.0, 0.0, 0.0).magnitude(), 0.0);
133 }
134
135 #[test]
138 fn magnitude_single_axis_equals_abs() {
139 for v in [1.0_f32, -1.0, 0.5, -3.25, 7.5, -1024.0, 65_536.0] {
140 assert_eq!(accel(v, 0.0, 0.0).magnitude(), v.abs(), "x = {v}");
141 assert_eq!(accel(0.0, v, 0.0).magnitude(), v.abs(), "y = {v}");
142 assert_eq!(accel(0.0, 0.0, v).magnitude(), v.abs(), "z = {v}");
143 }
144 }
145
146 #[test]
149 fn magnitude_negative_zero_yields_positive_zero() {
150 let m = accel(-0.0, -0.0, -0.0).magnitude();
151 assert_eq!(m, 0.0);
152 assert!(m.is_sign_positive(), "expected +0.0, got {m:?}");
153 }
154
155 #[test]
158 fn magnitude_is_sign_invariant() {
159 let base = accel(1.5, 2.5, 3.5).magnitude();
160 for &sx in &[1.0_f32, -1.0] {
161 for &sy in &[1.0_f32, -1.0] {
162 for &sz in &[1.0_f32, -1.0] {
163 let m = accel(1.5 * sx, 2.5 * sy, 3.5 * sz).magnitude();
164 assert_eq!(m.to_bits(), base.to_bits(), "signs {sx}/{sy}/{sz}");
165 }
166 }
167 }
168 }
169
170 #[test]
173 fn magnitude_is_never_negative() {
174 let vals = [
175 0.0_f32,
176 -0.0,
177 1.0,
178 -1.0,
179 9.81,
180 -9.81,
181 1e-20,
182 -1e-20,
183 1e20,
184 -1e20,
185 f32::MAX,
186 f32::MIN,
187 f32::MIN_POSITIVE,
188 f32::INFINITY,
189 f32::NEG_INFINITY,
190 ];
191 for &x in &vals {
192 for &y in &vals {
193 for &z in &vals {
194 let m = accel(x, y, z).magnitude();
195 assert!(!m.is_nan(), "unexpected NaN for ({x}, {y}, {z})");
196 assert!(m >= 0.0, "negative magnitude {m} for ({x}, {y}, {z})");
197 assert!(
198 m.is_sign_positive(),
199 "negatively-signed magnitude {m:?} for ({x}, {y}, {z})"
200 );
201 }
202 }
203 }
204 }
205
206 #[test]
208 fn magnitude_nan_on_any_axis_is_nan() {
209 assert!(accel(f32::NAN, 0.0, 0.0).magnitude().is_nan());
210 assert!(accel(0.0, f32::NAN, 0.0).magnitude().is_nan());
211 assert!(accel(0.0, 0.0, f32::NAN).magnitude().is_nan());
212 assert!(accel(f32::NAN, f32::NAN, f32::NAN).magnitude().is_nan());
213 assert!(accel(f32::INFINITY, f32::NAN, 0.0).magnitude().is_nan());
215 assert!(
216 accel(f32::NEG_INFINITY, 0.0, f32::NAN)
217 .magnitude()
218 .is_nan()
219 );
220 }
221
222 #[test]
224 fn magnitude_infinite_axis_is_positive_infinity() {
225 for inf in [f32::INFINITY, f32::NEG_INFINITY] {
226 assert_eq!(accel(inf, 0.0, 0.0).magnitude(), f32::INFINITY);
227 assert_eq!(accel(0.0, inf, 0.0).magnitude(), f32::INFINITY);
228 assert_eq!(accel(1.0, 2.0, inf).magnitude(), f32::INFINITY);
229 }
230 assert_eq!(
231 accel(f32::INFINITY, f32::NEG_INFINITY, f32::INFINITY).magnitude(),
232 f32::INFINITY
233 );
234 }
235
236 #[test]
241 fn magnitude_overflows_to_infinity_on_huge_finite_input() {
242 assert!(accel(f32::MAX, 0.0, 0.0).magnitude().is_infinite());
244 assert!(accel(0.0, f32::MIN, 0.0).magnitude().is_infinite());
245 assert!(
246 accel(f32::MAX, f32::MAX, f32::MAX)
247 .magnitude()
248 .is_infinite()
249 );
250 }
251
252 #[test]
255 fn magnitude_overflow_cliff_per_term_and_in_the_sum() {
256 let below = accel(1.8e19, 0.0, 0.0).magnitude();
258 assert!(below.is_finite(), "expected finite, got {below}");
259 assert!(approx(below, 1.8e19, 1e-5), "{below} !~ 1.8e19");
260
261 assert!(accel(1.9e19, 0.0, 0.0).magnitude().is_infinite());
263
264 let two_terms = accel(1.3e19, 1.3e19, 0.0).magnitude();
266 assert!(two_terms.is_finite(), "expected finite, got {two_terms}");
267 assert!(approx(two_terms, 1.838_477e19, 1e-4), "{two_terms}");
268 assert!(
269 accel(1.3e19, 1.3e19, 1.3e19).magnitude().is_infinite(),
270 "sum of three 1.69e38 squares must overflow to +inf"
271 );
272 }
273
274 #[test]
278 fn magnitude_underflows_to_zero_on_tiny_input() {
279 assert_eq!(accel(1e-30, 0.0, 0.0).magnitude(), 0.0);
280 assert_eq!(accel(0.0, -1e-30, 1e-30).magnitude(), 0.0);
281 assert_eq!(accel(f32::MIN_POSITIVE, 0.0, 0.0).magnitude(), 0.0);
282 assert_eq!(accel(f32::from_bits(1), 0.0, 0.0).magnitude(), 0.0);
284 }
285
286 #[test]
289 fn magnitude_near_underflow_cliff_is_finite_and_close() {
290 let m = accel(1e-19, 0.0, 0.0).magnitude();
291 assert!(m.is_finite() && m > 0.0, "expected small positive, got {m}");
292 assert!(approx(m, 1e-19, 1e-2), "{m} !~ 1e-19");
293 }
294
295 #[test]
298 fn magnitude_scaling_by_power_of_two_is_exact() {
299 let (x, y, z) = (1.5_f32, -2.25_f32, 0.75_f32);
300 let base = accel(x, y, z).magnitude();
301 let doubled = accel(2.0 * x, 2.0 * y, 2.0 * z).magnitude();
302 let halved = accel(0.5 * x, 0.5 * y, 0.5 * z).magnitude();
303 assert_eq!(doubled, 2.0 * base);
304 assert_eq!(halved, 0.5 * base);
305 }
306
307 #[test]
310 fn magnitude_is_permutation_stable() {
311 let (x, y, z) = (0.1_f32, 12_345.678_f32, -0.000_31_f32);
312 let base = accel(x, y, z).magnitude();
313 for (a, b, c) in [(x, z, y), (y, x, z), (y, z, x), (z, x, y), (z, y, x)] {
314 let m = accel(a, b, c).magnitude();
315 assert!(approx(m, base, 1e-6), "{m} !~ {base} for ({a}, {b}, {c})");
316 }
317 }
318
319 #[test]
322 fn magnitude_accelerometer_at_rest_is_about_9_81() {
323 for r in [
324 accel(0.0, -9.81, 0.0),
325 accel(9.81, 0.0, 0.0),
326 accel(0.0, 0.0, -9.81),
327 accel(5.663_8, -5.663_8, -5.663_8), ] {
329 let m = r.magnitude();
330 assert!(approx(m, 9.81, 1e-3), "{m} !~ 9.81 for {r:?}");
331 }
332 }
333
334 #[test]
337 fn magnitude_ignores_kind_and_timestamp() {
338 let expect = accel(1.0, 2.0, 2.0).magnitude().to_bits();
339 for kind in [
340 SensorKind::Accelerometer,
341 SensorKind::Gyroscope,
342 SensorKind::Magnetometer,
343 ] {
344 for timestamp_ms in [0_u64, 1, u64::MAX / 2, u64::MAX] {
345 let r = SensorReading {
346 kind,
347 x: 1.0,
348 y: 2.0,
349 z: 2.0,
350 timestamp_ms,
351 };
352 assert_eq!(r.magnitude().to_bits(), expect, "{kind:?} @ {timestamp_ms}");
353 }
354 }
355 }
356
357 #[test]
360 fn magnitude_is_deterministic_and_leaves_the_reading_intact() {
361 let r = accel(0.1, -0.2, 9.79);
362 let before = r;
363 let a = r.magnitude();
364 let b = r.magnitude();
365 assert_eq!(a.to_bits(), b.to_bits());
366 assert_eq!(r, before, "magnitude() must not mutate the reading");
367 }
368
369 #[test]
374 fn reading_with_nan_axis_is_not_equal_to_itself() {
375 let nan = accel(f32::NAN, 0.0, 0.0);
376 let bit_identical = nan; assert_ne!(nan, bit_identical, "a NaN axis makes PartialEq non-reflexive");
378
379 let opt = OptionSensorReading::Some(nan);
381 let opt_copy = opt;
382 assert_ne!(opt, opt_copy);
383
384 let ok = accel(1.0, 2.0, 2.0);
386 assert_eq!(ok, accel(1.0, 2.0, 2.0));
387 }
388
389 #[test]
392 fn sensor_kind_ordering_and_hash_are_consistent() {
393 use SensorKind::{Accelerometer, Gyroscope, Magnetometer};
394 assert!(Accelerometer < Gyroscope);
395 assert!(Gyroscope < Magnetometer);
396 assert!(Accelerometer < Magnetometer);
397
398 assert_eq!(hash_of(&Accelerometer), hash_of(&Accelerometer));
399 assert_ne!(hash_of(&Accelerometer), hash_of(&Gyroscope));
400 assert_ne!(hash_of(&Gyroscope), hash_of(&Magnetometer));
401
402 let k = Gyroscope;
404 let copied = k;
405 assert_eq!(k, copied);
406 }
407
408 #[test]
410 fn sensor_kind_is_a_c_int() {
411 assert_eq!(core::mem::size_of::<SensorKind>(), 4);
412 assert!(
413 core::mem::size_of::<OptionSensorReading>() > core::mem::size_of::<SensorReading>()
414 );
415 }
416
417 #[test]
420 fn option_default_is_none() {
421 let d = OptionSensorReading::default();
422 assert!(d.is_none());
423 assert!(!d.is_some());
424 assert_eq!(d.as_option(), None);
425 assert_eq!(d.as_ref(), None);
426 assert_eq!(d.into_option(), None);
427 }
428
429 #[test]
432 fn option_roundtrips_through_the_ffi_wrapper() {
433 let r = accel(1.0, 2.0, 2.0);
434
435 let wrapped: OptionSensorReading = Some(r).into();
436 assert!(wrapped.is_some());
437 assert_eq!(wrapped.into_option(), Some(r));
438 assert_eq!(Option::<SensorReading>::from(wrapped), Some(r));
439 assert_eq!(wrapped.as_option(), Some(&r));
440
441 let none: OptionSensorReading = None.into();
442 assert!(none.is_none());
443 assert_eq!(none.into_option(), None);
444 assert_eq!(Option::<SensorReading>::from(none), None);
445
446 for o in [wrapped, none] {
448 assert_ne!(o.is_some(), o.is_none());
449 }
450 }
451
452 #[test]
454 fn option_replace_returns_the_previous_value() {
455 let first = accel(1.0, 0.0, 0.0);
456 let second = accel(0.0, 3.0, 4.0);
457
458 let mut o = OptionSensorReading::None;
459 let prev = o.replace(first);
460 assert!(prev.is_none(), "replacing None must hand back None");
461 assert_eq!(o.into_option(), Some(first));
462
463 let prev = o.replace(second);
464 assert_eq!(prev.into_option(), Some(first));
465 assert_eq!(o.into_option(), Some(second));
466 assert_eq!(o.as_option().map(SensorReading::magnitude), Some(5.0));
467 }
468
469 #[test]
471 fn option_as_mut_mutates_the_payload() {
472 let mut o = OptionSensorReading::Some(accel(0.0, 0.0, 0.0));
473 assert_eq!(o.as_option().map(SensorReading::magnitude), Some(0.0));
474
475 let slot = o.as_mut().expect("Some");
476 slot.x = 3.0;
477 slot.y = 4.0;
478 assert_eq!(o.as_option().map(SensorReading::magnitude), Some(5.0));
479
480 let mut none = OptionSensorReading::None;
481 assert!(none.as_mut().is_none());
482 }
483
484 #[test]
487 fn option_map_and_and_then_are_lazy_on_none() {
488 let calls = Cell::new(0_u32);
489
490 let mapped = OptionSensorReading::None.map(|r| {
491 calls.set(calls.get() + 1);
492 r.magnitude()
493 });
494 assert_eq!(mapped, None);
495 assert_eq!(calls.get(), 0, "closure must not run on None");
496
497 let chained = OptionSensorReading::None.and_then(|r| Some(r.magnitude()));
498 assert_eq!(chained, None);
499
500 let some = OptionSensorReading::Some(accel(3.0, 4.0, 0.0));
501 let mapped = some.map(|r| {
502 calls.set(calls.get() + 1);
503 r.magnitude()
504 });
505 assert_eq!(mapped, Some(5.0));
506 assert_eq!(calls.get(), 1, "closure must run exactly once on Some");
507
508 assert_eq!(some.and_then(|r| Some(r.magnitude())), Some(5.0));
509 assert_eq!(
510 some.and_then(|_| Option::<f32>::None),
511 None,
512 "and_then must be able to collapse Some -> None"
513 );
514 }
515
516 #[test]
519 fn option_is_copy_and_the_source_survives() {
520 let o = OptionSensorReading::Some(accel(0.0, 3.0, 4.0));
521 let mut copy = o;
522 copy.replace(accel(1.0, 0.0, 0.0));
523
524 assert_eq!(o.as_option().map(SensorReading::magnitude), Some(5.0));
525 assert_eq!(copy.as_option().map(SensorReading::magnitude), Some(1.0));
526 }
527}