Show quat.c syntax highlighted
/* ______ ___ ___
* /\ _ \ /\_ \ /\_ \
* \ \ \L\ \\//\ \ \//\ \ __ __ _ __ ___
* \ \ __ \ \ \ \ \ \ \ /'__`\ /'_ `\/\`'__\/ __`\
* \ \ \/\ \ \_\ \_ \_\ \_/\ __//\ \L\ \ \ \//\ \L\ \
* \ \_\ \_\/\____\/\____\ \____\ \____ \ \_\\ \____/
* \/_/\/_/\/____/\/____/\/____/\/___L\ \/_/ \/___/
* /\____/
* \_/__/
*
* Quaternion manipulation routines.
*
* By Jason Wilkins.
*
* See readme.txt for copyright information.
*/
#include <math.h>
#include "allegro.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#define floatcos(x) ((float)(cos((x) * M_PI / 128.0)))
#define floatsin(x) ((float)(sin((x) * M_PI / 128.0)))
#define EPSILON (0.001)
AL_QUAT al_identity_quat = { 1.0, 0.0, 0.0, 0.0 };
/* al_quat_mul:
* Multiplies two quaternions, storing the result in out. The resulting
* quaternion will have the same effect as the combination of p and q,
* ie. when applied to a point, (point * out) = ((point * p) * q). Any
* number of rotations can be concatenated in this way. Note that quaternion
* multiplication is not commutative, ie. al_quat_mul(p, q) != al_quat_mul(q, p).
*/
void al_quat_mul(AL_CONST AL_QUAT *p, AL_CONST AL_QUAT *q, AL_QUAT *out)
{
AL_QUAT temp;
/* qp = ww' - v.v' + vxv' + wv' + w'v */
if (p == out) {
temp = *p;
p = &temp;
}
else if (q == out) {
temp = *q;
q = &temp;
}
/* w" = ww' - xx' - yy' - zz' */
out->w = (p->w * q->w) -
(p->x * q->x) -
(p->y * q->y) -
(p->z * q->z);
/* x" = wx' + xw' + yz' - zy' */
out->x = (p->w * q->x) +
(p->x * q->w) +
(p->y * q->z) -
(p->z * q->y);
/* y" = wy' + yw' + zx' - xz' */
out->y = (p->w * q->y) +
(p->y * q->w) +
(p->z * q->x) -
(p->x * q->z);
/* z" = wz' + zw' + xy' - yx' */
out->z = (p->w * q->z) +
(p->z * q->w) +
(p->x * q->y) -
(p->y * q->x);
}
/* al_get_x_rotate_quat:
* Construct X axis rotation quaternions, storing them in q. When applied to
* a point, these quaternions will rotate it about the X axis by the
* specified angle (given in binary, 256 degrees to a al_draw_circle format).
*/
void al_get_x_rotate_quat(AL_QUAT *q, float r)
{
q->w = floatcos(r / 2);
q->x = floatsin(r / 2);
q->y = 0;
q->z = 0;
}
/* al_get_y_rotate_quat:
* Construct Y axis rotation quaternions, storing them in q. When applied to
* a point, these quaternions will rotate it about the Y axis by the
* specified angle (given in binary, 256 degrees to a al_draw_circle format).
*/
void al_get_y_rotate_quat(AL_QUAT *q, float r)
{
q->w = floatcos(r / 2);
q->x = 0;
q->y = floatsin(r / 2);
q->z = 0;
}
/* al_get_z_rotate_quat:
* Construct Z axis rotation quaternions, storing them in q. When applied to
* a point, these quaternions will rotate it about the Z axis by the
* specified angle (given in binary, 256 degrees to a al_draw_circle format).
*/
void al_get_z_rotate_quat(AL_QUAT *q, float r)
{
q->w = floatcos(r / 2);
q->x = 0;
q->y = 0;
q->z = floatsin(r / 2);
}
/* al_get_rotation_quat:
* Constructs a quaternion which will rotate points around all three axis by
* the specified amounts (given in binary, 256 degrees to a al_draw_circle format).
*/
void al_get_rotation_quat(AL_QUAT *q, float x, float y, float z)
{
float sx;
float sy;
float sz;
float cx;
float cy;
float cz;
float cycz;
float sysz;
sx = floatsin(x / 2);
sy = floatsin(y / 2);
sz = floatsin(z / 2);
cx = floatcos(x / 2);
cy = floatcos(y / 2);
cz = floatcos(z / 2);
sysz = sy * sz;
cycz = cy * cz;
q->w = (cx * cycz) + (sx * sysz);
q->x = (sx * cycz) - (cx * sysz);
q->y = (cx * sy * cz) + (sx * cy * sz);
q->z = (cx * cy * sz) - (sx * sy * cz);
}
/* al_get_vector_rotation_quat:
* Constructs a quaternion which will rotate points around the specified
* x,y,z vector by the specified angle (given in binary, 256 degrees to a
* al_draw_circle format).
*/
void al_get_vector_rotation_quat(AL_QUAT *q, float x, float y, float z, float a)
{
float l;
float s;
l = al_vector_length_f(x, y, z);
/* NOTE: Passing (x, y, z) = (0, 0, 0) will cause l to equal 0 which will
* cause a divide-by-zero exception. Rotating something about the
* zero vector undefined.
*/
AL_ASSERT(l != 0);
x /= l;
y /= l;
z /= l;
q->w = floatcos(a / 2);
s = floatsin(a / 2);
q->x = s * x;
q->y = s * y;
q->z = s * z;
}
/* al_quat_to_matrix:
* Constructs a rotation matrix from a quaternion.
*/
void al_quat_to_matrix(AL_CONST AL_QUAT *q, AL_MATRIX_F *m)
{
float ww;
float xx;
float yy;
float zz;
float wx;
float wy;
float wz;
float xy;
float xz;
float yz;
/* This is the layout for converting the values in a quaternion to a
* matrix.
*
* | ww + xx - yy - zz 2xy + 2wz 2xz - 2wy |
* | 2xy - 2wz ww - xx + yy - zz 2yz - 2wx |
* | 2xz + 2wy 2yz - 2wx ww + xx - yy - zz |
*/
ww = q->w * q->w;
xx = q->x * q->x;
yy = q->y * q->y;
zz = q->z * q->z;
wx = q->w * q->x * 2;
wy = q->w * q->y * 2;
wz = q->w * q->z * 2;
xy = q->x * q->y * 2;
xz = q->x * q->z * 2;
yz = q->y * q->z * 2;
m->v[0][0] = ww + xx - yy - zz;
m->v[1][0] = xy - wz;
m->v[2][0] = xz + wy;
m->v[0][1] = xy + wz;
m->v[1][1] = ww - xx + yy - zz;
m->v[2][1] = yz - wx;
m->v[0][2] = xz - wy;
m->v[1][2] = yz + wx;
m->v[2][2] = ww - xx - yy + zz;
m->t[0] = 0.0;
m->t[1] = 0.0;
m->t[2] = 0.0;
}
/* al_matrix_to_quat:
* Constructs a quaternion from a rotation matrix. Translation is discarded
* during the conversion. Use al_get_align_matrix_f if the matrix is not
* orthonormalized, because strange things may happen otherwise.
*/
void al_matrix_to_quat(AL_CONST AL_MATRIX_F *m, AL_QUAT *q)
{
float diag;
float s;
int i;
int j;
int k;
float out[4];
static int next[3] = { 1, 2, 0 };
diag = m->v[0][0] + m->v[1][1] + m->v[2][2];
if (diag > 0.0f) {
s = (float)(sqrt(diag + 1.0));
q->w = s / 2.0;
s = 0.5 / s;
q->x = (m->v[1][2] - m->v[2][1]) * s;
q->y = (m->v[2][0] - m->v[0][2]) * s;
q->z = (m->v[0][1] - m->v[1][0]) * s;
}
else {
i = 0;
if (m->v[1][1] > m->v[0][0]) {
i = 1;
}
if (m->v[2][2] > m->v[i][i]) {
i = 2;
}
j = next[i];
k = next[j];
s = m->v[i][i] - (m->v[j][j] + m->v[k][k]);
/* NOTE: Passing non-orthonormalized matrices can result in odd things
* happening, like the calculation of s below trying to find the
* square-root of a negative number, which is imaginary. Some
* implementations of sqrt will crash, while others return this
* as not-a-number (NaN). NaN could be very subtle because it will
* not throw an exception on Intel processors.
*/
AL_ASSERT(s > 0.0);
s = (float)(sqrt(s) + 1.0);
out[i] = s / 2.0;
s = 0.5 / s;
out[j] = (m->v[i][j] + m->v[j][i]) * s;
out[k] = (m->v[i][k] + m->v[k][i]) * s;
out[3] = (m->v[j][k] - m->v[k][j]) * s;
q->x = out[0];
q->y = out[1];
q->z = out[2];
q->w = out[3];
}
}
/* quat_conjugate:
* A quaternion conjugate is analogous to a complex number conjugate, just
* negate the imaginary part
*/
static INLINE void quat_conjugate(AL_CONST AL_QUAT *q, AL_QUAT *out)
{
/* q^* = w - x - y - z */
out->w = (q->w);
out->x = -(q->x);
out->y = -(q->y);
out->z = -(q->z);
}
/* quat_normal:
* A quaternion normal is the sum of the squares of the components.
*/
static INLINE float quat_normal(AL_CONST AL_QUAT *q)
{
/* N(q) = ww + xx + yy + zz */
return ((q->w * q->w) + (q->x * q->x) + (q->y * q->y) + (q->z * q->z));
}
/* quat_inverse:
* A quaternion inverse is the conjugate divided by the normal.
*/
static INLINE void quat_inverse(AL_CONST AL_QUAT *q, AL_QUAT *out)
{
AL_QUAT con;
float norm;
/* q^-1 = q^* / N(q) */
quat_conjugate(q, &con);
norm = quat_normal(q);
/* NOTE: If the normal is 0 then a divide-by-zero exception will occur, we
* let this happen because the inverse of a zero quaternion is
* undefined
*/
AL_ASSERT(norm != 0);
out->w = con.w / norm;
out->x = con.x / norm;
out->y = con.y / norm;
out->z = con.z / norm;
}
/* al_apply_quat:
* Multiplies the point (x, y, z) by the quaternion q, storing the result in
* (*xout, *yout, *zout). This is quite a bit slower than al_apply_matrix_f.
* So, only use it if you only need to translate a handful of points.
* Otherwise it is much more efficient to call al_quat_to_matrix then use
* al_apply_matrix_f.
*/
void al_apply_quat(AL_CONST AL_QUAT *q, float x, float y, float z, float *xout, float *yout, float *zout)
{
AL_QUAT v;
AL_QUAT i;
AL_QUAT t;
/* v' = q * v * q^-1 */
v.w = 0;
v.x = x;
v.y = y;
v.z = z;
/* NOTE: Rotating about a zero quaternion is undefined. This can be shown
* by the fact that the inverse of a zero quaternion is undefined
* and therefore causes an exception below.
*/
AL_ASSERT(!((x == 0) && (y == 0) && (z == 0)));
quat_inverse(q, &i);
al_quat_mul(&i, &v, &t);
al_quat_mul(&t, q, &v);
*xout = v.x;
*yout = v.y;
*zout = v.z;
}
/* al_quat_slerp:
* Constructs a quaternion that represents a rotation between 'from' and
* 'to'. The argument 't' can be anything between 0 and 1 and represents
* where between from and to the result will be. 0 returns 'from', 1
* returns 'to', and 0.5 will return a rotation exactly in between. The
* result is copied to 'out'.
*
* The variable 'how' can be any one of the following:
*
* AL_QUAT_SHORT - This equivalent al_quat_interpolate, the rotation will
* be less than 180 degrees
* AL_QUAT_LONG - rotation will be greater than 180 degrees
* AL_QUAT_CW - rotation will be clockwise when viewed from above
* AL_QUAT_CCW - rotation will be counterclockwise when viewed
* from above
* AL_QUAT_USER - the quaternions are interpolated exactly as given
*/
void al_quat_slerp(AL_CONST AL_QUAT *from, AL_CONST AL_QUAT *to, float t, AL_QUAT *out, int how)
{
AL_QUAT to2;
double angle;
double cos_angle;
double scale_from;
double scale_to;
double sin_angle;
cos_angle = (from->x * to->x) +
(from->y * to->y) +
(from->z * to->z) +
(from->w * to->w);
if (((how == AL_QUAT_SHORT) && (cos_angle < 0.0)) ||
((how == AL_QUAT_LONG) && (cos_angle > 0.0)) ||
((how == AL_QUAT_CW) && (from->w > to->w)) ||
((how == AL_QUAT_CCW) && (from->w < to->w))) {
cos_angle = -cos_angle;
to2.w = -to->w;
to2.x = -to->x;
to2.y = -to->y;
to2.z = -to->z;
}
else {
to2.w = to->w;
to2.x = to->x;
to2.y = to->y;
to2.z = to->z;
}
if ((1.0 - ABS(cos_angle)) > EPSILON) {
/* spherical linear interpolation (SLERP) */
angle = acos(cos_angle);
sin_angle = sin(angle);
scale_from = sin((1.0 - t) * angle) / sin_angle;
scale_to = sin(t * angle) / sin_angle;
}
else {
/* to prevent divide-by-zero, resort to linear interpolation */
scale_from = 1.0 - t;
scale_to = t;
}
out->w = (float)((scale_from * from->w) + (scale_to * to2.w));
out->x = (float)((scale_from * from->x) + (scale_to * to2.x));
out->y = (float)((scale_from * from->y) + (scale_to * to2.y));
out->z = (float)((scale_from * from->z) + (scale_to * to2.z));
}
See more files for this project here