199 lines
5.1 KiB
C
199 lines
5.1 KiB
C
#include "fix16.h"
|
|
|
|
#if defined(FIXMATH_SIN_LUT)
|
|
#include "fix16_trig_sin_lut.h"
|
|
#elif !defined(FIXMATH_NO_CACHE)
|
|
static fix16_t _fix16_sin_cache_index[4096] = { 0 };
|
|
static fix16_t _fix16_sin_cache_value[4096] = { 0 };
|
|
#endif
|
|
|
|
#ifndef FIXMATH_NO_CACHE
|
|
static fix16_t _fix16_atan_cache_index[2][4096] = { { 0 }, { 0 } };
|
|
static fix16_t _fix16_atan_cache_value[4096] = { 0 };
|
|
#endif
|
|
|
|
|
|
|
|
fix16_t fix16_sin(fix16_t inAngle) {
|
|
fix16_t tempAngle = inAngle % (fix16_pi << 1);
|
|
|
|
#ifdef FIXMATH_SIN_LUT
|
|
if(tempAngle < 0)
|
|
tempAngle += (fix16_pi << 1);
|
|
|
|
fix16_t tempOut;
|
|
if(tempAngle >= fix16_pi) {
|
|
tempAngle -= fix16_pi;
|
|
if(tempAngle >= (fix16_pi >> 1))
|
|
tempAngle = fix16_pi - tempAngle;
|
|
tempOut = -_fix16_sin_lut[tempAngle];
|
|
} else {
|
|
if(tempAngle >= (fix16_pi >> 1))
|
|
tempAngle = fix16_pi - tempAngle;
|
|
tempOut = _fix16_sin_lut[tempAngle];
|
|
}
|
|
#else
|
|
if(tempAngle > fix16_pi)
|
|
tempAngle -= (fix16_pi << 1);
|
|
else if(tempAngle < -fix16_pi)
|
|
tempAngle += (fix16_pi << 1);
|
|
|
|
#ifndef FIXMATH_NO_CACHE
|
|
fix16_t tempIndex = ((inAngle >> 5) & 0x00000FFF);
|
|
if(_fix16_sin_cache_index[tempIndex] == inAngle)
|
|
return _fix16_sin_cache_value[tempIndex];
|
|
#endif
|
|
|
|
fix16_t tempAngleSq = fix16_mul(tempAngle, tempAngle);
|
|
|
|
#ifndef FIXMATH_FAST_SIN // Most accurate version, accurate to ~2.1%
|
|
fix16_t tempOut = tempAngle;
|
|
tempAngle = fix16_mul(tempAngle, tempAngleSq);
|
|
tempOut -= (tempAngle / 6);
|
|
tempAngle = fix16_mul(tempAngle, tempAngleSq);
|
|
tempOut += (tempAngle / 120);
|
|
tempAngle = fix16_mul(tempAngle, tempAngleSq);
|
|
tempOut -= (tempAngle / 5040);
|
|
tempAngle = fix16_mul(tempAngle, tempAngleSq);
|
|
tempOut += (tempAngle / 362880);
|
|
tempAngle = fix16_mul(tempAngle, tempAngleSq);
|
|
tempOut -= (tempAngle / 39916800);
|
|
#else // Fast implementation, runs at 159% the speed of above 'accurate' version with an slightly lower accuracy of ~2.3%
|
|
fix16_t tempOut;
|
|
tempOut = fix16_mul(-13, tempAngleSq) + 546;
|
|
tempOut = fix16_mul(tempOut, tempAngleSq) - 10923;
|
|
tempOut = fix16_mul(tempOut, tempAngleSq) + 65536;
|
|
tempOut = fix16_mul(tempOut, tempAngle);
|
|
#endif
|
|
|
|
/*// Broken implementation, meant to be slightly faster and much more accurate than the above 'accurate' implementation.
|
|
int64_t tempAngleSq = tempAngle * tempAngle;
|
|
#ifndef FIXMATH_NO_ROUNDING
|
|
tempAngleSq += (fix16_one >> 1);
|
|
#endif
|
|
tempAngleSq >>= 16;
|
|
|
|
int64_t tempOut;
|
|
tempOut = (-108 * tempAngleSq) + 11836;
|
|
#ifndef FIXMATH_NO_ROUNDING
|
|
tempOut += (fix16_one >> 1);
|
|
#endif
|
|
tempOut >>= 16;
|
|
|
|
tempOut = (tempOut * tempAngleSq) - 852176;
|
|
#ifndef FIXMATH_NO_ROUNDING
|
|
tempOut += (fix16_one >> 1);
|
|
#endif
|
|
tempOut >>= 16;
|
|
|
|
tempOut = (tempOut * tempAngleSq) + 35791394;
|
|
#ifndef FIXMATH_NO_ROUNDING
|
|
tempOut += (fix16_one >> 1);
|
|
#endif
|
|
tempOut >>= 16;
|
|
|
|
tempOut = (tempOut * tempAngleSq) - 715827883;
|
|
#ifndef FIXMATH_NO_ROUNDING
|
|
tempOut += (fix16_one >> 1);
|
|
#endif
|
|
tempOut >>= 16;
|
|
|
|
tempOut = fix16_mul(tempOut, tempAngle) + tempAngle;
|
|
*/
|
|
|
|
#ifndef FIXMATH_NO_CACHE
|
|
_fix16_sin_cache_index[tempIndex] = inAngle;
|
|
_fix16_sin_cache_value[tempIndex] = tempOut;
|
|
#endif
|
|
#endif
|
|
|
|
return tempOut;
|
|
}
|
|
|
|
fix16_t fix16_cos(fix16_t inAngle) {
|
|
return fix16_sin(inAngle + (fix16_pi >> 1));
|
|
}
|
|
|
|
fix16_t fix16_tan(fix16_t inAngle) {
|
|
return fix16_div(fix16_sin(inAngle), fix16_cos(inAngle));
|
|
}
|
|
|
|
fix16_t fix16_asin(fix16_t inValue) {
|
|
if((inValue > fix16_one) || (inValue < -fix16_one))
|
|
return 0;
|
|
fix16_t tempOut;
|
|
tempOut = (fix16_one - fix16_mul(inValue, inValue));
|
|
tempOut = fix16_div(inValue, fix16_sqrt(tempOut));
|
|
tempOut = fix16_atan(tempOut);
|
|
return tempOut;
|
|
}
|
|
|
|
fix16_t fix16_acos(fix16_t inValue) {
|
|
return ((fix16_pi >> 1) - fix16_asin(inValue));
|
|
}
|
|
|
|
fix16_t fix16_atan2(fix16_t inY , fix16_t inX) {
|
|
#ifndef FIXMATH_NO_CACHE
|
|
uintptr_t hash = (inX ^ inY);
|
|
hash ^= hash >> 20;
|
|
hash &= 0x0FFF;
|
|
if((_fix16_atan_cache_index[0][hash] == inX) && (_fix16_atan_cache_index[1][hash] == inY))
|
|
return _fix16_atan_cache_value[hash];
|
|
#endif
|
|
|
|
fix16_t absy = (inY < 0 ? -inY : inY);
|
|
int64_t i = inX + (inX >= 0 ? -absy : absy);
|
|
int64_t j = (inX >= 0 ? inX : -inX) + absy;
|
|
if(j == 0)
|
|
return (inY < 0 ? (-fix16_pi >> 1) : (fix16_pi >> 1));
|
|
|
|
int64_t is, js;
|
|
int64_t angle;
|
|
angle = (4216574283LL * -i) / j;
|
|
is = (i * i);
|
|
js = (j * j);
|
|
#ifndef FIXMATH_NO_ROUNDING
|
|
is += (fix16_one >> 1);
|
|
js += (fix16_one >> 1);
|
|
#endif
|
|
is >>= 16;
|
|
js >>= 16;
|
|
if((is | js) >> 32) {
|
|
if((is | js) >> 40) {
|
|
is >>= 16;
|
|
js >>= 16;
|
|
} else {
|
|
is >>= 8;
|
|
js >>= 8;
|
|
}
|
|
}
|
|
is = (is * i);
|
|
js = (js * j);
|
|
#ifndef FIXMATH_NO_ROUNDING
|
|
is += (fix16_one >> 1);
|
|
js += (fix16_one >> 1);
|
|
#endif
|
|
is >>= 16;
|
|
js >>= 16;
|
|
is = is * 51472LL;
|
|
angle += (is / js) << 14;
|
|
angle += (inX >= 0 ? 3373259426LL : 10119778278LL);
|
|
#ifndef FIXMATH_NO_ROUNDING
|
|
angle += (fix16_one >> 1);
|
|
#endif
|
|
angle >>= 16;
|
|
angle = (inY < 0 ? -angle : angle);
|
|
|
|
#ifndef FIXMATH_NO_CACHE
|
|
_fix16_atan_cache_index[0][hash] = inX;
|
|
_fix16_atan_cache_index[1][hash] = inY;
|
|
_fix16_atan_cache_value[hash] = angle;
|
|
#endif
|
|
|
|
return angle;
|
|
}
|
|
|
|
fix16_t fix16_atan(fix16_t inValue) {
|
|
return fix16_atan2(inValue, fix16_one);
|
|
}
|