
#ifndef H_QUATERNION_
#define H_QUATERNION_

#include <math.h>
#include <psptypes.h>

inline ScePspFQuaternion *quaternionNormalize(ScePspFQuaternion *q)
{
	float n = 1.0f / sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
	q->x *= n;
	q->y *= n;
	q->z *= n;
	q->w *= n;
	return q;
}

inline ScePspFQuaternion *quaternionMultiplyAssign(ScePspFQuaternion *lhs, const ScePspFQuaternion *rhs)
{
	float ww = lhs->w * rhs->w - (lhs->x * rhs->x + lhs->y * rhs->y + lhs->z * rhs->z);
	float vx = lhs->w * rhs->x + rhs->w * lhs->x + lhs->y * rhs->z - lhs->z * rhs->y;
	float vy = lhs->w * rhs->y + rhs->w * lhs->y + lhs->z * rhs->x - lhs->x * rhs->z;
	float vz = lhs->w * rhs->z + rhs->w * lhs->z + lhs->x * rhs->y - lhs->y * rhs->x;
	lhs->w = ww;
	lhs->x = vx;
	lhs->y = vy;
	lhs->z = vz;
	return quaternionNormalize(lhs);
}

inline ScePspFQuaternion quaternionMultiply(const ScePspFQuaternion *lhs, const ScePspFQuaternion *rhs)
{
	ScePspFQuaternion q = *lhs;
	return *quaternionMultiplyAssign(&q, rhs);
}

inline ScePspFQuaternion *quaternionFromEulerAngle(ScePspFQuaternion *q, const ScePspFVector3 *v)
{
	float cr = cos(v->x * 0.5f);
	float cp = cos(v->y * 0.5f);
	float cy = cos(v->z * 0.5f);
	float sr = sin(v->x * 0.5f);
	float sp = sin(v->y * 0.5f);
	float sy = sin(v->z * 0.5f);
	float cpcy = cp * cy;
	float spsy = sp * sy;
	q->x = sr * cpcy - cy * spsy;
	q->y = cr * sp * cy + sr * cp * sy;
	q->z = cr * cp * sy - sr * sp * cy;
	q->w = cr * cpcy + sr * spsy;
	return quaternionNormalize(q);
}

inline ScePspFMatrix4 *quaternionGetMatrix(const ScePspFQuaternion *q, ScePspFMatrix4 *m)
{
	float x2 = q->x * q->x * 2.0f;
	float y2 = q->y * q->y * 2.0f;
	float z2 = q->z * q->z * 2.0f;
	float xy = q->x * q->y * 2.0f;
	float yz = q->y * q->z * 2.0f;
	float zx = q->z * q->x * 2.0f;
	float xw = q->x * q->w * 2.0f;
	float yw = q->y * q->w * 2.0f;
	float zw = q->z * q->w * 2.0f;
	m->x.x = 1.0 - y2 - z2;
	m->x.y = xy + zw;
	m->x.z = zx - yw;
	m->x.w = 0.0f;
	m->y.x = xy - zw;
	m->y.y = 1.0 - z2 - x2;
	m->y.z = yz + xw;
	m->y.w = 0.0f;
	m->z.x = zx + yw;
	m->z.y = yz - xw;
	m->z.z = 1.0 - x2 - y2;
	m->z.w = 0.0f;
	m->w.x = 0.0f;
	m->w.y = 0.0f;
	m->w.z = 0.0f;
	m->w.w = 1.0f;
	return m;
}

#endif

