Move matrix.[ch] to shared

This means it can be used for the calibration tool.

3 files changed, 316 insertions, 1 deletions

diff --git a/shared/Makefile.am b/shared/Makefile.am
index b38cb95d..faf073db 100644
--- a/shared/Makefile.am
+++ b/shared/Makefile.am
@@ -7,7 +7,9 @@ libshared_la_SOURCES =				\
 	option-parser.c				\
 	config-parser.h				\
 	os-compatibility.c			\
-	os-compatibility.h
+	os-compatibility.h			\
+	matrix.c				\
+	matrix.h
 
 libshared_cairo_la_CFLAGS =			\
 	$(GCC_CFLAGS)				\
diff --git a/shared/matrix.c b/shared/matrix.c
new file mode 100644
index 00000000..91acdd3d
--- /dev/null
+++ b/shared/matrix.c
@@ -0,0 +1,249 @@
+/*
+ * Copyright © 2011 Intel Corporation
+ * Copyright © 2012 Collabora, Ltd.
+ *
+ * Permission to use, copy, modify, distribute, and sell this software and
+ * its documentation for any purpose is hereby granted without fee, provided
+ * that the above copyright notice appear in all copies and that both that
+ * copyright notice and this permission notice appear in supporting
+ * documentation, and that the name of the copyright holders not be used in
+ * advertising or publicity pertaining to distribution of the software
+ * without specific, written prior permission.  The copyright holders make
+ * no representations about the suitability of this software for any
+ * purpose.  It is provided "as is" without express or implied warranty.
+ *
+ * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS
+ * SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ * FITNESS, IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
+ * RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
+ * CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <wayland-server.h>
+
+#include "matrix.h"
+
+
+/*
+ * Matrices are stored in column-major order, that is the array indices are:
+ *  0  4  8 12
+ *  1  5  9 13
+ *  2  6 10 14
+ *  3  7 11 15
+ */
+
+WL_EXPORT void
+weston_matrix_init(struct weston_matrix *matrix)
+{
+	static const struct weston_matrix identity = {
+		{ 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1 }
+	};
+
+	memcpy(matrix, &identity, sizeof identity);
+}
+
+/* m <- n * m, that is, m is multiplied on the LEFT. */
+WL_EXPORT void
+weston_matrix_multiply(struct weston_matrix *m, const struct weston_matrix *n)
+{
+	struct weston_matrix tmp;
+	const float *row, *column;
+	div_t d;
+	int i, j;
+
+	for (i = 0; i < 16; i++) {
+		tmp.d[i] = 0;
+		d = div(i, 4);
+		row = m->d + d.quot * 4;
+		column = n->d + d.rem;
+		for (j = 0; j < 4; j++)
+			tmp.d[i] += row[j] * column[j * 4];
+	}
+	memcpy(m, &tmp, sizeof tmp);
+}
+
+WL_EXPORT void
+weston_matrix_translate(struct weston_matrix *matrix, float x, float y, float z)
+{
+	struct weston_matrix translate = {
+		{ 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  x, y, z, 1 }
+	};
+
+	weston_matrix_multiply(matrix, &translate);
+}
+
+WL_EXPORT void
+weston_matrix_scale(struct weston_matrix *matrix, float x, float y,float z)
+{
+	struct weston_matrix scale = {
+		{ x, 0, 0, 0,  0, y, 0, 0,  0, 0, z, 0,  0, 0, 0, 1 }
+	};
+
+	weston_matrix_multiply(matrix, &scale);
+}
+
+/* v <- m * v */
+WL_EXPORT void
+weston_matrix_transform(struct weston_matrix *matrix, struct weston_vector *v)
+{
+	int i, j;
+	struct weston_vector t;
+
+	for (i = 0; i < 4; i++) {
+		t.f[i] = 0;
+		for (j = 0; j < 4; j++)
+			t.f[i] += v->f[j] * matrix->d[i + j * 4];
+	}
+
+	*v = t;
+}
+
+static inline void
+swap_rows(double *a, double *b)
+{
+	unsigned k;
+	double tmp;
+
+	for (k = 0; k < 13; k += 4) {
+		tmp = a[k];
+		a[k] = b[k];
+		b[k] = tmp;
+	}
+}
+
+static inline void
+swap_unsigned(unsigned *a, unsigned *b)
+{
+	unsigned tmp;
+
+	tmp = *a;
+	*a = *b;
+	*b = tmp;
+}
+
+static inline unsigned
+find_pivot(double *column, unsigned k)
+{
+	unsigned p = k;
+	for (++k; k < 4; ++k)
+		if (fabs(column[p]) < fabs(column[k]))
+			p = k;
+
+	return p;
+}
+
+/*
+ * reference: Gene H. Golub and Charles F. van Loan. Matrix computations.
+ * 3rd ed. The Johns Hopkins University Press. 1996.
+ * LU decomposition, forward and back substitution: Chapter 3.
+ */
+
+MATRIX_TEST_EXPORT inline int
+matrix_invert(double *A, unsigned *p, const struct weston_matrix *matrix)
+{
+	unsigned i, j, k;
+	unsigned pivot;
+	double pv;
+
+	for (i = 0; i < 4; ++i)
+		p[i] = i;
+	for (i = 16; i--; )
+		A[i] = matrix->d[i];
+
+	/* LU decomposition with partial pivoting */
+	for (k = 0; k < 4; ++k) {
+		pivot = find_pivot(&A[k * 4], k);
+		if (pivot != k) {
+			swap_unsigned(&p[k], &p[pivot]);
+			swap_rows(&A[k], &A[pivot]);
+		}
+
+		pv = A[k * 4 + k];
+		if (fabs(pv) < 1e-9)
+			return -1; /* zero pivot, not invertible */
+
+		for (i = k + 1; i < 4; ++i) {
+			A[i + k * 4] /= pv;
+
+			for (j = k + 1; j < 4; ++j)
+				A[i + j * 4] -= A[i + k * 4] * A[k + j * 4];
+		}
+	}
+
+	return 0;
+}
+
+MATRIX_TEST_EXPORT inline void
+inverse_transform(const double *LU, const unsigned *p, float *v)
+{
+	/* Solve A * x = v, when we have P * A = L * U.
+	 * P * A * x = P * v  =>  L * U * x = P * v
+	 * Let U * x = b, then L * b = P * v.
+	 */
+	double b[4];
+	unsigned j;
+
+	/* Forward substitution, column version, solves L * b = P * v */
+	/* The diagonal of L is all ones, and not explicitly stored. */
+	b[0] = v[p[0]];
+	b[1] = (double)v[p[1]] - b[0] * LU[1 + 0 * 4];
+	b[2] = (double)v[p[2]] - b[0] * LU[2 + 0 * 4];
+	b[3] = (double)v[p[3]] - b[0] * LU[3 + 0 * 4];
+	b[2] -= b[1] * LU[2 + 1 * 4];
+	b[3] -= b[1] * LU[3 + 1 * 4];
+	b[3] -= b[2] * LU[3 + 2 * 4];
+
+	/* backward substitution, column version, solves U * y = b */
+#if 1
+	/* hand-unrolled, 25% faster for whole function */
+	b[3] /= LU[3 + 3 * 4];
+	b[0] -= b[3] * LU[0 + 3 * 4];
+	b[1] -= b[3] * LU[1 + 3 * 4];
+	b[2] -= b[3] * LU[2 + 3 * 4];
+
+	b[2] /= LU[2 + 2 * 4];
+	b[0] -= b[2] * LU[0 + 2 * 4];
+	b[1] -= b[2] * LU[1 + 2 * 4];
+
+	b[1] /= LU[1 + 1 * 4];
+	b[0] -= b[1] * LU[0 + 1 * 4];
+
+	b[0] /= LU[0 + 0 * 4];
+#else
+	for (j = 3; j > 0; --j) {
+		unsigned k;
+		b[j] /= LU[j + j * 4];
+		for (k = 0; k < j; ++k)
+			b[k] -= b[j] * LU[k + j * 4];
+	}
+
+	b[0] /= LU[0 + 0 * 4];
+#endif
+
+	/* the result */
+	for (j = 0; j < 4; ++j)
+		v[j] = b[j];
+}
+
+WL_EXPORT int
+weston_matrix_invert(struct weston_matrix *inverse,
+		     const struct weston_matrix *matrix)
+{
+	double LU[16];		/* column-major */
+	unsigned perm[4];	/* permutation */
+	unsigned c;
+
+	if (matrix_invert(LU, perm, matrix) < 0)
+		return -1;
+
+	weston_matrix_init(inverse);
+	for (c = 0; c < 4; ++c)
+		inverse_transform(LU, perm, &inverse->d[c * 4]);
+
+	return 0;
+}
diff --git a/shared/matrix.h b/shared/matrix.h
new file mode 100644
index 00000000..bacb7bf3
--- /dev/null
+++ b/shared/matrix.h
@@ -0,0 +1,64 @@
+/*
+ * Copyright © 2008-2011 Kristian Høgsberg
+ * Copyright © 2012 Collabora, Ltd.
+ *
+ * Permission to use, copy, modify, distribute, and sell this software and
+ * its documentation for any purpose is hereby granted without fee, provided
+ * that the above copyright notice appear in all copies and that both that
+ * copyright notice and this permission notice appear in supporting
+ * documentation, and that the name of the copyright holders not be used in
+ * advertising or publicity pertaining to distribution of the software
+ * without specific, written prior permission.  The copyright holders make
+ * no representations about the suitability of this software for any
+ * purpose.  It is provided "as is" without express or implied warranty.
+ *
+ * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS
+ * SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ * FITNESS, IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
+ * RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
+ * CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef WESTON_MATRIX_H
+#define WESTON_MATRIX_H
+
+struct weston_matrix {
+	float d[16];
+};
+
+struct weston_vector {
+	float f[4];
+};
+
+void
+weston_matrix_init(struct weston_matrix *matrix);
+void
+weston_matrix_multiply(struct weston_matrix *m, const struct weston_matrix *n);
+void
+weston_matrix_scale(struct weston_matrix *matrix, float x, float y, float z);
+void
+weston_matrix_translate(struct weston_matrix *matrix,
+			float x, float y, float z);
+void
+weston_matrix_transform(struct weston_matrix *matrix, struct weston_vector *v);
+
+int
+weston_matrix_invert(struct weston_matrix *inverse,
+		     const struct weston_matrix *matrix);
+
+#ifdef UNIT_TEST
+#  define MATRIX_TEST_EXPORT WL_EXPORT
+
+int
+matrix_invert(double *A, unsigned *p, const struct weston_matrix *matrix);
+
+void
+inverse_transform(const double *LU, const unsigned *p, float *v);
+
+#else
+#  define MATRIX_TEST_EXPORT static
+#endif
+
+#endif /* WESTON_MATRIX_H */