fish/362_rubik/trig.c

   1 /*      trig.c  fast integer trig functions etc. for rubik.c
   2         martin round    xmas eve 1989
   3 */
   4
   5 #include <math.h>
   6
   7 short sintab [360];
   8
   9 void init_trig()
  10         {
  11         double degtorad;
  12         int i;
  13
  14         degtorad = 3.14159/180.0;
  15         for (i=1; i<90; i++)
  16                 {
  17                 sintab[i] = sintab[180-i] = (short) (0.5 + 256.0 * sin(degtorad * i));
  18                 sintab[i+180] = sintab[360-i] = -sintab[i];
  19                 }
  20         sintab[0] = 0;
  21         sintab[90] = 256;
  22         sintab[180] = 0;
  23         sintab[270] = -256;
  24
  25         }
  26
  27 short mysin(int ang)    /*      works with degrees! return is 256*sin(ang)      */
  28         {
  29
  30         if (ang < 0)
  31                 return (sintab [ang + 360]);
  32
  33         if (ang >359)
  34                 return (sintab [ang - 360]);
  35
  36         return (sintab[ang]);
  37         }
  38
  39 short mycos(int ang)
  40         {
  41         if (ang < -90)
  42                 return (sintab [ang + 450]);
  43
  44         if (ang >269)
  45                 return (sintab [ang - 270]);
  46
  47         return (sintab [ang + 90]);
  48         }
  49
  50 unsigned short hypoten ( short x, short y)
  51         {
  52         register unsigned long square = x * x + y * y;
  53         register unsigned short root = x<0 ? -x : x;
  54         register unsigned short correction;
  55
  56         root += y<0 ? -y+1 : y+1;
  57
  58         do
  59                 {
  60                 root -= (correction = (root * root - square) / (root * 2));
  61                 } while (correction);
  62
  63         return (--root);
  64         }
  65
  66 short angle(short x, short y, unsigned short r) /*      find vector */
  67         {
  68         register short ang,s,e,sine;
  69         short flag;
  70
  71         if ((r * x * y) == 0)
  72                 {
  73                 if (r == 0)
  74                         return (0);
  75                 if (x == 0)
  76                         {
  77                         if (y >=0)
  78                                 return (90);
  79                         else
  80                                 return (-90);
  81                         }
  82                 if (x >=0)
  83                         return (0);
  84                 else
  85                         return (180);
  86                 }
  87
  88         ang = y<0 ? -y : y;
  89
  90         s = x<0 ? -x : x;
  91
  92         if ((flag = (s < ang)))
  93                 ang = s;
  94
  95         sine  = (ang << 8) / r;
  96
  97         ang = 21;
  98         s = 92;         /*      sintab [21]             */
  99         e = sine - s;
 100
 101         while ((e < -2) || (e > 2))
 102                 {
 103                 if (e > 4)
 104                         ang += ((46-ang)*e)/(184-s);
 105                 else if (e < -4)
 106                         ang -= ((1-ang)*e)/(s-4);
 107                 else if (e < 0)
 108                         {
 109                         --ang;
 110                         break;
 111                         }
 112                 else
 113                         {
 114                         ++ang;
 115                         break;
 116                         }
 117
 118                 e = sine-(s = sintab[ang]);
 119                 }
 120
 121         if (flag)
 122                 ang = 90 - ang;
 123
 124         if (x < 0)
 125                 ang = 180 - ang;
 126
 127         if (y < 0)
 128                 ang = -ang;
 129
 130         return (ang);
 131         }
 132
 133 void rotate (short *a, short *b, short ang)
 134         {
 135         static short olda,oldb,oldang,olda1,oldb1;
 136         unsigned short r;
 137
 138         if ((*a == olda) && (*b == oldb) && (ang == oldang))
 139                 {
 140                 *a = olda1;
 141                 *b = oldb1;
 142                 }
 143         else
 144                 {
 145                 olda = *a;
 146                 oldb = *b;
 147                 oldang = ang;
 148
 149                 ang += angle (*a, *b, (r = hypoten (*a, *b)));
 150
 151                 *a = olda1 = (r * mycos(ang)) >> 8;
 152                 *b = oldb1 = (r * mysin(ang)) >> 8;
 153                 }
 154         }