strafe-project/fixed_wide/src/fixed.rs

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

#[derive(Clone,Copy,Debug,Hash)]
/// 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())
	}
}

macro_rules! impl_additive_operator {
	( $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		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::from_bits(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>,
		{
			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)<<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 );

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

			fn $method(self, other: Self) -> Self::Output {
				//this can be done better but that is a job for later
				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_())
			}
		}
	};
}
macro_rules! impl_multiply_assign_operator_const {
	( $width:expr, $struct: ident, $trait: ident, $method: ident ) => {
		impl<const F:usize> core::ops::$trait for $struct<$width,F>{
			fn $method(&mut self, other: Self) {
				self.bits.$method(other.bits);
			}
		}
	};
}

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

			fn $method(self, other: Self) -> Self::Output {
				//this can be done better but that is a job for later
				//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_())
			}
		}
	};
}
macro_rules! impl_divide_assign_operator_const {
	( $width:expr, $struct: ident, $trait: ident, $method: ident ) => {
		impl<const F:usize> core::ops::$trait for $struct<$width,F>{
			fn $method(&mut self, other: Self) {
				self.bits.$method(other.bits);
			}
		}
	};
}

macro_rules! impl_multiplicatave_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_multiplicatave_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! impl_operator_16 {
	( $macro: ident, $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		$macro!(1,$struct,$trait,$method,$output);
		$macro!(2,$struct,$trait,$method,$output);
		$macro!(3,$struct,$trait,$method,$output);
		$macro!(4,$struct,$trait,$method,$output);
		$macro!(5,$struct,$trait,$method,$output);
		$macro!(6,$struct,$trait,$method,$output);
		$macro!(7,$struct,$trait,$method,$output);
		$macro!(8,$struct,$trait,$method,$output);
		$macro!(9,$struct,$trait,$method,$output);
		$macro!(10,$struct,$trait,$method,$output);
		$macro!(11,$struct,$trait,$method,$output);
		$macro!(12,$struct,$trait,$method,$output);
		$macro!(13,$struct,$trait,$method,$output);
		$macro!(14,$struct,$trait,$method,$output);
		$macro!(15,$struct,$trait,$method,$output);
		$macro!(16,$struct,$trait,$method,$output);
	}
}
macro_rules! impl_assign_operator_16 {
	( $macro: ident, $struct: ident, $trait: ident, $method: ident ) => {
		$macro!(1,$struct,$trait,$method);
		$macro!(2,$struct,$trait,$method);
		$macro!(3,$struct,$trait,$method);
		$macro!(4,$struct,$trait,$method);
		$macro!(5,$struct,$trait,$method);
		$macro!(6,$struct,$trait,$method);
		$macro!(7,$struct,$trait,$method);
		$macro!(8,$struct,$trait,$method);
		$macro!(9,$struct,$trait,$method);
		$macro!(10,$struct,$trait,$method);
		$macro!(11,$struct,$trait,$method);
		$macro!(12,$struct,$trait,$method);
		$macro!(13,$struct,$trait,$method);
		$macro!(14,$struct,$trait,$method);
		$macro!(15,$struct,$trait,$method);
		$macro!(16,$struct,$trait,$method);
	}
}

impl_assign_operator_16!( impl_multiply_assign_operator_const, Fixed, MulAssign, mul_assign );
impl_operator_16!( impl_multiply_operator_const, Fixed, Mul, mul, Self );
impl_assign_operator_16!( impl_divide_assign_operator_const, Fixed, DivAssign, div_assign );
impl_operator_16!( impl_divide_operator_const, Fixed, Div, div, Self );
impl_multiplicatave_assign_operator!( Fixed, MulAssign, mul_assign );
impl_multiplicatave_operator!( Fixed, Mul, mul, Self );
impl_multiplicatave_assign_operator!( Fixed, DivAssign, div_assign );
impl_multiplicatave_operator!( Fixed, Div, div, Self );

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 MUL: multiply into a wider type
// let a = I32F32::ONE;
// let b:I64F64 = a.wide_mul(a);
macro_rules! impl_wide_mul{
	($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}>{
					Fixed::from_bits(self.bits.as_::<BInt<{$lhs+$rhs}>>()*rhs.bits.as_::<BInt<{$lhs+$rhs}>>())
				}
			}
		}
	};
}

macro_rules! impl_wide_mul_all{
	($(($x:expr, $y:expr)),*)=>{
		$(
			impl_wide_mul!($x, $y);
		)*
	};
}

//const generics sidestepped wahoo
impl_wide_mul_all!(
	(1,1),(2,1),(3,1),(4,1),(5,1),(6,1),(7,1),(8,1),
	(1,2),(2,2),(3,2),(4,2),(5,2),(6,2),(7,2),(8,2),
	(1,3),(2,3),(3,3),(4,3),(5,3),(6,3),(7,3),(8,3),
	(1,4),(2,4),(3,4),(4,4),(5,4),(6,4),(7,4),(8,4),
	(1,5),(2,5),(3,5),(4,5),(5,5),(6,5),(7,5),(8,5),
	(1,6),(2,6),(3,6),(4,6),(5,6),(6,6),(7,6),(8,6),
	(1,7),(2,7),(3,7),(4,7),(5,7),(6,7),(7,7),(8,7),
	(1,8),(2,8),(3,8),(4,8),(5,8),(6,8),(7,8),(8,8)
);
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_const{
	($n:expr)=>{
		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_const!(1);
impl_const!(2);
impl_const!(3);
impl_const!(4);
impl_const!(5);
impl_const!(6);
impl_const!(7);
impl_const!(8);