1 files changed, 170 insertions, 0 deletions
diff --git a/common/mat33.c b/common/mat33.c
new file mode 100644
index 0000000000..8317ee8a97
--- /dev/null
+++ b/common/mat33.c
@@ -0,0 +1,170 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "common.h"
+#include "mat33.h"
+#include "math.h"
+#include "util.h"
+
+#define K_EPSILON 1E-5f
+
+void init_zero_matrix(mat33_t A)
+{
+	memset(A, 0, sizeof(mat33_t));
+}
+
+void init_diagonal_matrix(mat33_t A, float x)
+{
+	size_t i;
+	init_zero_matrix(A);
+
+	for (i = 0; i < 3; ++i)
+		A[i][i] = x;
+}
+
+void mat33_scalar_mul(mat33_t A, float c)
+{
+	size_t i;
+	for (i = 0; i < 3; ++i) {
+		size_t j;
+		for (j = 0; j < 3; ++j)
+			A[i][j] *= c;
+	}
+}
+
+void mat33_swap_rows(mat33_t A, const size_t i, const size_t j)
+{
+	const size_t N = 3;
+	size_t k;
+
+	if (i == j)
+		return;
+
+	for (k = 0; k < N; ++k) {
+		float tmp = A[i][k];
+		A[i][k] = A[j][k];
+		A[j][k] = tmp;
+	}
+}
+
+/*
+ * Returns the eigenvalues and corresponding eigenvectors of the _symmetric_
+ * matrix.
+ * The i-th eigenvalue corresponds to the eigenvector in the i-th _row_ of
+ * "eigenvecs".
+ */
+void mat33_get_eigenbasis(mat33_t S, vec3_t e_vals, mat33_t e_vecs)
+{
+	const size_t N = 3;
+	size3_t ind;
+	size_t i, j, k, l, m;
+
+	for (k = 0; k < N; ++k) {
+		ind[k] = mat33_maxind(S, k);
+		e_vals[k] = S[k][k];
+	}
+
+	init_diagonal_matrix(e_vecs, 1.0f);
+
+	for (;;) {
+		float y, t, s, c, p, sum;
+		m = 0;
+		for (k = 1; k + 1 < N; ++k) {
+			if (fabsf(S[k][ind[k]]) >
+			    fabsf(S[m][ind[m]])) {
+				m = k;
+			}
+		}
+
+		k = m;
+		l = ind[m];
+		p = S[k][l];
+
+		if (fabsf(p) < K_EPSILON)
+			break;
+
+		y = (e_vals[l] - e_vals[k]) * 0.5f;
+
+		t = fabsf(y) + sqrtf(p * p + y * y);
+		s = sqrtf(p * p + t * t);
+		c = t / s;
+		s = p / s;
+		t = p * p / t;
+
+		if (y < 0.0f) {
+			s = -s;
+			t = -t;
+		}
+
+		S[k][l] = 0.0f;
+
+		e_vals[k] -= t;
+		e_vals[l] += t;
+
+		for (i = 0; i < k; ++i)
+			mat33_rotate(S, c, s, i, k, i, l);
+
+		for (i = k + 1; i < l; ++i)
+			mat33_rotate(S, c, s, k, i, i, l);
+
+		for (i = l + 1; i < N; ++i)
+			mat33_rotate(S, c, s, k, i, l, i);
+
+		for (i = 0; i < N; ++i) {
+			float tmp = c * e_vecs[k][i] - s * e_vecs[l][i];
+			e_vecs[l][i] = s * e_vecs[k][i] + c * e_vecs[l][i];
+			e_vecs[k][i] = tmp;
+		}
+
+		ind[k] = mat33_maxind(S, k);
+		ind[l] = mat33_maxind(S, l);
+
+		sum = 0.0f;
+		for (i = 0; i < N; ++i)
+			for (j = i + 1; j < N; ++j)
+				sum += fabsf(S[i][j]);
+
+		if (sum < K_EPSILON)
+			break;
+	}
+
+	for (k = 0; k < N; ++k) {
+		m = k;
+		for (l = k + 1; l < N; ++l)
+			if (e_vals[l] > e_vals[m])
+				m = l;
+
+		if (k != m) {
+			float tmp = e_vals[k];
+			e_vals[k] = e_vals[m];
+			e_vals[m] = tmp;
+
+			mat33_swap_rows(e_vecs, k, m);
+		}
+	}
+}
+
+/* index of largest off-diagonal element in row k */
+size_t mat33_maxind(mat33_t A, size_t k)
+{
+	const size_t N = 3;
+	size_t i, m = k + 1;
+
+	for (i = k + 2; i < N; ++i)
+		if (fabsf(A[k][i]) > fabsf(A[k][m]))
+			m = i;
+
+	return m;
+}
+
+void mat33_rotate(mat33_t A, float c, float s,
+		  size_t k, size_t l, size_t i, size_t j)
+{
+	float tmp = c * A[k][l] - s * A[i][j];
+	A[i][j] = s * A[k][l] + c * A[i][j];
+	A[k][l] = tmp;
+}
+
+