// quaternions and quaternion operations -- by julian "jibb" smart

typedef struct QUATERNION {
	double w;
	double x;
	double y;
	double z;
} QUATERNION;

function quat_multiply(QUATERNION *end_result, QUATERNION *a, QUATERNION *b);
function ang_for_quat(ANGLE *a, QUATERNION *q);
function quat_for_ang(QUATERNION *q, ANGLE *a);
function quaternize(double a, VECTOR *v, QUATERNION *q);
function quat_to_unit(QUATERNION *q);
function quat_set(QUATERNION *q, QUATERNION *u);
function quat_add(QUATERNION *q, QUATERNION *u);
function quat_sub(QUATERNION *q, QUATERNION *u);
function quat_conjugate(QUATERNION *q, QUATERNION *u);
function vec_rot_by_quat(VECTOR *v, QUATERNION *q);
function vec_for_quat(VECTOR *v, QUATERNION *q);
function quat_nlerp(QUATERNION *q, QUATERNION *u, QUATERNION *v, double f);

// end_result is the result of quaternion "b" being rotated by quaternion "a"
function quat_multiply(QUATERNION *end_result, QUATERNION *a, QUATERNION *b) {
	// multiply two quaternions -- non-commutative!
	QUATERNION q;
	q.w = a->w*b->w - a->x*b->x - a->y*b->y - a->z*b->z;
	q.x = a->w*b->x + a->x*b->w + a->y*b->z - a->z*b->y;
	q.y = a->w*b->y - a->x*b->z + a->y*b->w + a->z*b->x;
	q.z = a->w*b->z + a->x*b->y - a->y*b->x + a->z*b->w;
	quat_set(end_result, &q);
}

function ang_for_quat(ANGLE *a, QUATERNION *q) {
	// finds a euler representation of a quaternion
	ang_for_axis(a, vector(q->x, q->y, q->z), acosv(q->w) * 2);
}

function quat_for_ang(QUATERNION *q, ANGLE *a) {
	q->w = cos((a->roll/2))*cos((-a->tilt/2))*cos((a->pan/2))+sin((a->roll/2))*sin((-a->tilt/2))*sin((a->pan/2));
	q->x = sin((a->roll/2))*cos((-a->tilt/2))*cos((a->pan/2))-cos((a->roll/2))*sin((-a->tilt/2))*sin((a->pan/2));
	q->y = cos((a->roll/2))*sin((-a->tilt/2))*cos((a->pan/2))+sin((a->roll/2))*cos((-a->tilt/2))*sin((a->pan/2));
	q->z = cos((a->roll/2))*cos((-a->tilt/2))*sin((a->pan/2))-sin((a->roll/2))*sin((-a->tilt/2))*cos((a->pan/2));
}

function quaternize(double a, VECTOR *v, QUATERNION *q) {
	// makes quaternion "q" contain the information in angle and vector
	double rotation = a/2;
	q->w = cos(rotation);
	q->x = v->x*sin(rotation);
	q->y = v->y*sin(rotation);
	q->z = v->z*sin(rotation);
}

function quat_to_unit(QUATERNION *q) {
	// converts a quaternion to a unit quaternion
	double magnitude = sqrt(q->x*q->x + q->y*q->y + q->z*q->z);
	if(magnitude == 0) {
		q->w = 1.0;
		return;
	}
	double scaleFactor = sqrt(1-q->w*q->w)/magnitude;
	q->x *= scaleFactor;
	q->y *= scaleFactor;
	q->z *= scaleFactor;
}

function quat_set(QUATERNION *q, QUATERNION *u) {
	// sets quat "q" to equal quat "u"
	q->w = u->w;
	q->x = u->x;
	q->y = u->y;
	q->z = u->z;
}

function quat_add(QUATERNION *q, QUATERNION *u) {
	// add two quaternions -- warning! does not equal a rotation!
	q->w += u->w;
	q->x += u->x;
	q->y += u->y;
	q->z += u->z;
}

function quat_sub(QUATERNION *q, QUATERNION *u) {
	// subtract the second quaternion from the first -- also does not equal a rotation!
	q->w -= u->w;
	q->x -= u->x;
	q->y -= u->y;
	q->z -= u->z;
}

function quat_conjugate(QUATERNION *q, QUATERNION *u) {
	// first quaternion equals the conjugate of the second quaternion 
	q->w = u->w;
	q->x = -u->x;
	q->y = -u->y;
	q->z = -u->z;
}

function vec_rot_by_quat(VECTOR *v, QUATERNION *q) {
	// rotate a vector by a quaternion
	VECTOR u;
	u.x = v->x*(q->w*q->w + q->x*q->x - q->y*q->y - q->z*q->z) + 2*(q->w*q->y*v->z + q->x*q->z*v->z + q->x*q->y*v->y - q->w*q->z*v->y);
	u.y = v->y*(q->w*q->w - q->x*q->x + q->y*q->y - q->z*q->z) + 2*(q->x*q->y*v->x + q->w*q->z*v->x + q->y*q->z*v->z - q->x*q->w*v->z);
	u.z = v->z*(q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z) + 2*(q->x*q->z*v->x - q->w*q->y*v->x + q->w*q->x*v->y + q->z*q->y*v->y);
	vec_set(v, &u);
}

function vec_for_quat(VECTOR *v, QUATERNION *q) {
	// find a unit vector pointing in the direction of the quaternion
	v->x = q->w*q->w + q->x*q->x - q->y*q->y - q->z*q->z;
	v->y = 2*(q->x*q->y + q->w*q->z);
	v->z = 2*(q->x*q->z - q->w*q->y);
}

function quat_nlerp(QUATERNION *q, QUATERNION *u, QUATERNION *v, double f) {
	// normalized linear interpolation of a quaternion between two others by a factor "f"
	QUATERNION r, s;
	quat_set(&r, u);
	r.w *= 1.0-f;
	r.x *= 1.0-f;
	r.y *= 1.0-f;
	r.z *= 1.0-f;
	quat_set(&s, v);
	s.w *= f;
	s.x *= f;
	s.y *= f;
	s.z *= f;
	quat_add(&r, &s);
	quat_to_unit(&r);
	quat_set(q, &r);
}