mirror of
https://github.com/nzp-team/dquakeplus.git
synced 2024-11-25 13:21:18 +00:00
35 lines
1.3 KiB
C
35 lines
1.3 KiB
C
|
#include "pspmath.h"
|
||
|
|
|
||
|
|
#define PI 3.14159265358979323846f
|
||
|
|
const float piover180 = PI/180.0f;
|
||
|
|
|
||
|
|
void vfpu_quaternion_from_euler(ScePspQuatMatrix *res, float x, float y, float z) {
|
||
|
|
__asm__ volatile (
|
||
|
|
"mtv %1, S000\n\t" // S000 = x
|
||
|
|
"mtv %2, S001\n\t" // S001 = y
|
||
|
|
"mtv %3, S002\n\t" // S002 = z
|
||
|
|
"lv.s S010, 0(%4)\n"
|
||
|
|
"vscl.t C000, C000, S010\n\t" // x *= pi/180, y *= pi/180, z *= pi/180
|
||
|
|
|
||
|
|
"vfim.s S010, 0.5\n\t" // S010 = 0.5
|
||
|
|
"vscl.t C000, C000, S010\n\t" // x *= 0.5, y *= 0.5, z *= 0.5
|
||
|
|
"vcst.s S010, VFPU_2_PI\n\t" // load 2/PI into S010, S011 and S012
|
||
|
|
"vscl.t C000, C000, S010\n\t" // x *= 2/PI, y *= 2/pi, z *= pi/2
|
||
|
|
|
||
|
|
"vrot.p C010, S000, [s, c]\n\t" // S010 = sr, S011 = cr
|
||
|
|
"vrot.p C020, S001, [s, c]\n\t" // S020 = sp, S021 = cp
|
||
|
|
"vrot.p C030, S002, [s, c]\n\t" // S030 = sy, S031 = cy
|
||
|
|
|
||
|
|
// fear the madness of prefixes
|
||
|
|
"vmul.q R100, C010[x,y,y,x], C020[y,x,x,y]\n"
|
||
|
|
"vmul.q R100, R100, C030[y,x,y,x]\n"
|
||
|
|
"vmul.q R101, C010[y,x,y,x], C020[y,x,y,x]\n"
|
||
|
|
"vmul.q R101, R101, C030[x,y,y,x]\n"
|
||
|
|
"vadd.q C000, R100[x,z,0,0], R100[-y,w,0,0]\n"
|
||
|
|
"vadd.q C000, C000, R101[0,0,x,z]\n"
|
||
|
|
"vadd.q C000, C000, R101[0,0,-y,w]\n"
|
||
|
|
"usv.q C000, %0\n\t"
|
||
|
|
:"=m"(*res) : "r"(x), "r"(y), "r"(z), "r"(&piover180));
|
||
|
|
vfpu_quaternion_normalize(res);
|
||
|
|
}
|