2020-12-17 08:26:44 +08:00
|
|
|
use super::{abs, shift_round, Complex};
|
2020-12-10 23:56:13 +08:00
|
|
|
use core::f64::consts::PI;
|
2020-12-08 02:22:09 +08:00
|
|
|
|
2020-12-11 00:17:09 +08:00
|
|
|
include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
|
|
|
|
|
2020-12-17 08:26:44 +08:00
|
|
|
/// 2-argument arctangent function.
|
|
|
|
///
|
|
|
|
/// This implementation uses all integer arithmetic for fast
|
|
|
|
/// computation. It is designed to have high accuracy near the axes
|
|
|
|
/// and lower away from the axes. It is additionally designed so that
|
|
|
|
/// the error changes slowly with respect to the angle.
|
|
|
|
///
|
|
|
|
/// # Arguments
|
|
|
|
///
|
|
|
|
/// * `y` - Y-axis component.
|
|
|
|
/// * `x` - X-axis component.
|
|
|
|
///
|
|
|
|
/// # Returns
|
|
|
|
///
|
|
|
|
/// The angle between the x-axis and the ray to the point (x,y). The
|
|
|
|
/// result range is from i32::MIN to i32::MAX, where i32::MIN
|
|
|
|
/// corresponds to an angle of -pi and i32::MAX corresponds to an
|
|
|
|
/// angle of +pi.
|
|
|
|
pub fn atan2(y: i32, x: i32) -> i32 {
|
|
|
|
let y = y >> 16;
|
|
|
|
let x = x >> 16;
|
|
|
|
|
|
|
|
let ux = abs::<i32>(x);
|
|
|
|
let uy = abs::<i32>(y);
|
|
|
|
|
|
|
|
// Uses the general procedure described in the following
|
|
|
|
// Mathematics stack exchange answer:
|
|
|
|
//
|
|
|
|
// https://math.stackexchange.com/a/1105038/583981
|
|
|
|
//
|
|
|
|
// The atan approximation method has been modified to be cheaper
|
|
|
|
// to compute and to be more compatible with integer
|
|
|
|
// arithmetic. The approximation technique used here is
|
|
|
|
//
|
|
|
|
// pi / 4 * x + 0.285 * x * (1 - abs(x))
|
|
|
|
//
|
|
|
|
// which is taken from Rajan 2006: Efficient Approximations for
|
|
|
|
// the Arctangent Function.
|
|
|
|
let (min, max) = if ux < uy { (ux, uy) } else { (uy, ux) };
|
|
|
|
|
|
|
|
if max == 0 {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
let ratio = (min << 15) / max;
|
|
|
|
|
|
|
|
let mut angle = {
|
|
|
|
// pi/4, referenced to i16::MAX
|
|
|
|
const PI_4_FACTOR: i32 = 25735;
|
|
|
|
// 0.285, referenced to i16::MAX
|
|
|
|
const FACTOR_0285: i32 = 9339;
|
|
|
|
// 1/pi, referenced to u16::MAX
|
|
|
|
const PI_INVERTED_FACTOR: i32 = 20861;
|
|
|
|
|
|
|
|
let r1 = shift_round(ratio * PI_4_FACTOR, 15);
|
|
|
|
let r2 = shift_round(
|
|
|
|
(shift_round(ratio * FACTOR_0285, 15)) * (i16::MAX as i32 - ratio),
|
|
|
|
15,
|
|
|
|
);
|
|
|
|
(r1 + r2) * PI_INVERTED_FACTOR
|
|
|
|
};
|
|
|
|
|
|
|
|
if uy > ux {
|
|
|
|
angle = (i32::MAX >> 1) - angle;
|
|
|
|
}
|
|
|
|
|
|
|
|
if x < 0 {
|
|
|
|
angle = i32::MAX - angle;
|
|
|
|
}
|
|
|
|
|
|
|
|
if y < 0 {
|
|
|
|
angle *= -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
angle
|
|
|
|
}
|
|
|
|
|
2020-12-08 02:22:09 +08:00
|
|
|
/// Compute the cosine and sine of an angle.
|
2020-12-10 23:56:13 +08:00
|
|
|
/// This is ported from the MiSoC cossin core.
|
|
|
|
/// (https://github.com/m-labs/misoc/blob/master/misoc/cores/cossin.py)
|
2020-12-08 02:22:09 +08:00
|
|
|
///
|
|
|
|
/// # Arguments
|
2020-12-10 23:56:13 +08:00
|
|
|
/// * `phase` - 32-bit phase.
|
2020-12-08 02:22:09 +08:00
|
|
|
///
|
|
|
|
/// # Returns
|
|
|
|
/// The cos and sin values of the provided phase as a `Complex<i32>`
|
2020-12-10 23:56:13 +08:00
|
|
|
/// value. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error
|
|
|
|
/// in each quadrature over 20 bit phase.
|
2020-12-08 02:22:09 +08:00
|
|
|
pub fn cossin(phase: i32) -> Complex<i32> {
|
2020-12-10 23:56:13 +08:00
|
|
|
// Phase bits excluding the three highes MSB
|
|
|
|
const OCTANT_BITS: usize = 32 - 3;
|
|
|
|
|
|
|
|
// This is a slightly more compact way to compute the four flags for
|
|
|
|
// octant mapping/unmapping used below.
|
|
|
|
let mut octant = (phase as u32) >> OCTANT_BITS;
|
|
|
|
octant ^= octant << 1;
|
|
|
|
|
|
|
|
// Mask off octant bits. This leaves the angle in the range [0, pi/4).
|
|
|
|
let mut phase = phase & ((1 << OCTANT_BITS) - 1);
|
|
|
|
|
|
|
|
if octant & 1 != 0 {
|
2020-12-08 02:22:09 +08:00
|
|
|
// phase = pi/4 - phase
|
2020-12-10 23:56:13 +08:00
|
|
|
phase = (1 << OCTANT_BITS) - 1 - phase;
|
2020-12-08 02:22:09 +08:00
|
|
|
}
|
|
|
|
|
2020-12-10 23:56:13 +08:00
|
|
|
let lookup = COSSIN[(phase >> (OCTANT_BITS - COSSIN_DEPTH)) as usize];
|
|
|
|
// 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.0 as i32 + u16::MAX as i32;
|
|
|
|
let mut sin = (lookup.1 as i32) << 1;
|
|
|
|
|
|
|
|
// 16 + 1 bits for cos/sin and 15 for dphi to saturate the i32 range.
|
|
|
|
const ALIGN_MSB: usize = 32 - 16 - 1;
|
|
|
|
phase >>= OCTANT_BITS - COSSIN_DEPTH - ALIGN_MSB;
|
|
|
|
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)) - (octant & 1) as i32;
|
|
|
|
// Fixed point pi/4.
|
|
|
|
const PI4: i32 = (PI / 4. * (1 << (32 - ALIGN_MSB)) as f64) as i32;
|
|
|
|
// No rounding bias necessary here since we keep enough low bits.
|
|
|
|
let dphi = (phase * PI4) >> (32 - ALIGN_MSB);
|
|
|
|
|
|
|
|
// Make room for the sign bit.
|
|
|
|
let dcos = (sin * dphi) >> (COSSIN_DEPTH + 1);
|
|
|
|
let dsin = (cos * dphi) >> (COSSIN_DEPTH + 1);
|
|
|
|
|
|
|
|
cos = (cos << (ALIGN_MSB - 1)) - dcos;
|
|
|
|
sin = (sin << (ALIGN_MSB - 1)) + dsin;
|
|
|
|
|
|
|
|
// Unmap using octant bits.
|
|
|
|
if octant & 2 != 0 {
|
|
|
|
core::mem::swap(&mut sin, &mut cos);
|
2020-12-08 02:22:09 +08:00
|
|
|
}
|
|
|
|
|
2020-12-10 23:56:13 +08:00
|
|
|
if octant & 4 != 0 {
|
2020-12-08 02:22:09 +08:00
|
|
|
cos *= -1;
|
|
|
|
}
|
|
|
|
|
2020-12-10 23:56:13 +08:00
|
|
|
if octant & 8 != 0 {
|
2020-12-08 02:22:09 +08:00
|
|
|
sin *= -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
(cos, sin)
|
|
|
|
}
|
|
|
|
|
|
|
|
#[cfg(test)]
|
|
|
|
mod tests {
|
|
|
|
use super::*;
|
2020-12-17 08:26:44 +08:00
|
|
|
use crate::testing::isclose;
|
|
|
|
use core::f64::consts::PI;
|
|
|
|
|
|
|
|
fn angle_to_axis(angle: f64) -> f64 {
|
|
|
|
let angle = angle % (PI / 2.);
|
|
|
|
(PI / 2. - angle).min(angle)
|
|
|
|
}
|
|
|
|
|
2020-12-08 02:22:09 +08:00
|
|
|
#[test]
|
2020-12-17 08:28:49 +08:00
|
|
|
fn cossin_error_max_rms_all_phase() {
|
2020-12-10 23:56:13 +08:00
|
|
|
// Constant amplitude error due to LUT data range.
|
|
|
|
const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as f64;
|
|
|
|
const MAX_PHASE: f64 = (1i64 << 32) as f64;
|
2020-12-08 02:22:09 +08:00
|
|
|
let mut rms_err: Complex<f64> = (0., 0.);
|
2020-12-10 23:56:13 +08:00
|
|
|
let mut sum_err: Complex<f64> = (0., 0.);
|
|
|
|
let mut max_err: Complex<f64> = (0., 0.);
|
|
|
|
let mut sum: Complex<f64> = (0., 0.);
|
|
|
|
let mut demod: Complex<f64> = (0., 0.);
|
|
|
|
|
2020-12-13 19:36:23 +08:00
|
|
|
// use std::{fs::File, io::{BufWriter, prelude::*}, path::Path};
|
|
|
|
// let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap());
|
2020-12-10 23:56:13 +08:00
|
|
|
|
|
|
|
const PHASE_DEPTH: usize = 20;
|
|
|
|
|
|
|
|
for phase in 0..(1 << PHASE_DEPTH) {
|
|
|
|
let phase = (phase << (32 - PHASE_DEPTH)) as i32;
|
|
|
|
let have = cossin(phase);
|
2020-12-13 19:36:23 +08:00
|
|
|
// file.write(&have.0.to_le_bytes()).unwrap();
|
|
|
|
// file.write(&have.1.to_le_bytes()).unwrap();
|
2020-12-10 23:56:13 +08:00
|
|
|
|
|
|
|
let have = (have.0 as f64 / AMPLITUDE, have.1 as f64 / AMPLITUDE);
|
2020-12-08 02:22:09 +08:00
|
|
|
|
2020-12-10 23:56:13 +08:00
|
|
|
let radian_phase = 2. * PI * phase as f64 / MAX_PHASE;
|
|
|
|
let want = (radian_phase.cos(), radian_phase.sin());
|
|
|
|
|
|
|
|
sum.0 += have.0;
|
|
|
|
sum.1 += have.1;
|
|
|
|
|
|
|
|
demod.0 += have.0 * want.0 - have.1 * want.1;
|
|
|
|
demod.1 += have.1 * want.0 + have.0 * want.1;
|
|
|
|
|
|
|
|
let err = (have.0 - want.0, have.1 - want.1);
|
|
|
|
|
|
|
|
sum_err.0 += err.0;
|
|
|
|
sum_err.1 += err.1;
|
|
|
|
|
|
|
|
rms_err.0 += err.0 * err.0;
|
|
|
|
rms_err.1 += err.1 * err.1;
|
|
|
|
|
|
|
|
max_err.0 = max_err.0.max(err.0.abs());
|
|
|
|
max_err.1 = max_err.1.max(err.1.abs());
|
2020-12-08 02:22:09 +08:00
|
|
|
}
|
2020-12-10 23:56:13 +08:00
|
|
|
rms_err.0 /= MAX_PHASE;
|
|
|
|
rms_err.1 /= MAX_PHASE;
|
|
|
|
|
|
|
|
println!("sum: {:.2e} {:.2e}", sum.0, sum.1);
|
|
|
|
println!("demod: {:.2e} {:.2e}", demod.0, demod.1);
|
|
|
|
println!("sum_err: {:.2e} {:.2e}", sum_err.0, sum_err.1);
|
|
|
|
println!("rms: {:.2e} {:.2e}", rms_err.0.sqrt(), rms_err.1.sqrt());
|
|
|
|
println!("max: {:.2e} {:.2e}", max_err.0, max_err.1);
|
|
|
|
|
|
|
|
assert!(sum.0.abs() < 4e-10);
|
|
|
|
assert!(sum.1.abs() < 4e-10);
|
|
|
|
|
|
|
|
assert!(demod.0.abs() < 4e-10);
|
|
|
|
assert!(demod.1.abs() < 4e-10);
|
|
|
|
|
|
|
|
assert!(sum_err.0.abs() < 4e-10);
|
|
|
|
assert!(sum_err.1.abs() < 4e-10);
|
|
|
|
|
|
|
|
assert!(rms_err.0.sqrt() < 6e-8);
|
|
|
|
assert!(rms_err.1.sqrt() < 6e-8);
|
|
|
|
|
|
|
|
assert!(max_err.0 < 1.1e-5);
|
|
|
|
assert!(max_err.1 < 1.1e-5);
|
2020-12-08 02:22:09 +08:00
|
|
|
}
|
2020-12-17 08:26:44 +08:00
|
|
|
|
|
|
|
#[test]
|
2020-12-17 08:28:49 +08:00
|
|
|
fn atan2_absolute_error() {
|
2020-12-17 08:26:44 +08:00
|
|
|
const NUM_VALS: usize = 1_001;
|
|
|
|
let mut test_vals: [f64; NUM_VALS] = [0.; NUM_VALS];
|
|
|
|
let val_bounds: (f64, f64) = (-1., 1.);
|
|
|
|
let val_delta: f64 =
|
|
|
|
(val_bounds.1 - val_bounds.0) / (NUM_VALS - 1) as f64;
|
|
|
|
for i in 0..NUM_VALS {
|
|
|
|
test_vals[i] = val_bounds.0 + i as f64 * val_delta;
|
|
|
|
}
|
|
|
|
|
|
|
|
for &x in test_vals.iter() {
|
|
|
|
for &y in test_vals.iter() {
|
|
|
|
let atol: f64 = 4e-5;
|
|
|
|
let rtol: f64 = 0.127;
|
|
|
|
let actual = (y.atan2(x) as f64 * i16::MAX as f64).round()
|
|
|
|
/ i16::MAX as f64;
|
|
|
|
let tol = atol + rtol * angle_to_axis(actual).abs();
|
|
|
|
let computed = (atan2(
|
|
|
|
((y * i16::MAX as f64) as i32) << 16,
|
|
|
|
((x * i16::MAX as f64) as i32) << 16,
|
|
|
|
) >> 16) as f64
|
|
|
|
/ i16::MAX as f64
|
|
|
|
* PI;
|
|
|
|
|
|
|
|
if !isclose(computed, actual, 0., tol) {
|
|
|
|
println!("(x, y) : {}, {}", x, y);
|
|
|
|
println!("actual : {}", actual);
|
|
|
|
println!("computed : {}", computed);
|
|
|
|
println!("tolerance: {}\n", tol);
|
|
|
|
assert!(false);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2020-12-08 02:22:09 +08:00
|
|
|
}
|