Compiler Explorer

Source code

#![no_std]

pub(crate) const COSSIN_DEPTH: usize = 7;
pub(crate) const COSSIN: [u32; 1 << COSSIN_DEPTH] = [
    13238269, 39583736, 65929199, 92274656,
    118620108, 144965555, 171310998, 197656435,
    223936332, 250281759, 276561646, 302907064,
    329186941, 355401277, 381681144, 407895470,
    434175327, 460389643, 486538418, 512752725,
    538901490, 564984715, 591133471, 617216686,
    643299896, 669317565, 695335229, 721352888,
    747305007, 773257121, 799143693, 825030261,
    850851288, 876672311, 902493328, 928248805,
    953938741, 979628672, 1005253062, 1030877447,
    1056436292, 1081929595, 1107422895, 1132850653,
    1158278406, 1183640619, 1208937291, 1234168422,
    1259399549, 1284565135, 1309730716, 1334765220,
    1359799720, 1384768679, 1409672097, 1434575511,
    1459347848, 1484120181, 1508826972, 1533468224,
    1558043934, 1582554104, 1607064269, 1631443358,
    1655822442, 1680070450, 1704318453, 1728500916,
    1752552302, 1776603684, 1800523989, 1824444289,
    1848233513, 1872022733, 1895680876, 1919339015,
    1942866078, 1966327600, 1989723581, 2013054022,
    2036253387, 2059452748, 2082521032, 2105523776,
    2128460980, 2151332643, 2174073230, 2196748277,
    2219357783, 2241901750, 2264314640, 2286661990,
    2308943799, 2331094533, 2353179726, 2375199380,
    2397087957, 2418910994, 2440668491, 2462294912,
    2483855793, 2505285598, 2526649863, 2547883051,
    2569050700, 2590152809, 2611123842, 2632029335,
    2652803753, 2673447094, 2694024895, 2714471621,
    2734852807, 2755102917, 2775287487, 2795340982,
    2815328936, 2835185815, 2854911618, 2874506346,
    2894035534, 2913499182, 2932766219, 2951967716,
    2971038137, 2990043019, 3008916826, 3027659556,
];

// #[inline(never)]
pub fn cossin(mut phase: i32) -> (i32, i32) {
    let mut octant = phase as u32;
    if octant & (1 << 29) != 0 {
        // phase = pi/4 - phase
        phase = !phase;
    }

// 16 + 1 bits for cos/sin and 15 for dphi to saturate the i32 range.
    const ALIGN_MSB: usize = 32 - 16 - 1;

// Mask off octant bits. This leaves the angle in the range [0, pi/4).
    phase = (((phase as u32) << 3) >> (32 - COSSIN_DEPTH - ALIGN_MSB)) as _;

let lookup = COSSIN[(phase >> ALIGN_MSB) as usize];

phase &= (1 << ALIGN_MSB) - 1;

// The phase values used for the LUT are at midpoint for the truncated phase.
    // Interpolate relative to the LUT entry midpoint.
    phase -= 1 << (ALIGN_MSB - 1);

// Cancel the -1 bias that was conditionally introduced above.
    // This lowers the DC spur from 2e-8 to 2e-10 magnitude.
    // phase += ((octant >> 29) & 1) as i32;

// Fixed point pi/4.
    const PI4: i32 = (core::f64::consts::FRAC_PI_4 * (1 << 16) as f64) as _;
    // No rounding bias necessary here since we keep enough low bits.
    let dphi = (phase * PI4) >> 16;

// 1/2 < cos(0 <= x <= pi/4) <= 1: Shift the cos
    // values and scale the sine values as encoded in the LUT.
    let mut cos = (lookup & 0xffff) as i32 + (1 << 16);
    let mut sin = (lookup >> 16) as i32;
    //let mut cos = (lookup.0) as i32 + (1 << 16);
    //let mut sin = (lookup.1) as i32;

let dcos = (sin * dphi) >> COSSIN_DEPTH;
    let dsin = (cos * dphi) >> (COSSIN_DEPTH + 1);

cos = (cos << (ALIGN_MSB - 1)) - dcos;
    sin = (sin << ALIGN_MSB) + dsin;

// Unmap using octant bits.
    octant ^= octant >> 1;
    if octant & (1 << 29) != 0 {
        (cos, sin) = (sin, cos);
    }
    if octant & (1 << 30) != 0 {
        cos = -cos;
    }
    if octant & (1 << 31) != 0 {
        sin = -sin;
    }

(cos, sin)
}