cogl/cogl-quaternion.c

   1 /*
   2  * Cogl
   3  *
   4  * An object oriented GL/GLES Abstraction/Utility Layer
   5  *
   6  * Copyright (C) 2010 Intel Corporation.
   7  *
   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.
  12  *
  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.
  17  *
  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.
  22  *
  23  * Authors:
  24  *   Robert Bragg <robert@linux.intel.com>
  25  *
  26  * Various references relating to quaternions:
  27  *
  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
  35  */
  36
  37 #ifdef HAVE_CONFIG_H
  38 #include "config.h"
  39 #endif
  40
  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>
  47
  48 #include <string.h>
  49 #include <math.h>
  50
  51 #define FLOAT_EPSILON 1e-03
  52
  53 static CoglQuaternion zero_quaternion =
  54 {
  55   0.0,  0.0, 0.0, 0.0,
  56 };
  57
  58 static CoglQuaternion identity_quaternion =
  59 {
  60   1.0,  0.0, 0.0, 0.0,
  61 };
  62
  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
  65    avoid a warning */
  66 void
  67 _cogl_quaternion_print (CoglQuaternion *quarternion);
  68
  69 void
  70 _cogl_quaternion_print (CoglQuaternion *quaternion)
  71 {
  72   g_print ("[ %6.4f (%6.4f, %6.4f, %6.4f)]\n",
  73            quaternion->w,
  74            quaternion->x,
  75            quaternion->y,
  76            quaternion->z);
  77 }
  78
  79 void
  80 cogl_quaternion_init (CoglQuaternion *quaternion,
  81                       float angle,
  82                       float x,
  83                       float y,
  84                       float z)
  85 {
  86   float axis[3] = { x, y, z};
  87   cogl_quaternion_init_from_angle_vector (quaternion, angle, axis);
  88 }
  89
  90 void
  91 cogl_quaternion_init_from_angle_vector (CoglQuaternion *quaternion,
  92                                         float angle,
  93                                         const float *axis3f_in)
  94 {
  95   /* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
  96    * in this form:
  97    * [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
  98    */
  99   float axis[3];
 100   float half_angle;
 101   float sin_half_angle;
 102
 103   /* XXX: Should we make cogl_vector3_normalize have separate in and
 104    * out args? */
 105   axis[0] = axis3f_in[0];
 106   axis[1] = axis3f_in[1];
 107   axis[2] = axis3f_in[2];
 108   cogl_vector3_normalize (axis);
 109
 110   half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
 111   sin_half_angle = sinf (half_angle);
 112
 113   quaternion->w = cosf (half_angle);
 114
 115   quaternion->x = axis[0] * sin_half_angle;
 116   quaternion->y = axis[1] * sin_half_angle;
 117   quaternion->z = axis[2] * sin_half_angle;
 118
 119   cogl_quaternion_normalize (quaternion);
 120 }
 121
 122 void
 123 cogl_quaternion_init_identity (CoglQuaternion *quaternion)
 124 {
 125   quaternion->w = 1.0;
 126
 127   quaternion->x = 0.0;
 128   quaternion->y = 0.0;
 129   quaternion->z = 0.0;
 130 }
 131
 132 void
 133 cogl_quaternion_init_from_array (CoglQuaternion *quaternion,
 134                                  const float *array)
 135 {
 136   quaternion->w = array[0];
 137   quaternion->x = array[1];
 138   quaternion->y = array[2];
 139   quaternion->z = array[3];
 140 }
 141
 142 void
 143 cogl_quaternion_init_from_x_rotation (CoglQuaternion *quaternion,
 144                                       float angle)
 145 {
 146   /* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
 147    * in this form:
 148    * [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
 149    */
 150   float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
 151
 152   quaternion->w = cosf (half_angle);
 153
 154   quaternion->x = sinf (half_angle);
 155   quaternion->y = 0.0f;
 156   quaternion->z = 0.0f;
 157 }
 158
 159 void
 160 cogl_quaternion_init_from_y_rotation (CoglQuaternion *quaternion,
 161                                       float angle)
 162 {
 163   /* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
 164    * in this form:
 165    * [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
 166    */
 167   float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
 168
 169   quaternion->w = cosf (half_angle);
 170
 171   quaternion->x = 0.0f;
 172   quaternion->y = sinf (half_angle);
 173   quaternion->z = 0.0f;
 174 }
 175
 176 void
 177 cogl_quaternion_init_from_z_rotation (CoglQuaternion *quaternion,
 178                                       float angle)
 179 {
 180   /* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
 181    * in this form:
 182    * [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
 183    */
 184   float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
 185
 186   quaternion->w = cosf (half_angle);
 187
 188   quaternion->x = 0.0f;
 189   quaternion->y = 0.0f;
 190   quaternion->z = sinf (half_angle);
 191 }
 192
 193 void
 194 cogl_quaternion_init_from_euler (CoglQuaternion *quaternion,
 195                                  const CoglEuler *euler)
 196 {
 197   /* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
 198    * in this form:
 199    * [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
 200    */
 201   float sin_heading =
 202     sinf (euler->heading * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
 203   float sin_pitch =
 204     sinf (euler->pitch * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
 205   float sin_roll =
 206     sinf (euler->roll * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
 207   float cos_heading =
 208     cosf (euler->heading * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
 209   float cos_pitch =
 210     cosf (euler->pitch * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
 211   float cos_roll =
 212     cosf (euler->roll * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
 213
 214   quaternion->w =
 215     cos_heading * cos_pitch * cos_roll +
 216     sin_heading * sin_pitch * sin_roll;
 217
 218   quaternion->x =
 219     cos_heading * sin_pitch * cos_roll +
 220     sin_heading * cos_pitch * sin_roll;
 221   quaternion->y =
 222     sin_heading * cos_pitch * cos_roll -
 223     cos_heading * sin_pitch * sin_roll;
 224   quaternion->z =
 225     cos_heading * cos_pitch * sin_roll -
 226     sin_heading * sin_pitch * cos_roll;
 227 }
 228
 229 void
 230 cogl_quaternion_init_from_quaternion (CoglQuaternion *quaternion,
 231                                       CoglQuaternion *src)
 232 {
 233   memcpy (quaternion, src, sizeof (float) * 4);
 234 }
 235
 236 /* XXX: it could be nice to make something like this public... */
 237 /*
 238  * COGL_MATRIX_READ:
 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
 242  *
 243  * Reads a value from the given matrix using integers to index
 244  * into the matrix.
 245  */
 246 #define COGL_MATRIX_READ(MATRIX, ROW, COLUMN) \
 247   (((const float *)matrix)[COLUMN * 4 + ROW])
 248
 249 void
 250 cogl_quaternion_init_from_matrix (CoglQuaternion *quaternion,
 251                                   const CoglMatrix *matrix)
 252 {
 253   /* Algorithm devised by Ken Shoemake, Ref:
 254    * http://campar.in.tum.de/twiki/pub/Chair/DwarfTutorial/quatut.pdf
 255    */
 256
 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;
 260   float root;
 261
 262   if (trace > 0.0f)
 263     {
 264       root = sqrtf (trace + 1);
 265       quaternion->w = root * 0.5f;
 266       root = 0.5f / root;
 267       quaternion->x = (matrix->zy - matrix->yz) * root;
 268       quaternion->y = (matrix->xz - matrix->zx) * root;
 269       quaternion->z = (matrix->yx - matrix->xy) * root;
 270     }
 271   else
 272     {
 273 #define X 0
 274 #define Y 1
 275 #define Z 2
 276 #define W 3
 277       int h = X;
 278       if (matrix->yy > matrix->xx)
 279         h = Y;
 280       if (matrix->zz > COGL_MATRIX_READ (matrix, h, h))
 281         h = Z;
 282       switch (h)
 283         {
 284 #define CASE_MACRO(i, j, k, I, J, K) \
 285         case I: \
 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;\
 291           root = 0.5f / root;\
 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;\
 298           break
 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);
 302 #undef CASE_MACRO
 303 #undef X
 304 #undef Y
 305 #undef Z
 306         }
 307     }
 308
 309   if (matrix->ww != 1.0f)
 310     {
 311       float s = 1.0 / sqrtf (matrix->ww);
 312       quaternion->w *= s;
 313       quaternion->x *= s;
 314       quaternion->y *= s;
 315       quaternion->z *= s;
 316     }
 317 }
 318
 319 gboolean
 320 cogl_quaternion_equal (gconstpointer v1, gconstpointer v2)
 321 {
 322   const CoglQuaternion *a = v1;
 323   const CoglQuaternion *b = v2;
 324
 325   _COGL_RETURN_VAL_IF_FAIL (v1 != NULL, FALSE);
 326   _COGL_RETURN_VAL_IF_FAIL (v2 != NULL, FALSE);
 327
 328   if (v1 == v2)
 329     return TRUE;
 330
 331   return (a->w == b->w &&
 332           a->x == b->x &&
 333           a->y == b->y &&
 334           a->z == b->z);
 335 }
 336
 337 CoglQuaternion *
 338 cogl_quaternion_copy (const CoglQuaternion *src)
 339 {
 340   if (G_LIKELY (src))
 341     {
 342       CoglQuaternion *new = g_slice_new (CoglQuaternion);
 343       memcpy (new, src, sizeof (float) * 4);
 344       return new;
 345     }
 346   else
 347     return NULL;
 348 }
 349
 350 void
 351 cogl_quaternion_free (CoglQuaternion *quaternion)
 352 {
 353   g_slice_free (CoglQuaternion, quaternion);
 354 }
 355
 356 float
 357 cogl_quaternion_get_rotation_angle (const CoglQuaternion *quaternion)
 358 {
 359   /* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
 360    * in this form:
 361    * [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
 362    */
 363
 364   /* FIXME: clamp [-1, 1] */
 365   return 2.0f * acosf (quaternion->w) * _COGL_QUATERNION_RADIANS_TO_DEGREES;
 366 }
 367
 368 void
 369 cogl_quaternion_get_rotation_axis (const CoglQuaternion *quaternion,
 370                                    float *vector3)
 371 {
 372   float sin_half_angle_sqr;
 373   float one_over_sin_angle_over_2;
 374
 375   /* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
 376    * in this form:
 377    * [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
 378    */
 379
 380   /* NB: sin²(𝜃) + cos²(𝜃) = 1 */
 381
 382   sin_half_angle_sqr = 1.0f - quaternion->w * quaternion->w;
 383
 384   if (sin_half_angle_sqr <= 0.0f)
 385     {
 386       /* Either an identity quaternion or numerical imprecision.
 387        * Either way we return an arbitrary vector. */
 388       vector3[0] = 1;
 389       vector3[1] = 0;
 390       vector3[2] = 0;
 391       return;
 392     }
 393
 394   /* Calculate 1 / sin(𝜃/2) */
 395   one_over_sin_angle_over_2 = 1.0f / sqrtf (sin_half_angle_sqr);
 396
 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;
 400 }
 401
 402 void
 403 cogl_quaternion_normalize (CoglQuaternion *quaternion)
 404 {
 405   float slen = _COGL_QUATERNION_NORM (quaternion);
 406   float factor = 1.0f / sqrtf (slen);
 407
 408   quaternion->x *= factor;
 409   quaternion->y *= factor;
 410   quaternion->z *= factor;
 411
 412   quaternion->w *= factor;
 413
 414   return;
 415 }
 416
 417 float
 418 cogl_quaternion_dot_product (const CoglQuaternion *a,
 419                              const CoglQuaternion *b)
 420 {
 421   return a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z;
 422 }
 423
 424 void
 425 cogl_quaternion_invert (CoglQuaternion *quaternion)
 426 {
 427   quaternion->x = -quaternion->x;
 428   quaternion->y = -quaternion->y;
 429   quaternion->z = -quaternion->z;
 430 }
 431
 432 void
 433 cogl_quaternion_multiply (CoglQuaternion *result,
 434                           const CoglQuaternion *a,
 435                           const CoglQuaternion *b)
 436 {
 437   result->w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z;
 438
 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;
 442 }
 443
 444 void
 445 cogl_quaternion_pow (CoglQuaternion *quaternion, float exponent)
 446 {
 447   float half_angle;
 448   float new_half_angle;
 449   float factor;
 450
 451   /* Try and identify and nop identity quaternions to avoid
 452    * dividing by zero */
 453   if (fabs (quaternion->w) > 0.9999f)
 454     return;
 455
 456   /* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
 457    * in this form:
 458    * [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
 459    */
 460
 461   /* FIXME: clamp [-1, 1] */
 462   /* Extract 𝜃/2 from w */
 463   half_angle = acosf (quaternion->w);
 464
 465   /* Compute the new 𝜃/2 */
 466   new_half_angle = half_angle * exponent;
 467
 468   /* Compute the new w value */
 469   quaternion->w = cosf (new_half_angle);
 470
 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;
 476 }
 477
 478 void
 479 cogl_quaternion_slerp (CoglQuaternion *result,
 480                        const CoglQuaternion *a,
 481                        const CoglQuaternion *b,
 482                        float t)
 483 {
 484   float cos_difference;
 485   float qb_w;
 486   float qb_x;
 487   float qb_y;
 488   float qb_z;
 489   float fa;
 490   float fb;
 491
 492   _COGL_RETURN_IF_FAIL (t >=0 && t <= 1.0f);
 493
 494   if (t == 0)
 495     {
 496       *result = *a;
 497       return;
 498     }
 499   else if (t == 1)
 500     {
 501       *result = *b;
 502       return;
 503     }
 504
 505   /* compute the cosine of the angle between the two given quaternions */
 506   cos_difference = cogl_quaternion_dot_product (a, b);
 507
 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
 510    * angle.
 511    */
 512   if (cos_difference < 0.0f)
 513     {
 514       qb_w = -b->w;
 515       qb_x = -b->x;
 516       qb_y = -b->y;
 517       qb_z = -b->z;
 518       cos_difference = -cos_difference;
 519     }
 520   else
 521     {
 522       qb_w = b->w;
 523       qb_x = b->x;
 524       qb_y = b->y;
 525       qb_z = b->z;
 526     }
 527
 528   /* If we have two unit quaternions the dot should be <= 1.0 */
 529   g_assert (cos_difference < 1.1f);
 530
 531
 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)
 535    */
 536
 537   if (cos_difference > 0.9999f)
 538     {
 539       fa = 1.0f - t;
 540       fb = t;
 541
 542       /* XXX: should we also normalize() at the end in this case? */
 543     }
 544   else
 545     {
 546       /* Calculate the sin of the angle between the two quaternions using the
 547        * trig identity: sin²(𝜃) + cos²(𝜃) = 1
 548        */
 549       float sin_difference =  sqrtf (1.0f - cos_difference * cos_difference);
 550
 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;
 555     }
 556
 557   /* Finally interpolate the two quaternions */
 558
 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;
 563 }
 564
 565 void
 566 cogl_quaternion_nlerp (CoglQuaternion *result,
 567                        const CoglQuaternion *a,
 568                        const CoglQuaternion *b,
 569                        float t)
 570 {
 571   float cos_difference;
 572   float qb_w;
 573   float qb_x;
 574   float qb_y;
 575   float qb_z;
 576   float fa;
 577   float fb;
 578
 579   _COGL_RETURN_IF_FAIL (t >=0 && t <= 1.0f);
 580
 581   if (t == 0)
 582     {
 583       *result = *a;
 584       return;
 585     }
 586   else if (t == 1)
 587     {
 588       *result = *b;
 589       return;
 590     }
 591
 592   /* compute the cosine of the angle between the two given quaternions */
 593   cos_difference = cogl_quaternion_dot_product (a, b);
 594
 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
 597    * angle.
 598    */
 599   if (cos_difference < 0.0f)
 600     {
 601       qb_w = -b->w;
 602       qb_x = -b->x;
 603       qb_y = -b->y;
 604       qb_z = -b->z;
 605       cos_difference = -cos_difference;
 606     }
 607   else
 608     {
 609       qb_w = b->w;
 610       qb_x = b->x;
 611       qb_y = b->y;
 612       qb_z = b->z;
 613     }
 614
 615   /* If we have two unit quaternions the dot should be <= 1.0 */
 616   g_assert (cos_difference < 1.1f);
 617
 618   fa = 1.0f - t;
 619   fb = t;
 620
 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;
 625
 626   cogl_quaternion_normalize (result);
 627 }
 628
 629 /**
 630  * cogl_quaternion_squad:
 631  *
 632  */
 633 void
 634 cogl_quaternion_squad (CoglQuaternion *result,
 635                        const CoglQuaternion *prev,
 636                        const CoglQuaternion *a,
 637                        const CoglQuaternion *b,
 638                        const CoglQuaternion *next,
 639                        float t)
 640 {
 641   CoglQuaternion slerp0;
 642   CoglQuaternion slerp1;
 643
 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));
 647 }
 648
 649 const CoglQuaternion *
 650 cogl_get_static_identity_quaternion (void)
 651 {
 652   return &identity_quaternion;
 653 }
 654
 655 const CoglQuaternion *
 656 cogl_get_static_zero_quaternion (void)
 657 {
 658   return &zero_quaternion;
 659 }
 660