[RFC weston 12/16] matrix: Add functions to decompose a transformation matrix into basic operations
Bryce Harrington
bryce at osg.samsung.com
Wed Oct 15 01:23:53 PDT 2014
On Fri, Sep 26, 2014 at 04:10:23PM -0500, Derek Foreman wrote:
> Adds an internal weston_matrix_decompose() which takes a matrix and decomposes
> translation, rotation, shear and scale parameters to re-create it.
>
> This information is cached the first time any helper functions are used to extract
> these values and dirtied whenever a matrix multiplication takes place.
>
> Note that these may not be the same values used to compose the matrix in the first
> place.
> ---
> shared/matrix.c | 359 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
> shared/matrix.h | 31 +++++
> 2 files changed, 390 insertions(+)
>
> diff --git a/shared/matrix.c b/shared/matrix.c
> index 4f0b6b7..dafffa5 100644
> --- a/shared/matrix.c
> +++ b/shared/matrix.c
> @@ -27,6 +27,7 @@
> #include <string.h>
> #include <stdlib.h>
> #include <math.h>
> +#include <stdbool.h>
>
> #ifdef IN_WESTON
> #include <wayland-server.h>
> @@ -51,6 +52,7 @@ weston_matrix_init(struct weston_matrix *matrix)
> static const struct weston_matrix identity = {
> .d = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
> .type = 0,
> + .dirty = true,
> };
>
> memcpy(matrix, &identity, sizeof identity);
> @@ -74,6 +76,7 @@ weston_matrix_multiply(struct weston_matrix *m, const struct weston_matrix *n)
> tmp.d[i] += row[j] * column[j * 4];
> }
> tmp.type = m->type | n->type;
> + tmp.dirty = true;
> memcpy(m, &tmp, sizeof tmp);
> }
>
> @@ -271,3 +274,359 @@ weston_matrix_invert(struct weston_matrix *inverse,
>
> return 0;
> }
> +
> +/*
> + * Some of the following matrix/vector operations are taken
> + * from Graphics Gems I. In the process of converting them
> + * to weston data types, some of the original generality may
> + * have been destroyed. None of them should be used for
> + * general purpose operations without careful inspection,
> + * as soom only operate on parts of vectors, or do nothing
some
> + * to ensure input matrices can be use as output matrices...
> + */
> +
> +static void
> +transpose(struct weston_matrix *dst, struct weston_matrix *src)
> +{
> + int i,j;
> +
> + for (i = 0; i < 4; i++)
> + for (j = 0; j < 4; j++)
> + dst->d[i * 4 + j] = src->d[i + j * 4];
> +}
> +
> +static double
> +dotn(int n, struct weston_vector *a, struct weston_vector *b)
> +{
> + int i;
> + double out = 0.0;
> +
> + for (i = 0; i < n; i++)
> + out += a->f[i] * b->f[i];
> +
> + return out;
> +}
for weston_vectors, n = 3 always, right? Why not simplify it down
further and just
+static double
+dot(struct weston_vector *a, struct weston_vector *b)
+{
+ return a->f[0] * b->f[0] +
+ a->f[1] * b->f[1] +
+ a->f[2] * b->f[2];
+}
> +static struct weston_vector *
> +cross3(struct weston_vector *a, struct weston_vector *b, struct weston_vector *out)
> +{
> + out->f[0] = a->f[1] * b->f[2] - a->f[2] * b->f[1];
> + out->f[1] = a->f[2] * b->f[0] - a->f[0] * b->f[2];
> + out->f[2] = a->f[0] * b->f[1] - a->f[1] * b->f[0];
> + out->f[3] = 1.0;
> +
> + return out;
> +}
> +
> +static double
> +weston_vector_length(struct weston_vector *in)
> +{
> + return sqrt(in->f[0] * in->f[0] +
> + in->f[1] * in->f[1] +
> + in->f[2] * in->f[2]);
> +}
Can't we do:
return sqrt( dot(in, in) );
> +static void
> +weston_vector_normalize(struct weston_vector *in)
> +{
> + double len = weston_vector_length(in);
> + if (len == 0)
> + len = 1;
> + in->f[0] = in->f[0] / len;
> + in->f[1] = in->f[1] / len;
> + in->f[2] = in->f[2] / len;
> +}
> +
> +static void
> +weston_vector_combine(struct weston_vector *out,
> + struct weston_vector *a,
> + struct weston_vector *b,
> + double scale_a,
> + double scale_b)
> +{
> + out->f[0] = scale_a * a->f[0] + scale_b * b->f[0];
> + out->f[1] = scale_a * a->f[1] + scale_b * b->f[1];
> + out->f[2] = scale_a * a->f[2] + scale_b * b->f[2];
> +}
> +
> +static double
> +det2x2(double a, double b, double c, double d)
> +{
> + return a * d - b * c;
> +}
> +
> +static double
> +det3x3(double a1, double a2, double a3,
> + double b1, double b2, double b3,
> + double c1, double c2, double c3 )
> +{
> + return a1 * det2x2( b2, b3, c2, c3 )
> + - b1 * det2x2( a2, a3, c2, c3 )
> + + c1 * det2x2( a2, a3, b2, b3 );
> +}
> +
> +/*
> + * double = det4x4( matrix )
> + *
> + * calculate the determinant of a 4x4 matrix.
> + */
> +static double
> +det4x4(struct weston_matrix *m)
> +{
> + double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
> +
> + /* assign to individual variable names to aid selecting */
> + /* correct elements */
> +
> + a1 = m->d[0];
> + b1 = m->d[1];
> + c1 = m->d[2];
> + d1 = m->d[3];
> +
> + a2 = m->d[4];
> + b2 = m->d[5];
> + c2 = m->d[6];
> + d2 = m->d[7];
> +
> + a3 = m->d[8];
> + b3 = m->d[9];
> + c3 = m->d[10];
> + d3 = m->d[11];
> +
> + a4 = m->d[12];
> + b4 = m->d[13];
> + c4 = m->d[14];
> + d4 = m->d[15];
> +
> + return a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
> + - b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
> + + c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
> + - d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
> +}
> +
> +/* Based on code from Graphics Gems II:
> + * unmatrix.c - given a 4x4 matrix, decompose it into standard operations.
> + *
> + * Author: Spencer W. Thomas
> + * University of Michigan
Is he providing the code under open source friendly terms?
Might want to be explicit about it if so.
> + */
> +enum weston_decomposition_indices {
> + WESTON_MATRIX_SCALEX,
> + WESTON_MATRIX_SCALEY,
> + WESTON_MATRIX_SCALEZ,
> + WESTON_MATRIX_SHEARXY,
> + WESTON_MATRIX_SHEARXZ,
> + WESTON_MATRIX_SHEARYZ,
> + WESTON_MATRIX_ROTATEX,
> + WESTON_MATRIX_ROTATEY,
> + WESTON_MATRIX_ROTATEZ,
> + WESTON_MATRIX_TRANSX,
> + WESTON_MATRIX_TRANSY,
> + WESTON_MATRIX_TRANSZ,
> + WESTON_MATRIX_PERSPX,
> + WESTON_MATRIX_PERSPY,
> + WESTON_MATRIX_PERSPZ,
> + WESTON_MATRIX_PERSPW
> +};
> +
> +/* Decompose a non-degenerate 4x4 transformation matrix into
> + * the sequence of transformations that produced it.
> + * [Sx][Sy][Sz][Shearx/y][Sx/z][Sz/y][Rx][Ry][Rz][Tx][Ty][Tz][P(x,y,z,w)]
> + *
> + * The coefficient of each transformation is returned in the corresponding
> + * element of the vector tran.
> + *
> + * Returns 1 upon success, 0 if the matrix is singular.
> + */
> +static int
> +weston_matrix_decompose(struct weston_matrix *mat)
> +{
> + int i;
> + struct weston_matrix locmat;
> + struct weston_matrix pmat, invpmat, tinvpmat;
> + struct weston_vector prhs;
> + struct weston_vector row[3], pdum3;
> + float *tran = mat->decomposition;
> +
> + if (!mat->dirty) {
> + return !mat->singular;
> + }
> + mat->dirty = false;
> + locmat = *mat;
> + /* Normalize the matrix. */
> + if (locmat.d[15] == 0)
> + goto singular;
> +
> + for (i = 0; i < 16; i++ )
> + locmat.d[i] /= locmat.d[15];
> + /* pmat is used to solve for perspective, but it also provides
> + * an easy way to test for singularity of the upper 3x3 component.
> + */
> + pmat = locmat;
> + for (i = 0; i < 3; i++)
> + pmat.d[i * 4 + 3] = 0;
> + pmat.d[15] = 1;
> +
> + if (det4x4(&pmat) == 0.0)
> + goto singular;
> +
> + /* First, isolate perspective. This is the messiest. */
> + if (locmat.d[3] != 0 || locmat.d[7] != 0 || locmat.d[11] != 0 ) {
> + /* prhs is the right hand side of the equation. */
> + prhs.f[0] = locmat.d[3];
> + prhs.f[1] = locmat.d[7];
> + prhs.f[2] = locmat.d[11];
> + prhs.f[3] = locmat.d[15];
> +
> + /* Solve the equation by inverting pmat and multiplying
> + * prhs by the inverse. (This is the easiest way, not
> + * necessarily the best.)
> + * inverse function (and det4x4, above) from the Matrix
> + * Inversion gem in the first volume.
> + */
> + if (weston_matrix_invert(&invpmat, &pmat) < 0)
> + goto singular;
> + transpose(&tinvpmat, &invpmat);
> + weston_matrix_transform(&tinvpmat, &prhs);
> +
> + /* Stuff the answer away. */
> + tran[WESTON_MATRIX_PERSPX] = prhs.f[0];
> + tran[WESTON_MATRIX_PERSPY] = prhs.f[1];
> + tran[WESTON_MATRIX_PERSPZ] = prhs.f[2];
> + tran[WESTON_MATRIX_PERSPW] = prhs.f[3];
> + /* Clear the perspective partition. */
> + locmat.d[3] = locmat.d[7] = locmat.d[11] = 0;
> + locmat.d[15] = 1;
> + } else /* No perspective. */
> + tran[WESTON_MATRIX_PERSPX] = tran[WESTON_MATRIX_PERSPY]
> + = tran[WESTON_MATRIX_PERSPZ]
> + = tran[WESTON_MATRIX_PERSPW] = 0;
> +
> + /* Next take care of translation (easy). */
> + for (i = 0; i < 3; i++) {
> + tran[WESTON_MATRIX_TRANSX + i] = locmat.d[12 + i];
> + locmat.d[12 + i] = 0;
> + }
> +
> + /* Now get scale and shear. */
> + for (i = 0; i < 3; i++) {
> + row[i].f[0] = locmat.d[i * 4];
> + row[i].f[1] = locmat.d[i * 4 + 1];
> + row[i].f[2] = locmat.d[i * 4 + 2];
> + }
> +
> + /* Compute X scale factor and normalize first row. */
> + tran[WESTON_MATRIX_SCALEX] = weston_vector_length(&row[0]);
> + weston_vector_normalize(&row[0]);
> +
> + /* Compute XY shear factor and make 2nd row orthogonal to 1st. */
> + tran[WESTON_MATRIX_SHEARXY] = dotn(3, &row[0], &row[1]);
> + weston_vector_combine(&row[1], &row[1], &row[0], 1.0, -tran[WESTON_MATRIX_SHEARXY]);
> +
> + /* Now, compute Y scale and normalize 2nd row. */
> + tran[WESTON_MATRIX_SCALEY] = weston_vector_length(&row[1]);
> + weston_vector_normalize(&row[1]);
> + tran[WESTON_MATRIX_SHEARXY] /= tran[WESTON_MATRIX_SCALEY];
> +
> + /* Compute XZ and YZ shears, orthogonalize 3rd row. */
> + tran[WESTON_MATRIX_SHEARXZ] = dotn(3, &row[0], &row[2]);
> + weston_vector_combine(&row[2], &row[2], &row[0], 1.0, -tran[WESTON_MATRIX_SHEARXZ]);
> + tran[WESTON_MATRIX_SHEARYZ] = dotn(3, &row[1], &row[2]);
> + weston_vector_combine(&row[2], &row[2], &row[1], 1.0, -tran[WESTON_MATRIX_SHEARYZ]);
> +
> + /* Next, get Z scale and normalize 3rd row. */
> + tran[WESTON_MATRIX_SCALEZ] = weston_vector_length(&row[2]);
> + weston_vector_normalize(&row[2]);
> + tran[WESTON_MATRIX_SHEARXZ] /= tran[WESTON_MATRIX_SCALEZ];
> + tran[WESTON_MATRIX_SHEARYZ] /= tran[WESTON_MATRIX_SCALEZ];
> +
> + /* At this point, the matrix (in rows[]) is orthonormal.
> + * Check for a coordinate system flip. If the determinant
> + * is -1, then negate the matrix and the scaling factors.
> + */
> + if ( dotn(3, &row[0], cross3(&row[1], &row[2], &pdum3) ) < 0 )
> + for ( i = 0; i < 3; i++ ) {
> + tran[WESTON_MATRIX_SCALEX+i] *= -1;
> + row[i].f[0] *= -1;
> + row[i].f[1] *= -1;
> + row[i].f[2] *= -1;
> + }
> +
> + /* Now, get the rotations out, as described in the gem. */
> + tran[WESTON_MATRIX_ROTATEY] = asin(-row[0].f[2]);
> + if ( cos(tran[WESTON_MATRIX_ROTATEY]) != 0 ) {
> + tran[WESTON_MATRIX_ROTATEX] = atan2(row[1].f[2], row[2].f[2]);
> + tran[WESTON_MATRIX_ROTATEZ] = atan2(row[0].f[1], row[0].f[0]);
> + } else {
> + tran[WESTON_MATRIX_ROTATEX] = atan2(-row[2].f[0], row[1].f[1]);
> + tran[WESTON_MATRIX_ROTATEZ] = 0;
> + }
> + /* All done! */
> + return 1;
> +singular:
> + mat->singular = true;
> + return 0;
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_scale_x(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_SCALEX];
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_scale_y(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_SCALEY];
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_scale_z(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_SCALEZ];
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_translation_x(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_TRANSX];
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_translation_y(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_TRANSY];
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_translation_z(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_TRANSZ];
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_rotation_x(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_ROTATEX];
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_rotation_y(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_ROTATEY];
> +}
> +
> +WL_EXPORT float
> +weston_matrix_get_rotation_z(struct weston_matrix *matrix)
> +{
> + weston_matrix_decompose(matrix);
> + return matrix->decomposition[WESTON_MATRIX_ROTATEZ];
> +}
> diff --git a/shared/matrix.h b/shared/matrix.h
> index e5cf636..d771ac0 100644
> --- a/shared/matrix.h
> +++ b/shared/matrix.h
> @@ -28,6 +28,8 @@
> extern "C" {
> #endif
>
> +#include <stdbool.h>
> +
> enum weston_matrix_transform_type {
> WESTON_MATRIX_TRANSFORM_TRANSLATE = (1 << 0),
> WESTON_MATRIX_TRANSFORM_SCALE = (1 << 1),
> @@ -38,6 +40,9 @@ enum weston_matrix_transform_type {
> struct weston_matrix {
> float d[16];
> unsigned int type;
> + bool dirty;
> + bool singular;
> + float decomposition[16];
> };
>
> struct weston_vector {
> @@ -61,6 +66,32 @@ weston_matrix_transform(struct weston_matrix *matrix, struct weston_vector *v);
> int
> weston_matrix_invert(struct weston_matrix *inverse,
> const struct weston_matrix *matrix);
> +float
> +weston_matrix_get_scale_x(struct weston_matrix *matrix);
> +
> +float
> +weston_matrix_get_scale_y(struct weston_matrix *matrix);
> +
> +float
> +weston_matrix_get_scale_z(struct weston_matrix *matrix);
> +
> +float
> +weston_matrix_get_translation_x(struct weston_matrix *matrix);
> +
> +float
> +weston_matrix_get_translation_y(struct weston_matrix *matrix);
> +
> +float
> +weston_matrix_get_translation_z(struct weston_matrix *matrix);
> +
> +float
> +weston_matrix_get_rotation_x(struct weston_matrix *matrix);
> +
> +float
> +weston_matrix_get_rotation_y(struct weston_matrix *matrix);
> +
> +float
> +weston_matrix_get_rotation_z(struct weston_matrix *matrix);
>
> #ifdef UNIT_TEST
> # define MATRIX_TEST_EXPORT WL_EXPORT
Other than these two trivial things, this all looks like it'll make for
a great refactoring. I read through all the rest of the patches but
didn't see anything to comment on, so for the set:
Reviewed-by: Bryce Harrington <bryce at osg.samsung.com>
> --
> 2.1.0
>
> _______________________________________________
> wayland-devel mailing list
> wayland-devel at lists.freedesktop.org
> http://lists.freedesktop.org/mailman/listinfo/wayland-devel
More information about the wayland-devel
mailing list