191 lines
8.5 KiB
C
191 lines
8.5 KiB
C
/*
|
|
* This file is subject to the terms of the GFX License. If a copy of
|
|
* the license was not distributed with this file, you can obtain one at:
|
|
*
|
|
* http://ugfx.org/license.html
|
|
*/
|
|
|
|
/**
|
|
* @file src/gmisc/gmisc_trig.c
|
|
* @brief GMISC Trig Functions.
|
|
*
|
|
* @addtogroup GMISC
|
|
* @{
|
|
*/
|
|
#include "gfx.h"
|
|
|
|
#if GFX_USE_GMISC
|
|
|
|
#if GMISC_NEED_FASTTRIG
|
|
const double sintabledouble[] = {
|
|
0.000000, 0.017452, 0.034899, 0.052336, 0.069756, 0.087156, 0.104528, 0.121869,
|
|
0.139173, 0.156434, 0.173648, 0.190809, 0.207912, 0.224951, 0.241922, 0.258819,
|
|
0.275637, 0.292372, 0.309017, 0.325568, 0.342020, 0.358368, 0.374607, 0.390731,
|
|
0.406737, 0.422618, 0.438371, 0.453990, 0.469472, 0.484810, 0.500000, 0.515038,
|
|
0.529919, 0.544639, 0.559193, 0.573576, 0.587785, 0.601815, 0.615661, 0.629320,
|
|
0.642788, 0.656059, 0.669131, 0.681998, 0.694658, 0.707107, 0.719340, 0.731354,
|
|
0.743145, 0.754710, 0.766044, 0.777146, 0.788011, 0.798636, 0.809017, 0.819152,
|
|
0.829038, 0.838671, 0.848048, 0.857167, 0.866025, 0.874620, 0.882948, 0.891007,
|
|
0.898794, 0.906308, 0.913545, 0.920505, 0.927184, 0.933580, 0.939693, 0.945519,
|
|
0.951057, 0.956305, 0.961262, 0.965926, 0.970296, 0.974370, 0.978148, 0.981627,
|
|
0.984808, 0.987688, 0.990268, 0.992546, 0.994522, 0.996195, 0.997564, 0.998630,
|
|
0.999391, 0.999848, 1.000000, 0.999848, 0.999391, 0.998630, 0.997564, 0.996195,
|
|
0.994522, 0.992546, 0.990268, 0.987688, 0.984808, 0.981627, 0.978148, 0.974370,
|
|
0.970296, 0.965926, 0.961262, 0.956305, 0.951057, 0.945519, 0.939693, 0.933580,
|
|
0.927184, 0.920505, 0.913545, 0.906308, 0.898794, 0.891007, 0.882948, 0.874620,
|
|
0.866025, 0.857167, 0.848048, 0.838671, 0.829038, 0.819152, 0.809017, 0.798636,
|
|
0.788011, 0.777146, 0.766044, 0.754710, 0.743145, 0.731354, 0.719340, 0.707107,
|
|
0.694658, 0.681998, 0.669131, 0.656059, 0.642788, 0.629320, 0.615661, 0.601815,
|
|
0.587785, 0.573576, 0.559193, 0.544639, 0.529919, 0.515038, 0.500000, 0.484810,
|
|
0.469472, 0.453990, 0.438371, 0.422618, 0.406737, 0.390731, 0.374607, 0.358368,
|
|
0.342020, 0.325568, 0.309017, 0.292372, 0.275637, 0.258819, 0.241922, 0.224951,
|
|
0.207912, 0.190809, 0.173648, 0.156434, 0.139173, 0.121869, 0.104528, 0.087156,
|
|
0.069756, 0.052336, 0.034899, 0.017452, 0.000000, -0.017452, -0.034899, -0.052336,
|
|
-0.069756, -0.087156, -0.104528, -0.121869, -0.139173, -0.156434, -0.173648, -0.190809,
|
|
-0.207912, -0.224951, -0.241922, -0.258819, -0.275637, -0.292372, -0.309017, -0.325568,
|
|
-0.342020, -0.358368, -0.374607, -0.390731, -0.406737, -0.422618, -0.438371, -0.453990,
|
|
-0.469472, -0.484810, -0.500000, -0.515038, -0.529919, -0.544639, -0.559193, -0.573576,
|
|
-0.587785, -0.601815, -0.615661, -0.629320, -0.642788, -0.656059, -0.669131, -0.681998,
|
|
-0.694658, -0.707107, -0.719340, -0.731354, -0.743145, -0.754710, -0.766044, -0.777146,
|
|
-0.788011, -0.798636, -0.809017, -0.819152, -0.829038, -0.838671, -0.848048, -0.857167,
|
|
-0.866025, -0.874620, -0.882948, -0.891007, -0.898794, -0.906308, -0.913545, -0.920505,
|
|
-0.927184, -0.933580, -0.939693, -0.945519, -0.951057, -0.956305, -0.961262, -0.965926,
|
|
-0.970296, -0.974370, -0.978148, -0.981627, -0.984808, -0.987688, -0.990268, -0.992546,
|
|
-0.994522, -0.996195, -0.997564, -0.998630, -0.999391, -0.999848, -1.000000, -0.999848,
|
|
-0.999391, -0.998630, -0.997564, -0.996195, -0.994522, -0.992546, -0.990268, -0.987688,
|
|
-0.984808, -0.981627, -0.978148, -0.974370, -0.970296, -0.965926, -0.961262, -0.956305,
|
|
-0.951057, -0.945519, -0.939693, -0.933580, -0.927184, -0.920505, -0.913545, -0.906308,
|
|
-0.898794, -0.891007, -0.882948, -0.874620, -0.866025, -0.857167, -0.848048, -0.838671,
|
|
-0.829038, -0.819152, -0.809017, -0.798636, -0.788011, -0.777146, -0.766044, -0.754710,
|
|
-0.743145, -0.731354, -0.719340, -0.707107, -0.694658, -0.681998, -0.669131, -0.656059,
|
|
-0.642788, -0.629320, -0.615661, -0.601815, -0.587785, -0.573576, -0.559193, -0.544639,
|
|
-0.529919, -0.515038, -0.500000, -0.484810, -0.469472, -0.453990, -0.438371, -0.422618,
|
|
-0.406737, -0.390731, -0.374607, -0.358368, -0.342020, -0.325568, -0.309017, -0.292372,
|
|
-0.275637, -0.258819, -0.241922, -0.224951, -0.207912, -0.190809, -0.173648, -0.156434,
|
|
-0.139173, -0.121869, -0.104528, -0.087156, -0.069756, -0.052336, -0.034899, -0.017452
|
|
};
|
|
|
|
double fsin(int degrees) {
|
|
if (degrees < 0)
|
|
degrees -= (degrees/360-1)*360;
|
|
else if (degrees >= 360)
|
|
degrees %= 360;
|
|
return sintabledouble[degrees];
|
|
}
|
|
|
|
double fcos(int degrees) {
|
|
return fsin(degrees+90);
|
|
}
|
|
|
|
#endif
|
|
|
|
#if GMISC_NEED_FIXEDTRIG
|
|
const fixed sintablefixed[] = {
|
|
0, 1143, 2287, 3429, 4571, 5711, 6850, 7986,
|
|
9120, 10252, 11380, 12504, 13625, 14742, 15854, 16961,
|
|
18064, 19160, 20251, 21336, 22414, 23486, 24550, 25606,
|
|
26655, 27696, 28729, 29752, 30767, 31772, 32767, 33753,
|
|
34728, 35693, 36647, 37589, 38521, 39440, 40347, 41243,
|
|
42125, 42995, 43852, 44695, 45525, 46340, 47142, 47929,
|
|
48702, 49460, 50203, 50931, 51643, 52339, 53019, 53683,
|
|
54331, 54963, 55577, 56175, 56755, 57319, 57864, 58393,
|
|
58903, 59395, 59870, 60326, 60763, 61183, 61583, 61965,
|
|
62328, 62672, 62997, 63302, 63589, 63856, 64103, 64331,
|
|
64540, 64729, 64898, 65047, 65176, 65286, 65376, 65446,
|
|
65496, 65526, 65536, 65526, 65496, 65446, 65376, 65286,
|
|
65176, 65047, 64898, 64729, 64540, 64331, 64103, 63856,
|
|
63589, 63302, 62997, 62672, 62328, 61965, 61583, 61183,
|
|
60763, 60326, 59870, 59395, 58903, 58393, 57864, 57319,
|
|
56755, 56175, 55577, 54963, 54331, 53683, 53019, 52339,
|
|
51643, 50931, 50203, 49460, 48702, 47929, 47142, 46340,
|
|
45525, 44695, 43852, 42995, 42125, 41243, 40347, 39440,
|
|
38521, 37589, 36647, 35693, 34728, 33753, 32767, 31772,
|
|
30767, 29752, 28729, 27696, 26655, 25606, 24550, 23486,
|
|
22414, 21336, 20251, 19160, 18064, 16961, 15854, 14742,
|
|
13625, 12504, 11380, 10252, 9120, 7986, 6850, 5711,
|
|
4571, 3429, 2287, 1143, 0, -1143, -2287, -3429,
|
|
-4571, -5711, -6850, -7986, -9120, -10252, -11380, -12504,
|
|
-13625, -14742, -15854, -16961, -18064, -19160, -20251, -21336,
|
|
-22414, -23486, -24550, -25606, -26655, -27696, -28729, -29752,
|
|
-30767, -31772, -32768, -33753, -34728, -35693, -36647, -37589,
|
|
-38521, -39440, -40347, -41243, -42125, -42995, -43852, -44695,
|
|
-45525, -46340, -47142, -47929, -48702, -49460, -50203, -50931,
|
|
-51643, -52339, -53019, -53683, -54331, -54963, -55577, -56175,
|
|
-56755, -57319, -57864, -58393, -58903, -59395, -59870, -60326,
|
|
-60763, -61183, -61583, -61965, -62328, -62672, -62997, -63302,
|
|
-63589, -63856, -64103, -64331, -64540, -64729, -64898, -65047,
|
|
-65176, -65286, -65376, -65446, -65496, -65526, -65536, -65526,
|
|
-65496, -65446, -65376, -65286, -65176, -65047, -64898, -64729,
|
|
-64540, -64331, -64103, -63856, -63589, -63302, -62997, -62672,
|
|
-62328, -61965, -61583, -61183, -60763, -60326, -59870, -59395,
|
|
-58903, -58393, -57864, -57319, -56755, -56175, -55577, -54963,
|
|
-54331, -53683, -53019, -52339, -51643, -50931, -50203, -49460,
|
|
-48702, -47929, -47142, -46340, -45525, -44695, -43852, -42995,
|
|
-42125, -41243, -40347, -39440, -38521, -37589, -36647, -35693,
|
|
-34728, -33753, -32768, -31772, -30767, -29752, -28729, -27696,
|
|
-26655, -25606, -24550, -23486, -22414, -21336, -20251, -19160,
|
|
-18064, -16961, -15854, -14742, -13625, -12504, -11380, -10252,
|
|
-9120, -7986, -6850, -5711, -4571, -3429, -2287, -1143
|
|
};
|
|
|
|
fixed ffsin(int degrees) {
|
|
if (degrees < 0)
|
|
degrees -= (degrees/360-1)*360;
|
|
else if (degrees >= 360)
|
|
degrees %= 360;
|
|
return sintablefixed[degrees];
|
|
}
|
|
|
|
fixed ffcos(int degrees) {
|
|
return ffsin(degrees+90);
|
|
}
|
|
|
|
#endif
|
|
|
|
#if GMISC_NEED_INVSQRT
|
|
// Algorithm based on Quake code.
|
|
#if GMISC_INVSQRT_REAL_SLOW
|
|
#include <math.h>
|
|
float invsqrt(float n) {
|
|
return 1.0/sqrt(n);
|
|
}
|
|
#else
|
|
float invsqrt(float n) {
|
|
int32_t i;
|
|
float x2;
|
|
|
|
x2 = n * 0.5F;
|
|
|
|
// Convert into an int32 (no binary format conversion)
|
|
#if GMISC_INVSQRT_MIXED_ENDIAN
|
|
((char *)&i)[0] = ((char *)&n)[3];
|
|
((char *)&i)[1] = ((char *)&n)[2];
|
|
((char *)&i)[2] = ((char *)&n)[1];
|
|
((char *)&i)[3] = ((char *)&n)[0];
|
|
#else
|
|
i = *(int32_t *)&n;
|
|
#endif
|
|
|
|
// evil floating point bit level hacking
|
|
i = 0x5F375A86 - (i >> 1); // The quake code used 0x5F3759DF but this is better.
|
|
|
|
// Convert back to a float (no binary format conversion)
|
|
#if GMISC_INVSQRT_MIXED_ENDIAN
|
|
((char *)&n)[0] = ((char *)&i)[3];
|
|
((char *)&n)[1] = ((char *)&i)[2];
|
|
((char *)&n)[2] = ((char *)&i)[1];
|
|
((char *)&n)[3] = ((char *)&i)[0];
|
|
#else
|
|
n = *(float *)&i;
|
|
#endif
|
|
|
|
n = n * (1.5F - (x2 * n * n)); // 1st iteration
|
|
//n = n * (1.5F - (x2 * n * n)); // 2nd iteration for extra precision, this can be removed
|
|
return n;
|
|
}
|
|
#endif
|
|
#endif
|
|
|
|
#endif /* GFX_USE_GMISC */
|
|
/** @} */
|