Merge branch 'main' into feature/add-savitzky-golay-filter

Author: gabrielfrasantos

README.md

@@ -26,7 +26,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal
 | [Neural Network](doc/neural_network/README.md)                     | Layers, activations, losses, model                                   |
 | [Optimization](doc/optimization/README.md)                         | Gradient Descent                                                     |
 | [Regularization](doc/regularization/README.md)                     | L1 (Lasso), L2 (Ridge)                                              |
-| [Math](doc/math/README.md)                                         | Quaternion                                                           |
+| [Math](doc/math/README.md)                                         | CORDIC, Quaternion                                                   |
 | [Solvers](doc/solvers/README.md)                                   | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE |
 | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD                                         |
 

ROADMAP.md

@@ -29,7 +29,6 @@ Difficulty legend:
 |----|------------------------------------------------------|---------------------------|------------|
 | 15 | Biquad / Second-Order-Section cascade                | `filters/passive`         | ★★★☆☆      |
 | 20 | Integral / servo state feedback (LQI)                | `controllers`             | ★★★☆☆      |
-| 23 | CORDIC                                               | `math`                    | ★★★☆☆      |
 | 24 | Runge-Kutta ODE integrators (RK4 + Dormand-Prince)   | `solvers`                 | ★★★☆☆      |
 | 25 | Real-input FFT (RFFT)                                | `analysis`                | ★★★☆☆      |
 | 26 | Controllability / Observability matrices & Gramians  | `control_analysis`        | ★★★☆☆      |

doc/math/Cordic.md

@@ -0,0 +1,132 @@
+# CORDIC
+
+## Overview & Motivation
+
+Trigonometric functions, magnitudes, and vector rotations are fundamental operations in motor
+control, radar processing, and navigation. On microcontrollers without a floating-point unit or
+hardware multiplier, library implementations of `sin`, `cos`, `atan2`, and `hypot` require
+expensive software-emulated multiplications. This limits their use in hard real-time loops where
+deterministic execution time is mandatory.
+
+CORDIC (COordinate Rotation DIgital Computer) solves this by expressing any planar rotation as a
+sum of progressively smaller elementary rotations, each of which requires only a bit-shift and an
+addition. The result is a trig engine that runs on shift-add hardware with a fixed, data-independent
+cycle count — exactly what a deterministic real-time loop demands.
+
+## Mathematical Theory
+
+### Elementary Rotations
+
+A rotation by angle $\theta$ in two dimensions transforms a vector $(x, y)$ to
+
+$$x' = x\cos\theta - y\sin\theta, \quad y' = y\cos\theta + x\sin\theta.$$
+
+Factoring out $\cos\theta$ gives
+
+$$x' = \cos\theta\,(x - y\tan\theta), \quad y' = \cos\theta\,(y + x\tan\theta).$$
+
+When $\tan\theta_i = \pm 2^{-i}$, the multiplication by $\tan\theta_i$ becomes a right-shift by $i$
+bits. The angles $\theta_i = \arctan(2^{-i})$ form the **CORDIC angle table**.
+
+### Rotation Mode (sin/cos)
+
+Starting from $(x_0, y_0, z_0) = (K, 0, \theta)$, each iteration steers the residual angle $z$
+toward zero:
+
+$$x_{i+1} = x_i - \sigma_i \, 2^{-i} y_i$$
+$$y_{i+1} = y_i + \sigma_i \, 2^{-i} x_i$$
+$$z_{i+1} = z_i - \sigma_i \, \theta_i$$
+
+where $\sigma_i = \text{sign}(z_i)$. After $N$ iterations, $x_N \approx \cos\theta$ and
+$y_N \approx \sin\theta$.
+
+### Vectoring Mode (atan2/magnitude)
+
+Starting from $(x_0, y_0, z_0) = (x, y, 0)$, each iteration steers $y$ toward zero:
+
+$$\sigma_i = -\text{sign}(y_i)$$
+
+After $N$ iterations, $z_N \approx \arctan(y/x)$ and $x_N \approx \|(x, y)\| / K$.
+
+### CORDIC Gain
+
+Each elementary rotation stretches the vector length by $\sqrt{1 + 2^{-2i}}$. The accumulated
+gain over $N$ iterations is
+
+$$A_N = \prod_{i=0}^{N-1} \sqrt{1 + 2^{-2i}}.$$
+
+The constant $K = 1/A_N \approx 0.6073$ compensates for this growth. In rotation mode the initial
+$x$ is pre-scaled by $K$; in vectoring mode the final $x$ is multiplied by $K$.
+
+### Convergence Domain
+
+The convergence domain is $|z| \leq \sum_{i=0}^{N-1} \arctan(2^{-i})$. For $N = 16$ this exceeds
+$\pi/2$, so inputs outside $[-\pi/2, \pi/2]$ must be range-reduced by shifting the angle by $\pm\pi$
+and inverting the output signs. Vectoring mode uses quadrant detection on the signs of $x$ and $y$
+to handle the full $[-\pi, \pi]$ range.
+
+## Complexity Analysis
+
+| Metric      | Value                                                 |
+|-------------|-------------------------------------------------------|
+| Time        | $O(N)$ — exactly $N$ shift-add steps per call         |
+| Space       | $O(N)$ — angle table in ROM; $O(1)$ working registers |
+| Cycle count | Fixed, data-independent — no branch on input value    |
+
+One additional bit of precision is gained per iteration. $N = 16$ yields approximately 16-bit
+accuracy; $N = 20$ reaches the limits of single-precision float.
+
+## Step-by-Step Walkthrough
+
+Compute $\sin(\pi/6) = 0.5$ with $N = 4$ for brevity (gain $K_4 \approx 0.6352$).
+
+| $i$ | $\theta_i$ | $\sigma_i$ | $x_i$  | $y_i$  | $z_i$   |
+|-----|------------|------------|--------|--------|---------|
+| —   | —          | —          | 0.6352 | 0.0000 | 0.5236  |
+| 0   | 0.7854     | +1         | 0.6352 | 0.6352 | −0.2618 |
+| 1   | 0.4636     | −1         | 0.7940 | 0.3176 | 0.2018  |
+| 2   | 0.2450     | +1         | 0.7147 | 0.5122 | −0.0432 |
+| 3   | 0.1244     | −1         | 0.8425 | 0.4248 | 0.0812  |
+
+After iteration 3: $y_4 \approx 0.43$, improving toward 0.5 as $N$ grows.
+
+## Pitfalls & Edge Cases
+
+The input to rotation mode must lie within the convergence domain after range reduction. Angles
+at exactly $\pm\pi/2$ sit at the edge of the domain and may accumulate an extra half-ulp error.
+
+In vectoring mode, $(x, y) = (0, 0)$ is degenerate; by convention the angle is returned as zero
+rather than causing a division-by-zero or NaN.
+
+The shift $2^{-i}$ eventually underflows in floating-point for large $i$; iterations beyond
+$\lfloor -\log_2(\epsilon) \rfloor$ contribute nothing and can be capped without loss of accuracy.
+
+## Variants & Generalizations
+
+**Hyperbolic CORDIC** replaces the elementary angle table with $\tanh^{-1}(2^{-i})$ and handles
+`sinh`, `cosh`, `exp`, and `ln`.
+
+**Linear CORDIC** uses shifts alone (no rotation) to implement multiply and divide.
+
+**Double-rotation trick** repeats certain iterations to extend the convergence domain to $(-\pi, \pi]$
+without a range-reduction step.
+
+## Applications
+
+- Field-oriented motor control: Park/Clarke transforms require `sin`/`cos` at carrier frequency.
+- Radar and sonar: Cartesian-to-polar conversion of sample streams.
+- Navigation: continuous `atan2` for heading on heading-constrained MCUs.
+- Audio synthesis: wavetable-free sine generation on FPU-less targets.
+
+## Connections to Other Algorithms
+
+`TrigonometricFunctions` provides a table-lookup alternative with lower iteration count but higher
+ROM usage for the same precision. `Quaternion` consumes CORDIC-generated `sin`/`cos` for axis-angle
+conversions. On targets with an FPU the standard library usually outperforms CORDIC; the advantage
+is exclusive to multiply-poor hardware.
+
+## References & Further Reading
+
+- J. E. Volder, "The CORDIC Trigonometric Computing Technique," *IRE Transactions on Electronic Computers*, EC-8(3), pp. 330–334, 1959.
+- R. Andraka, "A survey of CORDIC algorithms for FPGA-based computers," *Proc. ACM/SIGDA FPGA*, 1998, pp. 191–200.
+- J. S. Walther, "A unified algorithm for elementary functions," *AFIPS Spring Joint Computer Conference*, 1971.

doc/math/README.md

@@ -6,4 +6,5 @@ Core mathematical primitives for numerical computation.
 
 | Algorithm                   | Description                                                                                   |
 |-----------------------------|-----------------------------------------------------------------------------------------------|
+| [CORDIC](Cordic.md)         | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier |
 | [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions |

numerical/math/CMakeLists.txt

@@ -12,6 +12,7 @@ target_link_libraries(numerical.math ${NUMERICAL_VISIBILITY}
 target_sources(numerical.math PRIVATE
     AdvancedFunctions.hpp
     ComplexNumber.hpp
+    Cordic.hpp
     Geometry3D.hpp
     HyperbolicFunctions.hpp
     LinearTimeInvariant.hpp
@@ -28,6 +29,7 @@ target_sources(numerical.math PRIVATE
 
 numerical_add_coverage_sources(numerical.math
     ComplexNumber.cpp
+    Cordic.cpp
     LinearTimeInvariant.cpp
     Matrix.cpp
     QNumber.cpp

numerical/math/Cordic.cpp

@@ -0,0 +1,7 @@
+#include "numerical/math/Cordic.hpp"
+
+namespace math
+{
+    template class Cordic<float, 16>;
+    template class Cordic<float, 8>;
+}

numerical/math/Cordic.hpp

@@ -0,0 +1,169 @@
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "numerical/math/CompilerOptimizations.hpp"
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <numbers>
+#include <type_traits>
+
+namespace math
+{
+    template<typename T, std::size_t Iterations = 16>
+    class Cordic
+    {
+        static_assert(std::is_floating_point_v<T>, "Cordic supports floating-point types only");
+
+    public:
+        struct SinCos
+        {
+            T sin;
+            T cos;
+        };
+
+        struct PolarResult
+        {
+            T magnitude;
+            T angle;
+        };
+
+        OPTIMIZE_FOR_SPEED SinCos SineCosine(T angleRadians) const;
+        T Arctangent2(T y, T x) const;
+        T Magnitude(T y, T x) const;
+        std::array<T, 2> Rotate(std::array<T, 2> v, T angle) const;
+
+    private:
+        struct VectoringResult
+        {
+            T x;
+            T angle;
+        };
+
+        VectoringResult VectoringMode(T xv, T yv) const
+        {
+            T z{ T(0) };
+            for (std::size_t i = 0; i < Iterations; ++i)
+            {
+                T d{ (yv >= T(0)) ? T(-1) : T(1) };
+                T pow2i{ T(1) / static_cast<T>(std::size_t(1) << i) };
+                T xNew{ xv - d * yv * pow2i };
+                T yNew{ yv + d * xv * pow2i };
+                z -= d * atanTable[i];
+                xv = xNew;
+                yv = yNew;
+            }
+            return VectoringResult{ xv, z };
+        }
+
+        static std::array<T, Iterations> BuildAtanTable()
+        {
+            std::array<T, Iterations> table{};
+            for (std::size_t i = 0; i < Iterations; ++i)
+                table[i] = static_cast<T>(std::atan(std::pow(T(2), -static_cast<T>(i))));
+            return table;
+        }
+
+        static T ComputeK()
+        {
+            T k{ T(1) };
+            for (std::size_t i = 0; i < Iterations; ++i)
+                k *= static_cast<T>(std::cos(std::atan(std::pow(T(2), -static_cast<T>(i)))));
+            return k;
+        }
+
+        static inline const std::array<T, Iterations> atanTable{ BuildAtanTable() };
+        static inline const T K{ ComputeK() };
+    };
+
+    template<typename T, std::size_t Iterations>
+    OPTIMIZE_FOR_SPEED typename Cordic<T, Iterations>::SinCos Cordic<T, Iterations>::SineCosine(T angleRadians) const
+    {
+        const T pi{ std::numbers::pi_v<T> };
+        const T halfPi{ pi / T(2) };
+
+        T angle{ angleRadians };
+        T sinSign{ T(1) };
+        T cosSign{ T(1) };
+
+        if (angle > halfPi)
+        {
+            angle -= pi;
+            sinSign = T(-1);
+            cosSign = T(-1);
+        }
+        else if (angle < -halfPi)
+        {
+            angle += pi;
+            sinSign = T(-1);
+            cosSign = T(-1);
+        }
+
+        T x{ K };
+        T y{ T(0) };
+        T z{ angle };
+
+        for (std::size_t i = 0; i < Iterations; ++i)
+        {
+            T d{ (z >= T(0)) ? T(1) : T(-1) };
+            T pow2i{ T(1) / static_cast<T>(std::size_t(1) << i) };
+            T xNew{ x - d * y * pow2i };
+            T yNew{ y + d * x * pow2i };
+            z -= d * atanTable[i];
+            x = xNew;
+            y = yNew;
+        }
+
+        return SinCos{ sinSign * y, cosSign * x };
+    }
+
+    template<typename T, std::size_t Iterations>
+    T Cordic<T, Iterations>::Arctangent2(T y, T x) const
+    {
+        const T pi{ std::numbers::pi_v<T> };
+
+        T quadrantOffset{ T(0) };
+        T xv{ x };
+        T yv{ y };
+
+        if (xv < T(0) && yv >= T(0))
+        {
+            xv = -x;
+            yv = -y;
+            quadrantOffset = pi;
+        }
+        else if (xv < T(0) && yv < T(0))
+        {
+            xv = -x;
+            yv = -y;
+            quadrantOffset = -pi;
+        }
+
+        auto res = VectoringMode(xv, yv);
+        return res.angle + quadrantOffset;
+    }
+
+    template<typename T, std::size_t Iterations>
+    T Cordic<T, Iterations>::Magnitude(T y, T x) const
+    {
+        T xv{ (x < T(0)) ? -x : x };
+        T yv{ (y < T(0)) ? -y : y };
+        auto res = VectoringMode(xv, yv);
+        return K * res.x;
+    }
+
+    template<typename T, std::size_t Iterations>
+    std::array<T, 2> Cordic<T, Iterations>::Rotate(std::array<T, 2> v, T angle) const
+    {
+        auto sc = SineCosine(angle);
+        return std::array<T, 2>{ v[0] * sc.cos - v[1] * sc.sin, v[0] * sc.sin + v[1] * sc.cos };
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class Cordic<float, 16>;
+    extern template class Cordic<float, 8>;
+#endif
+}

numerical/math/test/CMakeLists.txt

@@ -9,6 +9,7 @@ target_link_libraries(numerical.math_test PUBLIC
 
 target_sources(numerical.math_test PRIVATE
     TestComplexNumber.cpp
+    TestCordic.cpp
     TestLinearTimeInvariant.cpp
     TestQNumber.cpp
     TestMatrix.cpp