eulerToQuat(float qroll, float qpitch, float qyaw, QUAT * quat)
{
float cr, cp, cy, sr, sp, sy, cpcy, spsy;
// calculate trig identities
cr = cos(qroll/2);
cp = cos(qpitch/2);
cy = cos(qyaw/2);
sr = sin(qroll/2);
sp = sin(qpitch/2);
sy = sin(qyaw/2);
cpcy = cp * cy;
spsy = sp * sy;
quat->w = cr * cpcy + sr * spsy;
quat->x = sr * cpcy - cr * spsy;
quat->y = cr * sp * cy + sr * cp * sy;
quat->z = cr * cp * sy - sr * sp * cy;
}
quatToMatrix(QUAT * quat)
{
float x2 = quat->x * quat->x;
float y2 = quat->y * quat->y;
float z2 = quat->z * quat->z;
float xy = quat->x * quat->y;
float xz = quat->x * quat->z;
float yz = quat->y * quat->z;
float wx = quat->w * quat->x;
float wy = quat->w * quat->y;
float wz = quat->w * quat->z;
// This calculation would be a lot more complicated for non-unit length quaternions
// Note: The constructor of Matrix4 expects the Matrix in column-major format like expected by
// OpenGL
return Matrix4( 1.0f - 2.0f * (y2 + z2), 2.0f * (xy - wz), 2.0f * (xz + wy), 0.0f,
2.0f * (xy + wz), 1.0f - 2.0f * (x2 + z2), 2.0f * (yz - wx), 0.0f,
2.0f * (xz - wy), 2.0f * (yz + wx), 1.0f - 2.0f * (x2 + y2), 0.0f,
0.0f, 0.0f, 0.0f, 1.0f)
}