diff --git a/src/integer.rs b/src/integer.rs index 758bdaf..d25e4ce 100644 --- a/src/integer.rs +++ b/src/integer.rs @@ -413,18 +413,31 @@ impl Angle32{ .wrapping_add(midpoint) ) } - /* #[inline] - pub fn cos(&self)->Unit32{ - //TODO: fix this rounding towards 0 - Unit32(unsafe{((self.0 as f64*ANGLE32_TO_FLOAT64_RADIANS).cos()*UNIT32_ONE_FLOAT64).to_int_unchecked()}) + pub fn cos_sin(&self)->(Planar64,Planar64){ + /* + //cordic + let a=self.0 as u32; + //initialize based on the quadrant + let (mut x,mut y)=match (a&(1<<31)!=0,a&(1<<30)!=0){ + (false,false)=>( 1i64<<32, 0i64 ),//TR + (false,true )=>( 0i64 , 1i64<<32),//TL + (true ,false)=>(-1i64<<32, 0i64 ),//BL + (true ,true )=>( 0i64 ,-1i64<<32),//BR + }; + println!("x={} y={}",Planar64::raw(x),Planar64::raw(y)); + for i in 0..30{ + if a&(1<<(29-i))!=0{ + (x,y)=(x-(y>>i),y+(x>>i)); + } + println!("i={i} t={} x={} y={}",(a&(1<<(29-i))!=0) as u8,Planar64::raw(x),Planar64::raw(y)); + } + //don't forget the gain + (Planar64::raw(x),Planar64::raw(y)) + */ + let (s,c)=(self.0 as f64*Self::ANGLE32_TO_FLOAT64_RADIANS).sin_cos(); + (Planar64::raw((c*((1u64<<32) as f64)) as i64),Planar64::raw((s*((1u64<<32) as f64)) as i64)) } - #[inline] - pub fn sin(&self)->Unit32{ - //TODO: fix this rounding towards 0 - Unit32(unsafe{((self.0 as f64*ANGLE32_TO_FLOAT64_RADIANS).sin()*UNIT32_ONE_FLOAT64).to_int_unchecked()}) - } - */ } impl Into for Angle32{ #[inline]