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 impl Neg for $name {
142 type Output = Self;
143 #[inline(always)]
144 fn neg(self) -> Self {
145 Self(-self.0)
146 }
147 }
148 };
149}
150
151impl Fixed {
152 #[inline]
156 pub const fn mul_div(&self, a: Self, b: Self) -> Self {
157 let mut sign = 1;
158 let mut su = self.0 as u64;
159 let mut au = a.0 as u64;
160 let mut bu = b.0 as u64;
161 if self.0 < 0 {
162 su = 0u64.wrapping_sub(su);
163 sign = -1;
164 }
165 if a.0 < 0 {
166 au = 0u64.wrapping_sub(au);
167 sign = -sign;
168 }
169 if b.0 < 0 {
170 bu = 0u64.wrapping_sub(bu);
171 sign = -sign;
172 }
173 let result = if bu > 0 {
174 su.wrapping_mul(au).wrapping_add(bu >> 1) / bu
175 } else {
176 0x7FFFFFFF
177 };
178 Self(if sign < 0 {
179 (result as i32).wrapping_neg()
180 } else {
181 result as i32
182 })
183 }
184}
185
186impl Mul for Fixed {
187 type Output = Self;
188
189 #[inline(always)]
190 fn mul(self, other: Self) -> Self::Output {
191 let ab = self.0 as i64 * other.0 as i64;
192 Self(((ab + 0x8000 - i64::from(ab < 0)) >> 16) as i32)
193 }
194}
195
196impl Div for Fixed {
197 type Output = Self;
198
199 fn div(self, other: Self) -> Self {
200 let sign = (self.0 < 0) ^ (other.0 < 0);
201 let au = self.0.unsigned_abs() as u64;
202 let bu = other.0.unsigned_abs() as u64;
203 let q = if bu == 0 {
204 0x7FFFFFFF_u32
205 } else {
206 (((au << 16) + (bu >> 1)) / bu) as u32
207 };
208 Self(if sign {
209 (q as i32).wrapping_neg()
210 } else {
211 q as i32
212 })
213 }
214}
215
216impl Mul for F26Dot6 {
217 type Output = Self;
218
219 #[inline(always)]
220 fn mul(self, other: Self) -> Self::Output {
221 let ab = self.0 as i64 * other.0 as i64;
222 Self(((ab + 32 - i64::from(ab < 0)) >> 6) as i32)
223 }
224}
225
226impl Div for F26Dot6 {
227 type Output = Self;
228
229 fn div(self, other: Self) -> Self {
230 let sign = (self.0 < 0) ^ (other.0 < 0);
231 let au = self.0.unsigned_abs() as u64;
232 let bu = other.0.unsigned_abs() as u64;
233 let q = if bu == 0 {
234 0x7FFFFFFF_u32
235 } else {
236 (((au << 6) + (bu >> 1)) / bu) as u32
237 };
238 Self(if sign {
239 (q as i32).wrapping_neg()
240 } else {
241 q as i32
242 })
243 }
244}
245
246macro_rules! fixed_mul_div_assign {
249 ($ty:ty) => {
250 impl MulAssign for $ty {
251 #[inline(always)]
252 fn mul_assign(&mut self, rhs: Self) {
253 *self = *self * rhs;
254 }
255 }
256
257 impl DivAssign for $ty {
258 #[inline(always)]
259 fn div_assign(&mut self, rhs: Self) {
260 *self = *self / rhs;
261 }
262 }
263 };
264}
265
266macro_rules! float_conv {
271 ($name:ident, $to:ident, $from:ident, $ty:ty) => {
273 float_conv!($name, $to, $from, $ty, no_fmt);
274
275 impl std::fmt::Display for $name {
277 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
278 self.$to().fmt(f)
279 }
280 }
281
282 impl std::fmt::Debug for $name {
283 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
284 self.$to().fmt(f)
285 }
286 }
287 };
288 ($name:ident, $to:ident, $from:ident, $ty:ty, no_fmt) => {
290 impl $name {
291 #[doc = concat!("Creates a fixed point value from a", stringify!($ty), ".")]
292 #[inline(always)]
296 pub fn $from(x: $ty) -> Self {
297 let frac = (x.is_sign_positive() as u8 as $ty) - 0.5;
300 Self((x * Self::ONE.0 as $ty + frac) as _)
301 }
302
303 #[doc = concat!("Returns the value as an ", stringify!($ty), ".")]
304 #[inline(always)]
308 pub fn $to(self) -> $ty {
309 let int = ((self.0 & Self::INT_MASK) >> Self::FRACT_BITS) as $ty;
310 let fract = (self.0 & !Self::INT_MASK) as $ty / Self::ONE.0 as $ty;
311 int + fract
312 }
313 }
314 };
315}
316
317fixed_impl!(F2Dot14, 16, 14, i16);
318fixed_impl!(F4Dot12, 16, 12, i16);
319fixed_impl!(F6Dot10, 16, 10, i16);
320fixed_impl!(Fixed, 32, 16, i32);
321fixed_impl!(F26Dot6, 32, 6, i32);
322
323fixed_mul_div_assign!(Fixed);
324fixed_mul_div_assign!(F26Dot6);
325
326float_conv!(F2Dot14, to_f32, from_f32, f32);
327float_conv!(F4Dot12, to_f32, from_f32, f32);
328float_conv!(F6Dot10, to_f32, from_f32, f32);
329float_conv!(F2Dot14, to_f64, from_f64, f64, no_fmt);
330float_conv!(F4Dot12, to_f64, from_f64, f64, no_fmt);
331float_conv!(F6Dot10, to_f64, from_f64, f64, no_fmt);
332
333float_conv!(Fixed, to_f64, from_f64, f64);
334float_conv!(F26Dot6, to_f64, from_f64, f64);
335crate::newtype_scalar!(F2Dot14, [u8; 2]);
336crate::newtype_scalar!(F4Dot12, [u8; 2]);
337crate::newtype_scalar!(F6Dot10, [u8; 2]);
338crate::newtype_scalar!(Fixed, [u8; 4]);
339
340impl Fixed {
341 #[inline(always)]
343 pub const fn from_i32(i: i32) -> Self {
344 Self(i << 16)
345 }
346
347 #[inline(always)]
350 pub const fn to_i32(self) -> i32 {
351 self.0.wrapping_add(0x8000) >> 16
352 }
353
354 #[inline(always)]
356 pub const fn to_f26dot6(self) -> F26Dot6 {
357 F26Dot6(self.0.wrapping_add(0x200) >> 10)
358 }
359
360 #[inline(always)]
368 pub const fn to_f2dot14(self) -> F2Dot14 {
369 F2Dot14((self.0.wrapping_add(2) >> 2) as _)
370 }
371
372 #[inline(always)]
377 pub fn to_f32(self) -> f32 {
378 const SCALE_FACTOR: f32 = 1.0 / 65536.0;
379 self.0 as f32 * SCALE_FACTOR
380 }
381}
382
383impl From<i32> for Fixed {
384 fn from(value: i32) -> Self {
385 Self::from_i32(value)
386 }
387}
388
389impl F26Dot6 {
390 #[inline(always)]
392 pub const fn from_i32(i: i32) -> Self {
393 Self(i << 6)
394 }
395
396 #[inline(always)]
399 pub const fn to_i32(self) -> i32 {
400 self.0.wrapping_add(32) >> 6
401 }
402
403 #[inline(always)]
408 pub fn to_f32(self) -> f32 {
409 const SCALE_FACTOR: f32 = 1.0 / 64.0;
410 self.0 as f32 * SCALE_FACTOR
411 }
412}
413
414impl F2Dot14 {
415 #[inline(always)]
417 pub const fn to_fixed(self) -> Fixed {
418 Fixed(self.0 as i32 * 4)
419 }
420}
421
422#[cfg(test)]
423mod tests {
424 #![allow(overflowing_literals)] use super::*;
426
427 #[test]
428 fn f2dot14_floats() {
429 assert_eq!(F2Dot14(0x7fff), F2Dot14::from_f32(1.999939));
431 assert_eq!(F2Dot14(0x7000), F2Dot14::from_f32(1.75));
432 assert_eq!(F2Dot14(0x0001), F2Dot14::from_f32(0.0000610356));
433 assert_eq!(F2Dot14(0x0000), F2Dot14::from_f32(0.0));
434 assert_eq!(F2Dot14(0xffff), F2Dot14::from_f32(-0.000061));
435 assert_eq!(F2Dot14(0x8000), F2Dot14::from_f32(-2.0));
436 }
437
438 #[test]
439 fn roundtrip_f2dot14() {
440 for i in i16::MIN..=i16::MAX {
441 let val = F2Dot14(i);
442 assert_eq!(val, F2Dot14::from_f32(val.to_f32()));
443 }
444 }
445
446 #[test]
447 fn round_f2dot14() {
448 assert_eq!(F2Dot14(0x7000).round(), F2Dot14::from_f32(-2.0));
449 assert_eq!(F2Dot14(0x1F00).round(), F2Dot14::from_f32(0.0));
450 assert_eq!(F2Dot14(0x2000).round(), F2Dot14::from_f32(1.0));
451 }
452
453 #[test]
454 fn round_fixed() {
455 assert_eq!(Fixed(0x0001_7FFE).round(), Fixed(0x0001_0000));
457 assert_eq!(Fixed(0x0001_7FFF).round(), Fixed(0x0001_0000));
458 assert_eq!(Fixed(0x0001_8000).round(), Fixed(0x0002_0000));
459 }
460
461 #[test]
471 fn fixed_floats() {
472 assert_eq!(Fixed(0x7fff_0000), Fixed::from_f64(32767.));
473 assert_eq!(Fixed(0x7000_0001), Fixed::from_f64(28672.00001525879));
474 assert_eq!(Fixed(0x0001_0000), Fixed::from_f64(1.0));
475 assert_eq!(Fixed(0x0000_0000), Fixed::from_f64(0.0));
476 assert_eq!(
477 Fixed(i32::from_be_bytes([0xff; 4])),
478 Fixed::from_f64(-0.000015259)
479 );
480 assert_eq!(Fixed(0x7fff_ffff), Fixed::from_f64(32768.0));
481 }
482
483 #[test]
487 fn fixed_floats_rounding() {
488 fn with_round_intrinsic(x: f64) -> Fixed {
489 Fixed((x * 65536.0).round() as i32)
490 }
491 let inputs = [0.05, 0.6, 0.2, 0.4, 0.67755];
493 for input in inputs {
494 assert_eq!(Fixed::from_f64(input), with_round_intrinsic(input));
495 assert_eq!(Fixed::from_f64(-input), with_round_intrinsic(-input));
497 }
498 }
499
500 #[test]
501 fn fixed_to_int() {
502 assert_eq!(Fixed::from_f64(1.0).to_i32(), 1);
503 assert_eq!(Fixed::from_f64(1.5).to_i32(), 2);
504 assert_eq!(F26Dot6::from_f64(1.0).to_i32(), 1);
505 assert_eq!(F26Dot6::from_f64(1.5).to_i32(), 2);
506 }
507
508 #[test]
509 fn fixed_from_int() {
510 assert_eq!(Fixed::from_i32(1000).to_bits(), 1000 << 16);
511 assert_eq!(F26Dot6::from_i32(1000).to_bits(), 1000 << 6);
512 }
513
514 #[test]
515 fn fixed_to_f26dot6() {
516 assert_eq!(Fixed::from_f64(42.5).to_f26dot6(), F26Dot6::from_f64(42.5));
517 }
518
519 #[test]
520 fn fixed_muldiv() {
521 assert_eq!(
522 Fixed::from_f64(0.5) * Fixed::from_f64(2.0),
523 Fixed::from_f64(1.0)
524 );
525 assert_eq!(
526 Fixed::from_f64(0.5) / Fixed::from_f64(2.0),
527 Fixed::from_f64(0.25)
528 );
529 }
530
531 #[test]
535 fn fixed_div_neg_overflow() {
536 let a = Fixed::from_f64(-92.5);
537 let b = Fixed::from_f64(0.0028228759765625);
538 let _ = a / b;
540 }
541
542 #[test]
543 fn fixed_mul_div_neg_overflow() {
544 let a = Fixed::from_f64(-92.5);
545 let b = Fixed::from_f64(0.0028228759765625);
546 let _ = a.mul_div(Fixed::ONE, b);
548 }
549
550 #[test]
551 fn fixed_div_min_value() {
552 let min = Fixed(i32::MIN);
554 let one = Fixed::ONE;
555 let _ = min / one;
557 let neg_one = Fixed(-Fixed::ONE.0);
559 let _ = min / neg_one;
560 }
561
562 #[test]
563 fn f26dot6_muldiv() {
564 assert_eq!(
565 F26Dot6::from_f64(0.5) * F26Dot6::from_f64(2.0),
566 F26Dot6::from_f64(1.0)
567 );
568 assert_eq!(
569 F26Dot6::from_f64(0.5) * F26Dot6::from_f64(-2.4),
570 F26Dot6::from_f64(-1.2)
571 );
572 assert_eq!(F26Dot6::ONE * F26Dot6::ONE, F26Dot6::ONE);
573 assert_eq!(
574 F26Dot6::from_f64(0.5) / F26Dot6::from_f64(2.0),
575 F26Dot6::from_f64(0.25)
576 );
577 assert_eq!(
578 F26Dot6::from_f64(0.5) / F26Dot6::from_f64(-2.4),
579 F26Dot6::from_f64(-0.20833333333333334)
580 );
581 assert_eq!(
582 F26Dot6::from_f64(2.0) / F26Dot6::from_f64(3.0),
583 F26Dot6::from_f64(0.6666666666666666)
584 );
585 assert_eq!(F26Dot6::ONE / F26Dot6::ONE, F26Dot6::ONE);
586 assert_eq!(F26Dot6::ONE / F26Dot6::ZERO, F26Dot6(0x7FFFFFFF));
587 assert_eq!(-F26Dot6::ONE / F26Dot6::ZERO, F26Dot6(-0x7FFFFFFF));
588 }
589}