1 /*
2 * Copyright © 2012 Collabora, Ltd.
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining
5 * a copy of this software and associated documentation files (the
6 * "Software"), to deal in the Software without restriction, including
7 * without limitation the rights to use, copy, modify, merge, publish,
8 * distribute, sublicense, and/or sell copies of the Software, and to
9 * permit persons to whom the Software is furnished to do so, subject to
10 * the following conditions:
11 *
12 * The above copyright notice and this permission notice (including the
13 * next paragraph) shall be included in all copies or substantial
14 * portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
20 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
21 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
22 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
23 * SOFTWARE.
24 */
25
26 #include "config.h"
27
28 #include <stdio.h>
29 #include <stdlib.h>
30 #include <math.h>
31 #include <unistd.h>
32 #include <signal.h>
33 #include <time.h>
34
35 #include <libweston/matrix.h>
36
37 struct inverse_matrix {
38 double LU[16]; /* column-major */
39 unsigned perm[4]; /* permutation */
40 };
41
42 static struct timespec begin_time;
43
44 static void
reset_timer(void)45 reset_timer(void)
46 {
47 clock_gettime(CLOCK_MONOTONIC, &begin_time);
48 }
49
50 static double
read_timer(void)51 read_timer(void)
52 {
53 struct timespec t;
54
55 clock_gettime(CLOCK_MONOTONIC, &t);
56 return (double)(t.tv_sec - begin_time.tv_sec) +
57 1e-9 * (t.tv_nsec - begin_time.tv_nsec);
58 }
59
60 static double
det3x3(const float * c0,const float * c1,const float * c2)61 det3x3(const float *c0, const float *c1, const float *c2)
62 {
63 return (double)
64 c0[0] * c1[1] * c2[2] +
65 c1[0] * c2[1] * c0[2] +
66 c2[0] * c0[1] * c1[2] -
67 c0[2] * c1[1] * c2[0] -
68 c1[2] * c2[1] * c0[0] -
69 c2[2] * c0[1] * c1[0];
70 }
71
72 static double
determinant(const struct weston_matrix * m)73 determinant(const struct weston_matrix *m)
74 {
75 double det = 0;
76 #if 1
77 /* develop on last row */
78 det -= m->d[3 + 0 * 4] * det3x3(&m->d[4], &m->d[8], &m->d[12]);
79 det += m->d[3 + 1 * 4] * det3x3(&m->d[0], &m->d[8], &m->d[12]);
80 det -= m->d[3 + 2 * 4] * det3x3(&m->d[0], &m->d[4], &m->d[12]);
81 det += m->d[3 + 3 * 4] * det3x3(&m->d[0], &m->d[4], &m->d[8]);
82 #else
83 /* develop on first row */
84 det += m->d[0 + 0 * 4] * det3x3(&m->d[5], &m->d[9], &m->d[13]);
85 det -= m->d[0 + 1 * 4] * det3x3(&m->d[1], &m->d[9], &m->d[13]);
86 det += m->d[0 + 2 * 4] * det3x3(&m->d[1], &m->d[5], &m->d[13]);
87 det -= m->d[0 + 3 * 4] * det3x3(&m->d[1], &m->d[5], &m->d[9]);
88 #endif
89 return det;
90 }
91
92 static void
print_permutation_matrix(const struct inverse_matrix * m)93 print_permutation_matrix(const struct inverse_matrix *m)
94 {
95 const unsigned *p = m->perm;
96 const char *row[4] = {
97 "1 0 0 0\n",
98 "0 1 0 0\n",
99 "0 0 1 0\n",
100 "0 0 0 1\n"
101 };
102
103 printf(" P =\n%s%s%s%s", row[p[0]], row[p[1]], row[p[2]], row[p[3]]);
104 }
105
106 static void
print_LU_decomposition(const struct inverse_matrix * m)107 print_LU_decomposition(const struct inverse_matrix *m)
108 {
109 unsigned r, c;
110
111 printf(" L "
112 " U\n");
113 for (r = 0; r < 4; ++r) {
114 double v;
115
116 for (c = 0; c < 4; ++c) {
117 if (c < r)
118 v = m->LU[r + c * 4];
119 else if (c == r)
120 v = 1.0;
121 else
122 v = 0.0;
123 printf(" %12.6f", v);
124 }
125
126 printf(" | ");
127
128 for (c = 0; c < 4; ++c) {
129 if (c >= r)
130 v = m->LU[r + c * 4];
131 else
132 v = 0.0;
133 printf(" %12.6f", v);
134 }
135 printf("\n");
136 }
137 }
138
139 static void
print_inverse_data_matrix(const struct inverse_matrix * m)140 print_inverse_data_matrix(const struct inverse_matrix *m)
141 {
142 unsigned r, c;
143
144 for (r = 0; r < 4; ++r) {
145 for (c = 0; c < 4; ++c)
146 printf(" %12.6f", m->LU[r + c * 4]);
147 printf("\n");
148 }
149
150 printf("permutation: ");
151 for (r = 0; r < 4; ++r)
152 printf(" %u", m->perm[r]);
153 printf("\n");
154 }
155
156 static void
print_matrix(const struct weston_matrix * m)157 print_matrix(const struct weston_matrix *m)
158 {
159 unsigned r, c;
160
161 for (r = 0; r < 4; ++r) {
162 for (c = 0; c < 4; ++c)
163 printf(" %14.6e", m->d[r + c * 4]);
164 printf("\n");
165 }
166 }
167
168 static double
frand(void)169 frand(void)
170 {
171 double r = random();
172 return r / (double)(RAND_MAX / 2) - 1.0f;
173 }
174
175 static void
randomize_matrix(struct weston_matrix * m)176 randomize_matrix(struct weston_matrix *m)
177 {
178 unsigned i;
179 for (i = 0; i < 16; ++i)
180 #if 1
181 m->d[i] = frand() * exp(10.0 * frand());
182 #else
183 m->d[i] = frand();
184 #endif
185 }
186
187 /* Take a matrix, compute inverse, multiply together
188 * and subtract the identity matrix to get the error matrix.
189 * Return the largest absolute value from the error matrix.
190 */
191 static double
test_inverse(struct weston_matrix * m)192 test_inverse(struct weston_matrix *m)
193 {
194 unsigned i;
195 struct inverse_matrix q;
196 double errsup = 0.0;
197
198 if (matrix_invert(q.LU, q.perm, m) != 0)
199 return INFINITY;
200
201 for (i = 0; i < 4; ++i)
202 inverse_transform(q.LU, q.perm, &m->d[i * 4]);
203
204 m->d[0] -= 1.0f;
205 m->d[5] -= 1.0f;
206 m->d[10] -= 1.0f;
207 m->d[15] -= 1.0f;
208
209 for (i = 0; i < 16; ++i) {
210 double err = fabs(m->d[i]);
211 if (err > errsup)
212 errsup = err;
213 }
214
215 return errsup;
216 }
217
218 enum {
219 TEST_OK,
220 TEST_NOT_INVERTIBLE_OK,
221 TEST_FAIL,
222 TEST_COUNT
223 };
224
225 static int
test(void)226 test(void)
227 {
228 struct weston_matrix m;
229 double det, errsup;
230
231 randomize_matrix(&m);
232 det = determinant(&m);
233
234 errsup = test_inverse(&m);
235 if (errsup < 1e-6)
236 return TEST_OK;
237
238 if (fabs(det) < 1e-5 && isinf(errsup))
239 return TEST_NOT_INVERTIBLE_OK;
240
241 printf("test fail, det: %g, error sup: %g\n", det, errsup);
242
243 return TEST_FAIL;
244 }
245
246 static int running;
247 static void
stopme(int n)248 stopme(int n)
249 {
250 running = 0;
251 }
252
253 static void
test_loop_precision(void)254 test_loop_precision(void)
255 {
256 int counts[TEST_COUNT] = { 0 };
257
258 printf("\nRunning a test loop for 10 seconds...\n");
259 running = 1;
260 alarm(10);
261 while (running) {
262 counts[test()]++;
263 }
264
265 printf("tests: %d ok, %d not invertible but ok, %d failed.\n"
266 "Total: %d iterations.\n",
267 counts[TEST_OK], counts[TEST_NOT_INVERTIBLE_OK],
268 counts[TEST_FAIL],
269 counts[TEST_OK] + counts[TEST_NOT_INVERTIBLE_OK] +
270 counts[TEST_FAIL]);
271 }
272
273 static void __attribute__((noinline))
test_loop_speed_matrixvector(void)274 test_loop_speed_matrixvector(void)
275 {
276 struct weston_matrix m;
277 struct weston_vector v = { { 0.5, 0.5, 0.5, 1.0 } };
278 unsigned long count = 0;
279 double t;
280
281 printf("\nRunning 3 s test on weston_matrix_transform()...\n");
282
283 weston_matrix_init(&m);
284
285 running = 1;
286 alarm(3);
287 reset_timer();
288 while (running) {
289 weston_matrix_transform(&m, &v);
290 count++;
291 }
292 t = read_timer();
293
294 printf("%lu iterations in %f seconds, avg. %.1f ns/iter.\n",
295 count, t, 1e9 * t / count);
296 }
297
298 static void __attribute__((noinline))
test_loop_speed_inversetransform(void)299 test_loop_speed_inversetransform(void)
300 {
301 struct weston_matrix m;
302 struct inverse_matrix inv;
303 struct weston_vector v = { { 0.5, 0.5, 0.5, 1.0 } };
304 unsigned long count = 0;
305 double t;
306
307 printf("\nRunning 3 s test on inverse_transform()...\n");
308
309 weston_matrix_init(&m);
310 matrix_invert(inv.LU, inv.perm, &m);
311
312 running = 1;
313 alarm(3);
314 reset_timer();
315 while (running) {
316 inverse_transform(inv.LU, inv.perm, v.f);
317 count++;
318 }
319 t = read_timer();
320
321 printf("%lu iterations in %f seconds, avg. %.1f ns/iter.\n",
322 count, t, 1e9 * t / count);
323 }
324
325 static void __attribute__((noinline))
test_loop_speed_invert(void)326 test_loop_speed_invert(void)
327 {
328 struct weston_matrix m;
329 struct inverse_matrix inv;
330 unsigned long count = 0;
331 double t;
332
333 printf("\nRunning 3 s test on matrix_invert()...\n");
334
335 weston_matrix_init(&m);
336
337 running = 1;
338 alarm(3);
339 reset_timer();
340 while (running) {
341 matrix_invert(inv.LU, inv.perm, &m);
342 count++;
343 }
344 t = read_timer();
345
346 printf("%lu iterations in %f seconds, avg. %.1f ns/iter.\n",
347 count, t, 1e9 * t / count);
348 }
349
350 static void __attribute__((noinline))
test_loop_speed_invert_explicit(void)351 test_loop_speed_invert_explicit(void)
352 {
353 struct weston_matrix m;
354 unsigned long count = 0;
355 double t;
356
357 printf("\nRunning 3 s test on weston_matrix_invert()...\n");
358
359 weston_matrix_init(&m);
360
361 running = 1;
362 alarm(3);
363 reset_timer();
364 while (running) {
365 weston_matrix_invert(&m, &m);
366 count++;
367 }
368 t = read_timer();
369
370 printf("%lu iterations in %f seconds, avg. %.1f ns/iter.\n",
371 count, t, 1e9 * t / count);
372 }
373
main(void)374 int main(void)
375 {
376 struct sigaction ding;
377 struct weston_matrix M;
378 struct inverse_matrix Q;
379 int ret;
380 double errsup;
381 double det;
382
383 ding.sa_handler = stopme;
384 sigemptyset(&ding.sa_mask);
385 ding.sa_flags = 0;
386 sigaction(SIGALRM, &ding, NULL);
387
388 srandom(13);
389
390 M.d[0] = 3.0; M.d[4] = 17.0; M.d[8] = 10.0; M.d[12] = 0.0;
391 M.d[1] = 2.0; M.d[5] = 4.0; M.d[9] = -2.0; M.d[13] = 0.0;
392 M.d[2] = 6.0; M.d[6] = 18.0; M.d[10] = -12; M.d[14] = 0.0;
393 M.d[3] = 0.0; M.d[7] = 0.0; M.d[11] = 0.0; M.d[15] = 1.0;
394
395 ret = matrix_invert(Q.LU, Q.perm, &M);
396 printf("ret = %d\n", ret);
397 printf("det = %g\n\n", determinant(&M));
398
399 if (ret != 0)
400 return 1;
401
402 print_inverse_data_matrix(&Q);
403 printf("P * A = L * U\n");
404 print_permutation_matrix(&Q);
405 print_LU_decomposition(&Q);
406
407
408 printf("a random matrix:\n");
409 randomize_matrix(&M);
410 det = determinant(&M);
411 print_matrix(&M);
412 errsup = test_inverse(&M);
413 printf("\nThe matrix multiplied by its inverse, error:\n");
414 print_matrix(&M);
415 printf("max abs error: %g, original determinant %g\n", errsup, det);
416
417 test_loop_precision();
418 test_loop_speed_matrixvector();
419 test_loop_speed_inversetransform();
420 test_loop_speed_invert();
421 test_loop_speed_invert_explicit();
422
423 return 0;
424 }
425