mirror of
https://github.com/nzp-team/dquakeplus.git
synced 2024-12-13 05:31:00 +00:00
26 lines
905 B
C
26 lines
905 B
C
#include "pspmath.h"
|
|
|
|
void vfpu_quaternion_ln(ScePspQuatMatrix *qout, ScePspQuatMatrix *qin) {
|
|
//float r = sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
|
|
//float t = r>0.00001f? atan2(r,a[3])/r: 0.f;
|
|
//return quat(t*a[0],t*a[1],t*a[2],0.5*log(norm(a)));
|
|
float r;
|
|
__asm__ volatile (
|
|
"lv.q C010, 0 + %1\n"
|
|
"vdot.t S020, C010, C010\n" // r = x^2 + y^2 + z^2
|
|
"vsqrt.s S020, S020\n" // r = sqrt(x^2 + y^2 + z^2)
|
|
"mfv %0, S020\n"
|
|
:"=r"(r): "m"(*qin));
|
|
r = vfpu_atan2f(r, qin->w)/r;
|
|
__asm__ volatile (
|
|
"mtv %1, S021\n" // t = atan2(r, w) / r
|
|
"vdot.q S022, C010, C010\n" // norm = x^2 + y^2 + z^2 + w^2
|
|
"vcst.s S023, VFPU_LOG2E\n"
|
|
"vrcp.s S023, S023\n"
|
|
"vlog2.s S013, S022\n"
|
|
"vmul.s S013, S013, S023\n"
|
|
"vmul.s S013, S013, S013[1/2]\n"
|
|
"vscl.t C010, C010, S021\n"
|
|
"sv.q C010, 0 + %0\n"
|
|
: "=m"(*qout) : "r"(r));
|
|
}
|