server/supernova/utilities/fast_approximations.hpp

   1 //  fast approximations different functions
   2 //  Copyright (C) 2008 Tim Blechmann
   3 //
   4 //  This program is free software; you can redistribute it and/or modify
   5 //  it under the terms of the GNU General Public License as published by
   6 //  the Free Software Foundation; either version 2 of the License, or
   7 //  (at your option) any later version.
   8 //
   9 //  This program is distributed in the hope that it will be useful,
  10 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 //  GNU General Public License for more details.
  13 //
  14 //  You should have received a copy of the GNU General Public License
  15 //  along with this program; see the file COPYING.  If not, write to
  16 //  the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  17 //  Boston, MA 02111-1307, USA.
  18
  19 #include <boost/type_traits.hpp>
  20 #include <boost/static_assert.hpp>
  21 #include <boost/math/constants/constants.hpp>
  22
  23 namespace nova
  24 {
  25
  26 /* fast atan2 approximation
  27  *
  28  * adapted from pseudocode posted in
  29  * http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
  30  *
  31  * max error of .072 rads, if abs(x) > 1e-8
  32  * */
  33 template <typename F>
  34 F faster_atan2(F y, F x)
  35 {
  36     BOOST_STATIC_ASSERT(boost::is_floating_point<F>::value);
  37
  38     F coeff_1 = boost::math::constants::pi<F>()/F(4);
  39     F coeff_2 = F(3)*coeff_1;
  40     F abs_y = std::abs(y) + F(1e-10);      // kludge to prevent 0/0 condition
  41
  42     F angle;
  43     if (x>=0)
  44     {
  45         F r = (x - abs_y) / (x + abs_y);
  46         angle = coeff_1 - coeff_1 * r;
  47     }
  48     else
  49     {
  50         F r = (x + abs_y) / (abs_y - x);
  51         angle = coeff_2 - coeff_1 * r;
  52     }
  53     if (y < 0)
  54         return(-angle);     // negate if in quad III or IV
  55     else
  56         return(angle);
  57 }
  58
  59 /* fast atan2 approximation
  60  *
  61  * adapted from pseudocode posted in
  62  * http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
  63  *
  64  * max error of .011 rads, if abs(x) > 1e-8
  65  * */
  66 template <typename F>
  67 F fast_atan2(F y, F x)
  68 {
  69     BOOST_STATIC_ASSERT(boost::is_floating_point<F>::value);
  70
  71     F coeff_1 = boost::math::constants::pi<F>()/F(4);
  72     F coeff_2 = F(3)*coeff_1;
  73     F abs_y = std::abs(y) + F(1e-10);      // kludge to prevent 0/0 condition
  74
  75     F angle;
  76     if (x>=0)
  77     {
  78         F r = (x - abs_y) / (x + abs_y);
  79         angle = F(0.1963) * r*r*r - F(0.9817) * r + coeff_1;
  80     }
  81     else
  82     {
  83         F r = (x + abs_y) / (abs_y - x);
  84         angle = F(0.1963) * r*r*r - F(0.9817) * r + coeff_2;
  85     }
  86     if (y < 0)
  87         return(-angle);     // negate if in quad III or IV
  88     else
  89         return(angle);
  90 }
  91
  92 } /* namespace nova */