Diff: SDR_Library/FastATan.hpp

Revision:

1:30d9fb51dec1

Child:

2:4bec6b2be809

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDR_Library/FastATan.hpp	Mon Sep 23 07:32:10 2019 +0000
@@ -0,0 +1,77 @@
+//--------------------------------------------------------------
+//  高速低精度 arctan 計算
+//      係数はミニマックス近似で求めたもの
+//      ただし，誤差は絶対誤差で評価した
+//
+//  2019/09/18, Copyright (c) 2019 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)
+        {
+            if (x > 0.0f) return 0.0f;      // 0
+            else          return PI;        // π
+        }
+        float abs_x = fabsf(x);
+        float abs_y = fabsf(y);
+
+        if (abs_x == abs_y)
+        {
+            if (x > 0.0f)
+            {
+                if (y > 0.0f) return PI_4;  // π/4
+                else          return -PI_4; // -π/4
+            }
+            else
+            {
+                if (y > 0.0f) return PI3_4;  // 3π/4
+                else          return -PI3_4; // -3π/4
+            }
+        }
+
+        if (abs_x > abs_y)  // |θ|< π/4，3π/4<|θ|<π
+        {
+            float u = ATanPoly(y/x);
+            if (x > 0.0f) return u;
+            if (y > 0.0f) return u + PI;
+            else          return u - PI;
+        }
+        else                // π/4 <|θ|<3π/4
+        {
+            float u = ATanPoly(x/y);
+            if (y > 0.0f) return -u + PI_2;
+            else          return -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