Diff: SDR_Library/FastATan.hpp

Revision:

0:9c28f5905a1a

diff -r 000000000000 -r 9c28f5905a1a SDR_Library/FastATan.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDR_Library/FastATan.hpp	Sat Aug 29 11:25:01 2020 +0000
@@ -0,0 +1,64 @@
+//--------------------------------------------------------------
+//  高速低精度 arctan 計算
+//      係数はミニマックス近似で求めたもの
+//      ただし，誤差は絶対誤差で評価した
+//
+//  2020/08/12, Copyright (c) 2020 MIKAMI, Naoki
+//--------------------------------------------------------------
+
+#include "mbed.h"
+
+#ifndef FAST_ARCTAN_LOW_PRECISION_HPP
+#define FAST_ARCTAN_LOW_PRECISION_HPP
+
+namespace Mikami
+{
+    inline float ATanPoly(float x);
+    
+    // 引数の与え方は，atan2() と同じ
+    float FastATan(float y, float x)
+    {
+        static const float PI    = 3.1415926536f;  // π
+        static const float PI_4  = PI/4.0f;        // π/4
+        static const float PI3_4 = 3.0f*PI/4.0f;   // 3π/4
+        static const float PI_2  = PI/2.0f;        // π/2
+
+        if ( (x == 0.0f) && (y == 0.0f) ) return 0.0f;
+        if (y == 0.0f) return (x > 0.0f) ? 0.0f : PI;
+
+        float abs_x = fabsf(x);
+        float abs_y = fabsf(y);
+
+        if (abs_x == abs_y)
+        {
+            if (x > 0.0f) return (y > 0.0f) ?  PI_4 : -PI_4;
+            else          return (y > 0.0f) ? PI3_4 : -PI3_4;
+        }
+
+        if (abs_x > abs_y)  // |θ|< π/4，3π/4<|θ|<π
+        {
+            float u = ATanPoly(y/x);
+            if (x > 0.0f) return u;
+            else          return (y > 0.0f) ? u + PI : u - PI;
+        }
+        else                // π/4 <|θ|<3π/4
+        {
+            float u = ATanPoly(x/y);
+            return (y > 0.0f) ? -u + PI_2 : -u - PI_2;
+        }
+    }
+
+    inline float ATanPoly(float x)
+    {
+        static const float A1 =  0.9992138f;    // a1
+        static const float A3 = -0.3211750f;    // a3
+        static const float A5 =  0.1462645f;    // a5
+        static const float A7 = -0.03898651f;   // a7
+        
+        float x2 = x*x;
+        float atanx = (((A7*x2 + A5)*x2 + A3)*x2 + A1)*x;
+        
+        return atanx;
+    }
+}
+#endif  // FAST_ARCTAN_LOW_PRECISION_HPP
\ No newline at end of file