126 lines
2.5 KiB
C++
126 lines
2.5 KiB
C++
#include "rmath.h"
|
|
|
|
const double DOUBLE_PI = 6.283185307179586;
|
|
const double SIN_CURVE_A = 0.0415896;
|
|
const double SIN_CURVE_B = 0.00129810625032;
|
|
|
|
float _float_cos(float x) {
|
|
if (x < 0) {
|
|
int q = -x / DOUBLE_PI;
|
|
q += 1;
|
|
float y = q * DOUBLE_PI;
|
|
x = -(x - y);
|
|
}
|
|
if (x >= DOUBLE_PI) {
|
|
int q = x / DOUBLE_PI;
|
|
float y = q * DOUBLE_PI;
|
|
x = x - y;
|
|
}
|
|
int s = 1;
|
|
if (x >= PI) {
|
|
s = -1;
|
|
x -= PI;
|
|
}
|
|
if (x > HALF_PI) {
|
|
x = PI - x;
|
|
s = -s;
|
|
}
|
|
float z = x * x;
|
|
float r = z * (z * (SIN_CURVE_A - SIN_CURVE_B * z) - 0.5) + 1.0;
|
|
if (r > 1.0) r = r - 2.0;
|
|
if (s > 0) return r;
|
|
else return -r;
|
|
}
|
|
|
|
float _float_sin(float x) {
|
|
return _float_cos(x - HALF_PI);
|
|
}
|
|
|
|
fp *sinTable = nullptr;
|
|
fp _fp_sin(fp x){
|
|
return fp(_float_sin((float)x));
|
|
}
|
|
|
|
void createSinTable(){
|
|
for(int i = 0; i < SIN_SAMPLES; i++){
|
|
sinTable[i] = _float_sin(i * (PI / float(SIN_SAMPLES)));
|
|
}
|
|
}
|
|
|
|
fp fp_sin(fp x){
|
|
bool up = true;
|
|
while(x >= fp(PI)){
|
|
x = x - fp(PI);
|
|
up = !up;
|
|
}
|
|
if(x < 0)
|
|
return fp(0)-fp_sin(fp(0)-x);
|
|
int i = x * fp(SIN_SAMPLES / PI);
|
|
if(i >= SIN_SAMPLES)
|
|
i = SIN_SAMPLES-1;
|
|
fp r = sinTable[i];
|
|
if(!up)
|
|
r = fp(0) - r;
|
|
return r;
|
|
}
|
|
|
|
fp fp_cos(fp x){
|
|
return fp_sin(x+fp(HALF_PI));
|
|
}
|
|
|
|
fp fp_tan(fp x){
|
|
return fp_sin(x) / fp_cos(x);
|
|
}
|
|
|
|
fp _fp_atan(fp x){
|
|
return fp(PI)/fp(4) * x - x * (fp_abs(x) - fp(1)) * (fp(0.245) + fp(0.066) * fp_abs(x));
|
|
}
|
|
fp fp_atan(fp x){
|
|
if(x < 1)
|
|
return _fp_atan(x);
|
|
else
|
|
return fp(HALF_PI) - _fp_atan(fp(1)/x);
|
|
}
|
|
fp fp_atan2(fp y, fp x){
|
|
if(x == 0 || y == 0)
|
|
return 0;
|
|
if(x >= 0) {
|
|
if(y >= 0) {
|
|
if(y < x) {
|
|
return fp_atan(y / x);
|
|
} else {
|
|
return fp(PI)*fp(2) - fp_atan(x / y);
|
|
}
|
|
} else {
|
|
if(fp(0)-y < x){
|
|
return fp_atan(y / x);
|
|
} else {
|
|
return fp(0)-fp(PI)*fp(2) - fp_atan(x / y);
|
|
}
|
|
}
|
|
} else {
|
|
if(y >= 0){
|
|
if(y < fp(0)-x){
|
|
return fp_atan(y / x) + fp(PI);
|
|
} else {
|
|
return fp(PI)*fp(2) - fp_atan(x / y);
|
|
}
|
|
} else {
|
|
if(fp(0)-y < fp(0)-x){
|
|
return fp_atan(y / x) - fp(PI);
|
|
} else {
|
|
return fp(0)-fp(PI)*fp(2) - fp_atan(x / y);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
float _isqrt(float x){
|
|
float xhalf = 0.5f * x;
|
|
int i = *(int*)&x; // store floating-point bits in integer
|
|
i = 0x5f3759df - (i >> 1); // initial guess for Newton's method
|
|
x = *(float*)&i; // convert new bits into float
|
|
x = x*(1.5f - xhalf*x*x); // One round of Newton's method
|
|
return x;
|
|
}
|