1use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
2use float_cmp::{ApproxEq, F32Margin};
3#[cfg(not(feature = "fixed_point"))]
4#[allow(unused_imports)]
5use micromath::F32Ext;
6
7/// Real.
8///
9/// `Real` is used to store real numbers as either floating point or 32 bit fixed point if the
10/// `fixed_point` cargo feature is enabled.
11/// ```
12pub(crate) use real_impl::{Real, FRAC_PI_2, PI, TAU};
13
14#[cfg(not(feature = "fixed_point"))]
15mod real_impl {
16 use core::f32;
17
18 #[allow(dead_code)]
19 pub(crate) const FRAC_PI_2: Real = Real(f32::consts::FRAC_PI_2);
20 pub(crate) const PI: Real = Real(f32::consts::PI);
21 pub(crate) const TAU: Real = Real(2.0 * f32::consts::PI);
22
23 #[derive(Copy, Clone, PartialEq, PartialOrd, Debug)]
24 #[cfg_attr(feature = "defmt", derive(::defmt::Format))]
25 pub(crate) struct Real(pub(super) f32);
26
27 impl From<f32> for Real {
28 fn from(src: f32) -> Self {
29 Self(src)
30 }
31 }
32
33 impl From<i32> for Real {
34 fn from(src: i32) -> Self {
35 Self(src as f32)
36 }
37 }
38
39 impl From<u32> for Real {
40 fn from(src: u32) -> Self {
41 Self(src as f32)
42 }
43 }
44
45 impl From<Real> for f32 {
46 fn from(src: Real) -> Self {
47 src.0
48 }
49 }
50
51 impl From<Real> for i32 {
52 fn from(src: Real) -> Self {
53 src.0 as i32
54 }
55 }
56
57 impl From<Real> for u32 {
58 fn from(src: Real) -> Self {
59 src.0 as u32
60 }
61 }
62}
63
64#[cfg(feature = "fixed_point")]
65mod real_impl {
66 use fixed::types::I16F16;
67
68 #[allow(dead_code)]
69 pub(crate) const FRAC_PI_2: Real = Real(I16F16::from_bits(102944));
70 pub(crate) const PI: Real = Real(I16F16::from_bits(205887));
71 pub(crate) const TAU: Real = Real(I16F16::from_bits(411775));
72
73 #[derive(Copy, Clone, PartialEq, PartialOrd, Debug)]
74 pub(crate) struct Real(pub(super) I16F16);
75
76 impl Real {
77 pub(crate) const fn zero() -> Self {
78 Self(I16F16::from_bits(0))
79 }
80 }
81
82 impl From<I16F16> for Real {
83 fn from(src: I16F16) -> Self {
84 Self(src)
85 }
86 }
87
88 impl From<f32> for Real {
89 fn from(src: f32) -> Self {
90 Self(I16F16::from_num(src))
91 }
92 }
93
94 impl From<i32> for Real {
95 fn from(src: i32) -> Self {
96 Self(I16F16::from_num(src))
97 }
98 }
99
100 impl From<u32> for Real {
101 fn from(src: u32) -> Self {
102 Self(I16F16::from_num(src))
103 }
104 }
105
106 impl From<Real> for f32 {
107 fn from(src: Real) -> Self {
108 src.0.to_num::<f32>()
109 }
110 }
111
112 impl From<Real> for i32 {
113 fn from(src: Real) -> Self {
114 src.0.round_to_zero().to_num::<i32>()
115 }
116 }
117
118 impl From<Real> for u32 {
119 fn from(src: Real) -> Self {
120 src.0.to_num::<u32>()
121 }
122 }
123
124 #[cfg(feature = "defmt")]
125 impl ::defmt::Format for Real {
126 fn format(&self, fmt: ::defmt::Formatter<'_>) {
127 ::defmt::write!(
128 fmt,
129 "Real({=i32:#X} ≈{=f32})",
130 self.0.to_bits(),
131 self.0.to_num()
132 )
133 }
134 }
135}
136
137impl Add for Real {
138 type Output = Real;
139
140 fn add(self, other: Real) -> Real {
141 Self(self.0 + other.0)
142 }
143}
144
145impl AddAssign for Real {
146 fn add_assign(&mut self, other: Real) {
147 self.0 += other.0;
148 }
149}
150
151impl Sub for Real {
152 type Output = Real;
153
154 fn sub(self, other: Real) -> Real {
155 Self(self.0 - other.0)
156 }
157}
158
159impl SubAssign for Real {
160 fn sub_assign(&mut self, other: Real) {
161 self.0 -= other.0;
162 }
163}
164
165impl Neg for Real {
166 type Output = Real;
167
168 fn neg(self) -> Real {
169 Self(-self.0)
170 }
171}
172
173impl Mul for Real {
174 type Output = Real;
175
176 fn mul(self, other: Real) -> Real {
177 Self(self.0 * other.0)
178 }
179}
180
181impl MulAssign for Real {
182 fn mul_assign(&mut self, other: Real) {
183 self.0 *= other.0
184 }
185}
186
187impl Div for Real {
188 type Output = Real;
189
190 fn div(self, other: Real) -> Real {
191 Self(self.0 / other.0)
192 }
193}
194
195impl DivAssign for Real {
196 fn div_assign(&mut self, other: Real) {
197 self.0 /= other.0
198 }
199}
200
201impl ApproxEq for Real {
202 type Margin = F32Margin;
203
204 fn approx_eq<M: Into<Self::Margin>>(self, other: Self, margin: M) -> bool {
205 let a: f32 = self.into();
206 let b: f32 = other.into();
207 a.approx_eq(other:b, margin.into())
208 }
209}
210
211impl Real {
212 pub(crate) fn abs(self) -> Self {
213 Self(self.0.abs())
214 }
215
216 pub(crate) fn rem_euclid(self, rhs: Real) -> Self {
217 let r: f32 = self.0 % rhs.0;
218 if r < 0.0 {
219 Real(r) + rhs.abs()
220 } else {
221 Real(r)
222 }
223 }
224
225 #[allow(unused)]
226 pub(crate) fn round(self) -> Self {
227 Self(self.0.round())
228 }
229}
230