mutter/cogl/cogl-quaternion.c
Robert Bragg 961832164b quaternions: Allow multiplication into 'a' arg
When multiplying two quaternions, we now implicitly copy the components
of the 'a' argument so that the result can be reliably written back to
the 'a' argument quaternion without conflicting with the multiplication
itself. This is consistent with the cogl_matrix_multiply() api which
allows the 'result' and 'a' arguments to point to the same matrix. In
debug builds Cogl will assert that the 'b' and 'result' arguments don't
point to the same quaternion.

Reviewed-by: Neil Roberts <neil@linux.intel.com>

(cherry picked from commit 207527313a8957789390069e84189254cf41e88f)
2012-08-06 18:51:32 +01:00

668 lines
17 KiB
C

/*
* Cogl
*
* An object oriented GL/GLES Abstraction/Utility Layer
*
* Copyright (C) 2010 Intel Corporation.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the
* Free Software Foundation, Inc., 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
* Authors:
* Robert Bragg <robert@linux.intel.com>
*
* Various references relating to quaternions:
*
* http://www.cs.caltech.edu/courses/cs171/quatut.pdf
* http://mathworld.wolfram.com/Quaternion.html
* http://www.gamedev.net/reference/articles/article1095.asp
* http://www.cprogramming.com/tutorial/3d/quaternions.html
* http://www.isner.com/tutorials/quatSpells/quaternion_spells_12.htm
* http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56
* 3D Maths Primer for Graphics and Game Development ISBN-10: 1556229119
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <cogl-util.h>
#include <cogl-quaternion.h>
#include <cogl-quaternion-private.h>
#include <cogl-matrix.h>
#include <cogl-vector.h>
#include <cogl-euler.h>
#include <string.h>
#include <math.h>
#define FLOAT_EPSILON 1e-03
static CoglQuaternion zero_quaternion =
{
0.0, 0.0, 0.0, 0.0,
};
static CoglQuaternion identity_quaternion =
{
1.0, 0.0, 0.0, 0.0,
};
/* This function is just here to be called from GDB so we don't really
want to put a declaration in a header and we just add it here to
avoid a warning */
void
_cogl_quaternion_print (CoglQuaternion *quarternion);
void
_cogl_quaternion_print (CoglQuaternion *quaternion)
{
g_print ("[ %6.4f (%6.4f, %6.4f, %6.4f)]\n",
quaternion->w,
quaternion->x,
quaternion->y,
quaternion->z);
}
void
cogl_quaternion_init (CoglQuaternion *quaternion,
float angle,
float x,
float y,
float z)
{
float axis[3] = { x, y, z};
cogl_quaternion_init_from_angle_vector (quaternion, angle, axis);
}
void
cogl_quaternion_init_from_angle_vector (CoglQuaternion *quaternion,
float angle,
const float *axis3f_in)
{
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
* in this form:
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
*/
float axis[3];
float half_angle;
float sin_half_angle;
/* XXX: Should we make cogl_vector3_normalize have separate in and
* out args? */
axis[0] = axis3f_in[0];
axis[1] = axis3f_in[1];
axis[2] = axis3f_in[2];
cogl_vector3_normalize (axis);
half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
sin_half_angle = sinf (half_angle);
quaternion->w = cosf (half_angle);
quaternion->x = axis[0] * sin_half_angle;
quaternion->y = axis[1] * sin_half_angle;
quaternion->z = axis[2] * sin_half_angle;
cogl_quaternion_normalize (quaternion);
}
void
cogl_quaternion_init_identity (CoglQuaternion *quaternion)
{
quaternion->w = 1.0;
quaternion->x = 0.0;
quaternion->y = 0.0;
quaternion->z = 0.0;
}
void
cogl_quaternion_init_from_array (CoglQuaternion *quaternion,
const float *array)
{
quaternion->w = array[0];
quaternion->x = array[1];
quaternion->y = array[2];
quaternion->z = array[3];
}
void
cogl_quaternion_init_from_x_rotation (CoglQuaternion *quaternion,
float angle)
{
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
* in this form:
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
*/
float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
quaternion->w = cosf (half_angle);
quaternion->x = sinf (half_angle);
quaternion->y = 0.0f;
quaternion->z = 0.0f;
}
void
cogl_quaternion_init_from_y_rotation (CoglQuaternion *quaternion,
float angle)
{
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
* in this form:
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
*/
float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
quaternion->w = cosf (half_angle);
quaternion->x = 0.0f;
quaternion->y = sinf (half_angle);
quaternion->z = 0.0f;
}
void
cogl_quaternion_init_from_z_rotation (CoglQuaternion *quaternion,
float angle)
{
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
* in this form:
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
*/
float half_angle = angle * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f;
quaternion->w = cosf (half_angle);
quaternion->x = 0.0f;
quaternion->y = 0.0f;
quaternion->z = sinf (half_angle);
}
void
cogl_quaternion_init_from_euler (CoglQuaternion *quaternion,
const CoglEuler *euler)
{
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
* in this form:
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
*/
float sin_heading =
sinf (euler->heading * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
float sin_pitch =
sinf (euler->pitch * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
float sin_roll =
sinf (euler->roll * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
float cos_heading =
cosf (euler->heading * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
float cos_pitch =
cosf (euler->pitch * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
float cos_roll =
cosf (euler->roll * _COGL_QUATERNION_DEGREES_TO_RADIANS * 0.5f);
quaternion->w =
cos_heading * cos_pitch * cos_roll +
sin_heading * sin_pitch * sin_roll;
quaternion->x =
cos_heading * sin_pitch * cos_roll +
sin_heading * cos_pitch * sin_roll;
quaternion->y =
sin_heading * cos_pitch * cos_roll -
cos_heading * sin_pitch * sin_roll;
quaternion->z =
cos_heading * cos_pitch * sin_roll -
sin_heading * sin_pitch * cos_roll;
}
void
cogl_quaternion_init_from_quaternion (CoglQuaternion *quaternion,
CoglQuaternion *src)
{
memcpy (quaternion, src, sizeof (float) * 4);
}
/* XXX: it could be nice to make something like this public... */
/*
* COGL_MATRIX_READ:
* @MATRIX: A 4x4 transformation matrix
* @ROW: The row of the value you want to read
* @COLUMN: The column of the value you want to read
*
* Reads a value from the given matrix using integers to index
* into the matrix.
*/
#define COGL_MATRIX_READ(MATRIX, ROW, COLUMN) \
(((const float *)matrix)[COLUMN * 4 + ROW])
void
cogl_quaternion_init_from_matrix (CoglQuaternion *quaternion,
const CoglMatrix *matrix)
{
/* Algorithm devised by Ken Shoemake, Ref:
* http://campar.in.tum.de/twiki/pub/Chair/DwarfTutorial/quatut.pdf
*/
/* 3D maths literature refers to the diagonal of a matrix as the
* "trace" of a matrix... */
float trace = matrix->xx + matrix->yy + matrix->zz;
float root;
if (trace > 0.0f)
{
root = sqrtf (trace + 1);
quaternion->w = root * 0.5f;
root = 0.5f / root;
quaternion->x = (matrix->zy - matrix->yz) * root;
quaternion->y = (matrix->xz - matrix->zx) * root;
quaternion->z = (matrix->yx - matrix->xy) * root;
}
else
{
#define X 0
#define Y 1
#define Z 2
#define W 3
int h = X;
if (matrix->yy > matrix->xx)
h = Y;
if (matrix->zz > COGL_MATRIX_READ (matrix, h, h))
h = Z;
switch (h)
{
#define CASE_MACRO(i, j, k, I, J, K) \
case I: \
root = sqrtf ((COGL_MATRIX_READ (matrix, I, I) - \
(COGL_MATRIX_READ (matrix, J, J) + \
COGL_MATRIX_READ (matrix, K, K))) + \
COGL_MATRIX_READ (matrix, W, W)); \
quaternion->i = root * 0.5f;\
root = 0.5f / root;\
quaternion->j = (COGL_MATRIX_READ (matrix, I, J) + \
COGL_MATRIX_READ (matrix, J, I)) * root; \
quaternion->k = (COGL_MATRIX_READ (matrix, K, I) + \
COGL_MATRIX_READ (matrix, I, K)) * root; \
quaternion->w = (COGL_MATRIX_READ (matrix, K, J) - \
COGL_MATRIX_READ (matrix, J, K)) * root;\
break
CASE_MACRO (x, y, z, X, Y, Z);
CASE_MACRO (y, z, x, Y, Z, X);
CASE_MACRO (z, x, y, Z, X, Y);
#undef CASE_MACRO
#undef X
#undef Y
#undef Z
}
}
if (matrix->ww != 1.0f)
{
float s = 1.0 / sqrtf (matrix->ww);
quaternion->w *= s;
quaternion->x *= s;
quaternion->y *= s;
quaternion->z *= s;
}
}
CoglBool
cogl_quaternion_equal (const void *v1, const void *v2)
{
const CoglQuaternion *a = v1;
const CoglQuaternion *b = v2;
_COGL_RETURN_VAL_IF_FAIL (v1 != NULL, FALSE);
_COGL_RETURN_VAL_IF_FAIL (v2 != NULL, FALSE);
if (v1 == v2)
return TRUE;
return (a->w == b->w &&
a->x == b->x &&
a->y == b->y &&
a->z == b->z);
}
CoglQuaternion *
cogl_quaternion_copy (const CoglQuaternion *src)
{
if (G_LIKELY (src))
{
CoglQuaternion *new = g_slice_new (CoglQuaternion);
memcpy (new, src, sizeof (float) * 4);
return new;
}
else
return NULL;
}
void
cogl_quaternion_free (CoglQuaternion *quaternion)
{
g_slice_free (CoglQuaternion, quaternion);
}
float
cogl_quaternion_get_rotation_angle (const CoglQuaternion *quaternion)
{
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
* in this form:
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
*/
/* FIXME: clamp [-1, 1] */
return 2.0f * acosf (quaternion->w) * _COGL_QUATERNION_RADIANS_TO_DEGREES;
}
void
cogl_quaternion_get_rotation_axis (const CoglQuaternion *quaternion,
float *vector3)
{
float sin_half_angle_sqr;
float one_over_sin_angle_over_2;
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
* in this form:
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
*/
/* NB: sin²(𝜃) + cos²(𝜃) = 1 */
sin_half_angle_sqr = 1.0f - quaternion->w * quaternion->w;
if (sin_half_angle_sqr <= 0.0f)
{
/* Either an identity quaternion or numerical imprecision.
* Either way we return an arbitrary vector. */
vector3[0] = 1;
vector3[1] = 0;
vector3[2] = 0;
return;
}
/* Calculate 1 / sin(𝜃/2) */
one_over_sin_angle_over_2 = 1.0f / sqrtf (sin_half_angle_sqr);
vector3[0] = quaternion->x * one_over_sin_angle_over_2;
vector3[1] = quaternion->y * one_over_sin_angle_over_2;
vector3[2] = quaternion->z * one_over_sin_angle_over_2;
}
void
cogl_quaternion_normalize (CoglQuaternion *quaternion)
{
float slen = _COGL_QUATERNION_NORM (quaternion);
float factor = 1.0f / sqrtf (slen);
quaternion->x *= factor;
quaternion->y *= factor;
quaternion->z *= factor;
quaternion->w *= factor;
return;
}
float
cogl_quaternion_dot_product (const CoglQuaternion *a,
const CoglQuaternion *b)
{
return a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z;
}
void
cogl_quaternion_invert (CoglQuaternion *quaternion)
{
quaternion->x = -quaternion->x;
quaternion->y = -quaternion->y;
quaternion->z = -quaternion->z;
}
void
cogl_quaternion_multiply (CoglQuaternion *result,
const CoglQuaternion *a,
const CoglQuaternion *b)
{
float w = a->w;
float x = a->x;
float y = a->y;
float z = a->z;
_COGL_RETURN_IF_FAIL (b != result);
result->w = w * b->w - x * b->x - y * b->y - z * b->z;
result->x = w * b->x + x * b->w + y * b->z - z * b->y;
result->y = w * b->y + y * b->w + z * b->x - x * b->z;
result->z = w * b->z + z * b->w + x * b->y - y * b->x;
}
void
cogl_quaternion_pow (CoglQuaternion *quaternion, float exponent)
{
float half_angle;
float new_half_angle;
float factor;
/* Try and identify and nop identity quaternions to avoid
* dividing by zero */
if (fabs (quaternion->w) > 0.9999f)
return;
/* NB: We are using quaternions to represent an axis (a), angle (𝜃) pair
* in this form:
* [w=cos(𝜃/2) ( x=sin(𝜃/2)*a.x, y=sin(𝜃/2)*a.y, z=sin(𝜃/2)*a.x )]
*/
/* FIXME: clamp [-1, 1] */
/* Extract 𝜃/2 from w */
half_angle = acosf (quaternion->w);
/* Compute the new 𝜃/2 */
new_half_angle = half_angle * exponent;
/* Compute the new w value */
quaternion->w = cosf (new_half_angle);
/* And new xyz values */
factor = sinf (new_half_angle) / sinf (half_angle);
quaternion->x *= factor;
quaternion->y *= factor;
quaternion->z *= factor;
}
void
cogl_quaternion_slerp (CoglQuaternion *result,
const CoglQuaternion *a,
const CoglQuaternion *b,
float t)
{
float cos_difference;
float qb_w;
float qb_x;
float qb_y;
float qb_z;
float fa;
float fb;
_COGL_RETURN_IF_FAIL (t >=0 && t <= 1.0f);
if (t == 0)
{
*result = *a;
return;
}
else if (t == 1)
{
*result = *b;
return;
}
/* compute the cosine of the angle between the two given quaternions */
cos_difference = cogl_quaternion_dot_product (a, b);
/* If negative, use -b. Two quaternions q and -q represent the same angle but
* may produce a different slerp. We choose b or -b to rotate using the acute
* angle.
*/
if (cos_difference < 0.0f)
{
qb_w = -b->w;
qb_x = -b->x;
qb_y = -b->y;
qb_z = -b->z;
cos_difference = -cos_difference;
}
else
{
qb_w = b->w;
qb_x = b->x;
qb_y = b->y;
qb_z = b->z;
}
/* If we have two unit quaternions the dot should be <= 1.0 */
g_assert (cos_difference < 1.1f);
/* Determine the interpolation factors for each quaternion, simply using
* linear interpolation for quaternions that are nearly exactly the same.
* (this will avoid divisions by zero)
*/
if (cos_difference > 0.9999f)
{
fa = 1.0f - t;
fb = t;
/* XXX: should we also normalize() at the end in this case? */
}
else
{
/* Calculate the sin of the angle between the two quaternions using the
* trig identity: sin²(𝜃) + cos²(𝜃) = 1
*/
float sin_difference = sqrtf (1.0f - cos_difference * cos_difference);
float difference = atan2f (sin_difference, cos_difference);
float one_over_sin_difference = 1.0f / sin_difference;
fa = sinf ((1.0f - t) * difference) * one_over_sin_difference;
fb = sinf (t * difference) * one_over_sin_difference;
}
/* Finally interpolate the two quaternions */
result->x = fa * a->x + fb * qb_x;
result->y = fa * a->y + fb * qb_y;
result->z = fa * a->z + fb * qb_z;
result->w = fa * a->w + fb * qb_w;
}
void
cogl_quaternion_nlerp (CoglQuaternion *result,
const CoglQuaternion *a,
const CoglQuaternion *b,
float t)
{
float cos_difference;
float qb_w;
float qb_x;
float qb_y;
float qb_z;
float fa;
float fb;
_COGL_RETURN_IF_FAIL (t >=0 && t <= 1.0f);
if (t == 0)
{
*result = *a;
return;
}
else if (t == 1)
{
*result = *b;
return;
}
/* compute the cosine of the angle between the two given quaternions */
cos_difference = cogl_quaternion_dot_product (a, b);
/* If negative, use -b. Two quaternions q and -q represent the same angle but
* may produce a different slerp. We choose b or -b to rotate using the acute
* angle.
*/
if (cos_difference < 0.0f)
{
qb_w = -b->w;
qb_x = -b->x;
qb_y = -b->y;
qb_z = -b->z;
cos_difference = -cos_difference;
}
else
{
qb_w = b->w;
qb_x = b->x;
qb_y = b->y;
qb_z = b->z;
}
/* If we have two unit quaternions the dot should be <= 1.0 */
g_assert (cos_difference < 1.1f);
fa = 1.0f - t;
fb = t;
result->x = fa * a->x + fb * qb_x;
result->y = fa * a->y + fb * qb_y;
result->z = fa * a->z + fb * qb_z;
result->w = fa * a->w + fb * qb_w;
cogl_quaternion_normalize (result);
}
/**
* cogl_quaternion_squad:
*
*/
void
cogl_quaternion_squad (CoglQuaternion *result,
const CoglQuaternion *prev,
const CoglQuaternion *a,
const CoglQuaternion *b,
const CoglQuaternion *next,
float t)
{
CoglQuaternion slerp0;
CoglQuaternion slerp1;
cogl_quaternion_slerp (&slerp0, a, b, t);
cogl_quaternion_slerp (&slerp1, prev, next, t);
cogl_quaternion_slerp (result, &slerp0, &slerp1, 2.0f * t * (1.0f - t));
}
const CoglQuaternion *
cogl_get_static_identity_quaternion (void)
{
return &identity_quaternion;
}
const CoglQuaternion *
cogl_get_static_zero_quaternion (void)
{
return &zero_quaternion;
}