1use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
4
5macro_rules! fixed_impl {
7 ($name:ident, $bits:literal, $fract_bits:literal, $ty:ty) => {
8 #[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord, Hash, Default)]
9 #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
10 #[cfg_attr(feature = "bytemuck", derive(bytemuck::AnyBitPattern, bytemuck::NoUninit))]
11 #[repr(transparent)]
12 #[doc = concat!(stringify!($bits), "-bit signed fixed point number with ", stringify!($fract_bits), " bits of fraction." )]
13 pub struct $name($ty);
14 impl $name {
15 pub const MIN: Self = Self(<$ty>::MIN);
17
18 pub const MAX: Self = Self(<$ty>::MAX);
20
21 pub const EPSILON: Self = Self(1);
23
24 pub const ZERO: Self = Self(0);
26
27 pub const ONE: Self = Self(1 << $fract_bits);
29
30 const INT_MASK: $ty = !0 << $fract_bits;
31 const ROUND: $ty = 1 << ($fract_bits - 1);
32 const FRACT_BITS: usize = $fract_bits;
33
34 #[inline(always)]
36 pub const fn from_bits(bits: $ty) -> Self {
37 Self(bits)
38 }
39
40 #[inline(always)]
42 pub const fn to_bits(self) -> $ty {
43 self.0
44 }
45
46 #[inline(always)]
49 pub const fn round(self) -> Self {
50 Self(self.0.wrapping_add(Self::ROUND) & Self::INT_MASK)
51 }
52
53 #[inline(always)]
55 pub const fn abs(self) -> Self {
56 Self(self.0.abs())
57 }
58
59 #[inline(always)]
61 pub const fn floor(self) -> Self {
62 Self(self.0 & Self::INT_MASK)
63 }
64
65 #[inline(always)]
67 pub const fn fract(self) -> Self {
68 Self(self.0 - self.floor().0)
69 }
70
71 #[inline(always)]
73 pub fn wrapping_add(self, other: Self) -> Self {
74 Self(self.0.wrapping_add(other.0))
75 }
76
77 #[inline(always)]
79 pub const fn saturating_add(self, other: Self) -> Self {
80 Self(self.0.saturating_add(other.0))
81 }
82
83 #[inline(always)]
85 pub fn checked_add(self, other: Self) -> Option<Self> {
86 self.0.checked_add(other.0).map(|inner| Self(inner))
87 }
88
89 #[inline(always)]
91 pub const fn wrapping_sub(self, other: Self) -> Self {
92 Self(self.0.wrapping_sub(other.0))
93 }
94
95 #[inline(always)]
97 pub const fn saturating_sub(self, other: Self) -> Self {
98 Self(self.0.saturating_sub(other.0))
99 }
100
101 #[inline(always)]
103 pub const fn to_be_bytes(self) -> [u8; $bits / 8] {
104 self.0.to_be_bytes()
105 }
106 }
107
108 impl Add for $name {
109 type Output = Self;
110 #[inline(always)]
111 fn add(self, other: Self) -> Self {
112 Self(self.0.wrapping_add(other.0))
113 }
114 }
115
116 impl AddAssign for $name {
117 #[inline(always)]
118 fn add_assign(&mut self, other: Self) {
119 *self = *self + other;
120 }
121 }
122
123 impl Sub for $name {
124 type Output = Self;
125 #[inline(always)]
126 fn sub(self, other: Self) -> Self {
127 Self(self.0.wrapping_sub(other.0))
128 }
129 }
130
131 impl SubAssign for $name {
132 #[inline(always)]
133 fn sub_assign(&mut self, other: Self) {
134 *self = *self - other;
135 }
136 }
137 };
138}
139
140macro_rules! fixed_mul_div {
142 ($ty:ty) => {
143 impl $ty {
144 #[inline]
148 pub const fn mul_div(&self, a: Self, b: Self) -> Self {
149 let mut sign = 1;
150 let mut su = self.0 as u64;
151 let mut au = a.0 as u64;
152 let mut bu = b.0 as u64;
153 if self.0 < 0 {
154 su = 0u64.wrapping_sub(su);
155 sign = -1;
156 }
157 if a.0 < 0 {
158 au = 0u64.wrapping_sub(au);
159 sign = -sign;
160 }
161 if b.0 < 0 {
162 bu = 0u64.wrapping_sub(bu);
163 sign = -sign;
164 }
165 let result = if bu > 0 {
166 su.wrapping_mul(au).wrapping_add(bu >> 1) / bu
167 } else {
168 0x7FFFFFFF
169 };
170 Self(if sign < 0 {
171 (result as i32).wrapping_neg()
172 } else {
173 result as i32
174 })
175 }
176 }
177
178 impl Mul for $ty {
179 type Output = Self;
180 #[inline(always)]
181 fn mul(self, other: Self) -> Self::Output {
182 let ab = self.0 as i64 * other.0 as i64;
183 Self(((ab + 0x8000 - i64::from(ab < 0)) >> 16) as i32)
184 }
185 }
186
187 impl MulAssign for $ty {
188 #[inline(always)]
189 fn mul_assign(&mut self, rhs: Self) {
190 *self = *self * rhs;
191 }
192 }
193
194 impl Div for $ty {
195 type Output = Self;
196 #[inline(always)]
197 fn div(self, other: Self) -> Self::Output {
198 let mut sign = 1;
199 let mut a = self.0;
200 let mut b = other.0;
201 if a < 0 {
202 a = -a;
203 sign = -1;
204 }
205 if b < 0 {
206 b = -b;
207 sign = -sign;
208 }
209 let q = if b == 0 {
210 0x7FFFFFFF
211 } else {
212 ((((a as u64) << 16) + ((b as u64) >> 1)) / (b as u64)) as u32
213 };
214 Self(if sign < 0 {
215 (q as i32).wrapping_neg()
216 } else {
217 q as i32
218 })
219 }
220 }
221
222 impl DivAssign for $ty {
223 #[inline(always)]
224 fn div_assign(&mut self, rhs: Self) {
225 *self = *self / rhs;
226 }
227 }
228
229 impl Neg for $ty {
230 type Output = Self;
231 #[inline(always)]
232 fn neg(self) -> Self {
233 Self(-self.0)
234 }
235 }
236 };
237}
238
239macro_rules! float_conv {
244 ($name:ident, $to:ident, $from:ident, $ty:ty) => {
245 impl $name {
246 #[doc = concat!("Creates a fixed point value from a", stringify!($ty), ".")]
247 #[inline(always)]
251 pub fn $from(x: $ty) -> Self {
252 let frac = (x.is_sign_positive() as u8 as $ty) - 0.5;
255 Self((x * Self::ONE.0 as $ty + frac) as _)
256 }
257
258 #[doc = concat!("Returns the value as an ", stringify!($ty), ".")]
259 #[inline(always)]
263 pub fn $to(self) -> $ty {
264 let int = ((self.0 & Self::INT_MASK) >> Self::FRACT_BITS) as $ty;
265 let fract = (self.0 & !Self::INT_MASK) as $ty / Self::ONE.0 as $ty;
266 int + fract
267 }
268 }
269
270 impl std::fmt::Display for $name {
272 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
273 self.$to().fmt(f)
274 }
275 }
276
277 impl std::fmt::Debug for $name {
278 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
279 self.$to().fmt(f)
280 }
281 }
282 };
283}
284
285fixed_impl!(F2Dot14, 16, 14, i16);
286fixed_impl!(F4Dot12, 16, 12, i16);
287fixed_impl!(F6Dot10, 16, 10, i16);
288fixed_impl!(Fixed, 32, 16, i32);
289fixed_impl!(F26Dot6, 32, 6, i32);
290fixed_mul_div!(Fixed);
291fixed_mul_div!(F26Dot6);
292float_conv!(F2Dot14, to_f32, from_f32, f32);
293float_conv!(F4Dot12, to_f32, from_f32, f32);
294float_conv!(F6Dot10, to_f32, from_f32, f32);
295float_conv!(Fixed, to_f64, from_f64, f64);
296float_conv!(F26Dot6, to_f64, from_f64, f64);
297crate::newtype_scalar!(F2Dot14, [u8; 2]);
298crate::newtype_scalar!(F4Dot12, [u8; 2]);
299crate::newtype_scalar!(F6Dot10, [u8; 2]);
300crate::newtype_scalar!(Fixed, [u8; 4]);
301
302impl Fixed {
303 #[inline(always)]
305 pub const fn from_i32(i: i32) -> Self {
306 Self(i << 16)
307 }
308
309 #[inline(always)]
312 pub const fn to_i32(self) -> i32 {
313 self.0.wrapping_add(0x8000) >> 16
314 }
315
316 #[inline(always)]
318 pub const fn to_f26dot6(self) -> F26Dot6 {
319 F26Dot6(self.0.wrapping_add(0x200) >> 10)
320 }
321
322 #[inline(always)]
330 pub const fn to_f2dot14(self) -> F2Dot14 {
331 F2Dot14((self.0.wrapping_add(2) >> 2) as _)
332 }
333
334 #[inline(always)]
339 pub fn to_f32(self) -> f32 {
340 const SCALE_FACTOR: f32 = 1.0 / 65536.0;
341 self.0 as f32 * SCALE_FACTOR
342 }
343}
344
345impl From<i32> for Fixed {
346 fn from(value: i32) -> Self {
347 Self::from_i32(value)
348 }
349}
350
351impl F26Dot6 {
352 #[inline(always)]
354 pub const fn from_i32(i: i32) -> Self {
355 Self(i << 6)
356 }
357
358 #[inline(always)]
361 pub const fn to_i32(self) -> i32 {
362 self.0.wrapping_add(32) >> 6
363 }
364
365 #[inline(always)]
370 pub fn to_f32(self) -> f32 {
371 const SCALE_FACTOR: f32 = 1.0 / 64.0;
372 self.0 as f32 * SCALE_FACTOR
373 }
374}
375
376impl F2Dot14 {
377 #[inline(always)]
379 pub const fn to_fixed(self) -> Fixed {
380 Fixed(self.0 as i32 * 4)
381 }
382}
383
384#[cfg(test)]
385mod tests {
386 #![allow(overflowing_literals)] use super::*;
388
389 #[test]
390 fn f2dot14_floats() {
391 assert_eq!(F2Dot14(0x7fff), F2Dot14::from_f32(1.999939));
393 assert_eq!(F2Dot14(0x7000), F2Dot14::from_f32(1.75));
394 assert_eq!(F2Dot14(0x0001), F2Dot14::from_f32(0.0000610356));
395 assert_eq!(F2Dot14(0x0000), F2Dot14::from_f32(0.0));
396 assert_eq!(F2Dot14(0xffff), F2Dot14::from_f32(-0.000061));
397 assert_eq!(F2Dot14(0x8000), F2Dot14::from_f32(-2.0));
398 }
399
400 #[test]
401 fn roundtrip_f2dot14() {
402 for i in i16::MIN..=i16::MAX {
403 let val = F2Dot14(i);
404 assert_eq!(val, F2Dot14::from_f32(val.to_f32()));
405 }
406 }
407
408 #[test]
409 fn round_f2dot14() {
410 assert_eq!(F2Dot14(0x7000).round(), F2Dot14::from_f32(-2.0));
411 assert_eq!(F2Dot14(0x1F00).round(), F2Dot14::from_f32(0.0));
412 assert_eq!(F2Dot14(0x2000).round(), F2Dot14::from_f32(1.0));
413 }
414
415 #[test]
416 fn round_fixed() {
417 assert_eq!(Fixed(0x0001_7FFE).round(), Fixed(0x0001_0000));
419 assert_eq!(Fixed(0x0001_7FFF).round(), Fixed(0x0001_0000));
420 assert_eq!(Fixed(0x0001_8000).round(), Fixed(0x0002_0000));
421 }
422
423 #[test]
433 fn fixed_floats() {
434 assert_eq!(Fixed(0x7fff_0000), Fixed::from_f64(32767.));
435 assert_eq!(Fixed(0x7000_0001), Fixed::from_f64(28672.00001525879));
436 assert_eq!(Fixed(0x0001_0000), Fixed::from_f64(1.0));
437 assert_eq!(Fixed(0x0000_0000), Fixed::from_f64(0.0));
438 assert_eq!(
439 Fixed(i32::from_be_bytes([0xff; 4])),
440 Fixed::from_f64(-0.000015259)
441 );
442 assert_eq!(Fixed(0x7fff_ffff), Fixed::from_f64(32768.0));
443 }
444
445 #[test]
449 fn fixed_floats_rounding() {
450 fn with_round_intrinsic(x: f64) -> Fixed {
451 Fixed((x * 65536.0).round() as i32)
452 }
453 let inputs = [0.05, 0.6, 0.2, 0.4, 0.67755];
455 for input in inputs {
456 assert_eq!(Fixed::from_f64(input), with_round_intrinsic(input));
457 assert_eq!(Fixed::from_f64(-input), with_round_intrinsic(-input));
459 }
460 }
461
462 #[test]
463 fn fixed_to_int() {
464 assert_eq!(Fixed::from_f64(1.0).to_i32(), 1);
465 assert_eq!(Fixed::from_f64(1.5).to_i32(), 2);
466 assert_eq!(F26Dot6::from_f64(1.0).to_i32(), 1);
467 assert_eq!(F26Dot6::from_f64(1.5).to_i32(), 2);
468 }
469
470 #[test]
471 fn fixed_from_int() {
472 assert_eq!(Fixed::from_i32(1000).to_bits(), 1000 << 16);
473 assert_eq!(F26Dot6::from_i32(1000).to_bits(), 1000 << 6);
474 }
475
476 #[test]
477 fn fixed_to_f26dot6() {
478 assert_eq!(Fixed::from_f64(42.5).to_f26dot6(), F26Dot6::from_f64(42.5));
479 }
480
481 #[test]
482 fn fixed_muldiv() {
483 assert_eq!(
484 Fixed::from_f64(0.5) * Fixed::from_f64(2.0),
485 Fixed::from_f64(1.0)
486 );
487 assert_eq!(
488 Fixed::from_f64(0.5) / Fixed::from_f64(2.0),
489 Fixed::from_f64(0.25)
490 );
491 }
492
493 #[test]
497 fn fixed_div_neg_overflow() {
498 let a = Fixed::from_f64(-92.5);
499 let b = Fixed::from_f64(0.0028228759765625);
500 let _ = a / b;
502 }
503
504 #[test]
505 fn fixed_mul_div_neg_overflow() {
506 let a = Fixed::from_f64(-92.5);
507 let b = Fixed::from_f64(0.0028228759765625);
508 let _ = a.mul_div(Fixed::ONE, b);
510 }
511}