fixed16.d

   1 /* Invisible Vector Library
   2  * coded by Ketmar // Invisible Vector <ketmar@ketmar.no-ip.org>
   3  * Understanding is not required. Only obedience.
   4  *
   5  * This program is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, version 3 of the License ONLY.
   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. If not, see <http://www.gnu.org/licenses/>.
  16  */
  17 // UNFINISHED! DO NOT USE!
  18 module iv.fixed16;
  19
  20 // ////////////////////////////////////////////////////////////////////////// //
  21 alias Fixnum = FixnumN!(int, 16);
  22 enum Fixed(int v) = Fixnum(v);
  23 enum Fixed(float v) = Fixnum(cast(double)v);
  24 enum Fixed(double v) = Fixnum(v);
  25
  26
  27 // ////////////////////////////////////////////////////////////////////////// //
  28 struct FixnumN(T=int, ubyte scl=16) if ((is(T == byte) || is(T == int) || is(T == long)) && scl > 0 && scl < T.sizeof*8-1) {
  29 private:
  30   alias Me = typeof(this);
  31
  32 public:
  33   enum mpl = cast(T)1<<scale;
  34   enum scale = scl;
  35   T value;
  36   static FixnumN make (T v) nothrow @safe @nogc { FixnumN fixed; fixed.value = v; return fixed; }
  37
  38 public:
  39   string toString () const { import std.format : format; return "%f".format(cast(double)value/mpl); }
  40
  41 public nothrow @safe @nogc:
  42   // min and max represent the smallest and larget possible values respectively
  43   enum min = make(-T.max);
  44   enum max = make(T.max);
  45   static if (is(T == int) && scl == 16) {
  46     enum One = make(1<<16);
  47     enum Half = make(1<<15);
  48     enum Nan = make(0x80000000);
  49     enum Inf = make(int.max);
  50     enum PI = make(0x3243f);
  51     enum PId2 = make(0x3243f>>1);
  52     enum PIx2 = make(0x3243f<<1);
  53     enum Sqrt2 = make(92682);
  54   }
  55
  56   this (T v) { pragma(inline, true); value = v*mpl; }
  57   this (double v) { pragma(inline, true); import core.math : rndtol; value = cast(T)rndtol(v*mpl); }
  58
  59   Me opUnary(string op:"++") () pure { pragma(inline, true); value += mpl; return this; }
  60   Me opUnary(string op:"--") () pure { pragma(inline, true); value -= mpl; return this; }
  61
  62   Me opUnary(string op) () const pure { pragma(inline, true); mixin("return make("~op~"value);"); }
  63
  64   bool opEquals (Me b) const pure { pragma(inline, true); return (value == b.value); }
  65   bool opEquals (T b) const pure { pragma(inline, true); return (value == b*mpl); }
  66   bool opEquals (double b) const pure { pragma(inline, true); return ((cast(double)value/mpl) == b); }
  67
  68   int opCmp (const Me b) const pure { pragma(inline, true); return (value < b.value ? -1 : value > b.value ? 1 : 0); }
  69   int opCmp (const T b) const pure { pragma(inline, true); return (value < b*mpl ? -1 : value > b*mpl ? 1 : 0); }
  70   int opCmp (const double b) const pure { pragma(inline, true); return (value < b*mpl ? -1 : value > b*mpl ? 1 : 0); }
  71
  72   void opAssign (Me v) pure { pragma(inline, true); value = v.value; }
  73   void opAssign (T v) pure { pragma(inline, true); value = v*mpl; }
  74   void opAssign (double v) pure { pragma(inline, true); value = cast(T)(v*mpl); }
  75
  76   void opOpAssign (string op) (Me v) pure if (op == "+" || op == "-") { pragma(inline, true); mixin("value "~op~"= v.value;"); }
  77   void opOpAssign (string op) (Me v) pure if (op == "*" || op == "/") { pragma(inline, true); mixin("value = cast(T)(cast(long)value"~op~"cast(long)v.value/mpl);"); }
  78
  79   void opOpAssign (string op) (T v) pure if (op == "+" || op == "-") { pragma(inline, true); mixin("value "~op~"= v*mpl;"); }
  80   void opOpAssign (string op) (T v) pure if (op == "*" || op == "/") { pragma(inline, true); mixin("value "~op~"= v;"); }
  81
  82   void opOpAssign (string op) (double v) pure if (op == "+" || op == "-") { pragma(inline, true); import core.math : rndtol; mixin("value "~op~"= rndtol(v*mpl);"); }
  83   void opOpAssign (string op) (double v) pure if (op == "*" || op == "/") { pragma(inline, true); import core.math : rndtol; mixin("value "~op~"= rndtol(v);"); }
  84
  85   T opCast(DT) () const pure if (is(DT == byte) || is(DT == ubyte) || is(DT == short) || is(DT == ushort) || is(DT == int) || is(DT == uint) || is(DT == long) || is(DT == ulong)) { pragma(inline, true); return cast(DT)(value/mpl); }
  86   T opCast(DT) () const pure if (is(DT == float) || is(DT == double)) { pragma(inline, true); return cast(DT)((cast(double)value)/mpl); }
  87   T opCast(DT) () const pure if (is(DT == bool)) { pragma(inline, true); return value != 0; }
  88
  89
  90   Me opBinary(string op) (Me b) const pure if (op == "+" || op == "-") { pragma(inline, true); mixin("return make(value"~op~"b.value);"); }
  91   Me opBinary(string op) (Me b) const pure if (op == "*" || op == "/") { pragma(inline, true); mixin("return make(cast(T)(cast(long)value"~op~"cast(long)b.value/mpl));"); }
  92
  93   Me opBinary(string op) (T b) const pure if (op == "+" || op == "-" || op == "%") { pragma(inline, true); mixin("return make(value"~op~"(b*mpl));"); }
  94   Me opBinary(string op) (T b) const pure if (op == "*" || op == "/") { pragma(inline, true); mixin("return make(value"~op~"b);"); }
  95
  96   Me opBinaryRight(string op) (T b) const pure if (op == "+" || op == "-" || op == "%") { pragma(inline, true); mixin("return make((b*mpl)"~op~"value);"); }
  97   Me opBinaryRight(string op) (T b) const pure if (op == "*") { pragma(inline, true); return make(cast(T)(b*value)); }
  98   Me opBinaryRight(string op) (T b) const pure if (op == "/") { pragma(inline, true); return make(cast(T)(cast(long)(b*mpl*mpl)/value)); }
  99
 100
 101   Me opBinary(string op) (double b) const pure if (op == "+" || op == "-" || op == "%") { pragma(inline, true); import core.math : rndtol; mixin("return make(value"~op~"cast(T)rndtol(b*mpl));"); }
 102   Me opBinary(string op) (double b) const pure if (op == "*" || op == "/") { pragma(inline, true); import core.math : rndtol; mixin("return make(value"~op~"cast(T)rndtol(b));"); }
 103
 104   Me opBinaryRight(string op) (double b) const pure if (op == "+" || op == "-" || op == "%") { pragma(inline, true); import core.math : rndtol; mixin("return make(cast(T)rndtol(b*mpl)"~op~"value);"); }
 105   Me opBinaryRight(string op) (double b) const pure if (op == "*") { pragma(inline, true); import core.math : rndtol; return make(cast(T)(cast(T)rndtol(b)*value)); }
 106   Me opBinaryRight(string op) (double b) const pure if (op == "/") { pragma(inline, true); import core.math : rndtol; return make(cast(T)(rndtol(b*mpl*mpl)/value)); }
 107
 108   @property Me abs () const pure { pragma(inline, true); return make(value >= 0 ? value : -value); }
 109   @property int sign () const pure { pragma(inline, true); return (value < 0 ? -1 : value > 0 ? 1 : 0); }
 110   @property isnan () const pure { pragma(inline, true); return (value == T.min); }
 111
 112   static if (is(T == int) && scl == 16) {
 113     static Me cos (Me rads) { pragma(inline, true); Me res; calcSinCosF16(rads.value, null, &res.value); return res; }
 114     static Me sin (Me rads) { pragma(inline, true); Me res; calcSinCosF16(rads.value, &res.value, null); return res; }
 115     static void sincos (Me rads, ref Me s, ref Me c) { pragma(inline, true); calcSinCosF16(rads.value, &s.value, &c.value); }
 116     static Me atan2 (Me y, Me x) { pragma(inline, true); return make(calcAtan2F16(y.value, x.value)); }
 117     static Me atan (Me x) { pragma(inline, true); return make(calcAtanF16(x.value)); }
 118     static Me asin (Me x) { pragma(inline, true); return make(calcAsinF16(x.value)); }
 119   }
 120
 121 private static:
 122   // code taken from tbox
 123   void calcSinCosF16 (int x, int* s, int* c) @trusted {
 124     // |angle| < 90 degrees
 125     // x0,y0: fixed30
 126     static void cordicRotation (int* x0, int* y0, int z0) {
 127       int i = 0;
 128       int atan2i = 0;
 129       int z = z0;
 130       int x = *x0; // fixed30
 131       int y = *y0; // fixed30
 132       int xi = 0; // fixed30
 133       int yi = 0; // fixed30
 134       immutable(int)* patan2i = tb_fixed16_cordic_atan2i_table.ptr;
 135       do {
 136         xi = x>>i;
 137         yi = y>>i;
 138         atan2i = *patan2i++;
 139         if (z >= 0) {
 140           x -= yi;
 141           y += xi;
 142           z -= atan2i;
 143         } else {
 144           x += yi;
 145           y -= xi;
 146           z += atan2i;
 147         }
 148       } while (++i < 16);
 149       *x0 = x;
 150       *y0 = y;
 151     }
 152
 153     // main
 154
 155     // (x0, y0) = (k, 0), k = 0.607252935 => fixed30
 156     int cos = 0x26dd3b6a; // fixed30
 157     int sin = 0; // fixed30
 158
 159     /* scale to 65536 degrees from x radians: x * 65536 / (2 * pi)
 160      *
 161      * 90:  0x40000000
 162      * 180: 0x80000000
 163      * 270: 0xc0000000
 164      */
 165     int ang = x*0x28be;
 166
 167     /* quadrant
 168      *
 169      * 1: 00 ...
 170      * 2: 01 ...
 171      * 3: 10 ...
 172      * 4: 11 ...
 173      *
 174      * quadrant++
 175      *
 176      * 1: 01 ...
 177      * 2: 10 ...
 178      * 3: 11 ...
 179      * 4: 00 ...
 180      *
 181      */
 182     int quadrant = ang>>30;
 183     ++quadrant;
 184
 185     /* quadrant == 2, 3, |angle| < 90
 186      *
 187      * 100 => -100 + 180 => 80
 188      * -200 => 200 + 180 => -20
 189      */
 190     if (quadrant&0x2) ang = -ang+0x80000000;
 191
 192     // rotation
 193     cordicRotation(&cos, &sin, ang);
 194
 195     // result
 196     if (s !is null) *s = sin>>14; // fixed30->fixed16
 197     if (c !is null) {
 198       // quadrant == 2, 3
 199       if (quadrant&0x2) cos = -cos;
 200       *c = cos>>14; // fixed30->fixed16
 201     }
 202   }
 203
 204   // |angle| < 90 degrees
 205   int cordicVectorAtan2 (int y0, int x0) @trusted {
 206     int i = 0;
 207     int atan2i = 0;
 208     int z = 0;
 209     int x = x0;
 210     int y = y0;
 211     int xi = 0;
 212     int yi = 0;
 213     immutable(int)* patan2i = tb_fixed16_cordic_atan2i_table.ptr;
 214     do {
 215       xi = x>>i;
 216       yi = y>>i;
 217       atan2i = *patan2i++;
 218       if (y < 0) {
 219         x -= yi;
 220         y += xi;
 221         z -= atan2i;
 222       } else {
 223         x += yi;
 224         y -= xi;
 225         z += atan2i;
 226       }
 227     } while (++i < 16);
 228     return z / 0x28be;
 229   }
 230
 231   // slope angle: [-180, 180]
 232   // the precision will be pool if x, y is too small
 233   int calcAtan2F16 (int y, int x) {
 234     if (!(x|y)) return 0;
 235     // abs
 236     int xs = tbGetSign(x);
 237     x = (x >= 0 ? x : -x); //tb_fixed30_abs(x);
 238     // quadrant: 1, 4
 239     int z = cordicVectorAtan2(y, x);
 240     // for quadrant: 2, 3
 241     if (xs) {
 242       int zs = tbGetSign(z);
 243       if (y == 0) zs = 0;
 244       int pi = tbSetSign(0x3243f/*TB_FIXED16_PI*/, zs);
 245       z = pi-z;
 246     }
 247     return z;
 248   }
 249
 250   // |angle| < 90
 251   // the precision will be pool if x is too large.
 252   int calcAtanF16 (int x) {
 253     if (!x) return 0;
 254     return cordicVectorAtan2(x, 1<<16/*TB_FIXED16_ONE*/);
 255   }
 256
 257   int calcAsinF16 (int x) {
 258     // |angle| < 90 degrees
 259     static int tb_fixed16_cordic_vector_asin (int m) @trusted {
 260       int i = 0;
 261       int atan2i = 0;
 262       int z = 0;
 263       int x = 0x18bde0bb; // k = 0.607252935
 264       int y = 0;
 265       int xi = 0;
 266       int yi = 0;
 267       immutable(int)* patan2i = tb_fixed16_cordic_atan2i_table.ptr;
 268       do {
 269         xi = x>>i;
 270         yi = y>>i;
 271         atan2i = *patan2i++;
 272         if (y < m) {
 273           x -= yi;
 274           y += xi;
 275           z -= atan2i;
 276         } else {
 277           x += yi;
 278           y -= xi;
 279           z += atan2i;
 280         }
 281       } while (++i < 16);
 282       return z / 0x28be;
 283     }
 284
 285     // abs
 286     int s = tbGetSign(x);
 287     x = (x >= 0 ? x : -1); //tb_fixed16_abs(x);
 288     if (x >= 1<<16/*TB_FIXED16_ONE*/) return tbSetSign((0x3243f/*TB_FIXED16_PI*/) >> 1, s);
 289     int z = tb_fixed16_cordic_vector_asin(x * 0x28be);
 290     return tbSetSign(z, ~s);
 291   }
 292
 293   // return -1 if x < 0, else return 0
 294   int tbGetSign (int x) pure nothrow @safe @nogc {
 295     pragma(inline, true);
 296     int s = (cast(int)(x) >> 31);
 297     //tb_assert((x < 0 && s == -1) || (x >= 0 && !s));
 298     return s;
 299   }
 300
 301   // if s == -1, return -x, else s must be 0, and return x.
 302   int tbSetSign (int x, int s) pure nothrow @safe @nogc {
 303     pragma(inline, true);
 304     //tb_assert(s == 0 || s == -1);
 305     return (x ^ s) - s;
 306   }
 307
 308   /+
 309   int invert (int x) {
 310     // is one?
 311     if (x == 1<<16/*TB_FIXED16_ONE*/) return 1<<16/*TB_FIXED16_ONE*/;
 312     // get sign
 313     int s = tbGetSign(x);
 314     // abs(x)
 315     x = (x >= 0 ? x : -x);
 316     // is infinity?
 317     if (x <= 2) return (s < 0 ? -T.max : T.max); //return tbSetSign(TB_FIXED16_MAX, s);
 318
 319     // normalize
 320     int cl0 = cast(int)tb_bits_cl0_u32_be(x);
 321     x = x << cl0 >> 16;
 322
 323     // compute 1 / x approximation (0.5 <= x < 1.0)
 324     // (2.90625 (~2.914) - 2 * x) >> 1
 325     uint r = 0x17400-x;
 326
 327     // newton-raphson iteration:
 328     // x = r * (2 - x * r) = ((r / 2) * (1 - x * r / 2)) * 4
 329     r = ((0x10000 - ((x * r) >> 16)) * r) >> 15;
 330     r = ((0x10000 - ((x * r) >> 16)) * r) >> (30 - cl0);
 331
 332     return tbSetSign(r, s);
 333   }
 334   +/
 335
 336 private:
 337   __gshared immutable int[30] tb_fixed16_cordic_atan2i_table = [
 338         0x20000000  // 45.000000
 339     ,   0x12e4051d  // 26.565051
 340     ,   0x9fb385b   // 14.036243
 341     ,   0x51111d4   // 7.125016
 342     ,   0x28b0d43   // 3.576334
 343     ,   0x145d7e1   // 1.789911
 344     ,   0xa2f61e    // 0.895174
 345     ,   0x517c55    // 0.447614
 346     ,   0x28be53    // 0.223811
 347     ,   0x145f2e    // 0.111906
 348     ,   0xa2f98     // 0.055953
 349     ,   0x517cc     // 0.027976
 350     ,   0x28be6     // 0.013988
 351     ,   0x145f3     // 0.006994
 352     ,   0xa2f9      // 0.003497
 353     ,   0x517c      // 0.001749
 354     ,   0x28be      // 0.000874
 355     ,   0x145f      // 0.000437
 356     ,   0xa2f       // 0.000219
 357     ,   0x517       // 0.000109
 358     ,   0x28b       // 0.000055
 359     ,   0x145       // 0.000027
 360     ,   0xa2        // 0.000014
 361     ,   0x51        // 0.000007
 362     ,   0x28        // 0.000003
 363     ,   0x14        // 0.000002
 364     ,   0xa         // 0.000001
 365     ,   0x5         // 0.000000
 366     ,   0x2         // 0.000000
 367     ,   0x1         // 0.000000
 368   ];
 369 }
 370
 371
 372 // ////////////////////////////////////////////////////////////////////////// //
 373 /+
 374 import iv.vfs.io;
 375 import std.math;
 376
 377 void main () {
 378   writeln(Fixnum.PI);
 379   //auto n = Fixed!42;
 380   Fixnum n = 42;
 381   Fixnum n1 = 42.2;
 382   Fixnum nd1 = 0.1;
 383   writeln(n);
 384   writeln(n1);
 385   n += 0.1;
 386   writeln(n);
 387   n += nd1;
 388   writeln(n);
 389
 390   writeln("sin=", Fixnum.sin(Fixnum(0.3)));
 391   writeln("dsin=", sin(0.3));
 392   writeln("cos=", Fixnum.cos(Fixnum(0.5)));
 393   writeln("dcos=", cos(0.5));
 394 }
 395 +/