mirror of
https://github.com/aicodix/dsp.git
synced 2026-04-27 14:30:36 +00:00
72 lines
2.3 KiB
C++
72 lines
2.3 KiB
C++
/*
|
|
CORDIC atan2 implementation
|
|
|
|
Copyright 2019 Ahmet Inan <inan@aicodix.de>
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
/*
|
|
for ((i = 0; i < 16; ++i)) ; do
|
|
echo -n "int(0.5+FAC*"
|
|
echo "scale=100; a((1/2)^$i)/(4*a(1))" | bc -l | head -n1 | sed 's/\\/L),/'
|
|
done
|
|
*/
|
|
|
|
namespace DSP {
|
|
|
|
template <typename TYPE>
|
|
class CORDIC
|
|
{
|
|
static const int BYTES = sizeof(TYPE);
|
|
static const int BITS = 8 * BYTES;
|
|
static_assert(BITS <= 16, "LUT not big enough");
|
|
static const int FAC = BYTES << BITS;
|
|
static constexpr int LUT[16] = {
|
|
int(0.5+FAC*.2500000000000000000000000000000000000000000000000000000000000000000L),
|
|
int(0.5+FAC*.1475836176504332741754010762247405259511345238869178945999223128627L),
|
|
int(0.5+FAC*.0779791303773693254605128897731301351165246187810070349907614355625L),
|
|
int(0.5+FAC*.0395834241605655420108516713400380263836230512014484657091255843474L),
|
|
int(0.5+FAC*.0198685243055408390593598828581045592395809198311117360197658161834L),
|
|
int(0.5+FAC*.0099439478235892739286124987559949706675355099351305855673205197301L),
|
|
int(0.5+FAC*.0049731872789504128535623156837751800756022858309425918885230220925L),
|
|
int(0.5+FAC*.0024867453936697392949686323634656804594951905170750175789691381407L),
|
|
int(0.5+FAC*.0012433916687141004173529641246144055092826557912518235730426698546L),
|
|
int(0.5+FAC*.0006216982059233715959748655390818054931061630817558971376245115402L),
|
|
int(0.5+FAC*.0003108493994100203787468007466901774427133407538770647518959302312L),
|
|
int(0.5+FAC*.0001554247367611315255509951068712763232641191255978216125229683284L),
|
|
int(0.5+FAC*.0000777123730125834146038201984686638046103667177724187710747868218L),
|
|
int(0.5+FAC*.0000388561870852939914306938515989779351910341528053916759630446151L),
|
|
int(0.5+FAC*.0000194280936150222836580154085598088977275755448309525831225639103L),
|
|
int(0.5+FAC*.0000097140468165580528976715963565423783044063210250363625335628380L),
|
|
};
|
|
public:
|
|
static TYPE atan2(TYPE y, TYPE x)
|
|
{
|
|
int angle = 0;
|
|
int real = x << 1;
|
|
int imag = y << 1;
|
|
if (x < 0)
|
|
real = -real;
|
|
for (int i = 0; i < BITS; ++i) {
|
|
int re = real;
|
|
int im = imag;
|
|
if (imag < 0) {
|
|
angle -= LUT[i];
|
|
re -= imag >> i;
|
|
im += real >> i;
|
|
} else {
|
|
angle += LUT[i];
|
|
re += imag >> i;
|
|
im -= real >> i;
|
|
}
|
|
real = re; imag = im;
|
|
}
|
|
if (x < 0)
|
|
angle = FAC - angle;
|
|
return angle >> BYTES;
|
|
}
|
|
};
|
|
|
|
}
|
|
|