source/maths/Fixed.cpp

   1 /* Copyright (C) 2010 Wildfire Games.
   2  * This file is part of 0 A.D.
   3  *
   4  * 0 A.D. 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  * 0 A.D. 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 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "precompiled.h"
  19
  20 #include "Fixed.h"
  21
  22 #include "ps/CStr.h"
  23
  24 #include <sstream>
  25
  26 template<>
  27 CFixed_15_16 CFixed_15_16::FromString(const CStr8& s)
  28 {
  29         // Parse a superset of the xsd:decimal syntax: [-+]?\d*(\.\d*)?
  30
  31         if (s.empty())
  32                 return CFixed_15_16::Zero();
  33
  34         bool neg = false;
  35         CFixed_15_16 r;
  36         const char* c = &s[0];
  37
  38         if (*c == '+')
  39         {
  40                 ++c;
  41         }
  42         else if (*c == '-')
  43         {
  44                 ++c;
  45                 neg = true;
  46         }
  47
  48         while (true)
  49         {
  50                 // Integer part:
  51                 if (*c >= '0' && *c <= '9')
  52                 {
  53                         r = r * 10; // TODO: handle overflow gracefully, maybe
  54                         r += CFixed_15_16::FromInt(*c - '0');
  55                         ++c;
  56                 }
  57                 else if (*c == '.')
  58                 {
  59                         ++c;
  60                         u32 frac = 0;
  61                         u32 div = 1;
  62                         // Fractional part
  63                         while (*c >= '0' && *c <= '9')
  64                         {
  65                                 frac *= 10;
  66                                 frac += (*c - '0');
  67                                 div *= 10;
  68                                 ++c;
  69                                 if (div >= 100000)
  70                                 {
  71                                         // any further digits will be too small to have any major effect
  72                                         break;
  73                                 }
  74                         }
  75                         // too many digits or invalid character or end of string - add the fractional part and stop
  76                         r += CFixed_15_16(((u64)frac << 16) / div);
  77                         break;
  78                 }
  79                 else
  80                 {
  81                         // invalid character or end of string
  82                         break;
  83                 }
  84         }
  85
  86         return (neg ? -r : r);
  87 }
  88
  89 template<>
  90 CFixed_15_16 CFixed_15_16::FromString(const CStrW& s)
  91 {
  92         return FromString(s.ToUTF8());
  93 }
  94
  95 template<>
  96 CStr8 CFixed_15_16::ToString() const
  97 {
  98         std::stringstream r;
  99
 100         u32 posvalue = abs(value);
 101         if (value < 0)
 102                 r << "-";
 103
 104         r << (posvalue >> fract_bits);
 105
 106         u16 fraction = posvalue & ((1 << fract_bits) - 1);
 107         if (fraction)
 108         {
 109                 r << ".";
 110
 111                 u32 frac = 0;
 112                 u32 div = 1;
 113
 114                 // Do the inverse of FromString: Keep adding digits until (frac<<16)/div == expected fraction
 115                 while (true)
 116                 {
 117                         frac *= 10;
 118                         div *= 10;
 119
 120                         // Low estimate of d such that ((frac+d)<<16)/div == fraction
 121                         u32 digit = (((u64)fraction*div) >> 16) - frac;
 122                         frac += digit;
 123
 124                         // If this gives the exact target, then add the digit and stop
 125                         if (((u64)frac << 16) / div == fraction)
 126                         {
 127                                 r << digit;
 128                                 break;
 129                         }
 130
 131                         // If the next higher digit gives the exact target, then add that digit and stop
 132                         if (digit <= 8 && (((u64)frac+1) << 16) / div == fraction)
 133                         {
 134                                 r << digit+1;
 135                                 break;
 136                         }
 137
 138                         // Otherwise add the digit and continue
 139                         r << digit;
 140                 }
 141         }
 142
 143         return r.str();
 144 }
 145
 146 // Based on http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
 147 CFixed_15_16 atan2_approx(CFixed_15_16 y, CFixed_15_16 x)
 148 {
 149         CFixed_15_16 zero;
 150
 151         // Special case to avoid division-by-zero
 152         if (x.IsZero() && y.IsZero())
 153                 return zero;
 154
 155         CFixed_15_16 c1;
 156         c1.SetInternalValue(51472); // pi/4 << 16
 157
 158         CFixed_15_16 c2;
 159         c2.SetInternalValue(154415); // 3*pi/4 << 16
 160
 161         CFixed_15_16 abs_y = y.Absolute();
 162
 163         CFixed_15_16 angle;
 164         if (x >= zero)
 165         {
 166                 CFixed_15_16 r = (x - abs_y) / (x + abs_y);
 167                 angle = c1 - c1.Multiply(r);
 168         }
 169         else
 170         {
 171                 CFixed_15_16 r = (x + abs_y) / (abs_y - x);
 172                 angle = c2 - c1.Multiply(r);
 173         }
 174
 175         if (y < zero)
 176                 return -angle;
 177         else
 178                 return angle;
 179 }
 180
 181 template<>
 182 CFixed_15_16 CFixed_15_16::Pi()
 183 {
 184         return CFixed_15_16(205887); // = pi << 16
 185 }
 186
 187 void sincos_approx(CFixed_15_16 a, CFixed_15_16& sin_out, CFixed_15_16& cos_out)
 188 {
 189         // Based on http://www.coranac.com/2009/07/sines/
 190
 191         // TODO: this could be made a bit more precise by being careful about scaling
 192
 193         typedef CFixed_15_16 fixed;
 194
 195         fixed c2_pi;
 196         c2_pi.SetInternalValue(41721); // = 2/pi << 16
 197
 198         // Map radians onto the range [0, 4)
 199         fixed z = a.Multiply(c2_pi) % fixed::FromInt(4);
 200
 201         // Map z onto the range [-1, +1] for sin, and the same with z+1 to compute cos
 202         fixed sz, cz;
 203         if (z >= fixed::FromInt(3)) // [3, 4)
 204         {
 205                 sz = z - fixed::FromInt(4);
 206                 cz = z - fixed::FromInt(3);
 207         }
 208         else if (z >= fixed::FromInt(2)) // [2, 3)
 209         {
 210                 sz = fixed::FromInt(2) - z;
 211                 cz = z - fixed::FromInt(3);
 212         }
 213         else if (z >= fixed::FromInt(1)) // [1, 2)
 214         {
 215                 sz = fixed::FromInt(2) - z;
 216                 cz = fixed::FromInt(1) - z;
 217         }
 218         else // [0, 1)
 219         {
 220                 sz = z;
 221                 cz = fixed::FromInt(1) - z;
 222         }
 223
 224         // Third-order (max absolute error ~0.02)
 225
 226 //      sin_out = (sz / 2).Multiply(fixed::FromInt(3) - sz.Multiply(sz));
 227 //      cos_out = (cz / 2).Multiply(fixed::FromInt(3) - cz.Multiply(cz));
 228
 229         // Fifth-order (max absolute error ~0.0005)
 230
 231         fixed sz2 = sz.Multiply(sz);
 232         sin_out = sz.Multiply(fixed::Pi() - sz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - sz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2;
 233
 234         fixed cz2 = cz.Multiply(cz);
 235         cos_out = cz.Multiply(fixed::Pi() - cz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - cz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2;
 236 }