1use core::fmt;
4
5pub type Scalar = f64;
11
12#[derive(Debug, Clone, Copy, PartialEq, Eq)]
14pub enum ToleranceError {
15 NotFiniteAndNonNegative,
17}
18
19impl fmt::Display for ToleranceError {
20 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
21 match self {
22 Self::NotFiniteAndNonNegative => {
23 f.write_str("tolerance values must be finite and non-negative")
24 }
25 }
26 }
27}
28
29impl std::error::Error for ToleranceError {}
30
31#[derive(Debug, Clone, Copy, PartialEq)]
36pub struct Tolerance {
37 linear: Scalar,
38 angular: Scalar,
39}
40
41impl Tolerance {
42 pub const ZERO: Self = Self {
46 linear: 0.0,
47 angular: 0.0,
48 };
49
50 pub const METRE: Self = Self {
53 linear: 1e-6,
54 angular: 1e-9,
55 };
56
57 pub const MILLIMETRE: Self = Self {
60 linear: 1e-3,
61 angular: 1e-9,
62 };
63
64 pub fn new(linear: Scalar, angular: Scalar) -> Result<Self, ToleranceError> {
66 if !linear.is_finite() || !angular.is_finite() || linear < 0.0 || angular < 0.0 {
67 return Err(ToleranceError::NotFiniteAndNonNegative);
68 }
69 Ok(Self { linear, angular })
70 }
71
72 #[inline]
74 pub const fn linear(self) -> Scalar {
75 self.linear
76 }
77
78 #[inline]
80 pub const fn angular(self) -> Scalar {
81 self.angular
82 }
83
84 #[inline]
86 pub fn eq(self, a: Scalar, b: Scalar) -> bool {
87 (a - b).abs() <= self.linear
88 }
89}
90
91#[cfg(test)]
92mod tests {
93 use super::*;
94
95 #[test]
96 fn rejects_invalid_policy() {
97 assert_eq!(
98 Tolerance::new(Scalar::NAN, 0.0),
99 Err(ToleranceError::NotFiniteAndNonNegative)
100 );
101 assert_eq!(
102 Tolerance::new(-1.0, 0.0),
103 Err(ToleranceError::NotFiniteAndNonNegative)
104 );
105 }
106
107 #[test]
108 fn policy_has_no_context_free_default() {
109 assert!(Tolerance::METRE.eq(1.0, 1.0 + 0.5e-6));
110 assert!(!Tolerance::METRE.eq(1.0, 1.0 + 2.0e-6));
111 }
112}