Sine and Cosine using CTFE

gistfile1.d

module cordicalgorithm;

import std.traits, std.conv;

immutable PI      = 3.14159265359;
immutable trigono = gen_trigonometric!double();

pure int degreeloop(int deg){
  while(deg < 0 || deg >= 360){
    (deg < 0) ? deg += 360 : deg -= 360;
  }
  return deg;
}

pure auto sin(int deg){
  deg = degreeloop(deg);
  return trigono[deg][0];
}
pure auto cos(int deg){ 
  deg = degreeloop(deg);
  return trigono[deg][1]; 
}

struct Coord(T = float) if (isFloatingPoint!T){
  T[2] d = [1.0, 0.0];
  
  T[2] opUnary(string s)() if (s == "-") {
    d = [ -d[0], -d[1] ];
    return d;
  }
  
  pure T[2] mult(in T a, in T precision = 1e-12){
    d = [d[0] * a, d[1] * a];
    if(d[0] < precision) d[0] = 0;
    if(d[1] < precision) d[1] = 0;
    return d;
  }

  pure T[2] mult(in T[2][2] R){
    d = [ R[0][0]*d[0] + R[0][1]*d[1], R[1][0]*d[0] + R[1][1]*d[1] ];
    return d;
  }
}

pure T[2][] gen_trigonometric(T = float)(in uint iter = 50)
in{ 
  assert(isFloatingPoint!T, "T must be a FloatingPoint type"); 
}
body{
  T[2][] result = new T[2][](360);
  debug pragma(msg, "Generating trigonometric functions");
  foreach(i; 0 .. 360){
    result[i] = cordic!T(degToRad!T(i), iter);
  }
  debug pragma(msg, "Done trigonometric functions");
  return result;
}

pure T[][] mult(T)(in T[][] A, in T[][] B)
in{
  assert(A.length > 0,"A has no length");
  assert(B.length > 0,"B has no length");
  foreach(i; 0 .. A.length){
    assert(A[i].length == B.length, "A[i].length != B.length");
  }
}
body{
  T[][] d;
  T tmp;
  foreach(i; 0 .. A.length){
    T[] row;
    foreach(j; 0 .. B[i].length){ 
      tmp = 0.0;
      foreach(k; 0 .. A[i].length){ 
        tmp += A[i][k] * B[k][j];
      }
      row ~= tmp;
    }
    d ~= row;
  }
  return d;
}

immutable Kangles = [
  0.78539816339745, 0.46364760900081, 0.24497866312686, 0.12435499454676, 0.06241880999596, 
  0.03123983343027, 0.01562372862048, 0.00781234106010, 0.00390623013197, 0.00195312251648, 
  0.00097656218956, 0.00048828121119, 0.00024414062015, 0.00012207031189, 0.00006103515617, 
  0.00003051757812, 0.00001525878906, 0.00000762939453, 0.00000381469727, 0.00000190734863,
  0.00000095367432, 0.00000047683716, 0.00000023841858, 0.00000011920929, 0.00000005960464, 
  0.00000002980232, 0.00000001490116, 0.00000000745058
];

immutable Kvalues = [
  0.70710678118655, 0.63245553203368, 0.61357199107790, 0.60883391251775, 0.60764825625617, 
  0.60735177014130, 0.60727764409353, 0.60725911229889, 0.60725447933256, 0.60725332108988, 
  0.60725303152913, 0.60725295913894, 0.60725294104140, 0.60725293651701, 0.60725293538591, 
  0.60725293510314, 0.60725293503245, 0.60725293501477, 0.60725293501035, 0.60725293500925,
  0.60725293500897, 0.60725293500890, 0.60725293500889, 0.60725293500888 
];

pure size_t min(in size_t x, in size_t y)
body{ return (x < y)?  x : y; }

pure U degToRad(U = float, V = int)(in V deg)
in{
  assert(isFloatingPoint!U, "U must be a FloatingPoint type");
  assert(isIntegral!V, "V must be a Integral type");
}
body{ return (deg * PI) / to!U(360/2.0); }

pure T[] cordic(T = float)(T beta, in uint iter = 50)
in{
  assert(isFloatingPoint!T, "T must be a FloatingPoint type");
}
body{
  Coord!T v;
  if(beta < -(.5*PI) || beta > (.5*PI)){
    T newbeta = (beta < 0)? (beta + PI) : (beta - PI); 
    v.d = cordic!T(newbeta, iter);
    return -v;
  }

  int sigma;
  float Kn = Kvalues[min(iter, (Kvalues.length-1))];
  float factor;
  T R[2][2];
  float onedivpowtwo = 1;
  float angle = Kangles[0];

  for(size_t j = 0; j < iter; j++){
    sigma  = (beta < 0)? -1 : 1; 
    factor = sigma * onedivpowtwo;
    R = [[1, -factor],[factor, 1]];
    v.mult(R);
    beta = beta - sigma * angle;
    onedivpowtwo = onedivpowtwo / 2.0;
    if(j+1 > (Kangles.length-1)){
      angle = angle / 2.0;
    }else{
      angle = Kangles[j+1];
    }
  }
  return v.mult(Kn);
}