4
* A Low Level GPU Graphics and Utilities API
6
* Copyright (C) 2010 Intel Corporation.
8
* Permission is hereby granted, free of charge, to any person
9
* obtaining a copy of this software and associated documentation
10
* files (the "Software"), to deal in the Software without
11
* restriction, including without limitation the rights to use, copy,
12
* modify, merge, publish, distribute, sublicense, and/or sell copies
13
* of the Software, and to permit persons to whom the Software is
14
* furnished to do so, subject to the following conditions:
16
* The above copyright notice and this permission notice shall be
17
* included in all copies or substantial portions of the Software.
19
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
20
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
21
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
22
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
23
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
24
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
25
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
29
* Robert Bragg <robert@linux.intel.com>
31
* Various references relating to quaternions:
33
* http://www.cs.caltech.edu/courses/cs171/quatut.pdf
34
* http://mathworld.wolfram.com/Quaternion.html
35
* http://www.gamedev.net/reference/articles/article1095.asp
36
* http://www.cprogramming.com/tutorial/3d/quaternions.html
37
* http://www.isner.com/tutorials/quatSpells/quaternion_spells_12.htm
38
* http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56
39
* 3D Maths Primer for Graphics and Game Development ISBN-10: 1556229119
46
#include <cogl-util.h>
47
#include <cogl-quaternion.h>
48
#include <cogl-quaternion-private.h>
49
#include <cogl-matrix.h>
50
#include <cogl-vector.h>
51
#include <cogl-euler.h>
52
#include "cogl-gtype-private.h"
57
#define FLOAT_EPSILON 1e-03
59
COGL_GTYPE_DEFINE_BOXED (Quaternion, quaternion,
61
cogl_quaternion_free);
63
static CoglQuaternion zero_quaternion =
68
static CoglQuaternion identity_quaternion =
73
/* This function is just here to be called from GDB so we don't really
74
want to put a declaration in a header and we just add it here to
77
_cogl_quaternion_print (CoglQuaternion *quarternion);
80
_cogl_quaternion_print (CoglQuaternion *quaternion)
82
g_print ("[ %6.4f (%6.4f, %6.4f, %6.4f)]\n",
90
cogl_quaternion_init (CoglQuaternion *quaternion,
96
float axis[3] = { x, y, z};
97
cogl_quaternion_init_from_angle_vector (quaternion, angle, axis);
101
cogl_quaternion_init_from_angle_vector (CoglQuaternion *quaternion,
103
const float *axis3f_in)
105
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
107
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
111
float sin_half_angle;
113
/* XXX: Should we make cogl_vector3_normalize have separate in and
115
axis[0] = axis3f_in[0];
116
axis[1] = axis3f_in[1];
117
axis[2] = axis3f_in[2];
118
cogl_vector3_normalize (axis);
120
half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
121
sin_half_angle = sinf (half_angle);
123
quaternion->w = cosf (half_angle);
125
quaternion->x = axis[0] * sin_half_angle;
126
quaternion->y = axis[1] * sin_half_angle;
127
quaternion->z = axis[2] * sin_half_angle;
129
cogl_quaternion_normalize (quaternion);
133
cogl_quaternion_init_identity (CoglQuaternion *quaternion)
143
cogl_quaternion_init_from_array (CoglQuaternion *quaternion,
146
quaternion->w = array[0];
147
quaternion->x = array[1];
148
quaternion->y = array[2];
149
quaternion->z = array[3];
153
cogl_quaternion_init_from_x_rotation (CoglQuaternion *quaternion,
156
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
158
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
160
float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
162
quaternion->w = cosf (half_angle);
164
quaternion->x = sinf (half_angle);
165
quaternion->y = 0.0f;
166
quaternion->z = 0.0f;
170
cogl_quaternion_init_from_y_rotation (CoglQuaternion *quaternion,
173
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
175
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
177
float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
179
quaternion->w = cosf (half_angle);
181
quaternion->x = 0.0f;
182
quaternion->y = sinf (half_angle);
183
quaternion->z = 0.0f;
187
cogl_quaternion_init_from_z_rotation (CoglQuaternion *quaternion,
190
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
192
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
194
float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
196
quaternion->w = cosf (half_angle);
198
quaternion->x = 0.0f;
199
quaternion->y = 0.0f;
200
quaternion->z = sinf (half_angle);
204
cogl_quaternion_init_from_euler (CoglQuaternion *quaternion,
205
const CoglEuler *euler)
207
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
209
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
212
sinf (euler->heading * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
214
sinf (euler->pitch * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
216
sinf (euler->roll * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
218
cosf (euler->heading * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
220
cosf (euler->pitch * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
222
cosf (euler->roll * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
225
cos_heading * cos_pitch * cos_roll +
226
sin_heading * sin_pitch * sin_roll;
229
cos_heading * sin_pitch * cos_roll +
230
sin_heading * cos_pitch * sin_roll;
232
sin_heading * cos_pitch * cos_roll -
233
cos_heading * sin_pitch * sin_roll;
235
cos_heading * cos_pitch * sin_roll -
236
sin_heading * sin_pitch * cos_roll;
240
cogl_quaternion_init_from_quaternion (CoglQuaternion *quaternion,
243
memcpy (quaternion, src, sizeof (float) * 4);
246
/* XXX: it could be nice to make something like this public... */
249
* @MATRIX: A 4x4 transformation matrix
250
* @ROW: The row of the value you want to read
251
* @COLUMN: The column of the value you want to read
253
* Reads a value from the given matrix using integers to index
256
#define COGL_MATRIX_READ(MATRIX, ROW, COLUMN) \
257
(((const float *)matrix)[COLUMN * 4 + ROW])
260
cogl_quaternion_init_from_matrix (CoglQuaternion *quaternion,
261
const CoglMatrix *matrix)
263
/* Algorithm devised by Ken Shoemake, Ref:
264
* http://campar.in.tum.de/twiki/pub/Chair/DwarfTutorial/quatut.pdf
267
/* 3D maths literature refers to the diagonal of a matrix as the
268
* "trace" of a matrix... */
269
float trace = matrix->xx + matrix->yy + matrix->zz;
274
root = sqrtf (trace + 1);
275
quaternion->w = root * 0.5f;
277
quaternion->x = (matrix->zy - matrix->yz) * root;
278
quaternion->y = (matrix->xz - matrix->zx) * root;
279
quaternion->z = (matrix->yx - matrix->xy) * root;
288
if (matrix->yy > matrix->xx)
290
if (matrix->zz > COGL_MATRIX_READ (matrix, h, h))
294
#define CASE_MACRO(i, j, k, I, J, K) \
296
root = sqrtf ((COGL_MATRIX_READ (matrix, I, I) - \
297
(COGL_MATRIX_READ (matrix, J, J) + \
298
COGL_MATRIX_READ (matrix, K, K))) + \
299
COGL_MATRIX_READ (matrix, W, W)); \
300
quaternion->i = root * 0.5f;\
302
quaternion->j = (COGL_MATRIX_READ (matrix, I, J) + \
303
COGL_MATRIX_READ (matrix, J, I)) * root; \
304
quaternion->k = (COGL_MATRIX_READ (matrix, K, I) + \
305
COGL_MATRIX_READ (matrix, I, K)) * root; \
306
quaternion->w = (COGL_MATRIX_READ (matrix, K, J) - \
307
COGL_MATRIX_READ (matrix, J, K)) * root;\
309
CASE_MACRO (x, y, z, X, Y, Z);
310
CASE_MACRO (y, z, x, Y, Z, X);
311
CASE_MACRO (z, x, y, Z, X, Y);
319
if (matrix->ww != 1.0f)
321
float s = 1.0 / sqrtf (matrix->ww);
330
cogl_quaternion_equal (const void *v1, const void *v2)
332
const CoglQuaternion *a = v1;
333
const CoglQuaternion *b = v2;
335
_COGL_RETURN_VAL_IF_FAIL (v1 != NULL, FALSE);
336
_COGL_RETURN_VAL_IF_FAIL (v2 != NULL, FALSE);
341
return (a->w == b->w &&
348
cogl_quaternion_copy (const CoglQuaternion *src)
352
CoglQuaternion *new = g_slice_new (CoglQuaternion);
353
memcpy (new, src, sizeof (float) * 4);
361
cogl_quaternion_free (CoglQuaternion *quaternion)
363
g_slice_free (CoglQuaternion, quaternion);
367
cogl_quaternion_get_rotation_angle (const CoglQuaternion *quaternion)
369
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
371
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
374
/* FIXME: clamp [-1, 1] */
375
return 2.0f * acosf (quaternion->w) * _COGL_QUATERNION_RADIANS_TO_DEGREES;
379
cogl_quaternion_get_rotation_axis (const CoglQuaternion *quaternion,
382
float sin_half_angle_sqr;
383
float one_over_sin_angle_over_2;
385
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
387
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
390
/* NB: sin²(𝜃) + cos²(𝜃) = 1 */
392
sin_half_angle_sqr = 1.0f - quaternion->w * quaternion->w;
394
if (sin_half_angle_sqr <= 0.0f)
396
/* Either an identity quaternion or numerical imprecision.
397
* Either way we return an arbitrary vector. */
404
/* Calculate 1 / sin(𝜃/2) */
405
one_over_sin_angle_over_2 = 1.0f / sqrtf (sin_half_angle_sqr);
407
vector3[0] = quaternion->x * one_over_sin_angle_over_2;
408
vector3[1] = quaternion->y * one_over_sin_angle_over_2;
409
vector3[2] = quaternion->z * one_over_sin_angle_over_2;
413
cogl_quaternion_normalize (CoglQuaternion *quaternion)
415
float slen = _COGL_QUATERNION_NORM (quaternion);
416
float factor = 1.0f / sqrtf (slen);
418
quaternion->x *= factor;
419
quaternion->y *= factor;
420
quaternion->z *= factor;
422
quaternion->w *= factor;
428
cogl_quaternion_dot_product (const CoglQuaternion *a,
429
const CoglQuaternion *b)
431
return a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z;
435
cogl_quaternion_invert (CoglQuaternion *quaternion)
437
quaternion->x = -quaternion->x;
438
quaternion->y = -quaternion->y;
439
quaternion->z = -quaternion->z;
443
cogl_quaternion_multiply (CoglQuaternion *result,
444
const CoglQuaternion *a,
445
const CoglQuaternion *b)
452
_COGL_RETURN_IF_FAIL (b != result);
454
result->w = w * b->w - x * b->x - y * b->y - z * b->z;
456
result->x = w * b->x + x * b->w + y * b->z - z * b->y;
457
result->y = w * b->y + y * b->w + z * b->x - x * b->z;
458
result->z = w * b->z + z * b->w + x * b->y - y * b->x;
462
cogl_quaternion_pow (CoglQuaternion *quaternion, float exponent)
465
float new_half_angle;
468
/* Try and identify and nop identity quaternions to avoid
469
* dividing by zero */
470
if (fabs (quaternion->w) > 0.9999f)
473
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
475
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
478
/* FIXME: clamp [-1, 1] */
479
/* Extract 𝜃/2 from w */
480
half_angle = acosf (quaternion->w);
482
/* Compute the new 𝜃/2 */
483
new_half_angle = half_angle * exponent;
485
/* Compute the new w value */
486
quaternion->w = cosf (new_half_angle);
488
/* And new xyz values */
489
factor = sinf (new_half_angle) / sinf (half_angle);
490
quaternion->x *= factor;
491
quaternion->y *= factor;
492
quaternion->z *= factor;
496
cogl_quaternion_slerp (CoglQuaternion *result,
497
const CoglQuaternion *a,
498
const CoglQuaternion *b,
501
float cos_difference;
509
_COGL_RETURN_IF_FAIL (t >=0 && t <= 1.0f);
522
/* compute the cosine of the angle between the two given quaternions */
523
cos_difference = cogl_quaternion_dot_product (a, b);
525
/* If negative, use -b. Two quaternions q and -q represent the same angle but
526
* may produce a different slerp. We choose b or -b to rotate using the acute
529
if (cos_difference < 0.0f)
535
cos_difference = -cos_difference;
545
/* If we have two unit quaternions the dot should be <= 1.0 */
546
g_assert (cos_difference < 1.1f);
549
/* Determine the interpolation factors for each quaternion, simply using
550
* linear interpolation for quaternions that are nearly exactly the same.
551
* (this will avoid divisions by zero)
554
if (cos_difference > 0.9999f)
559
/* XXX: should we also normalize() at the end in this case? */
563
/* Calculate the sin of the angle between the two quaternions using the
564
* trig identity: sin²(𝜃) + cos²(𝜃) = 1
566
float sin_difference = sqrtf (1.0f - cos_difference * cos_difference);
568
float difference = atan2f (sin_difference, cos_difference);
569
float one_over_sin_difference = 1.0f / sin_difference;
570
fa = sinf ((1.0f - t) * difference) * one_over_sin_difference;
571
fb = sinf (t * difference) * one_over_sin_difference;
574
/* Finally interpolate the two quaternions */
576
result->x = fa * a->x + fb * qb_x;
577
result->y = fa * a->y + fb * qb_y;
578
result->z = fa * a->z + fb * qb_z;
579
result->w = fa * a->w + fb * qb_w;
583
cogl_quaternion_nlerp (CoglQuaternion *result,
584
const CoglQuaternion *a,
585
const CoglQuaternion *b,
588
float cos_difference;
596
_COGL_RETURN_IF_FAIL (t >=0 && t <= 1.0f);
609
/* compute the cosine of the angle between the two given quaternions */
610
cos_difference = cogl_quaternion_dot_product (a, b);
612
/* If negative, use -b. Two quaternions q and -q represent the same angle but
613
* may produce a different slerp. We choose b or -b to rotate using the acute
616
if (cos_difference < 0.0f)
622
cos_difference = -cos_difference;
632
/* If we have two unit quaternions the dot should be <= 1.0 */
633
g_assert (cos_difference < 1.1f);
638
result->x = fa * a->x + fb * qb_x;
639
result->y = fa * a->y + fb * qb_y;
640
result->z = fa * a->z + fb * qb_z;
641
result->w = fa * a->w + fb * qb_w;
643
cogl_quaternion_normalize (result);
647
cogl_quaternion_squad (CoglQuaternion *result,
648
const CoglQuaternion *prev,
649
const CoglQuaternion *a,
650
const CoglQuaternion *b,
651
const CoglQuaternion *next,
654
CoglQuaternion slerp0;
655
CoglQuaternion slerp1;
657
cogl_quaternion_slerp (&slerp0, a, b, t);
658
cogl_quaternion_slerp (&slerp1, prev, next, t);
659
cogl_quaternion_slerp (result, &slerp0, &slerp1, 2.0f * t * (1.0f - t));
662
const CoglQuaternion *
663
cogl_get_static_identity_quaternion (void)
665
return &identity_quaternion;
668
const CoglQuaternion *
669
cogl_get_static_zero_quaternion (void)
671
return &zero_quaternion;
added
removed
cogl/cogl/cogl-quaternion.c
