[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

```