From a4c31b17838cfdfa17be2d2cf7858c0ba4778f4c Mon Sep 17 00:00:00 2001
From: Ahmet Inan <inan@aicodix.de>
Date: Wed, 13 Feb 2019 02:36:39 +0100
Subject: [PATCH] added fixed-point CORDIC atan2

---
 README.md            |  7 +++++
 cordic.hh            | 72 ++++++++++++++++++++++++++++++++++++++++++++
 tests/cordic_test.cc | 33 ++++++++++++++++++++
 3 files changed, 112 insertions(+)
 create mode 100644 cordic.hh
 create mode 100644 tests/cordic_test.cc

diff --git a/README.md b/README.md
index c046fe3..c01e7ea 100644
--- a/README.md
+++ b/README.md
@@ -56,6 +56,13 @@ The following [infinite impulse response](https://en.wikipedia.org/wiki/Infinite
 
 [atan](https://en.wikipedia.org/wiki/Inverse_trigonometric_functions) and [atan2](https://en.wikipedia.org/wiki/Atan2).
 
+### [cordic.hh](cordic.hh)
+
+When working on a device where multiplication is expensive, the [CORDIC](https://en.wikipedia.org/wiki/CORDIC) comes in handy for computing [trigonometric functions](https://en.wikipedia.org/wiki/Trigonometric_functions).
+
+The following implementations are a good (max 1 LSB error at full range) starting point for your own designs:
+* [Fixed-point](https://en.wikipedia.org/wiki/Fixed-point_arithmetic) [atan2](https://en.wikipedia.org/wiki/Atan2) implementation
+
 ### [trigger.hh](trigger.hh)
 
 Implemented are the following [trigger functions](https://en.wikipedia.org/wiki/Flip-flop_(electronics)):
diff --git a/cordic.hh b/cordic.hh
new file mode 100644
index 0000000..42c0105
--- /dev/null
+++ b/cordic.hh
@@ -0,0 +1,72 @@
+/*
+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;
+	}
+};
+
+}
+
diff --git a/tests/cordic_test.cc b/tests/cordic_test.cc
new file mode 100644
index 0000000..e4dfb68
--- /dev/null
+++ b/tests/cordic_test.cc
@@ -0,0 +1,33 @@
+/*
+Test for the CORDIC atan2 function
+
+Copyright 2019 Ahmet Inan <inan@aicodix.de>
+*/
+
+#include <cassert>
+#include <iostream>
+#include <cmath>
+#include "unit_circle.hh"
+#include "cordic.hh"
+#include "const.hh"
+
+template <typename TYPE>
+void test()
+{
+	const int N = 1 << (8*sizeof(TYPE));
+	for (int n = -N/2; n < N/2; ++n) {
+		TYPE x = nearbyint((N/2-1) * DSP::UnitCircle<double>::cos(n, N));
+		TYPE y = nearbyint((N/2-1) * DSP::UnitCircle<double>::sin(n, N));
+		TYPE a = DSP::CORDIC<TYPE>::atan2(y, x);
+		assert(abs(a-n) <= 1);
+	}
+}
+
+int main()
+{
+	test<char>();
+	test<short>();
+	std::cerr << "CORDIC atan2 test passed!" << std::endl;
+	return 0;
+}
+