mirror of
https://github.com/nzp-team/dquakeplus.git
synced 2024-12-12 13:12:16 +00:00
34 lines
1.9 KiB
C
34 lines
1.9 KiB
C
#include "pspmath.h"
|
|
|
|
void vfpu_quaternion_to_matrix(ScePspQuatMatrix *q, ScePspFMatrix4 *m) {
|
|
__asm__ volatile (
|
|
"lv.q C000, %1\n" // C000 = [x, y, z, w ]
|
|
"vmul.q C010, C000, C000\n" // C010 = [x2, y2, z2, w2]
|
|
"vcrs.t C020, C000, C000\n" // C020 = [yz, xz, xy ]
|
|
"vmul.q C030, C000[x,y,z,1], C000[w,w,w,2]\n" // C030 = [wx, wy, wz ]
|
|
|
|
"vadd.q C100, C020[0,z,y,0], C030[0,z,-y,0]\n" // C100 = [0, xy+wz, xz-wy]
|
|
"vadd.s S100, S011, S012\n" // C100 = [y2+z2, xy+wz, xz-wy]
|
|
|
|
"vadd.q C110, C020[z,0,x,0], C030[-z,0,x,0]\n" // C110 = [xy-wz, 0, yz+wx]
|
|
"vadd.s S111, S010, S012\n" // C110 = [xy-wz, x2+z2, yz+wx]
|
|
|
|
"vadd.q C120, C020[y,x,0,0], C030[y,-x,0,0]\n" // C120 = [xz+wy, yz-wx, 0 ]
|
|
"vadd.s S122, S010, S011\n" // C120 = [xz+wy, yz-wx, x2+y2]
|
|
|
|
"vmscl.t M100, M100, S033\n" // C100 = [2*(y2+z2), 2*(xy+wz), 2*(xz-wy)]
|
|
// C110 = [2*(xy-wz), 2*(x2+z2), 2*(yz+wx)]
|
|
// C120 = [2*(xz+wy), 2*(yz-wx), 2*(x2+y2)]
|
|
|
|
"vocp.s S100, S100\n" // C100 = [1-2*(y2+z2), 2*(xy+wz), 2*(xz-wy) ]
|
|
"vocp.s S111, S111\n" // C110 = [2*(xy-wz), 1-2*(x2+z2), 2*(yz+wx) ]
|
|
"vocp.s S122, S122\n" // C120 = [2*(xz+wy), 2*(yz-wx), 1-2*(x2+y2)]
|
|
|
|
"vidt.q C130\n" // C130 = [0, 0, 0, 1]
|
|
|
|
"sv.q R100, 0 + %0\n"
|
|
"sv.q R101, 16 + %0\n"
|
|
"sv.q R102, 32 + %0\n"
|
|
"sv.q R103, 48 + %0\n"
|
|
: "=m"(*m) : "m"(*q));
|
|
}
|