git.mutantstargoat.com Git - regis/blob - sincos.c

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include "fixed.h"
   4 #include "sincos.h"
   5
   6 int16_t sinlut[SINLUT_SIZE] = {
   7         0, 6, 12, 18, 25, 31, 37, 43, 50, 56, 62, 68, 75, 81, 87, 93,
   8         99, 106, 112, 118, 124, 130, 136, 142, 148, 154, 160, 166, 172, 178, 184, 190,
   9         195, 201, 207, 213, 218, 224, 230, 235, 241, 246, 252, 257, 263, 268, 273, 279,
  10         284, 289, 294, 299, 304, 310, 314, 319, 324, 329, 334, 339, 343, 348, 353, 357,
  11         362, 366, 370, 375, 379, 383, 387, 391, 395, 399, 403, 407, 411, 414, 418, 422,
  12         425, 429, 432, 435, 439, 442, 445, 448, 451, 454, 457, 460, 462, 465, 468, 470,
  13         473, 475, 477, 479, 482, 484, 486, 488, 489, 491, 493, 495, 496, 498, 499, 500,
  14         502, 503, 504, 505, 506, 507, 508, 508, 509, 510, 510, 511, 511, 511, 511, 511,
  15         512, 511, 511, 511, 511, 511, 510, 510, 509, 508, 508, 507, 506, 505, 504, 503,
  16         502, 500, 499, 498, 496, 495, 493, 491, 489, 488, 486, 484, 482, 479, 477, 475,
  17         473, 470, 468, 465, 462, 460, 457, 454, 451, 448, 445, 442, 439, 435, 432, 429,
  18         425, 422, 418, 414, 411, 407, 403, 399, 395, 391, 387, 383, 379, 375, 370, 366,
  19         362, 357, 353, 348, 343, 339, 334, 329, 324, 319, 314, 310, 304, 299, 294, 289,
  20         284, 279, 273, 268, 263, 257, 252, 246, 241, 235, 230, 224, 218, 213, 207, 201,
  21         195, 190, 184, 178, 172, 166, 160, 154, 148, 142, 136, 130, 124, 118, 112, 106,
  22         99, 93, 87, 81, 75, 68, 62, 56, 50, 43, 37, 31, 25, 18, 12, 6,
  23         0, -6, -12, -18, -25, -31, -37, -43, -50, -56, -62, -68, -75, -81, -87, -93,
  24         -99, -106, -112, -118, -124, -130, -136, -142, -148, -154, -160, -166, -172, -178, -184, -190,
  25         -195, -201, -207, -213, -218, -224, -230, -235, -241, -246, -252, -257, -263, -268, -273, -279,
  26         -284, -289, -294, -299, -304, -310, -314, -319, -324, -329, -334, -339, -343, -348, -353, -357,
  27         -362, -366, -370, -375, -379, -383, -387, -391, -395, -399, -403, -407, -411, -414, -418, -422,
  28         -425, -429, -432, -435, -439, -442, -445, -448, -451, -454, -457, -460, -462, -465, -468, -470,
  29         -473, -475, -477, -479, -482, -484, -486, -488, -489, -491, -493, -495, -496, -498, -499, -500,
  30         -502, -503, -504, -505, -506, -507, -508, -508, -509, -510, -510, -511, -511, -511, -511, -511,
  31         -512, -511, -511, -511, -511, -511, -510, -510, -509, -508, -508, -507, -506, -505, -504, -503,
  32         -502, -500, -499, -498, -496, -495, -493, -491, -489, -488, -486, -484, -482, -479, -477, -475,
  33         -473, -470, -468, -465, -462, -460, -457, -454, -451, -448, -445, -442, -439, -435, -432, -429,
  34         -425, -422, -418, -414, -411, -407, -403, -399, -395, -391, -387, -383, -379, -375, -370, -366,
  35         -362, -357, -353, -348, -343, -339, -334, -329, -324, -319, -314, -310, -304, -299, -294, -289,
  36         -284, -279, -273, -268, -263, -257, -252, -246, -241, -235, -230, -224, -218, -213, -207, -201,
  37         -195, -190, -184, -178, -172, -166, -160, -154, -148, -142, -136, -130, -124, -118, -112, -106,
  38         -99, -93, -87, -81, -75, -68, -62, -56, -50, -43, -37, -31, -25, -18, -12, -6
  39 };
  40
  41 void sincos_init(void)
  42 {
  43         int i;
  44
  45         printf("int16_t sinlut[SINLUT_SIZE] = {");
  46
  47         for(i=0; i<SINLUT_SIZE; i++) {
  48                 float angle = 2.0 * M_PI * ((float)i / (float)SINLUT_SIZE);
  49                 float val = sin(angle);
  50                 sinlut[i] = (int16_t)(val * SINLUT_SCALE);
  51
  52                 if((i & 0xf) == 0) {
  53                         printf("%s\t%d", i ? ",\n" : "\n", sinlut[i]);
  54                 } else {
  55                         printf(", %d", sinlut[i]);
  56                 }
  57         }
  58         printf("\n};");
  59 }
  60
  61 int16_t sin_int(int16_t norm_angle)
  62 {
  63         norm_angle %= SINLUT_SIZE;
  64         if(norm_angle < 0) {
  65                 norm_angle += SINLUT_SIZE;
  66         }
  67         return sinlut[norm_angle];
  68 }
  69
  70 int16_t cos_int(int16_t norm_angle)
  71 {
  72         return sin_int(norm_angle + SINLUT_SIZE / 4);
  73 }
  74
  75 int32_t sin_x16(int32_t radians)
  76 {
  77         int32_t na = x16div(radians, M_PI_X16 * 2);
  78         return (sin_int((na * SINLUT_SIZE) >> 16) << 16) / SINLUT_SCALE;
  79 }
  80
  81 int32_t cos_x16(int32_t radians)
  82 {
  83         int32_t na = x16div(radians, M_PI_X16 * 2);
  84         return (cos_int((na * SINLUT_SIZE) >> 16) << 16) / SINLUT_SCALE;
  85 }