FastATan.hpp

00001 //--------------------------------------------------------------
00002 //  高速低精度 arctan 計算
00003 //      係数はミニマックス近似で求めたもの
00004 //      ただし，誤差は絶対誤差で評価した
00005 //
00006 //  2020/08/12, Copyright (c) 2020 MIKAMI, Naoki
00007 //--------------------------------------------------------------
00008 
00009 #include "mbed.h"
00010 
00011 #ifndef FAST_ARCTAN_LOW_PRECISION_HPP
00012 #define FAST_ARCTAN_LOW_PRECISION_HPP
00013 
00014 namespace Mikami
00015 {
00016     inline float ATanPoly(float x);
00017     
00018     // 引数の与え方は，atan2() と同じ
00019     float FastATan(float y, float x)
00020     {
00021         static const float PI    = 3.1415926536f;  // π
00022         static const float PI_4  = PI/4.0f;        // π/4
00023         static const float PI3_4 = 3.0f*PI/4.0f;   // 3π/4
00024         static const float PI_2  = PI/2.0f;        // π/2
00025 
00026         if ( (x == 0.0f) && (y == 0.0f) ) return 0.0f;
00027         if (y == 0.0f) return (x > 0.0f) ? 0.0f : PI;
00028 
00029         float abs_x = fabsf(x);
00030         float abs_y = fabsf(y);
00031 
00032         if (abs_x == abs_y)
00033         {
00034             if (x > 0.0f) return (y > 0.0f) ?  PI_4 : -PI_4;
00035             else          return (y > 0.0f) ? PI3_4 : -PI3_4;
00036         }
00037 
00038         if (abs_x > abs_y)  // |θ|< π/4，3π/4<|θ|<π
00039         {
00040             float u = ATanPoly(y/x);
00041             if (x > 0.0f) return u;
00042             else          return (y > 0.0f) ? u + PI : u - PI;
00043         }
00044         else                // π/4 <|θ|<3π/4
00045         {
00046             float u = ATanPoly(x/y);
00047             return (y > 0.0f) ? -u + PI_2 : -u - PI_2;
00048         }
00049     }
00050 
00051     inline float ATanPoly(float x)
00052     {
00053         static const float A1 =  0.9992138f;    // a1
00054         static const float A3 = -0.3211750f;    // a3
00055         static const float A5 =  0.1462645f;    // a5
00056         static const float A7 = -0.03898651f;   // a7
00057         
00058         float x2 = x*x;
00059         float atanx = (((A7*x2 + A5)*x2 + A3)*x2 + A1)*x;
00060         
00061         return atanx;
00062     }
00063 }
00064 #endif  // FAST_ARCTAN_LOW_PRECISION_HPP