[RFC weston 12/16] matrix: Add functions to decompose a transformation matrix into basic operations
Derek Foreman
derekf at osg.samsung.com
Fri Sep 26 14:10:23 PDT 2014
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
+ * 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;
+}
+
+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]);
+}
+
+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
+ */
+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
--
2.1.0
More information about the wayland-devel
mailing list