4 * An object oriented GL/GLES Abstraction/Utility Layer
6 * Copyright (C) 2010 Intel Corporation.
8 * This library is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU Lesser General Public
10 * License as published by the Free Software Foundation; either
11 * version 2 of the License, or (at your option) any later version.
13 * This library is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 * Lesser General Public License for more details.
18 * You should have received a copy of the GNU Lesser General Public
19 * License along with this library; if not, write to the
20 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
21 * Boston, MA 02111-1307, USA.
24 * Robert Bragg <robert@linux.intel.com>
26 * Various references relating to quaternions:
28 * http://www.cs.caltech.edu/courses/cs171/quatut.pdf
29 * http://mathworld.wolfram.com/Quaternion.html
30 * http://www.gamedev.net/reference/articles/article1095.asp
31 * http://www.cprogramming.com/tutorial/3d/quaternions.html
32 * http://www.isner.com/tutorials/quatSpells/quaternion_spells_12.htm
33 * http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56
34 * 3D Maths Primer for Graphics and Game Development ISBN-10: 1556229119
41 #include <cogl-util.h>
42 #include <cogl-quaternion.h>
43 #include <cogl-quaternion-private.h>
44 #include <cogl-matrix.h>
45 #include <cogl-vector.h>
46 #include <cogl-euler.h>
51 #define FLOAT_EPSILON 1e-03
53 static CoglQuaternion zero_quaternion =
58 static CoglQuaternion identity_quaternion =
63 /* This function is just here to be called from GDB so we don't really
64 want to put a declaration in a header and we just add it here to
67 _cogl_quaternion_print (CoglQuaternion *quarternion);
70 _cogl_quaternion_print (CoglQuaternion *quaternion)
72 g_print ("[ %6.4f (%6.4f, %6.4f, %6.4f)]\n",
80 cogl_quaternion_init (CoglQuaternion *quaternion,
86 float axis[3] = { x, y, z};
87 cogl_quaternion_init_from_angle_vector (quaternion, angle, axis);
91 cogl_quaternion_init_from_angle_vector (CoglQuaternion *quaternion,
93 const float *axis3f_in)
95 /* NB: We are using quaternions to represent an axis (a), angle (π) pair
97 * [w=cos(π/2) ( x=sin(π/2)*a.x, y=sin(π/2)*a.y, z=sin(π/2)*a.x )]
101 float sin_half_angle;
103 /* XXX: Should we make cogl_vector3_normalize have separate in and
105 axis[0] = axis3f_in[0];
106 axis[1] = axis3f_in[1];
107 axis[2] = axis3f_in[2];
108 cogl_vector3_normalize (axis);
110 half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
111 sin_half_angle = sinf (half_angle);
113 quaternion->w = cosf (half_angle);
115 quaternion->x = axis[0] * sin_half_angle;
116 quaternion->y = axis[1] * sin_half_angle;
117 quaternion->z = axis[2] * sin_half_angle;
119 cogl_quaternion_normalize (quaternion);
123 cogl_quaternion_init_identity (CoglQuaternion *quaternion)
133 cogl_quaternion_init_from_array (CoglQuaternion *quaternion,
136 quaternion->w = array[0];
137 quaternion->x = array[1];
138 quaternion->y = array[2];
139 quaternion->z = array[3];
143 cogl_quaternion_init_from_x_rotation (CoglQuaternion *quaternion,
146 /* NB: We are using quaternions to represent an axis (a), angle (π) pair
148 * [w=cos(π/2) ( x=sin(π/2)*a.x, y=sin(π/2)*a.y, z=sin(π/2)*a.x )]
150 float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
152 quaternion->w = cosf (half_angle);
154 quaternion->x = sinf (half_angle);
155 quaternion->y = 0.0f;
156 quaternion->z = 0.0f;
160 cogl_quaternion_init_from_y_rotation (CoglQuaternion *quaternion,
163 /* NB: We are using quaternions to represent an axis (a), angle (π) pair
165 * [w=cos(π/2) ( x=sin(π/2)*a.x, y=sin(π/2)*a.y, z=sin(π/2)*a.x )]
167 float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
169 quaternion->w = cosf (half_angle);
171 quaternion->x = 0.0f;
172 quaternion->y = sinf (half_angle);
173 quaternion->z = 0.0f;
177 cogl_quaternion_init_from_z_rotation (CoglQuaternion *quaternion,
180 /* NB: We are using quaternions to represent an axis (a), angle (π) pair
182 * [w=cos(π/2) ( x=sin(π/2)*a.x, y=sin(π/2)*a.y, z=sin(π/2)*a.x )]
184 float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
186 quaternion->w = cosf (half_angle);
188 quaternion->x = 0.0f;
189 quaternion->y = 0.0f;
190 quaternion->z = sinf (half_angle);
194 cogl_quaternion_init_from_euler (CoglQuaternion *quaternion,
195 const CoglEuler *euler)
197 /* NB: We are using quaternions to represent an axis (a), angle (π) pair
199 * [w=cos(π/2) ( x=sin(π/2)*a.x, y=sin(π/2)*a.y, z=sin(π/2)*a.x )]
202 sinf (euler->heading * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
204 sinf (euler->pitch * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
206 sinf (euler->roll * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
208 cosf (euler->heading * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
210 cosf (euler->pitch * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
212 cosf (euler->roll * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
215 cos_heading * cos_pitch * cos_roll +
216 sin_heading * sin_pitch * sin_roll;
219 cos_heading * sin_pitch * cos_roll +
220 sin_heading * cos_pitch * sin_roll;
222 sin_heading * cos_pitch * cos_roll -
223 cos_heading * sin_pitch * sin_roll;
225 cos_heading * cos_pitch * sin_roll -
226 sin_heading * sin_pitch * cos_roll;
230 cogl_quaternion_init_from_quaternion (CoglQuaternion *quaternion,
233 memcpy (quaternion, src, sizeof (float) * 4);
236 /* XXX: it could be nice to make something like this public... */
239 * @MATRIX: A 4x4 transformation matrix
240 * @ROW: The row of the value you want to read
241 * @COLUMN: The column of the value you want to read
243 * Reads a value from the given matrix using integers to index
246 #define COGL_MATRIX_READ(MATRIX, ROW, COLUMN) \
247 (((const float *)matrix)[COLUMN * 4 + ROW])
250 cogl_quaternion_init_from_matrix (CoglQuaternion *quaternion,
251 const CoglMatrix *matrix)
253 /* Algorithm devised by Ken Shoemake, Ref:
254 * http://campar.in.tum.de/twiki/pub/Chair/DwarfTutorial/quatut.pdf
257 /* 3D maths literature refers to the diagonal of a matrix as the
258 * "trace" of a matrix... */
259 float trace = matrix->xx + matrix->yy + matrix->zz;
264 root = sqrtf (trace + 1);
265 quaternion->w = root * 0.5f;
267 quaternion->x = (matrix->zy - matrix->yz) * root;
268 quaternion->y = (matrix->xz - matrix->zx) * root;
269 quaternion->z = (matrix->yx - matrix->xy) * root;
278 if (matrix->yy > matrix->xx)
280 if (matrix->zz > COGL_MATRIX_READ (matrix, h, h))
284 #define CASE_MACRO(i, j, k, I, J, K) \
286 root = sqrtf ((COGL_MATRIX_READ (matrix, I, I) - \
287 (COGL_MATRIX_READ (matrix, J, J) + \
288 COGL_MATRIX_READ (matrix, K, K))) + \
289 COGL_MATRIX_READ (matrix, W, W)); \
290 quaternion->i = root * 0.5f;\
292 quaternion->j = (COGL_MATRIX_READ (matrix, I, J) + \
293 COGL_MATRIX_READ (matrix, J, I)) * root; \
294 quaternion->k = (COGL_MATRIX_READ (matrix, K, I) + \
295 COGL_MATRIX_READ (matrix, I, K)) * root; \
296 quaternion->w = (COGL_MATRIX_READ (matrix, K, J) - \
297 COGL_MATRIX_READ (matrix, J, K)) * root;\
299 CASE_MACRO (x, y, z, X, Y, Z);
300 CASE_MACRO (y, z, x, Y, Z, X);
301 CASE_MACRO (z, x, y, Z, X, Y);
309 if (matrix->ww != 1.0f)
311 float s = 1.0 / sqrtf (matrix->ww);
320 cogl_quaternion_equal (gconstpointer v1, gconstpointer v2)
322 const CoglQuaternion *a = v1;
323 const CoglQuaternion *b = v2;
325 _COGL_RETURN_VAL_IF_FAIL (v1 != NULL, FALSE);
326 _COGL_RETURN_VAL_IF_FAIL (v2 != NULL, FALSE);
331 return (a->w == b->w &&
338 cogl_quaternion_copy (const CoglQuaternion *src)
342 CoglQuaternion *new = g_slice_new (CoglQuaternion);
343 memcpy (new, src, sizeof (float) * 4);
351 cogl_quaternion_free (CoglQuaternion *quaternion)
353 g_slice_free (CoglQuaternion, quaternion);
357 cogl_quaternion_get_rotation_angle (const CoglQuaternion *quaternion)
359 /* NB: We are using quaternions to represent an axis (a), angle (π) pair
361 * [w=cos(π/2) ( x=sin(π/2)*a.x, y=sin(π/2)*a.y, z=sin(π/2)*a.x )]
364 /* FIXME: clamp [-1, 1] */
365 return 2.0f * acosf (quaternion->w) * _COGL_QUATERNION_RADIANS_TO_DEGREES;
369 cogl_quaternion_get_rotation_axis (const CoglQuaternion *quaternion,
372 float sin_half_angle_sqr;
373 float one_over_sin_angle_over_2;
375 /* NB: We are using quaternions to represent an axis (a), angle (π) pair
377 * [w=cos(π/2) ( x=sin(π/2)*a.x, y=sin(π/2)*a.y, z=sin(π/2)*a.x )]
380 /* NB: sinΒ²(π) + cosΒ²(π) = 1 */
382 sin_half_angle_sqr = 1.0f - quaternion->w * quaternion->w;
384 if (sin_half_angle_sqr <= 0.0f)
386 /* Either an identity quaternion or numerical imprecision.
387 * Either way we return an arbitrary vector. */
394 /* Calculate 1 / sin(π/2) */
395 one_over_sin_angle_over_2 = 1.0f / sqrtf (sin_half_angle_sqr);
397 vector3[0] = quaternion->x * one_over_sin_angle_over_2;
398 vector3[1] = quaternion->y * one_over_sin_angle_over_2;
399 vector3[2] = quaternion->z * one_over_sin_angle_over_2;
403 cogl_quaternion_normalize (CoglQuaternion *quaternion)
405 float slen = _COGL_QUATERNION_NORM (quaternion);
406 float factor = 1.0f / sqrtf (slen);
408 quaternion->x *= factor;
409 quaternion->y *= factor;
410 quaternion->z *= factor;
412 quaternion->w *= factor;
418 cogl_quaternion_dot_product (const CoglQuaternion *a,
419 const CoglQuaternion *b)
421 return a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z;
425 cogl_quaternion_invert (CoglQuaternion *quaternion)
427 quaternion->x = -quaternion->x;
428 quaternion->y = -quaternion->y;
429 quaternion->z = -quaternion->z;
433 cogl_quaternion_multiply (CoglQuaternion *result,
434 const CoglQuaternion *a,
435 const CoglQuaternion *b)
437 result->w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z;
439 result->x = a->w * b->x + a->x * b->w + a->y * b->z - a->z * b->y;
440 result->y = a->w * b->y + a->y * b->w + a->z * b->x - a->x * b->z;
441 result->z = a->w * b->z + a->z * b->w + a->x * b->y - a->y * b->x;
445 cogl_quaternion_pow (CoglQuaternion *quaternion, float exponent)
448 float new_half_angle;
451 /* Try and identify and nop identity quaternions to avoid
452 * dividing by zero */
453 if (fabs (quaternion->w) > 0.9999f)
456 /* NB: We are using quaternions to represent an axis (a), angle (π) pair
458 * [w=cos(π/2) ( x=sin(π/2)*a.x, y=sin(π/2)*a.y, z=sin(π/2)*a.x )]
461 /* FIXME: clamp [-1, 1] */
462 /* Extract π/2 from w */
463 half_angle = acosf (quaternion->w);
465 /* Compute the new π/2 */
466 new_half_angle = half_angle * exponent;
468 /* Compute the new w value */
469 quaternion->w = cosf (new_half_angle);
471 /* And new xyz values */
472 factor = sinf (new_half_angle) / sinf (half_angle);
473 quaternion->x *= factor;
474 quaternion->y *= factor;
475 quaternion->z *= factor;
479 cogl_quaternion_slerp (CoglQuaternion *result,
480 const CoglQuaternion *a,
481 const CoglQuaternion *b,
484 float cos_difference;
492 _COGL_RETURN_IF_FAIL (t >=0 && t <= 1.0f);
505 /* compute the cosine of the angle between the two given quaternions */
506 cos_difference = cogl_quaternion_dot_product (a, b);
508 /* If negative, use -b. Two quaternions q and -q represent the same angle but
509 * may produce a different slerp. We choose b or -b to rotate using the acute
512 if (cos_difference < 0.0f)
518 cos_difference = -cos_difference;
528 /* If we have two unit quaternions the dot should be <= 1.0 */
529 g_assert (cos_difference < 1.1f);
532 /* Determine the interpolation factors for each quaternion, simply using
533 * linear interpolation for quaternions that are nearly exactly the same.
534 * (this will avoid divisions by zero)
537 if (cos_difference > 0.9999f)
542 /* XXX: should we also normalize() at the end in this case? */
546 /* Calculate the sin of the angle between the two quaternions using the
547 * trig identity: sinΒ²(π) + cosΒ²(π) = 1
549 float sin_difference = sqrtf (1.0f - cos_difference * cos_difference);
551 float difference = atan2f (sin_difference, cos_difference);
552 float one_over_sin_difference = 1.0f / sin_difference;
553 fa = sinf ((1.0f - t) * difference) * one_over_sin_difference;
554 fb = sinf (t * difference) * one_over_sin_difference;
557 /* Finally interpolate the two quaternions */
559 result->x = fa * a->x + fb * qb_x;
560 result->y = fa * a->y + fb * qb_y;
561 result->z = fa * a->z + fb * qb_z;
562 result->w = fa * a->w + fb * qb_w;
566 cogl_quaternion_nlerp (CoglQuaternion *result,
567 const CoglQuaternion *a,
568 const CoglQuaternion *b,
571 float cos_difference;
579 _COGL_RETURN_IF_FAIL (t >=0 && t <= 1.0f);
592 /* compute the cosine of the angle between the two given quaternions */
593 cos_difference = cogl_quaternion_dot_product (a, b);
595 /* If negative, use -b. Two quaternions q and -q represent the same angle but
596 * may produce a different slerp. We choose b or -b to rotate using the acute
599 if (cos_difference < 0.0f)
605 cos_difference = -cos_difference;
615 /* If we have two unit quaternions the dot should be <= 1.0 */
616 g_assert (cos_difference < 1.1f);
621 result->x = fa * a->x + fb * qb_x;
622 result->y = fa * a->y + fb * qb_y;
623 result->z = fa * a->z + fb * qb_z;
624 result->w = fa * a->w + fb * qb_w;
626 cogl_quaternion_normalize (result);
630 * cogl_quaternion_squad:
634 cogl_quaternion_squad (CoglQuaternion *result,
635 const CoglQuaternion *prev,
636 const CoglQuaternion *a,
637 const CoglQuaternion *b,
638 const CoglQuaternion *next,
641 CoglQuaternion slerp0;
642 CoglQuaternion slerp1;
644 cogl_quaternion_slerp (&slerp0, a, b, t);
645 cogl_quaternion_slerp (&slerp1, prev, next, t);
646 cogl_quaternion_slerp (result, &slerp0, &slerp1, 2.0f * t * (1.0f - t));
649 const CoglQuaternion *
650 cogl_get_static_identity_quaternion (void)
652 return &identity_quaternion;
655 const CoglQuaternion *
656 cogl_get_static_zero_quaternion (void)
658 return &zero_quaternion;