#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));
}