static void
graphene_matrix_transpose_transform_vec4 (const graphene_matrix_t *m,
const graphene_vec4_t *v,
graphene_vec4_t *res)
{
float x, y, z, w;
x = graphene_simd4f_get_x (graphene_simd4f_sum (graphene_simd4f_mul (m->value.x, v->value)));
y = graphene_simd4f_get_x (graphene_simd4f_sum (graphene_simd4f_mul (m->value.y, v->value)));
z = graphene_simd4f_get_x (graphene_simd4f_sum (graphene_simd4f_mul (m->value.z, v->value)));
w = graphene_simd4f_get_x (graphene_simd4f_sum (graphene_simd4f_mul (m->value.w, v->value)));
graphene_vec4_init (res, x, y, z, w);
}
float
graphene_matrix_get_x_scale (const graphene_matrix_t *m)
{
g_return_val_if_fail (m != NULL, 0.f);
return graphene_simd4f_get_x (m->value.x);
}
/**
* graphene_quaternion_slerp:
* @a: a #graphene_quaternion_t
* @b: a #graphene_quaternion_t
* @factor: the linear interpolation factor
* @res: (out caller-allocates): return location for the interpolated
* quaternion
*
* Interpolates between the two given quaternions using a spherical
* linear interpolation, or [SLERP](http://en.wikipedia.org/wiki/Slerp),
* using the given interpolation @factor.
*
* Since: 1.0
*/
void
graphene_quaternion_slerp (const graphene_quaternion_t *a,
const graphene_quaternion_t *b,
float factor,
graphene_quaternion_t *res)
{
float theta, r_sin_theta, right_v, left_v, dot;
graphene_simd4f_t v_a, v_b, left, right, sum;
v_a = graphene_simd4f_init (a->x, a->y, a->z, a->w);
v_b = graphene_simd4f_init (b->x, b->y, b->z, b->w);
dot = CLAMP (graphene_simd4f_get_x (graphene_simd4f_dot4 (v_a, v_b)), -1.f, 1.f);
if (dot == 1.f)
{
*res = *a;
return;
}
theta = acos (dot);
r_sin_theta = 1.f / sqrtf (1.f - dot * dot);
right_v = sinf (factor * theta) * r_sin_theta;
left_v = cosf (factor * theta) - dot * right_v;
left = graphene_simd4f_init (a->x, a->y, a->z, a->w);
right = graphene_simd4f_init (b->x, b->y, b->z, b->w);
left = graphene_simd4f_mul (left, graphene_simd4f_splat (left_v));
right = graphene_simd4f_mul (right, graphene_simd4f_splat (right_v));
sum = graphene_simd4f_add (left, right);
graphene_quaternion_init_from_simd4f (res, sum);
}
float
graphene_matrix_get_value (const graphene_matrix_t *m,
unsigned int row,
unsigned int col)
{
graphene_simd4f_t r;
float c = 0.f;
g_return_val_if_fail (m != NULL, 0.f);
g_return_val_if_fail (row >= 0 && row < 4, 0.f);
g_return_val_if_fail (col >= 0 && row < 4, 0.f);
switch (row)
{
case 0:
r = m->value.x;
break;
case 1:
r = m->value.y;
break;
case 2:
r = m->value.z;
break;
case 3:
r = m->value.w;
break;
default:
g_assert_not_reached ();
}
switch (col)
{
case 0:
c = graphene_simd4f_get_x (r);
break;
case 1:
c = graphene_simd4f_get_y (r);
break;
case 2:
c = graphene_simd4f_get_z (r);
break;
case 3:
c = graphene_simd4f_get_w (r);
break;
default:
g_assert_not_reached ();
}
return c;
}
/**
* graphene_matrix_determinant:
* @m: a #graphene_matrix_t
*
* Computes the determinant of the given matrix.
*
* Returns: the value of the determinant
*
* Since: 1.0
*/
float
graphene_matrix_determinant (const graphene_matrix_t *m)
{
graphene_simd4f_t det;
graphene_simd4x4f_determinant (&m->value, &det, NULL);
return graphene_simd4f_get_x (det);
}
static graphene_quaternion_t *
graphene_quaternion_init_from_simd4f (graphene_quaternion_t *q,
graphene_simd4f_t v)
{
q->x = graphene_simd4f_get_x (v);
q->y = graphene_simd4f_get_y (v);
q->z = graphene_simd4f_get_z (v);
q->w = graphene_simd4f_get_w (v);
return q;
}
/**
* graphene_matrix_get_value:
* @m: a #grapheme_matrix_t
* @row: the row index
* @col: the column index
*
* Retrieves the value at the given @row and @col index.
*
* Returns: the value at the given indices
*
* Since: 1.0
*/
float
graphene_matrix_get_value (const graphene_matrix_t *m,
unsigned int row,
unsigned int col)
{
graphene_simd4f_t r;
float c = 0.f;
switch (row)
{
case 0:
r = m->value.x;
break;
case 1:
r = m->value.y;
break;
case 2:
r = m->value.z;
break;
case 3:
r = m->value.w;
break;
default:
return 0.f;
}
switch (col)
{
case 0:
c = graphene_simd4f_get_x (r);
break;
case 1:
c = graphene_simd4f_get_y (r);
break;
case 2:
c = graphene_simd4f_get_z (r);
break;
case 3:
c = graphene_simd4f_get_w (r);
break;
default:
return 0;
}
return c;
}
float
graphene_matrix_determinant (const graphene_matrix_t *m)
{
graphene_simd4f_t det;
g_return_val_if_fail (m != NULL, 0.f);
graphene_simd4x4f_determinant (&m->value, &det, NULL);
return graphene_simd4f_get_x (det);
}
/**
* graphene_quaternion_dot:
* @a: a #graphene_quaternion_t
* @b: a #graphene_quaternion_t
*
* Computes the dot product of two #graphene_quaternion_t.
*
* Returns: the value of the dot products
*
* Since: 1.0
*/
float
graphene_quaternion_dot (const graphene_quaternion_t *a,
const graphene_quaternion_t *b)
{
graphene_simd4f_t v_a, v_b;
v_a = graphene_simd4f_init (a->x, a->y, a->z, a->w);
v_b = graphene_simd4f_init (b->x, b->y, b->z, b->w);
return graphene_simd4f_get_x (graphene_simd4f_dot4 (v_a, v_b));
}
/**
* graphene_matrix_transform_point:
* @m: a #graphene_matrix_t
* @p: a #graphene_point_t
* @res: (out caller-allocates): return location for the
* transformed #graphene_point_t
*
* Transforms the given #graphene_point_t using the matrix @m.
*
* Since: 1.0
*/
void
graphene_matrix_transform_point (const graphene_matrix_t *m,
const graphene_point_t *p,
graphene_point_t *res)
{
graphene_simd4f_t vec3;
vec3 = graphene_simd4f_init (p->x, p->y, 0.0f, 0.0f);
graphene_simd4x4f_vec3_mul (&m->value, &vec3, &vec3);
res->x = graphene_simd4f_get_x (vec3);
res->y = graphene_simd4f_get_y (vec3);
}
static void
matrix_project (gpointer data_)
{
MatrixBench *data = data_;
int i;
for (i = 0; i < N_ROUNDS; i++)
{
graphene_simd4f_t pback, qback, uback;
float t, x, y;
graphene_simd4x4f_vec3_mul (&(data->a[i]), &(data->pa[i]), &pback);
graphene_simd4x4f_vec3_mul (&(data->a[i]), &(data->qa[i]), &qback);
uback = graphene_simd4f_sub (data->pa[i], pback);
t = -1.0f * graphene_simd4f_get_z (pback) / graphene_simd4f_get_z (uback);
x = graphene_simd4f_get_x (pback) + t * graphene_simd4f_get_x (uback);
y = graphene_simd4f_get_y (pback) + t * graphene_simd4f_get_y (uback);
data->ra[i] = graphene_simd4f_init (x, y, 0.f, 0.f);
}
}
/**
* graphene_point_distance:
* @a: a #graphene_point_t
* @b: a #graphene_point_t
* @d_x: (out) (optional): distance component on the X axis
* @d_y: (out) (optional): distance component on the Y axis
*
* Computes the distance between @a and @b.
*
* Returns: the distance between the two points
*
* Since: 1.0
*/
float
graphene_point_distance (const graphene_point_t *a,
const graphene_point_t *b,
float *d_x,
float *d_y)
{
graphene_simd4f_t s_a, s_b, res;
if (a == b)
return 0.f;
s_a = graphene_simd4f_init (a->x, a->y, 0.f, 0.f);
s_b = graphene_simd4f_init (b->x, b->y, 0.f, 0.f);
res = graphene_simd4f_sub (s_a, s_b);
if (d_x != NULL)
*d_x = fabsf (graphene_simd4f_get_x (res));
if (d_y != NULL)
*d_y = fabsf (graphene_simd4f_get_y (res));
return graphene_simd4f_get_x (graphene_simd4f_length2 (res));
}
/**
* graphene_point_near:
* @a: a #graphene_point_t
* @b: a #graphene_point_t
* @epsilon: threshold between the two points
*
* Checks whether the two points @a and @b are within
* the threshold of @epsilon.
*
* Returns: `true` if the distance is within @epsilon
*
* Since: 1.0
*/
bool
graphene_point_near (const graphene_point_t *a,
const graphene_point_t *b,
float epsilon)
{
graphene_simd4f_t s_a, s_b, res;
if (a == b)
return true;
s_a = graphene_simd4f_init (a->x, a->y, 0.f, 0.f);
s_b = graphene_simd4f_init (b->x, b->y, 0.f, 0.f);
res = graphene_simd4f_sub (s_a, s_b);
return fabsf (graphene_simd4f_get_x (res)) < epsilon &&
fabsf (graphene_simd4f_get_y (res)) < epsilon;
}
/**
* graphene_vec2_near:
* @v1: a #graphene_vec2_t
* @v2: a #graphene_vec2_t
* @epsilon: the threshold between the two vectors
*
* Compares the two given #graphene_vec2_t vectors and checks
* whether their values are within the given @epsilon.
*
* Returns: `true` if the two vectors are near each other
*
* Since: 1.2
*/
bool
graphene_vec2_near (const graphene_vec2_t *v1,
const graphene_vec2_t *v2,
float epsilon)
{
float epsilon_sq = epsilon * epsilon;
graphene_simd4f_t d;
if (v1 == v2)
return true;
if (v1 == NULL || v2 == NULL)
return false;
d = graphene_simd4f_sub (v1->value, v2->value);
return graphene_simd4f_get_x (graphene_simd4f_dot2 (d, d)) < epsilon_sq;
}
/**
* graphene_quaternion_init_from_angle_vec3:
* @q: a #graphene_quaternion_t
* @angle: the rotation on a given axis, in degrees
* @axis: the axis of rotation, expressed as a vector
*
* Initializes a #graphene_quaternion_t using an @angle on a
* specific @axis.
*
* Returns: (transfer none): the initialized quaternion
*
* Since: 1.0
*/
graphene_quaternion_t *
graphene_quaternion_init_from_angle_vec3 (graphene_quaternion_t *q,
float angle,
const graphene_vec3_t *axis)
{
float rad, sin_a, cos_a;
graphene_simd4f_t axis_n;
rad = GRAPHENE_DEG_TO_RAD (angle);
sin_a = sinf (rad / 2.f);
cos_a = cosf (rad / 2.f);
axis_n = graphene_simd4f_mul (graphene_simd4f_normalize3 (axis->value),
graphene_simd4f_splat (sin_a));
q->x = graphene_simd4f_get_x (axis_n);
q->y = graphene_simd4f_get_y (axis_n);
q->z = graphene_simd4f_get_z (axis_n);
q->w = cos_a;
return q;
}
static gboolean
matrix_decompose_3d (const graphene_matrix_t *m,
graphene_point3d_t *scale_r,
float shear_r[3],
graphene_quaternion_t *rotate_r,
graphene_point3d_t *translate_r,
graphene_vec4_t *perspective_r)
{
graphene_matrix_t local, perspective;
float shear_xy, shear_xz, shear_yz;
float scale_x, scale_y, scale_z;
graphene_simd4f_t dot, cross;
if (graphene_matrix_get_value (m, 3, 3) == 0.f)
return FALSE;
local = *m;
/* normalize the matrix */
graphene_matrix_normalize (&local, &local);
/* perspective is used to solve for the perspective component,
* but it also provides an easy way to test for singularity of
* the upper 3x3 component
*/
perspective = local;
perspective.value.w = graphene_simd4f_init (0.f, 0.f, 0.f, 1.f);
if (graphene_matrix_determinant (&perspective) == 0.f)
return FALSE;
/* isolate the perspective */
if (graphene_simd4f_is_zero3 (local.value.w))
{
graphene_matrix_t tmp;
/* perspective_r is the right hand side of the equation */
perspective_r->value = local.value.w;
/* solve the equation by inverting perspective and multiplying
* the inverse with the perspective vector
*/
graphene_matrix_inverse (&perspective, &tmp);
graphene_matrix_transpose_transform_vec4 (&tmp, perspective_r, perspective_r);
/* clear the perspective partition */
local.value.w = graphene_simd4f_init (0.f, 0.f, 0.f, 1.f);
}
else
graphene_vec4_init (perspective_r, 0.f, 0.f, 0.f, 1.f);
/* next, take care of the translation partition */
translate_r->x = graphene_simd4f_get_x (local.value.w);
translate_r->y = graphene_simd4f_get_y (local.value.w);
translate_r->z = graphene_simd4f_get_z (local.value.w);
local.value.w = graphene_simd4f_init (0.f, 0.f, 0.f, graphene_simd4f_get_w (local.value.w));
/* now get scale and shear */
/* compute the X scale factor and normalize the first row */
scale_x = graphene_simd4f_get_x (graphene_simd4f_length4 (local.value.x));
local.value.x = graphene_simd4f_div (local.value.x, graphene_simd4f_splat (scale_x));
/* compute XY shear factor and the second row orthogonal to the first */
shear_xy = graphene_simd4f_get_x (graphene_simd4f_dot4 (local.value.x, local.value.y));
local.value.y = graphene_simd4f_sub (local.value.y, graphene_simd4f_mul (local.value.x, graphene_simd4f_splat (shear_xy)));
/* now, compute the Y scale factor and normalize the second row */
scale_y = graphene_simd4f_get_x (graphene_simd4f_length4 (local.value.y));
local.value.y = graphene_simd4f_div (local.value.y, graphene_simd4f_splat (scale_y));
shear_xy /= scale_y;
/* compute XZ and YZ shears, make the third row orthogonal */
shear_xz = graphene_simd4f_get_x (graphene_simd4f_dot4 (local.value.x, local.value.z));
local.value.z = graphene_simd4f_sub (local.value.z, graphene_simd4f_mul (local.value.x, graphene_simd4f_splat (shear_xz)));
shear_yz = graphene_simd4f_get_x (graphene_simd4f_dot4 (local.value.y, local.value.z));
local.value.z = graphene_simd4f_sub (local.value.z, graphene_simd4f_mul (local.value.y, graphene_simd4f_splat (shear_yz)));
/* next, get the Z scale and normalize the third row */
scale_z = graphene_simd4f_get_x (graphene_simd4f_length4 (local.value.z));
local.value.z = graphene_simd4f_div (local.value.z, graphene_simd4f_splat (scale_z));
shear_xz /= scale_z;
shear_yz /= scale_z;
shear_r[XY_SHEAR] = shear_xy;
shear_r[XZ_SHEAR] = shear_xz;
shear_r[YZ_SHEAR] = shear_yz;
/* at this point, the matrix is orthonormal. we check for a
* coordinate system flip. if the determinant is -1, then
* negate the matrix and the scaling factors
*/
dot = graphene_simd4f_cross3 (local.value.y, local.value.z);
cross = graphene_simd4f_dot4 (local.value.x, dot);
if (graphene_simd4f_get_x (cross) < 0.f)
{
scale_x *= -1.f;
scale_y *= -1.f;
scale_z *= -1.f;
graphene_simd4f_mul (local.value.x, graphene_simd4f_splat (-1.f));
graphene_simd4f_mul (local.value.y, graphene_simd4f_splat (-1.f));
graphene_simd4f_mul (local.value.z, graphene_simd4f_splat (-1.f));
}
graphene_point3d_init (scale_r, scale_x, scale_y, scale_z);
/* get the rotations out */
graphene_quaternion_init_from_matrix (rotate_r, &local);
return TRUE;
}
/**
* graphene_vec3_get_x:
* @v: a #graphene_vec3_t
*
* Retrieves the first component of the given vector @v.
*
* Returns: the value of the first component of the vector
*
* Since: 1.0
*/
float
graphene_vec3_get_x (const graphene_vec3_t *v)
{
return graphene_simd4f_get_x (v->value);
}
/**
* graphene_vec3_length:
* @v: a #graphene_vec3_t
*
* Retrieves the length of the given vector @v.
*
* Returns: the value of the length of the vector
*
* Since: 1.0
*/
float
graphene_vec3_length (const graphene_vec3_t *v)
{
return graphene_simd4f_get_x (graphene_simd4f_length3 (v->value));
}
/**
* graphene_vec3_dot:
* @a: a #graphene_vec3_t
* @b: a #graphene_vec3_t
*
* Computes the dot product of the two given vectors.
*
* Returns: the value of the dot product
*
* Since: 1.0
*/
float
graphene_vec3_dot (const graphene_vec3_t *a,
const graphene_vec3_t *b)
{
return graphene_simd4f_get_x (graphene_simd4f_dot3 (a->value, b->value));
}
/**
* graphene_simd4f_get_x:
* @s: a #graphene_simd4f_t
*
* Retrieves the first component of @s.
*
* Returns: the first component of a #graphene_simd4f_t
*
* Since: 1.0
*/
float
(graphene_simd4f_get_x) (const graphene_simd4f_t s)
{
return graphene_simd4f_get_x (s);
}
/**
* graphene_matrix_get_x_scale:
* @m: a #graphene_matrix_t
*
* Retrieves the scaling factor on the X axis in @m.
*
* Returns: the value of the scaling factor
*
* Since: 1.0
*/
float
graphene_matrix_get_x_scale (const graphene_matrix_t *m)
{
return graphene_simd4f_get_x (m->value.x);
}