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 pub const NEG_ONE: Self = Self((!0 << $fract_bits) as $ty);
32
33 const INT_MASK: $ty = !0 << $fract_bits;
34 const ROUND: $ty = 1 << ($fract_bits - 1);
35 const FRACT_BITS: usize = $fract_bits;
36
37 #[inline(always)]
39 pub const fn from_bits(bits: $ty) -> Self {
40 Self(bits)
41 }
42
43 #[inline(always)]
45 pub const fn to_bits(self) -> $ty {
46 self.0
47 }
48
49 #[inline(always)]
52 pub const fn round(self) -> Self {
53 Self(self.0.wrapping_add(Self::ROUND) & Self::INT_MASK)
54 }
55
56 #[inline(always)]
58 pub const fn abs(self) -> Self {
59 Self(self.0.abs())
60 }
61
62 #[inline(always)]
64 pub const fn floor(self) -> Self {
65 Self(self.0 & Self::INT_MASK)
66 }
67
68 #[inline(always)]
70 pub const fn fract(self) -> Self {
71 Self(self.0 - self.floor().0)
72 }
73
74 #[inline(always)]
76 pub fn wrapping_add(self, other: Self) -> Self {
77 Self(self.0.wrapping_add(other.0))
78 }
79
80 #[inline(always)]
82 pub const fn saturating_add(self, other: Self) -> Self {
83 Self(self.0.saturating_add(other.0))
84 }
85
86 #[inline(always)]
88 pub fn checked_add(self, other: Self) -> Option<Self> {
89 self.0.checked_add(other.0).map(|inner| Self(inner))
90 }
91
92 #[inline(always)]
94 pub const fn wrapping_sub(self, other: Self) -> Self {
95 Self(self.0.wrapping_sub(other.0))
96 }
97
98 #[inline(always)]
100 pub const fn saturating_sub(self, other: Self) -> Self {
101 Self(self.0.saturating_sub(other.0))
102 }
103
104 #[inline(always)]
106 pub const fn to_be_bytes(self) -> [u8; $bits / 8] {
107 self.0.to_be_bytes()
108 }
109 }
110
111 impl Add for $name {
112 type Output = Self;
113 #[inline(always)]
114 fn add(self, other: Self) -> Self {
115 Self(self.0.wrapping_add(other.0))
116 }
117 }
118
119 impl AddAssign for $name {
120 #[inline(always)]
121 fn add_assign(&mut self, other: Self) {
122 *self = *self + other;
123 }
124 }
125
126 impl Sub for $name {
127 type Output = Self;
128 #[inline(always)]
129 fn sub(self, other: Self) -> Self {
130 Self(self.0.wrapping_sub(other.0))
131 }
132 }
133
134 impl SubAssign for $name {
135 #[inline(always)]
136 fn sub_assign(&mut self, other: Self) {
137 *self = *self - other;
138 }
139 }
140 };
141}
142
143macro_rules! fixed_mul_div {
145 ($ty:ty) => {
146 impl $ty {
147 #[inline]
151 pub const fn mul_div(&self, a: Self, b: Self) -> Self {
152 let mut sign = 1;
153 let mut su = self.0 as u64;
154 let mut au = a.0 as u64;
155 let mut bu = b.0 as u64;
156 if self.0 < 0 {
157 su = 0u64.wrapping_sub(su);
158 sign = -1;
159 }
160 if a.0 < 0 {
161 au = 0u64.wrapping_sub(au);
162 sign = -sign;
163 }
164 if b.0 < 0 {
165 bu = 0u64.wrapping_sub(bu);
166 sign = -sign;
167 }
168 let result = if bu > 0 {
169 su.wrapping_mul(au).wrapping_add(bu >> 1) / bu
170 } else {
171 0x7FFFFFFF
172 };
173 Self(if sign < 0 {
174 (result as i32).wrapping_neg()
175 } else {
176 result as i32
177 })
178 }
179 }
180
181 impl Mul for $ty {
182 type Output = Self;
183 #[inline(always)]
184 fn mul(self, other: Self) -> Self::Output {
185 let ab = self.0 as i64 * other.0 as i64;
186 Self(((ab + 0x8000 - i64::from(ab < 0)) >> 16) as i32)
187 }
188 }
189
190 impl MulAssign for $ty {
191 #[inline(always)]
192 fn mul_assign(&mut self, rhs: Self) {
193 *self = *self * rhs;
194 }
195 }
196
197 impl Div for $ty {
198 type Output = Self;
199 #[inline(always)]
200 fn div(self, other: Self) -> Self::Output {
201 let mut sign = 1;
202 let mut a = self.0;
203 let mut b = other.0;
204 if a < 0 {
205 a = -a;
206 sign = -1;
207 }
208 if b < 0 {
209 b = -b;
210 sign = -sign;
211 }
212 let q = if b == 0 {
213 0x7FFFFFFF
214 } else {
215 ((((a as u64) << 16) + ((b as u64) >> 1)) / (b as u64)) as u32
216 };
217 Self(if sign < 0 {
218 (q as i32).wrapping_neg()
219 } else {
220 q as i32
221 })
222 }
223 }
224
225 impl DivAssign for $ty {
226 #[inline(always)]
227 fn div_assign(&mut self, rhs: Self) {
228 *self = *self / rhs;
229 }
230 }
231
232 impl Neg for $ty {
233 type Output = Self;
234 #[inline(always)]
235 fn neg(self) -> Self {
236 Self(-self.0)
237 }
238 }
239 };
240}
241
242macro_rules! float_conv {
247 ($name:ident, $to:ident, $from:ident, $ty:ty) => {
248 impl $name {
249 #[doc = concat!("Creates a fixed point value from a", stringify!($ty), ".")]
250 #[inline(always)]
254 pub fn $from(x: $ty) -> Self {
255 let frac = (x.is_sign_positive() as u8 as $ty) - 0.5;
258 Self((x * Self::ONE.0 as $ty + frac) as _)
259 }
260
261 #[doc = concat!("Returns the value as an ", stringify!($ty), ".")]
262 #[inline(always)]
266 pub fn $to(self) -> $ty {
267 let int = ((self.0 & Self::INT_MASK) >> Self::FRACT_BITS) as $ty;
268 let fract = (self.0 & !Self::INT_MASK) as $ty / Self::ONE.0 as $ty;
269 int + fract
270 }
271 }
272
273 impl std::fmt::Display for $name {
275 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
276 self.$to().fmt(f)
277 }
278 }
279
280 impl std::fmt::Debug for $name {
281 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
282 self.$to().fmt(f)
283 }
284 }
285 };
286}
287
288fixed_impl!(F2Dot14, 16, 14, i16);
289fixed_impl!(F4Dot12, 16, 12, i16);
290fixed_impl!(F6Dot10, 16, 10, i16);
291fixed_impl!(Fixed, 32, 16, i32);
292fixed_impl!(F26Dot6, 32, 6, i32);
293fixed_mul_div!(Fixed);
294fixed_mul_div!(F26Dot6);
295float_conv!(F2Dot14, to_f32, from_f32, f32);
296float_conv!(F4Dot12, to_f32, from_f32, f32);
297float_conv!(F6Dot10, to_f32, from_f32, f32);
298float_conv!(Fixed, to_f64, from_f64, f64);
299float_conv!(F26Dot6, to_f64, from_f64, f64);
300crate::newtype_scalar!(F2Dot14, [u8; 2]);
301crate::newtype_scalar!(F4Dot12, [u8; 2]);
302crate::newtype_scalar!(F6Dot10, [u8; 2]);
303crate::newtype_scalar!(Fixed, [u8; 4]);
304
305impl Fixed {
306 #[inline(always)]
308 pub const fn from_i32(i: i32) -> Self {
309 Self(i << 16)
310 }
311
312 #[inline(always)]
315 pub const fn to_i32(self) -> i32 {
316 self.0.wrapping_add(0x8000) >> 16
317 }
318
319 #[inline(always)]
321 pub const fn to_f26dot6(self) -> F26Dot6 {
322 F26Dot6(self.0.wrapping_add(0x200) >> 10)
323 }
324
325 #[inline(always)]
333 pub const fn to_f2dot14(self) -> F2Dot14 {
334 F2Dot14((self.0.wrapping_add(2) >> 2) as _)
335 }
336
337 #[inline(always)]
342 pub fn to_f32(self) -> f32 {
343 const SCALE_FACTOR: f32 = 1.0 / 65536.0;
344 self.0 as f32 * SCALE_FACTOR
345 }
346}
347
348impl From<i32> for Fixed {
349 fn from(value: i32) -> Self {
350 Self::from_i32(value)
351 }
352}
353
354impl F26Dot6 {
355 #[inline(always)]
357 pub const fn from_i32(i: i32) -> Self {
358 Self(i << 6)
359 }
360
361 #[inline(always)]
364 pub const fn to_i32(self) -> i32 {
365 self.0.wrapping_add(32) >> 6
366 }
367
368 #[inline(always)]
373 pub fn to_f32(self) -> f32 {
374 const SCALE_FACTOR: f32 = 1.0 / 64.0;
375 self.0 as f32 * SCALE_FACTOR
376 }
377}
378
379impl F2Dot14 {
380 #[inline(always)]
382 pub const fn to_fixed(self) -> Fixed {
383 Fixed(self.0 as i32 * 4)
384 }
385}
386
387#[cfg(test)]
388mod tests {
389 #![allow(overflowing_literals)] use super::*;
391
392 #[test]
393 fn f2dot14_floats() {
394 assert_eq!(F2Dot14(0x7fff), F2Dot14::from_f32(1.999939));
396 assert_eq!(F2Dot14(0x7000), F2Dot14::from_f32(1.75));
397 assert_eq!(F2Dot14(0x0001), F2Dot14::from_f32(0.0000610356));
398 assert_eq!(F2Dot14(0x0000), F2Dot14::from_f32(0.0));
399 assert_eq!(F2Dot14(0xffff), F2Dot14::from_f32(-0.000061));
400 assert_eq!(F2Dot14(0x8000), F2Dot14::from_f32(-2.0));
401 }
402
403 #[test]
404 fn roundtrip_f2dot14() {
405 for i in i16::MIN..=i16::MAX {
406 let val = F2Dot14(i);
407 assert_eq!(val, F2Dot14::from_f32(val.to_f32()));
408 }
409 }
410
411 #[test]
412 fn round_f2dot14() {
413 assert_eq!(F2Dot14(0x7000).round(), F2Dot14::from_f32(-2.0));
414 assert_eq!(F2Dot14(0x1F00).round(), F2Dot14::from_f32(0.0));
415 assert_eq!(F2Dot14(0x2000).round(), F2Dot14::from_f32(1.0));
416 }
417
418 #[test]
419 fn round_fixed() {
420 assert_eq!(Fixed(0x0001_7FFE).round(), Fixed(0x0001_0000));
422 assert_eq!(Fixed(0x0001_7FFF).round(), Fixed(0x0001_0000));
423 assert_eq!(Fixed(0x0001_8000).round(), Fixed(0x0002_0000));
424 }
425
426 #[test]
436 fn fixed_floats() {
437 assert_eq!(Fixed(0x7fff_0000), Fixed::from_f64(32767.));
438 assert_eq!(Fixed(0x7000_0001), Fixed::from_f64(28672.00001525879));
439 assert_eq!(Fixed(0x0001_0000), Fixed::from_f64(1.0));
440 assert_eq!(Fixed(0x0000_0000), Fixed::from_f64(0.0));
441 assert_eq!(
442 Fixed(i32::from_be_bytes([0xff; 4])),
443 Fixed::from_f64(-0.000015259)
444 );
445 assert_eq!(Fixed(0x7fff_ffff), Fixed::from_f64(32768.0));
446 }
447
448 #[test]
452 fn fixed_floats_rounding() {
453 fn with_round_intrinsic(x: f64) -> Fixed {
454 Fixed((x * 65536.0).round() as i32)
455 }
456 let inputs = [0.05, 0.6, 0.2, 0.4, 0.67755];
458 for input in inputs {
459 assert_eq!(Fixed::from_f64(input), with_round_intrinsic(input));
460 assert_eq!(Fixed::from_f64(-input), with_round_intrinsic(-input));
462 }
463 }
464
465 #[test]
466 fn fixed_to_int() {
467 assert_eq!(Fixed::from_f64(1.0).to_i32(), 1);
468 assert_eq!(Fixed::from_f64(1.5).to_i32(), 2);
469 assert_eq!(F26Dot6::from_f64(1.0).to_i32(), 1);
470 assert_eq!(F26Dot6::from_f64(1.5).to_i32(), 2);
471 }
472
473 #[test]
474 fn fixed_from_int() {
475 assert_eq!(Fixed::from_i32(1000).to_bits(), 1000 << 16);
476 assert_eq!(F26Dot6::from_i32(1000).to_bits(), 1000 << 6);
477 }
478
479 #[test]
480 fn fixed_to_f26dot6() {
481 assert_eq!(Fixed::from_f64(42.5).to_f26dot6(), F26Dot6::from_f64(42.5));
482 }
483
484 #[test]
485 fn fixed_muldiv() {
486 assert_eq!(
487 Fixed::from_f64(0.5) * Fixed::from_f64(2.0),
488 Fixed::from_f64(1.0)
489 );
490 assert_eq!(
491 Fixed::from_f64(0.5) / Fixed::from_f64(2.0),
492 Fixed::from_f64(0.25)
493 );
494 }
495
496 #[test]
500 fn fixed_div_neg_overflow() {
501 let a = Fixed::from_f64(-92.5);
502 let b = Fixed::from_f64(0.0028228759765625);
503 let _ = a / b;
505 }
506
507 #[test]
508 fn fixed_mul_div_neg_overflow() {
509 let a = Fixed::from_f64(-92.5);
510 let b = Fixed::from_f64(0.0028228759765625);
511 let _ = a.mul_div(Fixed::ONE, b);
513 }
514}