matrix.c 6.27 KB
Newer Older
1
2
/*
 * Copyright © 2011 Intel Corporation
3
 * Copyright © 2012 Collabora, Ltd.
4
 *
5
6
7
8
9
10
11
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
12
 *
13
14
15
16
17
18
19
20
21
22
23
24
 * The above copyright notice and this permission notice (including the
 * next paragraph) shall be included in all copies or substantial
 * portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT.  IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
25
26
 */

27
28
#include "config.h"

29
#include <float.h>
30
31
#include <string.h>
#include <stdlib.h>
32
#include <math.h>
33

34
#ifdef UNIT_TEST
35
#define WL_EXPORT
36
37
#else
#include <wayland-server.h>
38
#endif
39
40
41

#include "matrix.h"

42

43
44
45
46
47
48
49
50
51
52
53
54
/*
 * 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 = {
55
56
		.d = { 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1 },
		.type = 0,
57
58
59
60
61
62
63
64
65
66
	};

	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;
67
	const float *row, *column;
68
69
70
71
72
73
74
75
76
77
78
	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];
	}
79
	tmp.type = m->type | n->type;
80
81
82
83
	memcpy(m, &tmp, sizeof tmp);
}

WL_EXPORT void
84
weston_matrix_translate(struct weston_matrix *matrix, float x, float y, float z)
85
86
{
	struct weston_matrix translate = {
87
88
		.d = { 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  x, y, z, 1 },
		.type = WESTON_MATRIX_TRANSFORM_TRANSLATE,
89
90
91
92
93
94
	};

	weston_matrix_multiply(matrix, &translate);
}

WL_EXPORT void
95
weston_matrix_scale(struct weston_matrix *matrix, float x, float y,float z)
96
97
{
	struct weston_matrix scale = {
98
99
		.d = { x, 0, 0, 0,  0, y, 0, 0,  0, 0, z, 0,  0, 0, 0, 1 },
		.type = WESTON_MATRIX_TRANSFORM_SCALE,
100
101
102
103
104
	};

	weston_matrix_multiply(matrix, &scale);
}

105
106
107
108
109
110
111
112
113
114
115
WL_EXPORT void
weston_matrix_rotate_xy(struct weston_matrix *matrix, float cos, float sin)
{
	struct weston_matrix translate = {
		.d = { cos, sin, 0, 0,  -sin, cos, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1 },
		.type = WESTON_MATRIX_TRANSFORM_ROTATE,
	};

	weston_matrix_multiply(matrix, &translate);
}

116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
/* 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;
}
131

132
133
134
135
136
137
138
139
140
141
142
143
144
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;
	}
}

145
146
147
148
149
150
151
152
153
154
static inline void
swap_unsigned(unsigned *a, unsigned *b)
{
	unsigned tmp;

	tmp = *a;
	*a = *b;
	*b = tmp;
}

155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
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.
 */

172
173
MATRIX_TEST_EXPORT inline int
matrix_invert(double *A, unsigned *p, const struct weston_matrix *matrix)
174
{
175
176
177
178
	unsigned i, j, k;
	unsigned pivot;
	double pv;

179
180
	for (i = 0; i < 4; ++i)
		p[i] = i;
181
182
183
184
185
186
187
	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) {
188
			swap_unsigned(&p[k], &p[pivot]);
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
			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;
}

207
MATRIX_TEST_EXPORT inline void
208
inverse_transform(const double *LU, const unsigned *p, float *v)
209
210
211
212
213
214
{
	/* 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];
Pekka Paalanen's avatar
Pekka Paalanen committed
215
	unsigned j;
216
217
218

	/* Forward substitution, column version, solves L * b = P * v */
	/* The diagonal of L is all ones, and not explicitly stored. */
219
220
221
222
	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];
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
	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) {
Pekka Paalanen's avatar
Pekka Paalanen committed
245
		unsigned k;
246
247
248
249
250
251
252
253
254
255
		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)
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
		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]);
273
	inverse->type = matrix->type;
274
275

	return 0;
276
}