StrafesNET/fixed_wide_vectors
3
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

cordic sqrt

Browse Source
This commit is contained in:
Quaternions 2024-08-29 10:12:08 -07:00
parent 446de71c30
commit 9f6dffafda
2 changed files with 60 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
fixed_wide/src/fixed.rs
View File

@ -9,11 +9,15 @@ pub struct Fixed<const CHUNKS:usize,Frac>{
}
impl<const CHUNKS:usize,Frac:Unsigned> Fixed<CHUNKS,Frac>{
pub const ZERO:Self=Self{bits:BInt::<CHUNKS>::ZERO,frac:PhantomData};
pub const ONE:Self=Self{bits:BInt::<CHUNKS>::ONE.shl(Frac::U32),frac:PhantomData};
pub const NEG_ONE:Self=Self{bits:BInt::<CHUNKS>::NEG_ONE.shl(Frac::U32),frac:PhantomData};
pub const MAX:Self=Self{bits:BInt::<CHUNKS>::MAX,frac:PhantomData};
pub const MIN:Self=Self{bits:BInt::<CHUNKS>::MIN,frac:PhantomData};
pub const ZERO:Self=Self{bits:BInt::<CHUNKS>::ZERO,frac:PhantomData};
pub const ONE:Self=Self{bits:BInt::<CHUNKS>::ONE.shl(Frac::U32),frac:PhantomData};
pub const TWO:Self=Self{bits:BInt::<CHUNKS>::TWO.shl(Frac::U32),frac:PhantomData};
pub const HALF:Self=Self{bits:BInt::<CHUNKS>::ONE.shl(Frac::U32-1),frac:PhantomData};
pub const NEG_ONE:Self=Self{bits:BInt::<CHUNKS>::NEG_ONE.shl(Frac::U32),frac:PhantomData};
pub const NEG_TWO:Self=Self{bits:BInt::<CHUNKS>::NEG_TWO.shl(Frac::U32),frac:PhantomData};
pub const NEG_HALF:Self=Self{bits:BInt::<CHUNKS>::NEG_ONE.shl(Frac::U32-1),frac:PhantomData};
}
impl<const CHUNKS:usize,Frac:Unsigned,T> From<T> for Fixed<CHUNKS,Frac>
@ -148,3 +152,50 @@ 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 );
impl<const CHUNKS:usize,Frac:Unsigned> Fixed<CHUNKS,Frac>{
pub fn sqrt_unchecked(self)->Self{
let mut pow2=if self==Self::ZERO{
return Self::ZERO;
}else if self<Self::ONE{
//find pow2 more powerful than self
let mut pow2=Self::ONE;
while self<=pow2{
pow2>>=1;
}
pow2
}else if self==Self::ONE{
return Self::ONE;
}else{
//find pow2 more powerful than self
let mut pow2=Self::ONE;
while pow2<=self{
pow2<<=1;
}
pow2
};
let mut result=pow2;
while pow2!=Self::ZERO{
pow2>>=1;
let new_result=result+pow2;
if new_result*new_result<=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())
}
}
}

6
fixed_wide/src/tests.rs
View File

@ -12,3 +12,9 @@ fn test_bint(){
let a=crate::types::I32F32::ONE;
assert_eq!(a*2,crate::types::I32F32::from(2));
}
#[test]
fn test_sqrt(){
let a=crate::types::I32F32::ONE*4;
assert_eq!(a.sqrt(),crate::types::I32F32::from(2));
}