fixed_dsp/basic/
cos.rs

1use crate::common::tables::{SIN_TABLE_U16, SIN_TABLE_U32};
2
3const Q15_SHIFT: i32 = 16 - 10;
4const Q31_SHIFT: i32 = 32 - 10;
5
6/// x is in Q15 format (1.15 fixed-point), representing angles in the range [0, 2pi).
7pub fn cos_i16(x: i16) -> i16 {
8    // Add 0.25 turn (pi/2) to map cosine onto sine lookup table.
9    let mut x_bits = (x as u16).wrapping_add(0x2000);
10    if (x_bits as i16) < 0 {
11        x_bits = x_bits.wrapping_add(0x8000);
12    }
13
14    let x_mapped = x_bits as i32;
15    let index = ((x_bits as u32) >> Q15_SHIFT) as usize;
16    let fract = (x_mapped - ((index as i32) << Q15_SHIFT)) << 9;
17
18    let a = SIN_TABLE_U16[index] as i16 as i32;
19    let b = SIN_TABLE_U16[index + 1] as i16 as i32;
20
21    let mut cos_val = ((0x8000_i32 - fract) * a) >> 16;
22    cos_val = ((cos_val << 16) + (fract * b)) >> 16;
23
24    (cos_val << 1) as i16
25}
26
27/// x is in Q31 format (1.31 fixed-point), representing angles in the range [0, 2pi).
28pub fn cos_i32(x: i32) -> i32 {
29    // Add 0.25 turn (pi/2) to map cosine onto sine lookup table.
30    let mut x_bits = (x as u32).wrapping_add(0x2000_0000);
31    if (x_bits as i32) < 0 {
32        x_bits = x_bits.wrapping_add(0x8000_0000);
33    }
34
35    let x_mapped = x_bits as i64;
36    let index = (x_bits >> Q31_SHIFT) as usize;
37    let fract = (x_mapped - ((index as i64) << Q31_SHIFT)) << 9;
38
39    let a = SIN_TABLE_U32[index] as i32 as i64;
40    let b = SIN_TABLE_U32[index + 1] as i32 as i64;
41
42    let mut cos_val = ((0x8000_0000_i64 - fract) * a) >> 32;
43    cos_val = ((cos_val << 32) + (fract * b)) >> 32;
44
45    (cos_val << 1) as i32
46}
fixed_dsp/basic/cos.rs

fixed_dsp/basic/
cos.rs
