474 lines
14 KiB
Rust
474 lines
14 KiB
Rust
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>
|
|
{
|
|
#[inline]
|
|
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>{
|
|
#[inline]
|
|
fn eq(&self,other:&Self)->bool{
|
|
self.bits.eq(&other.bits)
|
|
}
|
|
}
|
|
impl<const N:usize,const F:usize,T> PartialEq<T> for Fixed<N,F>
|
|
where
|
|
T:Copy,
|
|
BInt::<N>:From<T>,
|
|
{
|
|
#[inline]
|
|
fn eq(&self,&other:&T)->bool{
|
|
self.bits.eq(&other.into())
|
|
}
|
|
}
|
|
impl<const N:usize,const F:usize> Eq for Fixed<N,F>{}
|
|
|
|
impl<const N:usize,const F:usize> PartialOrd for Fixed<N,F>{
|
|
#[inline]
|
|
fn partial_cmp(&self,other:&Self)->Option<std::cmp::Ordering>{
|
|
self.bits.partial_cmp(&other.bits)
|
|
}
|
|
}
|
|
impl<const N:usize,const F:usize,T> PartialOrd<T> for Fixed<N,F>
|
|
where
|
|
T:Copy,
|
|
BInt::<N>:From<T>,
|
|
{
|
|
#[inline]
|
|
fn partial_cmp(&self,&other:&T)->Option<std::cmp::Ordering>{
|
|
self.bits.partial_cmp(&other.into())
|
|
}
|
|
}
|
|
impl<const N:usize,const F:usize> Ord for Fixed<N,F>{
|
|
#[inline]
|
|
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;
|
|
#[inline]
|
|
fn neg(self)->Self{
|
|
Self::from_bits(self.bits.neg())
|
|
}
|
|
}
|
|
impl<const N:usize,const F:usize> std::iter::Sum for Fixed<N,F>{
|
|
#[inline]
|
|
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;
|
|
#[inline]
|
|
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;
|
|
#[inline]
|
|
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>{
|
|
#[inline]
|
|
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>,
|
|
{
|
|
#[inline]
|
|
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;
|
|
#[inline]
|
|
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>{
|
|
#[inline]
|
|
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="deferred-division")))]
|
|
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;
|
|
#[inline]
|
|
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,
|
|
{
|
|
#[inline]
|
|
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="deferred-division")]
|
|
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=ratio_ops::ratio::Ratio<Fixed<LHS_N,LHS_F>,Fixed<RHS_N,RHS_F>>;
|
|
#[inline]
|
|
fn div(self, other: Fixed<RHS_N,RHS_F>)->Self::Output{
|
|
ratio_ops::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;
|
|
#[inline]
|
|
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>{
|
|
#[inline]
|
|
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}>;
|
|
#[inline]
|
|
fn mul(self, other: Fixed<$rhs,{$rhs*32}>)->Self::Output{
|
|
paste::item!{
|
|
self.[<wide_mul_ $lhs _ $rhs>](other)
|
|
}
|
|
}
|
|
}
|
|
#[cfg(not(feature="deferred-division"))]
|
|
impl core::ops::Div<Fixed<$rhs,{$rhs*32}>> for Fixed<$lhs,{$lhs*32}>{
|
|
type Output=Fixed<{$lhs+$rhs},{($lhs+$rhs)*32}>;
|
|
#[inline]
|
|
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!{
|
|
#[inline]
|
|
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.
|
|
#[inline]
|
|
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>{
|
|
#[inline]
|
|
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}>{
|
|
#[inline]
|
|
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!{
|
|
#[inline]
|
|
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)
|
|
//TODO: use resize method
|
|
let wide_self:<Self as core::ops::Mul>::Output=self*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*new_result<=wide_self{
|
|
result=new_result;
|
|
}
|
|
}
|
|
result
|
|
}
|
|
}
|
|
#[inline]
|
|
pub fn sqrt(self)->Self{
|
|
if self<Self::ZERO{
|
|
panic!("Square root less than zero")
|
|
}else{
|
|
self.sqrt_unchecked()
|
|
}
|
|
}
|
|
#[inline]
|
|
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);
|