1 /* ______ ___ ___
2 * /\ _ \ /\_ \ /\_ \
3 * \ \ \L\ \\//\ \ \//\ \ __ __ _ __ ___
4 * \ \ __ \ \ \ \ \ \ \ /'__`\ /'_ `\/\`'__\/ __`\
5 * \ \ \/\ \ \_\ \_ \_\ \_/\ __//\ \L\ \ \ \//\ \L\ \
6 * \ \_\ \_\/\____\/\____\ \____\ \____ \ \_\\ \____/
7 * \/_/\/_/\/____/\/____/\/____/\/___L\ \/_/ \/___/
8 * /\____/
9 * \_/__/
10 *
11 * Quaternion manipulation routines.
12 *
13 * By Jason Wilkins.
14 *
15 * See readme.txt for copyright information.
16 */
17
18
19 #include <math.h>
20
21 #include "allegro.h"
22
23
24
25 #define FLOATSINCOS(x, s, c) _AL_SINCOS((x) * AL_PI / 128.0, s ,c)
26
27
28 #define EPSILON (0.001)
29
30
31
32 QUAT identity_quat = { 1.0, 0.0, 0.0, 0.0 };
33
34
35
36 /* quat_mul:
37 * Multiplies two quaternions, storing the result in out. The resulting
38 * quaternion will have the same effect as the combination of p and q,
39 * ie. when applied to a point, (point * out) = ((point * p) * q). Any
40 * number of rotations can be concatenated in this way. Note that quaternion
41 * multiplication is not commutative, ie. quat_mul(p, q) != quat_mul(q, p).
42 */
quat_mul(AL_CONST QUAT * p,AL_CONST QUAT * q,QUAT * out)43 void quat_mul(AL_CONST QUAT *p, AL_CONST QUAT *q, QUAT *out)
44 {
45 QUAT temp;
46 ASSERT(p);
47 ASSERT(q);
48 ASSERT(out);
49
50 /* qp = ww' - v.v' + vxv' + wv' + w'v */
51
52 if (p == out) {
53 temp = *p;
54 p = &temp;
55 }
56 else if (q == out) {
57 temp = *q;
58 q = &temp;
59 }
60
61 /* w" = ww' - xx' - yy' - zz' */
62 out->w = (p->w * q->w) -
63 (p->x * q->x) -
64 (p->y * q->y) -
65 (p->z * q->z);
66
67 /* x" = wx' + xw' + yz' - zy' */
68 out->x = (p->w * q->x) +
69 (p->x * q->w) +
70 (p->y * q->z) -
71 (p->z * q->y);
72
73 /* y" = wy' + yw' + zx' - xz' */
74 out->y = (p->w * q->y) +
75 (p->y * q->w) +
76 (p->z * q->x) -
77 (p->x * q->z);
78
79 /* z" = wz' + zw' + xy' - yx' */
80 out->z = (p->w * q->z) +
81 (p->z * q->w) +
82 (p->x * q->y) -
83 (p->y * q->x);
84 }
85
86
87
88 /* get_x_rotate_quat:
89 * Construct X axis rotation quaternions, storing them in q. When applied to
90 * a point, these quaternions will rotate it about the X axis by the
91 * specified angle (given in binary, 256 degrees to a circle format).
92 */
get_x_rotate_quat(QUAT * q,float r)93 void get_x_rotate_quat(QUAT *q, float r)
94 {
95 ASSERT(q);
96
97 FLOATSINCOS(r/2, q->x, q->w);
98 q->y = 0;
99 q->z = 0;
100 }
101
102
103
104 /* get_y_rotate_quat:
105 * Construct Y axis rotation quaternions, storing them in q. When applied to
106 * a point, these quaternions will rotate it about the Y axis by the
107 * specified angle (given in binary, 256 degrees to a circle format).
108 */
get_y_rotate_quat(QUAT * q,float r)109 void get_y_rotate_quat(QUAT *q, float r)
110 {
111 ASSERT(q);
112
113 FLOATSINCOS(r/2, q->y, q->w);
114 q->x = 0;
115 q->z = 0;
116 }
117
118
119
120 /* get_z_rotate_quat:
121 * Construct Z axis rotation quaternions, storing them in q. When applied to
122 * a point, these quaternions will rotate it about the Z axis by the
123 * specified angle (given in binary, 256 degrees to a circle format).
124 */
get_z_rotate_quat(QUAT * q,float r)125 void get_z_rotate_quat(QUAT *q, float r)
126 {
127 ASSERT(q);
128
129 FLOATSINCOS(r/2, q->z, q->w);
130 q->x = 0;
131 q->y = 0;
132 }
133
134
135
136 /* get_rotation_quat:
137 * Constructs a quaternion which will rotate points around all three axis by
138 * the specified amounts (given in binary, 256 degrees to a circle format).
139 */
get_rotation_quat(QUAT * q,float x,float y,float z)140 void get_rotation_quat(QUAT *q, float x, float y, float z)
141 {
142 float sx;
143 float sy;
144 float sz;
145 float cx;
146 float cy;
147 float cz;
148 float cycz;
149 float sysz;
150
151 ASSERT(q);
152
153 FLOATSINCOS(x/2, sx, cx);
154 FLOATSINCOS(y/2, sy, cy);
155 FLOATSINCOS(z/2, sz, cz);
156
157 sysz = sy * sz;
158 cycz = cy * cz;
159
160 q->w = (cx * cycz) + (sx * sysz);
161 q->x = (sx * cycz) - (cx * sysz);
162 q->y = (cx * sy * cz) + (sx * cy * sz);
163 q->z = (cx * cy * sz) - (sx * sy * cz);
164 }
165
166
167
168 /* get_vector_rotation_quat:
169 * Constructs a quaternion which will rotate points around the specified
170 * x,y,z vector by the specified angle (given in binary, 256 degrees to a
171 * circle format).
172 */
get_vector_rotation_quat(QUAT * q,float x,float y,float z,float a)173 void get_vector_rotation_quat(QUAT *q, float x, float y, float z, float a)
174 {
175 float l;
176 float s;
177
178 ASSERT(q);
179
180 l = vector_length_f(x, y, z);
181
182 /* NOTE: Passing (x, y, z) = (0, 0, 0) will cause l to equal 0 which will
183 * cause a divide-by-zero exception. Rotating something about the
184 * zero vector undefined.
185 */
186 ASSERT(l != 0);
187
188 x /= l;
189 y /= l;
190 z /= l;
191
192 FLOATSINCOS(a/2, s, q->w);
193 q->x = s * x;
194 q->y = s * y;
195 q->z = s * z;
196 }
197
198
199
200 /* quat_to_matrix:
201 * Constructs a rotation matrix from a quaternion.
202 */
quat_to_matrix(AL_CONST QUAT * q,MATRIX_f * m)203 void quat_to_matrix(AL_CONST QUAT *q, MATRIX_f *m)
204 {
205 float ww;
206 float xx;
207 float yy;
208 float zz;
209 float wx;
210 float wy;
211 float wz;
212 float xy;
213 float xz;
214 float yz;
215 ASSERT(q);
216 ASSERT(m);
217
218 /* This is the layout for converting the values in a quaternion to a
219 * matrix.
220 *
221 * | ww + xx - yy - zz 2xy + 2wz 2xz - 2wy |
222 * | 2xy - 2wz ww - xx + yy - zz 2yz - 2wx |
223 * | 2xz + 2wy 2yz - 2wx ww + xx - yy - zz |
224 */
225
226 ww = q->w * q->w;
227 xx = q->x * q->x;
228 yy = q->y * q->y;
229 zz = q->z * q->z;
230 wx = q->w * q->x * 2;
231 wy = q->w * q->y * 2;
232 wz = q->w * q->z * 2;
233 xy = q->x * q->y * 2;
234 xz = q->x * q->z * 2;
235 yz = q->y * q->z * 2;
236
237 m->v[0][0] = ww + xx - yy - zz;
238 m->v[1][0] = xy - wz;
239 m->v[2][0] = xz + wy;
240
241 m->v[0][1] = xy + wz;
242 m->v[1][1] = ww - xx + yy - zz;
243 m->v[2][1] = yz - wx;
244
245 m->v[0][2] = xz - wy;
246 m->v[1][2] = yz + wx;
247 m->v[2][2] = ww - xx - yy + zz;
248
249 m->t[0] = 0.0;
250 m->t[1] = 0.0;
251 m->t[2] = 0.0;
252 }
253
254
255
256 /* matrix_to_quat:
257 * Constructs a quaternion from a rotation matrix. Translation is discarded
258 * during the conversion. Use get_align_matrix_f if the matrix is not
259 * orthonormalized, because strange things may happen otherwise.
260 */
matrix_to_quat(AL_CONST MATRIX_f * m,QUAT * q)261 void matrix_to_quat(AL_CONST MATRIX_f *m, QUAT *q)
262 {
263 float trace = m->v[0][0] + m->v[1][1] + m->v[2][2] + 1.0f;
264
265 if (trace > EPSILON) {
266 float s = 0.5f / (float)sqrt(trace);
267 q->w = 0.25f / s;
268 q->x = (m->v[2][1] - m->v[1][2]) * s;
269 q->y = (m->v[0][2] - m->v[2][0]) * s;
270 q->z = (m->v[1][0] - m->v[0][1]) * s;
271 }
272 else {
273 if (m->v[0][0] > m->v[1][1] && m->v[0][0] > m->v[2][2]) {
274 float s = 2.0f * (float)sqrt(1.0f + m->v[0][0] - m->v[1][1] - m->v[2][2]);
275 q->x = 0.25f * s;
276 q->y = (m->v[0][1] + m->v[1][0]) / s;
277 q->z = (m->v[0][2] + m->v[2][0]) / s;
278 q->w = (m->v[1][2] - m->v[2][1]) / s;
279 }
280 else if (m->v[1][1] > m->v[2][2]) {
281 float s = 2.0f * (float)sqrt(1.0f + m->v[1][1] - m->v[0][0] - m->v[2][2]);
282 q->x = (m->v[0][1] + m->v[1][0]) / s;
283 q->y = 0.25f * s;
284 q->z = (m->v[1][2] + m->v[2][1]) / s;
285 q->w = (m->v[0][2] - m->v[2][0]) / s;
286 }
287 else {
288 float s = 2.0f * (float)sqrt(1.0f + m->v[2][2] - m->v[0][0] - m->v[1][1]);
289 q->x = (m->v[0][2] + m->v[2][0]) / s;
290 q->y = (m->v[1][2] + m->v[2][1]) / s;
291 q->z = 0.25f * s;
292 q->w = (m->v[0][1] - m->v[1][0]) / s;
293 }
294 }
295 }
296
297
298
299 /* quat_conjugate:
300 * A quaternion conjugate is analogous to a complex number conjugate, just
301 * negate the imaginary part
302 */
quat_conjugate(AL_CONST QUAT * q,QUAT * out)303 static INLINE void quat_conjugate(AL_CONST QUAT *q, QUAT *out)
304 {
305 ASSERT(q);
306 ASSERT(out);
307
308 /* q^* = w - x - y - z */
309 out->w = (q->w);
310 out->x = -(q->x);
311 out->y = -(q->y);
312 out->z = -(q->z);
313 }
314
315
316
317 /* quat_normal:
318 * A quaternion normal is the sum of the squares of the components.
319 */
quat_normal(AL_CONST QUAT * q)320 static INLINE float quat_normal(AL_CONST QUAT *q)
321 {
322 ASSERT(q);
323
324 /* N(q) = ww + xx + yy + zz */
325 return ((q->w * q->w) + (q->x * q->x) + (q->y * q->y) + (q->z * q->z));
326 }
327
328
329
330 /* quat_inverse:
331 * A quaternion inverse is the conjugate divided by the normal.
332 */
quat_inverse(AL_CONST QUAT * q,QUAT * out)333 static INLINE void quat_inverse(AL_CONST QUAT *q, QUAT *out)
334 {
335 QUAT con;
336 float norm;
337 ASSERT(q);
338 ASSERT(out);
339
340 /* q^-1 = q^* / N(q) */
341
342 quat_conjugate(q, &con);
343 norm = quat_normal(q);
344
345 /* NOTE: If the normal is 0 then a divide-by-zero exception will occur, we
346 * let this happen because the inverse of a zero quaternion is
347 * undefined
348 */
349 ASSERT(norm != 0);
350
351 out->w = con.w / norm;
352 out->x = con.x / norm;
353 out->y = con.y / norm;
354 out->z = con.z / norm;
355 }
356
357
358
359 /* apply_quat:
360 * Multiplies the point (x, y, z) by the quaternion q, storing the result in
361 * (*xout, *yout, *zout). This is quite a bit slower than apply_matrix_f.
362 * So, only use it if you only need to translate a handful of points.
363 * Otherwise it is much more efficient to call quat_to_matrix then use
364 * apply_matrix_f.
365 */
apply_quat(AL_CONST QUAT * q,float x,float y,float z,float * xout,float * yout,float * zout)366 void apply_quat(AL_CONST QUAT *q, float x, float y, float z, float *xout, float *yout, float *zout)
367 {
368 QUAT v;
369 QUAT i;
370 QUAT t;
371 ASSERT(q);
372 ASSERT(xout);
373 ASSERT(yout);
374 ASSERT(zout);
375
376 /* v' = q * v * q^-1 */
377
378 v.w = 0;
379 v.x = x;
380 v.y = y;
381 v.z = z;
382
383 /* NOTE: Rotating about a zero quaternion is undefined. This can be shown
384 * by the fact that the inverse of a zero quaternion is undefined
385 * and therefore causes an exception below.
386 */
387 ASSERT(!((q->x == 0.0) && (q->y == 0.0) && (q->z == 0.0) && (q->w == 0.0)));
388
389 quat_inverse(q, &i);
390 quat_mul(&i, &v, &t);
391 quat_mul(&t, q, &v);
392
393 *xout = v.x;
394 *yout = v.y;
395 *zout = v.z;
396 }
397
398
399
400 /* quat_slerp:
401 * Constructs a quaternion that represents a rotation between 'from' and
402 * 'to'. The argument 't' can be anything between 0 and 1 and represents
403 * where between from and to the result will be. 0 returns 'from', 1
404 * returns 'to', and 0.5 will return a rotation exactly in between. The
405 * result is copied to 'out'.
406 *
407 * The variable 'how' can be any one of the following:
408 *
409 * QUAT_SHORT - This equivalent quat_interpolate, the rotation will
410 * be less than 180 degrees
411 * QUAT_LONG - rotation will be greater than 180 degrees
412 * QUAT_CW - rotation will be clockwise when viewed from above
413 * QUAT_CCW - rotation will be counterclockwise when viewed
414 * from above
415 * QUAT_USER - the quaternions are interpolated exactly as given
416 */
quat_slerp(AL_CONST QUAT * from,AL_CONST QUAT * to,float t,QUAT * out,int how)417 void quat_slerp(AL_CONST QUAT *from, AL_CONST QUAT *to, float t, QUAT *out, int how)
418 {
419 QUAT to2;
420 double angle;
421 double cos_angle;
422 double scale_from;
423 double scale_to;
424 double sin_angle;
425 ASSERT(from);
426 ASSERT(to);
427 ASSERT(out);
428
429 cos_angle = (from->x * to->x) +
430 (from->y * to->y) +
431 (from->z * to->z) +
432 (from->w * to->w);
433
434 if (((how == QUAT_SHORT) && (cos_angle < 0.0)) ||
435 ((how == QUAT_LONG) && (cos_angle > 0.0)) ||
436 ((how == QUAT_CW) && (from->w > to->w)) ||
437 ((how == QUAT_CCW) && (from->w < to->w))) {
438 cos_angle = -cos_angle;
439 to2.w = -to->w;
440 to2.x = -to->x;
441 to2.y = -to->y;
442 to2.z = -to->z;
443 }
444 else {
445 to2.w = to->w;
446 to2.x = to->x;
447 to2.y = to->y;
448 to2.z = to->z;
449 }
450
451 if ((1.0 - ABS(cos_angle)) > EPSILON) {
452 /* spherical linear interpolation (SLERP) */
453 angle = acos(cos_angle);
454 sin_angle = sin(angle);
455 scale_from = sin((1.0 - t) * angle) / sin_angle;
456 scale_to = sin(t * angle) / sin_angle;
457 }
458 else {
459 /* to prevent divide-by-zero, resort to linear interpolation */
460 scale_from = 1.0 - t;
461 scale_to = t;
462 }
463
464 out->w = (float)((scale_from * from->w) + (scale_to * to2.w));
465 out->x = (float)((scale_from * from->x) + (scale_to * to2.x));
466 out->y = (float)((scale_from * from->y) + (scale_to * to2.y));
467 out->z = (float)((scale_from * from->z) + (scale_to * to2.z));
468 }
469
470
471
472