src/utils_math.c

   1 /*
   2  *  Copyright (C) 1998,1999  Helmut Dersch <der@fh-furtwangen.de>
   3  *  Copyright (C) 2007  Florian octo Forster <octo at verplant.org>
   4  *
   5  *  This program is free software; you can redistribute it and/or modify it
   6  *  under the terms of the GNU General Public License as published by the Free
   7  *  Software Foundation; either version 2 of the License, or (at your option)
   8  *  any later version.
   9  *
  10  *  This program is distributed in the hope that it will be useful, but
  11  *  WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  12  *  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  13  *  for more details.
  14  *
  15  *  You should have received a copy of the GNU General Public License along
  16  *  with this program; if not, write to the Free Software Foundation, Inc., 51
  17  *  Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  18  **/
  19
  20 #include <stdlib.h>
  21 #include <stdio.h>
  22 #include <math.h>
  23
  24 #include "utils_math.h"
  25
  26 // Lookup Tables for Trig-functions and interpolator
  27
  28 #define ATAN_TABLE_SIZE 4096
  29 #define SQRT_TABLE_SIZE 4096
  30
  31 static int *atan_table = NULL;
  32 static int *sqrt_table = NULL;
  33
  34 static int setup_sqrt_table (void)
  35 {
  36   double z;
  37   int i;
  38
  39   sqrt_table = (int *) malloc (SQRT_TABLE_SIZE * sizeof (int));
  40   if (sqrt_table == NULL)
  41     return (-1);
  42
  43   /* Calculate the table entries. */
  44   for (i = 0; i < SQRT_TABLE_SIZE; i++)
  45   {
  46     z = ((double) i) / ((double) (SQRT_TABLE_SIZE - 1));
  47     sqrt_table[i] = (int) (sqrt( 1.0 + z*z ) * 4096.0 + .5);
  48   }
  49
  50   return (0);
  51 } /* int setup_sqrt_table */
  52
  53 /*
  54  * Calculate
  55  *   256 * sqrt(x1^2 + x2^2)
  56  * efficiently
  57  */
  58 int utils_sqrt2 (int x1, int x2)
  59 {
  60   int ret;
  61
  62   if (sqrt_table == NULL)
  63     if (setup_sqrt_table () != 0)
  64       return (-1);
  65
  66   if ((x1 == 0) && (x2 == 0))
  67   {
  68     ret = 0;
  69   }
  70   else if (x1 > x2)
  71   {
  72     ret = x1 * sqrt_table[(SQRT_TABLE_SIZE - 1) * x2 / x1] / 16;
  73   }
  74   else /* (x2 < x1) */
  75   {
  76     ret = x2 * sqrt_table[(SQRT_TABLE_SIZE - 1) * x1 / x2] / 16;
  77   }
  78
  79   return (ret);
  80 } /* int utils_sqrt2 */
  81
  82 static int setup_atan_table (void)
  83 {
  84   int i;
  85   double z;
  86
  87   atan_table = (int *) malloc (ATAN_TABLE_SIZE * sizeof (int));
  88   if (atan_table == NULL)
  89     return (-1);
  90
  91   for (i = 0; i < (ATAN_TABLE_SIZE - 1); i++)
  92   {
  93     z = ((double) i) / ((double) (ATAN_TABLE_SIZE - 1));
  94     atan_table[i] = atan ( z / (1.0 - z ) ) * ATAN_TABLE_SIZE * 256;
  95   }
  96   atan_table[ATAN_TABLE_SIZE - 1] = M_PI_4 * ATAN_TABLE_SIZE * 256;
  97
  98   return 0;
  99 } /* int setup_atan_table */
 100
 101 int utils_atan2 (int y, int x)
 102 {
 103   if (atan_table == NULL)
 104     if (setup_atan_table () != 0)
 105       return (-1);
 106
 107   // return atan2(y,x) * 256*ATAN_TABLE_SIZE;
 108   if( x > 0 )
 109   {
 110     if( y > 0 )
 111     {
 112       return  atan_table[(int)( ATAN_TABLE_SIZE * y / ( x + y ))];
 113     }
 114     else
 115     {
 116       return -atan_table[ (int)(ATAN_TABLE_SIZE * (-y) / ( x - y ))];
 117     }
 118   }
 119
 120   if( x == 0 )
 121   {
 122     if( y > 0 )
 123       return  (int)(256*ATAN_TABLE_SIZE * M_PI_2);
 124     else
 125       return  -(int)(256*ATAN_TABLE_SIZE * M_PI_2);
 126   }
 127
 128   if( y < 0 )
 129   {
 130     return  atan_table[(int)( ATAN_TABLE_SIZE * y / ( x + y ))] - (int)(M_PI*256*ATAN_TABLE_SIZE);
 131   }
 132   else
 133   {
 134     return -atan_table[ (int)(ATAN_TABLE_SIZE * (-y) / ( x - y ))] + (int)(M_PI*256*ATAN_TABLE_SIZE);
 135   }
 136
 137 }
 138
 139 /*
 140  * vim: set tabstop=8 softtabstop=2 shiftwidth=2 :
 141  */