GRAPHENE_TEST_UNIT_END
GRAPHENE_TEST_UNIT_BEGIN (euler_quaternion_roundtrip)
{
graphene_euler_t values[3];
unsigned int i;
graphene_euler_init_with_order (&values[0], 0.f, 0.f, 0.f, GRAPHENE_EULER_ORDER_XYZ);
graphene_euler_init_with_order (&values[1], 1.f, 0.f, 0.f, GRAPHENE_EULER_ORDER_XYZ);
graphene_euler_init_with_order (&values[2], 0.f, 1.f, 0.f, GRAPHENE_EULER_ORDER_ZYX);
for (i = 0; i < G_N_ELEMENTS (values); i++)
{
graphene_quaternion_t q, check;
graphene_euler_t e;
graphene_quaternion_init_from_euler (&q, &values[i]);
graphene_euler_init_from_quaternion (&e, &q, graphene_euler_get_order (&values[i]));
graphene_quaternion_init_from_euler (&check, &e);
g_assert_true (graphene_quaternion_equal (&q, &check));
}
}
/**
* graphene_euler_init_from_quaternion:
* @e: a #graphene_euler_t
* @q: (nullable): a normalized #graphene_quaternion_t
* @order: the order used to apply the rotations
*
* Initializes a #graphene_euler_t using the given normalized quaternion.
*
* If the #graphene_quaternion_t @q is %NULL, the #graphene_euler_t will
* be initialized with all angles set to 0.
*
* Returns: (transfer none): the initialized #graphene_euler_t
*
* Since: 1.2
*/
graphene_euler_t *
graphene_euler_init_from_quaternion (graphene_euler_t *e,
const graphene_quaternion_t *q,
graphene_euler_order_t order)
{
float sqx, sqy, sqz, sqw;
float x, y, z;
if (q == NULL)
return graphene_euler_init_with_order (e, 0.f, 0.f, 0.f, order);
sqx = q->x * q->x;
sqy = q->y * q->y;
sqz = q->z * q->z;
sqw = q->w * q->w;
x = y = z = 0.f;
e->order = order;
switch (graphene_euler_get_order (e))
{
case GRAPHENE_EULER_ORDER_XYZ:
x = atan2f (2.f * (q->x * q->w - q->y * q->z), (sqw - sqx - sqy + sqz));
y = asinf (CLAMP (2.f * (q->x * q->z + q->y * q->w ), -1.f, 1.f));
z = atan2f (2.f * (q->z * q->w - q->x * q->y), (sqw + sqx - sqy - sqz));
break;
case GRAPHENE_EULER_ORDER_YXZ:
x = asinf (CLAMP (2.f * (q->x * q->w - q->y * q->z), -1.f, 1.f));
y = atan2f (2.f * (q->x * q->z + q->y * q->w), (sqw - sqx - sqy + sqz));
z = atan2f (2.f * (q->x * q->y + q->z * q->w), (sqw - sqx + sqy - sqz));
break;
case GRAPHENE_EULER_ORDER_ZXY:
x = asinf (CLAMP (2.f * (q->x * q->w + q->y * q->z), -1.f, 1.f));
y = atan2f (2.f * (q->y * q->w - q->z * q->x), (sqw - sqx - sqy + sqz));
z = atan2f (2.f * (q->z * q->w - q->x * q->y), (sqw - sqx + sqy - sqz));
break;
case GRAPHENE_EULER_ORDER_ZYX:
x = atan2f (2.f * (q->x * q->w + q->z * q->y), (sqw - sqx - sqy + sqz));
y = asinf (CLAMP (2.f * (q->y * q->w - q->x * q->z), -1.f, 1.f));
z = atan2f (2.f * (q->x * q->y + q->z * q->w), (sqw + sqx - sqy - sqz));
break;
case GRAPHENE_EULER_ORDER_YZX:
x = atan2f (2.f * (q->x * q->w - q->z * q->y), (sqw - sqx + sqy - sqz));
y = atan2f (2.f * (q->y * q->w - q->x * q->z), (sqw + sqx - sqy - sqz));
z = asinf (CLAMP (2.f * (q->x * q->y + q->z * q->w ), -1.f, 1.f));
break;
case GRAPHENE_EULER_ORDER_XZY:
x = atan2f (2.f * (q->x * q->w + q->y * q->z), (sqw - sqx + sqy - sqz));
y = atan2f (2.f * (q->x * q->z + q->y * q->w), (sqw + sqx - sqy - sqz));
z = asinf (CLAMP (2.f * (q->z * q->w - q->x * q->y), -1.f, 1.f));
break;
case GRAPHENE_EULER_ORDER_DEFAULT:
break;
}
graphene_vec3_init (&e->angles, x, y, z);
return e;
}
/**
* graphene_euler_to_matrix:
* @e: a #graphene_euler_t
* @res: (out caller-allocates): return location for a #graphene_matrix_t
*
* Converts a #graphene_euler_t into a transformation matrix expressing
* the extrinsic composition of rotations described by the Euler angles.
*
* The rotations are applied over the reference frame axes in the order
* associated with the #graphene_euler_t; for instance, if the order
* used to initialize @e is %GRAPHENE_EULER_ORDER_XYZ:
*
* * the first rotation moves the body around the X axis with
* an angle φ
* * the second rotation moves the body around the Y axis with
* an angle of ϑ
* * the third rotation moves the body around the Z axis with
* an angle of ψ
*
* The rotation sign convention is left-handed, to preserve compatibility
* between Euler-based, quaternion-based, and angle-axis-based rotations.
*
* Since: 1.2
*/
void
graphene_euler_to_matrix (const graphene_euler_t *e,
graphene_matrix_t *res)
{
graphene_euler_order_t order = graphene_euler_get_order (e);
const float x = graphene_vec3_get_x (&e->angles);
const float y = graphene_vec3_get_y (&e->angles);
const float z = graphene_vec3_get_z (&e->angles);
float c1, s1, c2, s2, c3, s3;
float c3c2, s3c1, c3s2s1, s3s1;
float c3s2c1, s3c2, c3c1, s3s2s1;
float c3s1, s3s2c1, c2s1, c2c1;
graphene_sincos (x, &c1, &s1);
graphene_sincos (y, &c2, &s2);
graphene_sincos (z, &c3, &s3);
c3c2 = c3 * c2;
s3c1 = s3 * c1;
c3s2s1 = c3 * s2 * s1;
s3s1 = s3 * s1;
c3s2c1 = c3 * s2 * c1;
s3c2 = s3 * c2;
c3c1 = c3 * c1;
s3s2s1 = s3 * s2 * s1;
c3s1 = c3 * s1;
s3s2c1 = s3 * s2 * c1;
c2s1 = c2 * s1;
c2c1 = c2 * c1;
switch (order)
{
case GRAPHENE_EULER_ORDER_XYZ:
{
/* ⎡ c3 s3 0 ⎤ ⎡ c2 0 -s2 ⎤ ⎡ 1 0 0 ⎤
* ⎢ -s3 c3 0 ⎥ ⎢ 0 1 0 ⎥ ⎢ 0 c1 s1 ⎥
* ⎣ 0 0 1 ⎦ ⎣ s2 0 c2 ⎦ ⎣ 0 -s1 c1 ⎦
*/
res->value.x = graphene_simd4f_init ( c3c2, s3c1 + c3s2s1, s3s1 - c3s2c1, 0.f);
res->value.y = graphene_simd4f_init (-s3c2, c3c1 - s3s2s1, c3s1 + s3s2c1, 0.f);
res->value.z = graphene_simd4f_init ( s2, -c2s1, c2c1, 0.f);
res->value.w = graphene_simd4f_init ( 0.f, 0.f, 0.f, 1.f);
}
break;
case GRAPHENE_EULER_ORDER_YXZ:
{
/* ⎡ c3 s3 0 ⎤ ⎡ 1 0 0 ⎤ ⎡ c1 0 -s1 ⎤
* ⎢ -s2 c3 0 ⎥ ⎢ 0 c2 s2 ⎥ ⎢ 0 1 0 ⎥
* ⎣ 0 0 1 ⎦ ⎣ 0 -s2 c2 ⎦ ⎣ s1 0 c1 ⎦
*/
res->value.x = graphene_simd4f_init ( c3c1 + s3s2s1, s3c2, -c3s1 + s3s2c1, 0.f);
res->value.y = graphene_simd4f_init (-s3c1 + c3s2s1, c3c2, s3s1 + c3s2c1, 0.f);
res->value.z = graphene_simd4f_init ( c2s1, -s2, c2c1, 0.f);
res->value.w = graphene_simd4f_init ( 0.f, 0.f, 0.f, 1.f);
}
break;
case GRAPHENE_EULER_ORDER_ZXY:
{
/* ⎡ 1 0 0 ⎤ ⎡ c2 0 -s2 ⎤ ⎡ c1 s1 0 ⎤
* ⎢ 0 c3 s3 ⎥ ⎢ 0 1 0 ⎥ ⎢ -s1 c1 0 ⎥
* ⎣ 0 -s3 c3 ⎦ ⎣ s2 0 c2 ⎦ ⎣ 0 0 1 ⎦
*/
res->value.x = graphene_simd4f_init (c3c1 - s3s2s1, c3s1 + s3s2c1, -s3c2, 0.f);
res->value.y = graphene_simd4f_init ( -c2s1, c2c1, s2, 0.f);
res->value.z = graphene_simd4f_init (s3c1 + c3s2s1, s3s1 - c3s2c1, c3c2, 0.f);
res->value.w = graphene_simd4f_init ( 0.f, 0.f, 0.f, 1.f);
}
break;
case GRAPHENE_EULER_ORDER_ZYX:
{
/* ⎡ 1 0 0 ⎤ ⎡ c2 0 -s2 ⎤ ⎡ c1 s1 0 ⎤
* ⎢ 0 c3 s3 ⎥ ⎢ 0 1 0 ⎥ ⎢ -s1 c1 0 ⎥
* ⎣ 0 -s3 c3 ⎦ ⎣ s2 0 c2 ⎦ ⎣ 0 0 1 ⎦
*/
res->value.x = graphene_simd4f_init ( c2c1, c2s1, -s2, 0.f);
res->value.y = graphene_simd4f_init (s3s2c1 - c3s1, s3s2s1 + c3c1, s3c2, 0.f);
res->value.z = graphene_simd4f_init (c3s2c1 + s3s1, c3s2s1 - s3c1, c3c2, 0.f);
res->value.w = graphene_simd4f_init ( 0.f, 0.f, 0.f, 1.f);
}
break;
case GRAPHENE_EULER_ORDER_YZX:
{
/* ⎡ 1 0 0 ⎤ ⎡ c2 s2 0 ⎤ ⎡ c1 0 -s1 ⎤
* ⎢ 0 c3 s3 ⎥ ⎢ -s2 c2 0 ⎥ ⎢ 0 1 0 ⎥
* ⎣ 0 -s3 c3 ⎦ ⎣ 0 0 1 ⎦ ⎣ s1 0 c1 ⎦
*/
res->value.x = graphene_simd4f_init ( c2c1, s2, -c2s1, 0.f);
res->value.y = graphene_simd4f_init (-c3s2c1 + s3s1, c3c2, c3s2s1 + s3c1, 0.f);
res->value.z = graphene_simd4f_init ( s3s2c1 + c3s1, -s3c2, -s3s2s1 + c3c1, 0.f);
res->value.w = graphene_simd4f_init ( 0.f, 0.f, 0.f, 1.f);
}
break;
case GRAPHENE_EULER_ORDER_XZY:
{
/* ⎡ c3 0 -s3 ⎤ ⎡ c2 s2 0 ⎤ ⎡ 1 0 0 ⎤
* ⎢ 0 1 0 ⎥ ⎢ -s2 c2 0 ⎥ ⎢ 0 c1 s1 ⎥
* ⎣ s3 0 c3 ⎦ ⎣ 0 0 1 ⎦ ⎣ 0 -s1 c1 ⎦
*/
res->value.x = graphene_simd4f_init (c3c2, c3s2c1 + s3s1, c3s2s1 - s3c1, 0.f);
res->value.y = graphene_simd4f_init ( -s2, c2c1, c2s1, 0.f);
res->value.z = graphene_simd4f_init (s3c2, s3s2c1 - c3s1, s3s2s1 + c3c1, 0.f);
res->value.w = graphene_simd4f_init ( 0.f, 0.f, 0.f, 1.f);
}
break;
default:
graphene_matrix_init_identity (res);
break;
}
}
/**
* graphene_euler_init_from_matrix:
* @e: the #graphene_euler_t to initialize
* @m: (nullable): a rotation matrix
* @order: the order used to apply the rotations
*
* Initializes a #graphene_euler_t using the given rotation matrix.
*
* If the #graphene_matrix_t @m is %NULL, the #graphene_euler_t will
* be initialized with all angles set to 0.
*
* Returns: (transfer none): the initialized #graphene_euler_t
*
* Since: 1.2
*/
graphene_euler_t *
graphene_euler_init_from_matrix (graphene_euler_t *e,
const graphene_matrix_t *m,
graphene_euler_order_t order)
{
float me[16];
float m11, m12, m13;
float m21, m22, m23;
float m31, m32, m33;
float x, y, z;
if (m == NULL)
return graphene_euler_init_with_order (e, 0.f, 0.f, 0.f, order);
graphene_matrix_to_float (m, me);
/* isolate the rotation components */
m11 = me[0]; m21 = me[4]; m31 = me[8];
m12 = me[1]; m22 = me[5]; m32 = me[9];
m13 = me[2]; m23 = me[6]; m33 = me[10];
x = y = z = 0.f;
e->order = order;
switch (graphene_euler_get_order (e))
{
case GRAPHENE_EULER_ORDER_XYZ:
y = asinf (CLAMP (m13, -1.f, 1.f));
if (fabsf (m13) < 1.f)
{
x = atan2f (-1.f * m23, m33);
z = atan2f (-1.f * m12, m11);
}
else
{
x = atan2f (m32, m22);
z = 0.f;
}
break;
case GRAPHENE_EULER_ORDER_YXZ:
x = asinf (-1.f * CLAMP (m23, -1, 1));
if (fabsf (m23) < 1.f)
{
y = atan2f (m13, m33);
z = atan2f (m21, m22);
}
else
{
y = atan2f (-1.f * m31, m11);
z = 0.f;
}
break;
case GRAPHENE_EULER_ORDER_ZXY:
x = asinf (CLAMP (m32, -1.f, 1.f));
if (fabsf (m32) < 1.f)
{
y = atan2f (-1.f * m31, m33);
z = atan2f (-1.f * m12, m22);
}
else
{
y = 0.f;
z = atan2f (m21, m11);
}
break;
case GRAPHENE_EULER_ORDER_ZYX:
y = asinf (-1.f * CLAMP (m31, -1.f, 1.f));
if (fabsf (m31) < 1.f)
{
x = atan2f (m32, m33);
z = atan2f (m21, m11);
}
else
{
x = 0.f;
z = atan2f (-1.f * m12, m22);
}
break;
case GRAPHENE_EULER_ORDER_YZX:
z = asinf (CLAMP (m21, -1.f, 1.f));
if (fabsf (m21) < 1.f)
{
x = atan2f (-1.f * m23, m22);
y = atan2f (-1.f * m31, m11);
}
else
{
x = 0.f;
y = atan2f (m13, m33);
}
break;
case GRAPHENE_EULER_ORDER_XZY:
z = asinf (-1.f * CLAMP (m12, -1.f, 1.f));
if (fabsf (m12) < 1.f)
{
x = atan2f (m32, m22);
y = atan2f (m13, m11);
}
else
{
x = atan2f (-1.f * m23, m33);
y = 0.f;
}
break;
case GRAPHENE_EULER_ORDER_DEFAULT:
break;
}
graphene_vec3_init (&e->angles, x, y, z);
return e;
}
