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