1 /*
2 * Copyright © 2011 Intel Corporation
3 * Copyright © 2012 Collabora, Ltd.
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining
6 * a copy of this software and associated documentation files (the
7 * "Software"), to deal in the Software without restriction, including
8 * without limitation the rights to use, copy, modify, merge, publish,
9 * distribute, sublicense, and/or sell copies of the Software, and to
10 * permit persons to whom the Software is furnished to do so, subject to
11 * the following conditions:
12 *
13 * The above copyright notice and this permission notice (including the
14 * next paragraph) shall be included in all copies or substantial
15 * portions of the Software.
16 *
17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
18 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
20 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
21 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
22 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
23 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24 * SOFTWARE.
25 */
26
27 #include "config.h"
28
29 #include <float.h>
30 #include <string.h>
31 #include <stdlib.h>
32 #include <math.h>
33
34 #ifdef UNIT_TEST
35 #define WL_EXPORT
36 #else
37 #include <wayland-server.h>
38 #endif
39
40 #include <libweston/matrix.h>
41
42
43 /*
44 * Matrices are stored in column-major order, that is the array indices are:
45 * 0 4 8 12
46 * 1 5 9 13
47 * 2 6 10 14
48 * 3 7 11 15
49 */
50
51 WL_EXPORT void
weston_matrix_init(struct weston_matrix * matrix)52 weston_matrix_init(struct weston_matrix *matrix)
53 {
54 static const struct weston_matrix identity = {
55 .d = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
56 .type = 0,
57 };
58
59 memcpy(matrix, &identity, sizeof identity);
60 }
61
62 /* m <- n * m, that is, m is multiplied on the LEFT. */
63 WL_EXPORT void
weston_matrix_multiply(struct weston_matrix * m,const struct weston_matrix * n)64 weston_matrix_multiply(struct weston_matrix *m, const struct weston_matrix *n)
65 {
66 struct weston_matrix tmp;
67 const float *row, *column;
68 div_t d;
69 int i, j;
70
71 for (i = 0; i < 16; i++) {
72 tmp.d[i] = 0;
73 d = div(i, 4);
74 row = m->d + d.quot * 4;
75 column = n->d + d.rem;
76 for (j = 0; j < 4; j++)
77 tmp.d[i] += row[j] * column[j * 4];
78 }
79 tmp.type = m->type | n->type;
80 memcpy(m, &tmp, sizeof tmp);
81 }
82
83 WL_EXPORT void
weston_matrix_translate(struct weston_matrix * matrix,float x,float y,float z)84 weston_matrix_translate(struct weston_matrix *matrix, float x, float y, float z)
85 {
86 struct weston_matrix translate = {
87 .d = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, x, y, z, 1 },
88 .type = WESTON_MATRIX_TRANSFORM_TRANSLATE,
89 };
90
91 weston_matrix_multiply(matrix, &translate);
92 }
93
94 WL_EXPORT void
weston_matrix_scale(struct weston_matrix * matrix,float x,float y,float z)95 weston_matrix_scale(struct weston_matrix *matrix, float x, float y,float z)
96 {
97 struct weston_matrix scale = {
98 .d = { x, 0, 0, 0, 0, y, 0, 0, 0, 0, z, 0, 0, 0, 0, 1 },
99 .type = WESTON_MATRIX_TRANSFORM_SCALE,
100 };
101
102 weston_matrix_multiply(matrix, &scale);
103 }
104
105 WL_EXPORT void
weston_matrix_rotate_xy(struct weston_matrix * matrix,float cos,float sin)106 weston_matrix_rotate_xy(struct weston_matrix *matrix, float cos, float sin)
107 {
108 struct weston_matrix translate = {
109 .d = { cos, sin, 0, 0, -sin, cos, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
110 .type = WESTON_MATRIX_TRANSFORM_ROTATE,
111 };
112
113 weston_matrix_multiply(matrix, &translate);
114 }
115
116 /* v <- m * v */
117 WL_EXPORT void
weston_matrix_transform(struct weston_matrix * matrix,struct weston_vector * v)118 weston_matrix_transform(struct weston_matrix *matrix, struct weston_vector *v)
119 {
120 int i, j;
121 struct weston_vector t;
122
123 for (i = 0; i < 4; i++) {
124 t.f[i] = 0;
125 for (j = 0; j < 4; j++)
126 t.f[i] += v->f[j] * matrix->d[i + j * 4];
127 }
128
129 *v = t;
130 }
131
132 static inline void
swap_rows(double * a,double * b)133 swap_rows(double *a, double *b)
134 {
135 unsigned k;
136 double tmp;
137
138 for (k = 0; k < 13; k += 4) {
139 tmp = a[k];
140 a[k] = b[k];
141 b[k] = tmp;
142 }
143 }
144
145 static inline void
swap_unsigned(unsigned * a,unsigned * b)146 swap_unsigned(unsigned *a, unsigned *b)
147 {
148 unsigned tmp;
149
150 tmp = *a;
151 *a = *b;
152 *b = tmp;
153 }
154
155 static inline unsigned
find_pivot(double * column,unsigned k)156 find_pivot(double *column, unsigned k)
157 {
158 unsigned p = k;
159 for (++k; k < 4; ++k)
160 if (fabs(column[p]) < fabs(column[k]))
161 p = k;
162
163 return p;
164 }
165
166 /*
167 * reference: Gene H. Golub and Charles F. van Loan. Matrix computations.
168 * 3rd ed. The Johns Hopkins University Press. 1996.
169 * LU decomposition, forward and back substitution: Chapter 3.
170 */
171
172 MATRIX_TEST_EXPORT inline int
matrix_invert(double * A,unsigned * p,const struct weston_matrix * matrix)173 matrix_invert(double *A, unsigned *p, const struct weston_matrix *matrix)
174 {
175 unsigned i, j, k;
176 unsigned pivot;
177 double pv;
178
179 for (i = 0; i < 4; ++i)
180 p[i] = i;
181 for (i = 16; i--; )
182 A[i] = matrix->d[i];
183
184 /* LU decomposition with partial pivoting */
185 for (k = 0; k < 4; ++k) {
186 pivot = find_pivot(&A[k * 4], k);
187 if (pivot != k) {
188 swap_unsigned(&p[k], &p[pivot]);
189 swap_rows(&A[k], &A[pivot]);
190 }
191
192 pv = A[k * 4 + k];
193 if (fabs(pv) < 1e-9)
194 return -1; /* zero pivot, not invertible */
195
196 for (i = k + 1; i < 4; ++i) {
197 A[i + k * 4] /= pv;
198
199 for (j = k + 1; j < 4; ++j)
200 A[i + j * 4] -= A[i + k * 4] * A[k + j * 4];
201 }
202 }
203
204 return 0;
205 }
206
207 MATRIX_TEST_EXPORT inline void
inverse_transform(const double * LU,const unsigned * p,float * v)208 inverse_transform(const double *LU, const unsigned *p, float *v)
209 {
210 /* Solve A * x = v, when we have P * A = L * U.
211 * P * A * x = P * v => L * U * x = P * v
212 * Let U * x = b, then L * b = P * v.
213 */
214 double b[4];
215 unsigned j;
216
217 /* Forward substitution, column version, solves L * b = P * v */
218 /* The diagonal of L is all ones, and not explicitly stored. */
219 b[0] = v[p[0]];
220 b[1] = (double)v[p[1]] - b[0] * LU[1 + 0 * 4];
221 b[2] = (double)v[p[2]] - b[0] * LU[2 + 0 * 4];
222 b[3] = (double)v[p[3]] - b[0] * LU[3 + 0 * 4];
223 b[2] -= b[1] * LU[2 + 1 * 4];
224 b[3] -= b[1] * LU[3 + 1 * 4];
225 b[3] -= b[2] * LU[3 + 2 * 4];
226
227 /* backward substitution, column version, solves U * y = b */
228 #if 1
229 /* hand-unrolled, 25% faster for whole function */
230 b[3] /= LU[3 + 3 * 4];
231 b[0] -= b[3] * LU[0 + 3 * 4];
232 b[1] -= b[3] * LU[1 + 3 * 4];
233 b[2] -= b[3] * LU[2 + 3 * 4];
234
235 b[2] /= LU[2 + 2 * 4];
236 b[0] -= b[2] * LU[0 + 2 * 4];
237 b[1] -= b[2] * LU[1 + 2 * 4];
238
239 b[1] /= LU[1 + 1 * 4];
240 b[0] -= b[1] * LU[0 + 1 * 4];
241
242 b[0] /= LU[0 + 0 * 4];
243 #else
244 for (j = 3; j > 0; --j) {
245 unsigned k;
246 b[j] /= LU[j + j * 4];
247 for (k = 0; k < j; ++k)
248 b[k] -= b[j] * LU[k + j * 4];
249 }
250
251 b[0] /= LU[0 + 0 * 4];
252 #endif
253
254 /* the result */
255 for (j = 0; j < 4; ++j)
256 v[j] = b[j];
257 }
258
259 WL_EXPORT int
weston_matrix_invert(struct weston_matrix * inverse,const struct weston_matrix * matrix)260 weston_matrix_invert(struct weston_matrix *inverse,
261 const struct weston_matrix *matrix)
262 {
263 double LU[16]; /* column-major */
264 unsigned perm[4]; /* permutation */
265 unsigned c;
266
267 if (matrix_invert(LU, perm, matrix) < 0)
268 return -1;
269
270 weston_matrix_init(inverse);
271 for (c = 0; c < 4; ++c)
272 inverse_transform(LU, perm, &inverse->d[c * 4]);
273 inverse->type = matrix->type;
274
275 return 0;
276 }
277