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 = "bytemuck", derive(bytemuck::AnyBitPattern, bytemuck::NoUninit))]
10 #[repr(transparent)]
11 #[doc = concat!(stringify!($bits), "-bit signed fixed point number with ", stringify!($fract_bits), " bits of fraction." )]
12 pub struct $name($ty);
13 impl $name {
14 pub const MIN: Self = Self(<$ty>::MIN);
16
17 pub const MAX: Self = Self(<$ty>::MAX);
19
20 pub const EPSILON: Self = Self(1);
22
23 pub const ZERO: Self = Self(0);
25
26 pub const ONE: Self = Self(1 << $fract_bits);
28
29 pub const NEG_ONE: Self = Self((!0 << $fract_bits) as $ty);
31
32 const INT_MASK: $ty = !0 << $fract_bits;
33 const ROUND: $ty = 1 << ($fract_bits - 1);
34 const FRACT_BITS: usize = $fract_bits;
35
36 #[inline(always)]
38 pub const fn from_bits(bits: $ty) -> Self {
39 Self(bits)
40 }
41
42 #[inline(always)]
44 pub const fn to_bits(self) -> $ty {
45 self.0
46 }
47
48 #[inline(always)]
51 pub const fn round(self) -> Self {
52 Self(self.0.wrapping_add(Self::ROUND) & Self::INT_MASK)
53 }
54
55 #[inline(always)]
57 pub const fn abs(self) -> Self {
58 Self(self.0.abs())
59 }
60
61 #[inline(always)]
63 pub const fn floor(self) -> Self {
64 Self(self.0 & Self::INT_MASK)
65 }
66
67 #[inline(always)]
69 pub const fn fract(self) -> Self {
70 Self(self.0 - self.floor().0)
71 }
72
73 #[inline(always)]
75 pub fn wrapping_add(self, other: Self) -> Self {
76 Self(self.0.wrapping_add(other.0))
77 }
78
79 #[inline(always)]
81 pub const fn saturating_add(self, other: Self) -> Self {
82 Self(self.0.saturating_add(other.0))
83 }
84
85 #[inline(always)]
87 pub fn checked_add(self, other: Self) -> Option<Self> {
88 self.0.checked_add(other.0).map(|inner| Self(inner))
89 }
90
91 #[inline(always)]
93 pub const fn wrapping_sub(self, other: Self) -> Self {
94 Self(self.0.wrapping_sub(other.0))
95 }
96
97 #[inline(always)]
99 pub const fn saturating_sub(self, other: Self) -> Self {
100 Self(self.0.saturating_sub(other.0))
101 }
102
103 #[inline(always)]
105 pub const fn to_be_bytes(self) -> [u8; $bits / 8] {
106 self.0.to_be_bytes()
107 }
108 }
109
110 impl Add for $name {
111 type Output = Self;
112 #[inline(always)]
113 fn add(self, other: Self) -> Self {
114 Self(self.0.wrapping_add(other.0))
115 }
116 }
117
118 impl AddAssign for $name {
119 #[inline(always)]
120 fn add_assign(&mut self, other: Self) {
121 *self = *self + other;
122 }
123 }
124
125 impl Sub for $name {
126 type Output = Self;
127 #[inline(always)]
128 fn sub(self, other: Self) -> Self {
129 Self(self.0.wrapping_sub(other.0))
130 }
131 }
132
133 impl SubAssign for $name {
134 #[inline(always)]
135 fn sub_assign(&mut self, other: Self) {
136 *self = *self - other;
137 }
138 }
139
140 impl Neg for $name {
141 type Output = Self;
142 #[inline(always)]
143 fn neg(self) -> Self {
144 Self(-self.0)
145 }
146 }
147 };
148}
149
150impl Fixed {
151 #[inline]
155 pub const fn mul_div(&self, a: Self, b: Self) -> Self {
156 let mut sign = 1;
157 let mut su = self.0 as u64;
158 let mut au = a.0 as u64;
159 let mut bu = b.0 as u64;
160 if self.0 < 0 {
161 su = 0u64.wrapping_sub(su);
162 sign = -1;
163 }
164 if a.0 < 0 {
165 au = 0u64.wrapping_sub(au);
166 sign = -sign;
167 }
168 if b.0 < 0 {
169 bu = 0u64.wrapping_sub(bu);
170 sign = -sign;
171 }
172 let result = if bu > 0 {
173 su.wrapping_mul(au).wrapping_add(bu >> 1) / bu
174 } else {
175 0x7FFFFFFF
176 };
177 Self(if sign < 0 {
178 (result as i32).wrapping_neg()
179 } else {
180 result as i32
181 })
182 }
183}
184
185impl Mul for Fixed {
186 type Output = Self;
187
188 #[inline(always)]
189 fn mul(self, other: Self) -> Self::Output {
190 let ab = self.0 as i64 * other.0 as i64;
191 Self(((ab + 0x8000 - i64::from(ab < 0)) >> 16) as i32)
192 }
193}
194
195impl Div for Fixed {
196 type Output = Self;
197
198 fn div(self, other: Self) -> Self {
199 let sign = (self.0 < 0) ^ (other.0 < 0);
200 let au = self.0.unsigned_abs() as u64;
201 let bu = other.0.unsigned_abs() as u64;
202 let q = if bu == 0 {
203 0x7FFFFFFF_u32
204 } else {
205 (((au << 16) + (bu >> 1)) / bu) as u32
206 };
207 Self(if sign {
208 (q as i32).wrapping_neg()
209 } else {
210 q as i32
211 })
212 }
213}
214
215impl Mul for F26Dot6 {
216 type Output = Self;
217
218 #[inline(always)]
219 fn mul(self, other: Self) -> Self::Output {
220 let ab = self.0 as i64 * other.0 as i64;
221 Self(((ab + 32 - i64::from(ab < 0)) >> 6) as i32)
222 }
223}
224
225impl Div for F26Dot6 {
226 type Output = Self;
227
228 fn div(self, other: Self) -> Self {
229 let sign = (self.0 < 0) ^ (other.0 < 0);
230 let au = self.0.unsigned_abs() as u64;
231 let bu = other.0.unsigned_abs() as u64;
232 let q = if bu == 0 {
233 0x7FFFFFFF_u32
234 } else {
235 (((au << 6) + (bu >> 1)) / bu) as u32
236 };
237 Self(if sign {
238 (q as i32).wrapping_neg()
239 } else {
240 q as i32
241 })
242 }
243}
244
245macro_rules! fixed_mul_div_assign {
248 ($ty:ty) => {
249 impl MulAssign for $ty {
250 #[inline(always)]
251 fn mul_assign(&mut self, rhs: Self) {
252 *self = *self * rhs;
253 }
254 }
255
256 impl DivAssign for $ty {
257 #[inline(always)]
258 fn div_assign(&mut self, rhs: Self) {
259 *self = *self / rhs;
260 }
261 }
262 };
263}
264
265macro_rules! float_conv {
270 ($name:ident, $to:ident, $from:ident, $ty:ty) => {
272 float_conv!($name, $to, $from, $ty, no_fmt);
273
274 impl std::fmt::Display for $name {
276 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
277 self.$to().fmt(f)
278 }
279 }
280
281 impl std::fmt::Debug for $name {
282 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
283 self.$to().fmt(f)
284 }
285 }
286
287 #[cfg(feature = "serde")]
288 impl ::serde::Serialize for $name {
289 fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
290 where
291 S: ::serde::Serializer,
292 {
293 <$ty>::serialize(&$name::$to(*self), serializer)
294 }
295 }
296
297 #[cfg(feature = "serde")]
298 impl<'de> ::serde::Deserialize<'de> for $name {
299 fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
300 where
301 D: ::serde::Deserializer<'de>,
302 {
303 <$ty>::deserialize(deserializer).map($name::$from)
304 }
305 }
306 };
307 ($name:ident, $to:ident, $from:ident, $ty:ty, no_fmt) => {
310 impl $name {
311 #[doc = concat!("Creates a fixed point value from a ", stringify!($ty), ".")]
312 #[inline(always)]
316 pub fn $from(x: $ty) -> Self {
317 let frac = (x.is_sign_positive() as u8 as $ty) - 0.5;
320 Self((x * Self::ONE.0 as $ty + frac) as _)
321 }
322
323 #[doc = concat!("Returns the value as an ", stringify!($ty), ".")]
324 #[inline(always)]
328 pub fn $to(self) -> $ty {
329 let int = ((self.0 & Self::INT_MASK) >> Self::FRACT_BITS) as $ty;
330 let fract = (self.0 & !Self::INT_MASK) as $ty / Self::ONE.0 as $ty;
331 int + fract
332 }
333 }
334 };
335}
336
337fixed_impl!(F2Dot14, 16, 14, i16);
338fixed_impl!(F4Dot12, 16, 12, i16);
339fixed_impl!(F6Dot10, 16, 10, i16);
340fixed_impl!(Fixed, 32, 16, i32);
341fixed_impl!(F26Dot6, 32, 6, i32);
342
343fixed_mul_div_assign!(Fixed);
344fixed_mul_div_assign!(F26Dot6);
345
346float_conv!(F2Dot14, to_f32, from_f32, f32);
347float_conv!(F4Dot12, to_f32, from_f32, f32);
348float_conv!(F6Dot10, to_f32, from_f32, f32);
349float_conv!(F2Dot14, to_f64, from_f64, f64, no_fmt);
350float_conv!(F4Dot12, to_f64, from_f64, f64, no_fmt);
351float_conv!(F6Dot10, to_f64, from_f64, f64, no_fmt);
352
353float_conv!(Fixed, to_f64, from_f64, f64);
354float_conv!(F26Dot6, to_f64, from_f64, f64);
355crate::newtype_scalar!(F2Dot14, [u8; 2]);
356crate::newtype_scalar!(F4Dot12, [u8; 2]);
357crate::newtype_scalar!(F6Dot10, [u8; 2]);
358crate::newtype_scalar!(Fixed, [u8; 4]);
359
360impl Fixed {
361 #[inline(always)]
363 pub const fn from_i32(i: i32) -> Self {
364 Self(i << 16)
365 }
366
367 #[inline(always)]
370 pub const fn to_i32(self) -> i32 {
371 self.0.wrapping_add(0x8000) >> 16
372 }
373
374 #[inline(always)]
376 pub const fn to_f26dot6(self) -> F26Dot6 {
377 F26Dot6(self.0.wrapping_add(0x200) >> 10)
378 }
379
380 #[inline(always)]
388 pub const fn to_f2dot14(self) -> F2Dot14 {
389 F2Dot14((self.0.wrapping_add(2) >> 2) as _)
390 }
391
392 #[inline(always)]
397 pub fn to_f32(self) -> f32 {
398 const SCALE_FACTOR: f32 = 1.0 / 65536.0;
399 self.0 as f32 * SCALE_FACTOR
400 }
401}
402
403impl From<i32> for Fixed {
404 fn from(value: i32) -> Self {
405 Self::from_i32(value)
406 }
407}
408
409impl F26Dot6 {
410 #[inline(always)]
412 pub const fn from_i32(i: i32) -> Self {
413 Self(i << 6)
414 }
415
416 #[inline(always)]
419 pub const fn to_i32(self) -> i32 {
420 self.0.wrapping_add(32) >> 6
421 }
422
423 #[inline(always)]
428 pub fn to_f32(self) -> f32 {
429 const SCALE_FACTOR: f32 = 1.0 / 64.0;
430 self.0 as f32 * SCALE_FACTOR
431 }
432}
433
434impl F2Dot14 {
435 #[inline(always)]
437 pub const fn to_fixed(self) -> Fixed {
438 Fixed(self.0 as i32 * 4)
439 }
440}
441
442#[cfg(test)]
443mod tests {
444 #![allow(overflowing_literals)] use super::*;
446
447 #[test]
448 fn f2dot14_floats() {
449 assert_eq!(F2Dot14(0x7fff), F2Dot14::from_f32(1.999939));
451 assert_eq!(F2Dot14(0x7000), F2Dot14::from_f32(1.75));
452 assert_eq!(F2Dot14(0x0001), F2Dot14::from_f32(0.0000610356));
453 assert_eq!(F2Dot14(0x0000), F2Dot14::from_f32(0.0));
454 assert_eq!(F2Dot14(0xffff), F2Dot14::from_f32(-0.000061));
455 assert_eq!(F2Dot14(0x8000), F2Dot14::from_f32(-2.0));
456 }
457
458 #[test]
459 fn roundtrip_f2dot14() {
460 for i in i16::MIN..=i16::MAX {
461 let val = F2Dot14(i);
462 assert_eq!(val, F2Dot14::from_f32(val.to_f32()));
463 }
464 }
465
466 #[test]
467 fn round_f2dot14() {
468 assert_eq!(F2Dot14(0x7000).round(), F2Dot14::from_f32(-2.0));
469 assert_eq!(F2Dot14(0x1F00).round(), F2Dot14::from_f32(0.0));
470 assert_eq!(F2Dot14(0x2000).round(), F2Dot14::from_f32(1.0));
471 }
472
473 #[test]
474 fn round_fixed() {
475 assert_eq!(Fixed(0x0001_7FFE).round(), Fixed(0x0001_0000));
477 assert_eq!(Fixed(0x0001_7FFF).round(), Fixed(0x0001_0000));
478 assert_eq!(Fixed(0x0001_8000).round(), Fixed(0x0002_0000));
479 }
480
481 #[test]
491 fn fixed_floats() {
492 assert_eq!(Fixed(0x7fff_0000), Fixed::from_f64(32767.));
493 assert_eq!(Fixed(0x7000_0001), Fixed::from_f64(28672.00001525879));
494 assert_eq!(Fixed(0x0001_0000), Fixed::from_f64(1.0));
495 assert_eq!(Fixed(0x0000_0000), Fixed::from_f64(0.0));
496 assert_eq!(
497 Fixed(i32::from_be_bytes([0xff; 4])),
498 Fixed::from_f64(-0.000015259)
499 );
500 assert_eq!(Fixed(0x7fff_ffff), Fixed::from_f64(32768.0));
501 }
502
503 #[test]
507 fn fixed_floats_rounding() {
508 fn with_round_intrinsic(x: f64) -> Fixed {
509 Fixed((x * 65536.0).round() as i32)
510 }
511 let inputs = [0.05, 0.6, 0.2, 0.4, 0.67755];
513 for input in inputs {
514 assert_eq!(Fixed::from_f64(input), with_round_intrinsic(input));
515 assert_eq!(Fixed::from_f64(-input), with_round_intrinsic(-input));
517 }
518 }
519
520 #[test]
521 fn fixed_to_int() {
522 assert_eq!(Fixed::from_f64(1.0).to_i32(), 1);
523 assert_eq!(Fixed::from_f64(1.5).to_i32(), 2);
524 assert_eq!(F26Dot6::from_f64(1.0).to_i32(), 1);
525 assert_eq!(F26Dot6::from_f64(1.5).to_i32(), 2);
526 }
527
528 #[test]
529 fn fixed_from_int() {
530 assert_eq!(Fixed::from_i32(1000).to_bits(), 1000 << 16);
531 assert_eq!(F26Dot6::from_i32(1000).to_bits(), 1000 << 6);
532 }
533
534 #[test]
535 fn fixed_to_f26dot6() {
536 assert_eq!(Fixed::from_f64(42.5).to_f26dot6(), F26Dot6::from_f64(42.5));
537 }
538
539 #[test]
540 fn fixed_muldiv() {
541 assert_eq!(
542 Fixed::from_f64(0.5) * Fixed::from_f64(2.0),
543 Fixed::from_f64(1.0)
544 );
545 assert_eq!(
546 Fixed::from_f64(0.5) / Fixed::from_f64(2.0),
547 Fixed::from_f64(0.25)
548 );
549 }
550
551 #[test]
555 fn fixed_div_neg_overflow() {
556 let a = Fixed::from_f64(-92.5);
557 let b = Fixed::from_f64(0.0028228759765625);
558 let _ = a / b;
560 }
561
562 #[test]
563 fn fixed_mul_div_neg_overflow() {
564 let a = Fixed::from_f64(-92.5);
565 let b = Fixed::from_f64(0.0028228759765625);
566 let _ = a.mul_div(Fixed::ONE, b);
568 }
569
570 #[test]
571 fn fixed_div_min_value() {
572 let min = Fixed(i32::MIN);
574 let one = Fixed::ONE;
575 let _ = min / one;
577 let neg_one = Fixed(-Fixed::ONE.0);
579 let _ = min / neg_one;
580 }
581
582 #[test]
583 fn f26dot6_muldiv() {
584 assert_eq!(
585 F26Dot6::from_f64(0.5) * F26Dot6::from_f64(2.0),
586 F26Dot6::from_f64(1.0)
587 );
588 assert_eq!(
589 F26Dot6::from_f64(0.5) * F26Dot6::from_f64(-2.4),
590 F26Dot6::from_f64(-1.2)
591 );
592 assert_eq!(F26Dot6::ONE * F26Dot6::ONE, F26Dot6::ONE);
593 assert_eq!(
594 F26Dot6::from_f64(0.5) / F26Dot6::from_f64(2.0),
595 F26Dot6::from_f64(0.25)
596 );
597 assert_eq!(
598 F26Dot6::from_f64(0.5) / F26Dot6::from_f64(-2.4),
599 F26Dot6::from_f64(-0.20833333333333334)
600 );
601 assert_eq!(
602 F26Dot6::from_f64(2.0) / F26Dot6::from_f64(3.0),
603 F26Dot6::from_f64(0.6666666666666666)
604 );
605 assert_eq!(F26Dot6::ONE / F26Dot6::ONE, F26Dot6::ONE);
606 assert_eq!(F26Dot6::ONE / F26Dot6::ZERO, F26Dot6(0x7FFFFFFF));
607 assert_eq!(-F26Dot6::ONE / F26Dot6::ZERO, F26Dot6(-0x7FFFFFFF));
608 }
609
610 #[cfg(feature = "serde")]
611 mod serde {
612 use super::*;
613
614 macro_rules! roundtrip_one {
615 ($fixed:ident) => {{
616 let before = <$fixed>::ONE;
617 let serialized = ::serde_json::to_string(&before).expect("should serialize");
618 assert_eq!(&serialized, "1.0");
619 let after = ::serde_json::from_str(&serialized).expect("should deserialize");
620 assert_eq!(before, after);
621 }};
622 }
623
624 #[test]
625 fn one_is_one_f2dot14() {
626 roundtrip_one!(F2Dot14);
627 }
628
629 #[test]
630 fn one_is_one_f4dot12() {
631 roundtrip_one!(F4Dot12);
632 }
633
634 #[test]
635 fn one_is_one_f6dot10() {
636 roundtrip_one!(F6Dot10);
637 }
638
639 #[test]
640 fn one_is_one_fixed() {
641 roundtrip_one!(Fixed);
642 }
643
644 #[test]
645 fn one_is_one_f26dot6() {
646 roundtrip_one!(F26Dot6);
647 }
648
649 macro_rules! roundtrip_all {
650 ($fixed:ident, $ty:ty) => {
651 for raw in <$ty>::MIN..=<$ty>::MAX {
652 let fixed = $fixed(raw);
653 let fixed_float = fixed.to_f64();
654
655 let json_value = ::serde_json::to_value(&fixed).expect("should serialize");
656 let json_float = json_value
657 .as_f64()
658 .expect("serde didn't serialize the value to a float");
659
660 assert_eq!(
664 fixed_float,
665 json_float,
666 "failed on {raw} ({fixed_type}({raw:#X})): {json_float} != {fixed_float}",
667 fixed_type = ::std::stringify!($fixed),
668 );
669 }
670 };
671 }
672
673 #[test]
674 fn roundtrip_all_f2dot14() {
675 roundtrip_all!(F2Dot14, i16);
676 }
677
678 #[test]
679 fn roundtrip_all_f4dot12() {
680 roundtrip_all!(F4Dot12, i16);
681 }
682
683 #[test]
684 fn roundtrip_all_f6dot10() {
685 roundtrip_all!(F6Dot10, i16);
686 }
687
688 #[test]
689 #[ignore = "enumerating all i32 values takes a while"]
690 fn roundtrip_all_fixed() {
691 roundtrip_all!(Fixed, i32);
692 }
693
694 #[test]
695 #[ignore = "enumerating all i32 values takes a while"]
696 fn roundtrip_all_f26dot6() {
697 roundtrip_all!(F26Dot6, i32);
698 }
699 }
700}