mirror of
https://github.com/nzp-team/dquakeplus.git
synced 2024-11-15 00:31:43 +00:00
57 lines
1.1 KiB
C
57 lines
1.1 KiB
C
#include "pspmath.h"
|
|
|
|
float vfpu_atanf(float x) {
|
|
float result;
|
|
// result = asinf(x/sqrt(x*x+1))
|
|
__asm__ volatile (
|
|
"mtv %1, S000\n"
|
|
"vmul.s S001, S000, S000\n"
|
|
"vadd.s S001, S001, S001[1]\n"
|
|
"vrsq.s S001, S001\n"
|
|
"vmul.s S000, S000, S001\n"
|
|
"vasin.s S000, S000\n"
|
|
"vcst.s S001, VFPU_PI_2\n"
|
|
"vmul.s S000, S000, S001\n"
|
|
"mfv %0, S000\n"
|
|
: "=r"(result) : "r"(x));
|
|
return result;
|
|
}
|
|
|
|
|
|
#define PI 3.14159265358979f
|
|
#define PI_2 1.57079632679489f
|
|
|
|
inline static float fabsf(float x) {
|
|
float r;
|
|
__asm__ volatile( "abs.s %0, %1" : "=f"(r) :"f"(x):"memory");
|
|
return r;
|
|
}
|
|
|
|
/* double a;
|
|
|
|
if (fabs(x) >= fabs(y)) {
|
|
a = atan(y/x) ;
|
|
if (x < 0.0) {
|
|
if (y >= 0.0) a += _pi ;
|
|
else a -= _pi ;
|
|
}
|
|
}
|
|
else {
|
|
a = -atan(x/y) ;
|
|
if (y < 0.0) a -= _halfpi ;
|
|
else a += _halfpi ;
|
|
}
|
|
return a ;*/
|
|
|
|
float vfpu_atan2f(float y, float x) {
|
|
float r;
|
|
|
|
if (fabsf(x) >= fabsf(y)) {
|
|
r = vfpu_atanf(y/x);
|
|
if (x < 0.0f) r += (y>=0.0f ? PI : -PI);
|
|
} else {
|
|
r = -vfpu_atanf(x/y);
|
|
r += (y < 0.0f ? -PI_2 : PI_2);
|
|
}
|
|
return r;
|
|
}
|