375 lines
12 KiB
Rust
375 lines
12 KiB
Rust
use bnum::{BInt,cast::As};
|
|
use typenum::{Sum,Unsigned};
|
|
use crate::traits::WideMul;
|
|
|
|
#[derive(Clone,Copy,Debug,Hash)]
|
|
pub struct Fixed<const CHUNKS:usize,Frac>{
|
|
pub(crate)bits:BInt<{CHUNKS}>,
|
|
pub(crate)frac:std::marker::PhantomData<Frac>,
|
|
}
|
|
|
|
impl<const CHUNKS:usize,Frac:Unsigned> Fixed<CHUNKS,Frac>{
|
|
pub const MAX:Self=Self::from_bits(BInt::<CHUNKS>::MAX);
|
|
pub const MIN:Self=Self::from_bits(BInt::<CHUNKS>::MIN);
|
|
pub const ZERO:Self=Self::from_bits(BInt::<CHUNKS>::ZERO);
|
|
pub const EPSILON:Self=Self::from_bits(BInt::<CHUNKS>::ONE);
|
|
pub const NEG_EPSILON:Self=Self::from_bits(BInt::<CHUNKS>::NEG_ONE);
|
|
pub const ONE:Self=Self::from_bits(BInt::<CHUNKS>::ONE.shl(Frac::U32));
|
|
pub const TWO:Self=Self::from_bits(BInt::<CHUNKS>::TWO.shl(Frac::U32));
|
|
pub const HALF:Self=Self::from_bits(BInt::<CHUNKS>::ONE.shl(Frac::U32-1));
|
|
pub const NEG_ONE:Self=Self::from_bits(BInt::<CHUNKS>::NEG_ONE.shl(Frac::U32));
|
|
pub const NEG_TWO:Self=Self::from_bits(BInt::<CHUNKS>::NEG_TWO.shl(Frac::U32));
|
|
pub const NEG_HALF:Self=Self::from_bits(BInt::<CHUNKS>::NEG_ONE.shl(Frac::U32-1));
|
|
}
|
|
impl<const CHUNKS:usize,Frac> Fixed<CHUNKS,Frac>{
|
|
#[inline]
|
|
pub const fn from_bits(bits:BInt::<CHUNKS>)->Self{
|
|
Self{
|
|
bits,
|
|
frac:std::marker::PhantomData,
|
|
}
|
|
}
|
|
#[inline]
|
|
pub const fn to_bits(self)->BInt<CHUNKS>{
|
|
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 CHUNKS:usize,Frac:Unsigned,T> From<T> for Fixed<CHUNKS,Frac>
|
|
where
|
|
BInt<CHUNKS>:From<T>
|
|
{
|
|
fn from(value:T)->Self{
|
|
Self::from_bits(BInt::<{CHUNKS}>::from(value)<<Frac::U32)
|
|
}
|
|
}
|
|
|
|
impl<const CHUNKS:usize,Frac> PartialEq for Fixed<CHUNKS,Frac>{
|
|
fn eq(&self,other:&Self)->bool{
|
|
self.bits.eq(&other.bits)
|
|
}
|
|
}
|
|
impl<const CHUNKS:usize,Frac> Eq for Fixed<CHUNKS,Frac>{}
|
|
|
|
impl<const CHUNKS:usize,Frac> PartialOrd for Fixed<CHUNKS,Frac>{
|
|
fn partial_cmp(&self,other:&Self)->Option<std::cmp::Ordering>{
|
|
self.bits.partial_cmp(&other.bits)
|
|
}
|
|
}
|
|
impl<const CHUNKS:usize,Frac> Ord for Fixed<CHUNKS,Frac>{
|
|
fn cmp(&self,other:&Self)->std::cmp::Ordering{
|
|
self.bits.cmp(&other.bits)
|
|
}
|
|
}
|
|
|
|
impl<const CHUNKS:usize,Frac> std::ops::Neg for Fixed<CHUNKS,Frac>{
|
|
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 CHUNKS:usize,Frac> core::ops::$trait for $struct<CHUNKS,Frac>{
|
|
type Output = $output;
|
|
|
|
fn $method(self, other: Self) -> Self::Output {
|
|
Self::from_bits(self.bits.$method(other.bits))
|
|
}
|
|
}
|
|
impl<const CHUNKS:usize,Frac:Unsigned,U> core::ops::$trait<U> for $struct<CHUNKS,Frac>
|
|
where
|
|
BInt::<CHUNKS>:From<U>,
|
|
{
|
|
type Output = $output;
|
|
|
|
fn $method(self, other: U) -> Self::Output {
|
|
Self::from_bits(self.bits.$method(BInt::<CHUNKS>::from(other)<<Frac::U32))
|
|
}
|
|
}
|
|
};
|
|
}
|
|
macro_rules! impl_additive_assign_operator {
|
|
( $struct: ident, $trait: ident, $method: ident ) => {
|
|
impl<const CHUNKS:usize,Frac> core::ops::$trait for $struct<CHUNKS,Frac>{
|
|
fn $method(&mut self, other: Self) {
|
|
self.bits.$method(other.bits);
|
|
}
|
|
}
|
|
impl<const CHUNKS:usize,Frac:Unsigned,U> core::ops::$trait<U> for $struct<CHUNKS,Frac>
|
|
where
|
|
BInt::<CHUNKS>:From<U>,
|
|
{
|
|
fn $method(&mut self, other: U) {
|
|
self.bits.$method(BInt::<CHUNKS>::from(other)<<Frac::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<Frac:Unsigned> core::ops::$trait for $struct<$width,Frac>{
|
|
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(Frac::U32).as_())
|
|
}
|
|
}
|
|
};
|
|
}
|
|
macro_rules! impl_multiply_assign_operator_const {
|
|
( $width:expr, $struct: ident, $trait: ident, $method: ident ) => {
|
|
impl<Frac> core::ops::$trait for $struct<$width,Frac>{
|
|
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<Frac:Unsigned> core::ops::$trait for $struct<$width,Frac>{
|
|
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+Frac::U32/64+1 but MUH CONST GENERICS!!!!!
|
|
let lhs=self.bits.as_::<BInt::<{$width*2}>>().shl(Frac::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<Frac> core::ops::$trait for $struct<$width,Frac>{
|
|
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 CHUNKS:usize,Frac,U> core::ops::$trait<U> for $struct<CHUNKS,Frac>
|
|
where
|
|
BInt::<CHUNKS>:From<U>+core::ops::$trait,
|
|
{
|
|
type Output = $output;
|
|
|
|
fn $method(self, other: U) -> Self::Output {
|
|
Self::from_bits(self.bits.$method(BInt::<CHUNKS>::from(other)))
|
|
}
|
|
}
|
|
};
|
|
}
|
|
macro_rules! impl_multiplicatave_assign_operator {
|
|
( $struct: ident, $trait: ident, $method: ident ) => {
|
|
impl<const CHUNKS:usize,Frac,U> core::ops::$trait<U> for $struct<CHUNKS,Frac>
|
|
where
|
|
BInt::<CHUNKS>:From<U>+core::ops::$trait,
|
|
{
|
|
fn $method(&mut self, other: U) {
|
|
self.bits.$method(BInt::<CHUNKS>::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 CHUNKS:usize,Frac> core::ops::$trait<u32> for $struct<CHUNKS,Frac>{
|
|
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 CHUNKS:usize,Frac> core::ops::$trait<u32> for $struct<CHUNKS,Frac>{
|
|
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<A,B> WideMul<Fixed<$rhs,B>> for Fixed<$lhs,A>
|
|
where
|
|
A:std::ops::Add<B>,
|
|
B:Unsigned,
|
|
{
|
|
type Output=Fixed<{$lhs+$rhs},Sum<A,B>>;
|
|
fn wide_mul(self,rhs:Fixed<$rhs,B>)->Self::Output{
|
|
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,Frac> Fixed<SRC,Frac>{
|
|
pub fn resize_into<const DST:usize>(self)->Fixed<DST,Frac>{
|
|
Fixed::from_bits(self.bits.as_::<BInt<DST>>())
|
|
}
|
|
}
|
|
|
|
macro_rules! impl_const{
|
|
($n:expr)=>{
|
|
impl<F:Unsigned> Fixed<$n,F>{
|
|
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-F::I32;
|
|
let max_shift=((used_bits>>1)+F::I32) as u32;
|
|
let mut result=Self::ZERO;
|
|
|
|
//multiply by one to make the types match (hack)
|
|
let wide_self=self.wide_mul(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(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);
|