strafe-project/fixed_wide/src/fixed.rs

use bnum::{BInt,cast::As};

#[derive(Clone,Copy,Debug,Hash)]
/// A Fixed point number for which multiply operations widen the bits in the output. (when the wide-mul feature is enabled)
/// N is the number of u64s to use
/// F is the number of fractional bits (always N*32 lol)
pub struct Fixed<const N:usize,const F:usize>{
	pub(crate)bits:BInt<{N}>,
}

impl<const N:usize,const F:usize> Fixed<N,F>{
	pub const MAX:Self=Self::from_bits(BInt::<N>::MAX);
	pub const MIN:Self=Self::from_bits(BInt::<N>::MIN);
	pub const ZERO:Self=Self::from_bits(BInt::<N>::ZERO);
	pub const EPSILON:Self=Self::from_bits(BInt::<N>::ONE);
	pub const NEG_EPSILON:Self=Self::from_bits(BInt::<N>::NEG_ONE);
	pub const ONE:Self=Self::from_bits(BInt::<N>::ONE.shl(F as u32));
	pub const TWO:Self=Self::from_bits(BInt::<N>::TWO.shl(F as u32));
	pub const HALF:Self=Self::from_bits(BInt::<N>::ONE.shl(F as u32-1));
	pub const NEG_ONE:Self=Self::from_bits(BInt::<N>::NEG_ONE.shl(F as u32));
	pub const NEG_TWO:Self=Self::from_bits(BInt::<N>::NEG_TWO.shl(F as u32));
	pub const NEG_HALF:Self=Self::from_bits(BInt::<N>::NEG_ONE.shl(F as u32-1));
}
impl<const N:usize,const F:usize> Fixed<N,F>{
	#[inline]
	pub const fn from_bits(bits:BInt::<N>)->Self{
		Self{
			bits,
		}
	}
	#[inline]
	pub const fn to_bits(self)->BInt<N>{
		self.bits
	}
	#[inline]
	pub const fn raw(value:i64)->Self{
		Self::from_bits(BInt::from_bits(bnum::BUint::from_digit(value as u64)))
	}
}

impl<const N:usize,const F:usize,T> From<T> for Fixed<N,F>
	where
		BInt<N>:From<T>
{
	fn from(value:T)->Self{
		Self::from_bits(BInt::<{N}>::from(value)<<F as u32)
	}
}

impl<const N:usize,const F:usize> PartialEq for Fixed<N,F>{
	fn eq(&self,other:&Self)->bool{
		self.bits.eq(&other.bits)
	}
}
impl<const N:usize,const F:usize> Eq for Fixed<N,F>{}

impl<const N:usize,const F:usize> PartialOrd for Fixed<N,F>{
	fn partial_cmp(&self,other:&Self)->Option<std::cmp::Ordering>{
		self.bits.partial_cmp(&other.bits)
	}
}
impl<const N:usize,const F:usize> Ord for Fixed<N,F>{
	fn cmp(&self,other:&Self)->std::cmp::Ordering{
		self.bits.cmp(&other.bits)
	}
}

impl<const N:usize,const F:usize> std::ops::Neg for Fixed<N,F>{
	type Output=Self;
	fn neg(self)->Self{
		Self::from_bits(self.bits.neg())
	}
}
impl<const N:usize,const F:usize> std::iter::Sum for Fixed<N,F>{
	fn sum<I:Iterator<Item=Self>>(iter:I)->Self{
		let mut sum=Self::ZERO;
		for elem in iter{
			sum+=elem;
		}
		sum
	}
}

macro_rules! impl_additive_operator {
	( $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		impl<const N:usize,const F:usize> $struct<N,F>{
			#[inline]
			pub const fn $method(self, other: Self) -> Self {
				Self::from_bits(self.bits.$method(other.bits))
			}
		}
		impl<const N:usize,const F:usize> core::ops::$trait for $struct<N,F>{
			type Output = $output;
			fn $method(self, other: Self) -> Self::Output {
				self.$method(other)
			}
		}
		impl<const N:usize,const F:usize,U> core::ops::$trait<U> for $struct<N,F>
			where
				BInt::<N>:From<U>,
		{
			type Output = $output;
			fn $method(self, other: U) -> Self::Output {
				Self::from_bits(self.bits.$method(BInt::<N>::from(other).shl(F as u32)))
			}
		}
	};
}
macro_rules! impl_additive_assign_operator {
	( $struct: ident, $trait: ident, $method: ident ) => {
		impl<const N:usize,const F:usize> core::ops::$trait for $struct<N,F>{
			fn $method(&mut self, other: Self) {
				self.bits.$method(other.bits);
			}
		}
		impl<const N:usize,const F:usize,U> core::ops::$trait<U> for $struct<N,F>
		where
				BInt::<N>:From<U>,
		{
			fn $method(&mut self, other: U) {
				self.bits.$method(BInt::<N>::from(other).shl(F as u32));
			}
		}
	};
}

// Impl arithmetic pperators
impl_additive_assign_operator!( Fixed, AddAssign, add_assign );
impl_additive_operator!( Fixed, Add, add, Self );
impl_additive_assign_operator!( Fixed, SubAssign, sub_assign );
impl_additive_operator!( Fixed, Sub, sub, Self );
impl_additive_assign_operator!( Fixed, RemAssign, rem_assign );
impl_additive_operator!( Fixed, Rem, rem, Self );

// Impl bitwise operators
impl_additive_assign_operator!( Fixed, BitAndAssign, bitand_assign );
impl_additive_operator!( Fixed, BitAnd, bitand, Self );
impl_additive_assign_operator!( Fixed, BitOrAssign, bitor_assign );
impl_additive_operator!( Fixed, BitOr, bitor, Self );
impl_additive_assign_operator!( Fixed, BitXorAssign, bitxor_assign );
impl_additive_operator!( Fixed, BitXor, bitxor, Self );

// non-wide operators.  The result is the same width as the inputs.

// This macro is not used in the default configuration.
#[allow(unused_macros)]
macro_rules! impl_multiplicative_operator_not_const_generic {
	( ($struct: ident, $trait: ident, $method: ident, $output: ty ), $width:expr ) => {
		impl<const F:usize> core::ops::$trait for $struct<$width,F>{
			type Output = $output;
			fn $method(self, other: Self) -> Self::Output {
				paste::item!{
				self.[<fixed_ $method>](other)
				}
			}
		}
	};
}
macro_rules! impl_multiplicative_assign_operator_not_const_generic {
	( ($struct: ident, $trait: ident, $method: ident, $non_assign_method: ident ), $width:expr ) => {
		impl<const F:usize> core::ops::$trait for $struct<$width,F>{
			fn $method(&mut self, other: Self) {
				paste::item!{
				*self=self.[<fixed_ $non_assign_method>](other);
				}
			}
		}
	};
}

macro_rules! impl_multiply_operator_not_const_generic {
	( ($struct: ident, $trait: ident, $method: ident, $output: ty ), $width:expr ) => {
		impl<const F:usize> $struct<$width,F>{
			paste::item!{
			#[inline]
			pub fn [<fixed_ $method>](self, other: Self) -> Self {
				let lhs=self.bits.as_::<BInt::<{$width*2}>>();
				let rhs=other.bits.as_::<BInt::<{$width*2}>>();
				Self::from_bits(lhs.mul(rhs).shr(F as u32).as_())
			}
			}
		}
		#[cfg(not(feature="wide-mul"))]
		impl_multiplicative_operator_not_const_generic!(($struct, $trait, $method, $output ), $width);
	}
}
macro_rules! impl_divide_operator_not_const_generic {
	( ($struct: ident, $trait: ident, $method: ident, $output: ty ), $width:expr ) => {
		impl<const F:usize> $struct<$width,F>{
			paste::item!{
			#[inline]
			pub fn [<fixed_ $method>](self, other: Self) -> Self {
				//this only needs to be $width+F as u32/64+1 but MUH CONST GENERICS!!!!!
				let lhs=self.bits.as_::<BInt::<{$width*2}>>().shl(F as u32);
				let rhs=other.bits.as_::<BInt::<{$width*2}>>();
				Self::from_bits(lhs.div(rhs).as_())
			}
			}
		}
		#[cfg(all(not(feature="wide-mul"),not(feature="ratio")))]
		impl_multiplicative_operator_not_const_generic!(($struct, $trait, $method, $output ), $width);
	};
}

macro_rules! impl_multiplicative_operator {
	( $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		impl<const N:usize,const F:usize,U> core::ops::$trait<U> for $struct<N,F>
			where
				BInt::<N>:From<U>+core::ops::$trait,
		{
			type Output = $output;

			fn $method(self, other: U) -> Self::Output {
				Self::from_bits(self.bits.$method(BInt::<N>::from(other)))
			}
		}
	};
}
macro_rules! impl_multiplicative_assign_operator {
	( $struct: ident, $trait: ident, $method: ident ) => {
		impl<const N:usize,const F:usize,U> core::ops::$trait<U> for $struct<N,F>
			where
				BInt::<N>:From<U>+core::ops::$trait,
		{
			fn $method(&mut self, other: U) {
				self.bits.$method(BInt::<N>::from(other));
			}
		}
	};
}

macro_rules! macro_repeated{
	(
		$macro:ident,
		$any:tt,
		$($repeated:tt),*
	)=>{
		$(
			$macro!($any, $repeated);
		)*
	};
}

macro_rules! macro_16 {
	( $macro: ident, $any:tt ) => {
		macro_repeated!($macro,$any,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16);
	}
}

macro_16!( impl_multiplicative_assign_operator_not_const_generic, (Fixed, MulAssign, mul_assign, mul) );
macro_16!( impl_multiply_operator_not_const_generic, (Fixed, Mul, mul, Self) );
macro_16!( impl_multiplicative_assign_operator_not_const_generic, (Fixed, DivAssign, div_assign, div) );
macro_16!( impl_divide_operator_not_const_generic, (Fixed, Div, div, Self) );
impl_multiplicative_assign_operator!( Fixed, MulAssign, mul_assign );
impl_multiplicative_operator!( Fixed, Mul, mul, Self );
impl_multiplicative_assign_operator!( Fixed, DivAssign, div_assign );
impl_multiplicative_operator!( Fixed, Div, div, Self );
#[cfg(feature="ratio")]
impl<const LHS_N:usize,const LHS_F:usize,const RHS_N:usize,const RHS_F:usize> core::ops::Div<Fixed<RHS_N,RHS_F>> for Fixed<LHS_N,LHS_F>{
	type Output=crate::ratio::Ratio<Fixed<LHS_N,LHS_F>,Fixed<RHS_N,RHS_F>>;
	fn div(self, other: Fixed<RHS_N,RHS_F>)->Self::Output{
		crate::ratio::Ratio::new(self,other)
	}
}

macro_rules! impl_shift_operator {
	( $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		impl<const N:usize,const F:usize> core::ops::$trait<u32> for $struct<N,F>{
			type Output = $output;

			fn $method(self, other: u32) -> Self::Output {
				Self::from_bits(self.bits.$method(other))
			}
		}
	};
}
macro_rules! impl_shift_assign_operator {
	( $struct: ident, $trait: ident, $method: ident ) => {
		impl<const N:usize,const F:usize> core::ops::$trait<u32> for $struct<N,F>{
			fn $method(&mut self, other: u32) {
				self.bits.$method(other);
			}
		}
	};
}
impl_shift_assign_operator!( Fixed, ShlAssign, shl_assign );
impl_shift_operator!( Fixed, Shl, shl, Self );
impl_shift_assign_operator!( Fixed, ShrAssign, shr_assign );
impl_shift_operator!( Fixed, Shr, shr, Self );

// wide operators.  The result width is the sum of the input widths, i.e. none of the multiplication

macro_rules! impl_wide_operators{
	($lhs:expr,$rhs:expr)=>{
		impl core::ops::Mul<Fixed<$rhs,{$rhs*32}>> for Fixed<$lhs,{$lhs*32}>{
			type Output=Fixed<{$lhs+$rhs},{($lhs+$rhs)*32}>;
			fn mul(self, other: Fixed<$rhs,{$rhs*32}>)->Self::Output{
				paste::item!{
				self.[<wide_mul_ $lhs _ $rhs>](other)
				}
			}
		}
		#[cfg(not(feature="ratio"))]
		impl core::ops::Div<Fixed<$rhs,{$rhs*32}>> for Fixed<$lhs,{$lhs*32}>{
			type Output=Fixed<{$lhs+$rhs},{($lhs+$rhs)*32}>;
			fn div(self, other: Fixed<$rhs,{$rhs*32}>)->Self::Output{
				paste::item!{
				self.[<wide_div_ $lhs _ $rhs>](other)
				}
			}
		}
	}
}

// WIDE MUL: multiply into a wider type
// let a = I32F32::ONE;
// let b:I64F64 = a.wide_mul(a);
macro_rules! impl_wide_not_const_generic{
	(
		(),
		($lhs:expr,$rhs:expr)
	)=>{
		impl Fixed<$lhs,{$lhs*32}>
		{
			paste::item!{
				pub fn [<wide_mul_ $lhs _ $rhs>](self,rhs:Fixed<$rhs,{$rhs*32}>)->Fixed<{$lhs+$rhs},{($lhs+$rhs)*32}>{
					let lhs=self.bits.as_::<BInt<{$lhs+$rhs}>>();
					let rhs=rhs.bits.as_::<BInt<{$lhs+$rhs}>>();
					Fixed::from_bits(lhs*rhs)
				}
				/// This operation cannot represent the fraction exactly,
				/// but it shapes the output to have precision for the
				/// largest and smallest possible fractions.
				pub fn [<wide_div_ $lhs _ $rhs>](self,rhs:Fixed<$rhs,{$rhs*32}>)->Fixed<{$lhs+$rhs},{($lhs+$rhs)*32}>{
					// (lhs/2^LHS_FRAC)/(rhs/2^RHS_FRAC)
					let lhs=self.bits.as_::<BInt<{$lhs+$rhs}>>().shl($rhs*64);
					let rhs=rhs.bits.as_::<BInt<{$lhs+$rhs}>>();
					Fixed::from_bits(lhs/rhs)
				}
			}
		}
		#[cfg(feature="wide-mul")]
		impl_wide_operators!($lhs,$rhs);
	};
}

//const generics sidestepped wahoo
macro_repeated!(
	impl_wide_not_const_generic,(),
	(1,1),(2,1),(3,1),(4,1),(5,1),(6,1),(7,1),(8,1),(9,1),(10,1),(11,1),(12,1),(13,1),(14,1),(15,1),
	(1,2),(2,2),(3,2),(4,2),(5,2),(6,2),(7,2),(8,2),(9,2),(10,2),(11,2),(12,2),(13,2),(14,2),
	(1,3),(2,3),(3,3),(4,3),(5,3),(6,3),(7,3),(8,3),(9,3),(10,3),(11,3),(12,3),(13,3),
	(1,4),(2,4),(3,4),(4,4),(5,4),(6,4),(7,4),(8,4),(9,4),(10,4),(11,4),(12,4),
	(1,5),(2,5),(3,5),(4,5),(5,5),(6,5),(7,5),(8,5),(9,5),(10,5),(11,5),
	(1,6),(2,6),(3,6),(4,6),(5,6),(6,6),(7,6),(8,6),(9,6),(10,6),
	(1,7),(2,7),(3,7),(4,7),(5,7),(6,7),(7,7),(8,7),(9,7),
	(1,8),(2,8),(3,8),(4,8),(5,8),(6,8),(7,8),(8,8),
	(1,9),(2,9),(3,9),(4,9),(5,9),(6,9),(7,9),
	(1,10),(2,10),(3,10),(4,10),(5,10),(6,10),
	(1,11),(2,11),(3,11),(4,11),(5,11),
	(1,12),(2,12),(3,12),(4,12),
	(1,13),(2,13),(3,13),
	(1,14),(2,14),
	(1,15)
);
impl<const SRC:usize,const F:usize> Fixed<SRC,F>{
	pub fn resize_into<const DST:usize>(self)->Fixed<DST,F>{
		Fixed::from_bits(self.bits.as_::<BInt<DST>>())
	}
}

macro_rules! impl_not_const_generic{
	($n:expr)=>{
		impl Fixed<{$n*2},{$n*2*32}>{
			pub fn halve_precision(self)->Fixed<$n,{$n*32}>{
				Fixed::from_bits(bnum::cast::As::as_(self.bits.shr($n*32)))
			}
		}
		impl Fixed<$n,{$n*32}>{
			paste::item!{
			pub fn sqrt_unchecked(self)->Self{
				//1<<max_shift must be the minimum power of two which when squared is greater than self
				//calculating max_shift:
				//1. count "used" bits to the left of the decimal, not including the sign bit (so -1)
				//2. divide by 2 via >>1 (sqrt-ish)
				//3. add on fractional offset
				//Voila
				let used_bits=self.bits.bits() as i32-1-($n*32) as i32;
				let max_shift=((used_bits>>1)+($n*32) as i32) as u32;
				let mut result=Self::ZERO;

				//multiply by one to make the types match (hack)
				let wide_self=self.[<wide_mul_ $n _ $n>](Self::ONE);
				//descend down the bits and check if flipping each bit would push the square over the input value
				for shift in (0..=max_shift).rev(){
					let new_result=result|Self::from_bits(BInt::from_bits(bnum::BUint::power_of_two(shift)));
					if new_result.[<wide_mul_ $n _ $n>](new_result)<=wide_self{
						result=new_result;
					}
				}
				result
			}
			}
			pub fn sqrt(self)->Self{
				if self<Self::ZERO{
					panic!("Square root less than zero")
				}else{
					self.sqrt_unchecked()
				}
			}
			pub fn sqrt_checked(self)->Option<Self>{
				if self<Self::ZERO{
					None
				}else{
					Some(self.sqrt_unchecked())
				}
			}
		}
	}
}
impl_not_const_generic!(1);
impl_not_const_generic!(2);
impl_not_const_generic!(3);
impl_not_const_generic!(4);
impl_not_const_generic!(5);
impl_not_const_generic!(6);
impl_not_const_generic!(7);
impl_not_const_generic!(8);