1/*
2Copyright (c) 2012, Broadcom Europe Ltd
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without
6modification, are permitted provided that the following conditions are met:
7 * Redistributions of source code must retain the above copyright
8 notice, this list of conditions and the following disclaimer.
9 * Redistributions in binary form must reproduce the above copyright
10 notice, this list of conditions and the following disclaimer in the
11 documentation and/or other materials provided with the distribution.
12 * Neither the name of the copyright holder nor the
13 names of its contributors may be used to endorse or promote products
14 derived from this software without specific prior written permission.
15
16THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26*/
27
28#include "interface/khronos/common/khrn_int_common.h"
29#include "interface/khronos/vg/vg_int_mat3x3.h"
30#include "interface/khronos/common/khrn_int_math.h"
31#include "interface/khronos/common/khrn_int_util.h"
32
33/*
34 Preconditions:
35
36 -
37
38 Postconditions:
39
40 a is the identity matrix.
41*/
42
43void vg_mat3x3_set_identity(VG_MAT3X3_T *a)
44{
45 a->m[0][0] = 1.0f; a->m[0][1] = 0.0f; a->m[0][2] = 0.0f;
46 a->m[1][0] = 0.0f; a->m[1][1] = 1.0f; a->m[1][2] = 0.0f;
47 a->m[2][0] = 0.0f; a->m[2][1] = 0.0f; a->m[2][2] = 1.0f;
48}
49
50/*
51 copies matrix (column-major) into a, "cleaning" the elements (see
52 clean_float). forces the bottom row of a to (0, 0, 1) if force_affine is
53 true.
54
55 Preconditions:
56
57 -
58
59 Postconditions:
60
61 no elements of a are nan or infinity.
62 if force_affine is true, a is affine.
63*/
64
65void vg_mat3x3_set_clean(VG_MAT3X3_T *a, const float *matrix, bool force_affine)
66{
67 a->m[0][0] = clean_float(matrix[0]);
68 a->m[0][1] = clean_float(matrix[3]);
69 a->m[0][2] = clean_float(matrix[6]);
70
71 a->m[1][0] = clean_float(matrix[1]);
72 a->m[1][1] = clean_float(matrix[4]);
73 a->m[1][2] = clean_float(matrix[7]);
74
75 if (force_affine) {
76 a->m[2][0] = 0.0f;
77 a->m[2][1] = 0.0f;
78 a->m[2][2] = 1.0f;
79 } else {
80 a->m[2][0] = clean_float(matrix[2]);
81 a->m[2][1] = clean_float(matrix[5]);
82 a->m[2][2] = clean_float(matrix[8]);
83 }
84}
85
86/*
87 Preconditions:
88
89 -
90
91 Postconditions:
92
93 matrix (column-major) is set to a.
94*/
95
96void vg_mat3x3_get(const VG_MAT3X3_T *a, float *matrix)
97{
98 matrix[0] = a->m[0][0];
99 matrix[3] = a->m[0][1];
100 matrix[6] = a->m[0][2];
101
102 matrix[1] = a->m[1][0];
103 matrix[4] = a->m[1][1];
104 matrix[7] = a->m[1][2];
105
106 matrix[2] = a->m[2][0];
107 matrix[5] = a->m[2][1];
108 matrix[8] = a->m[2][2];
109}
110
111/*
112 Preconditions:
113
114 -
115
116 Postconditions:
117
118 returns true iff a and b are bitwise identical.
119*/
120
121bool vg_mat3x3_identical(const VG_MAT3X3_T *a, const VG_MAT3X3_T *b)
122{
123 return floats_identical(a->m[0][0], b->m[0][0]) && floats_identical(a->m[0][1], b->m[0][1]) && floats_identical(a->m[0][2], b->m[0][2]) &&
124 floats_identical(a->m[1][0], b->m[1][0]) && floats_identical(a->m[1][1], b->m[1][1]) && floats_identical(a->m[1][2], b->m[1][2]) &&
125 floats_identical(a->m[2][0], b->m[2][0]) && floats_identical(a->m[2][1], b->m[2][1]) && floats_identical(a->m[2][2], b->m[2][2]);
126}
127
128/*
129 Preconditions:
130
131 a does not point to the same matrix as b or c.
132
133 Postconditions:
134
135 a is set to b * c.
136*/
137
138void vg_mat3x3_mul(VG_MAT3X3_T *a, const VG_MAT3X3_T *b, const VG_MAT3X3_T *c)
139{
140 uint32_t j, i;
141 for (j = 0; j != 3; ++j) {
142 for (i = 0; i != 3; ++i) {
143 a->m[j][i] =
144 (b->m[j][0] * c->m[0][i]) +
145 (b->m[j][1] * c->m[1][i]) +
146 (b->m[j][2] * c->m[2][i]);
147 }
148 }
149}
150
151/*
152 Preconditions:
153
154 -
155
156 Postconditions:
157
158 a is set to a * translation_matrix(x, y).
159*/
160
161void vg_mat3x3_postmul_translate(VG_MAT3X3_T *a, float x, float y)
162{
163 a->m[0][2] += (a->m[0][0] * x) + (a->m[0][1] * y);
164 a->m[1][2] += (a->m[1][0] * x) + (a->m[1][1] * y);
165 a->m[2][2] += (a->m[2][0] * x) + (a->m[2][1] * y);
166}
167
168/*
169 Preconditions:
170
171 -
172
173 Postconditions:
174
175 a is set to a * scale_matrix(x, y).
176*/
177
178void vg_mat3x3_postmul_scale(VG_MAT3X3_T *a, float x, float y)
179{
180 a->m[0][0] *= x;
181 a->m[0][1] *= y;
182
183 a->m[1][0] *= x;
184 a->m[1][1] *= y;
185
186 a->m[2][0] *= x;
187 a->m[2][1] *= y;
188}
189
190/*
191 Preconditions:
192
193 -
194
195 Postconditions:
196
197 a is set to a * shear_matrix(x, y).
198*/
199
200void vg_mat3x3_postmul_shear(VG_MAT3X3_T *a, float x, float y)
201{
202 float m00 = a->m[0][0], m10 = a->m[1][0], m20 = a->m[2][0];
203
204 a->m[0][0] += a->m[0][1] * y;
205 a->m[0][1] += m00 * x;
206
207 a->m[1][0] += a->m[1][1] * y;
208 a->m[1][1] += m10 * x;
209
210 a->m[2][0] += a->m[2][1] * y;
211 a->m[2][1] += m20 * x;
212}
213
214/*
215 Preconditions:
216
217 angle is in radians.
218
219 Postconditions:
220
221 a is set to a * rotation_matrix(angle).
222*/
223
224void vg_mat3x3_postmul_rotate(VG_MAT3X3_T *a, float angle)
225{
226 float s, c;
227 sin_cos_(&s, &c, angle);
228 vg_mat3x3_postmul_rotate_sc(a, s, c);
229}
230
231/*
232 Preconditions:
233
234 there is some angle such that:
235 s = sin(angle)
236 c = cos(angle)
237
238 Postconditions:
239
240 a is set to a * rotation_matrix(angle).
241*/
242
243void vg_mat3x3_postmul_rotate_sc(VG_MAT3X3_T *a, float s, float c)
244{
245 float m00 = a->m[0][0], m10 = a->m[1][0], m20 = a->m[2][0];
246
247 a->m[0][0] = (m00 * c) + (a->m[0][1] * s);
248 a->m[0][1] = (a->m[0][1] * c) - (m00 * s);
249
250 a->m[1][0] = (m10 * c) + (a->m[1][1] * s);
251 a->m[1][1] = (a->m[1][1] * c) - (m10 * s);
252
253 a->m[2][0] = (m20 * c) + (a->m[2][1] * s);
254 a->m[2][1] = (a->m[2][1] * c) - (m20 * s);
255}
256
257/*
258 Preconditions:
259
260 -
261
262 Postconditions:
263
264 a is set to translation_matrix(x, y) * a.
265*/
266
267void vg_mat3x3_premul_translate(VG_MAT3X3_T *a, float x, float y)
268{
269 a->m[0][0] += a->m[2][0] * x;
270 a->m[0][1] += a->m[2][1] * x;
271 a->m[0][2] += a->m[2][2] * x;
272
273 a->m[1][0] += a->m[2][0] * y;
274 a->m[1][1] += a->m[2][1] * y;
275 a->m[1][2] += a->m[2][2] * y;
276}
277
278/*
279 Preconditions:
280
281 -
282
283 Postconditions:
284
285 a is set to scale_matrix(x, y) * a.
286*/
287
288void vg_mat3x3_premul_scale(VG_MAT3X3_T *a, float x, float y)
289{
290 a->m[0][0] *= x;
291 a->m[0][1] *= x;
292 a->m[0][2] *= x;
293
294 a->m[1][0] *= y;
295 a->m[1][1] *= y;
296 a->m[1][2] *= y;
297}
298
299/*
300 Preconditions:
301
302 -
303
304 Postconditions:
305
306 a is set to shear_matrix(x, y) * a.
307*/
308
309void vg_mat3x3_premul_shear(VG_MAT3X3_T *a, float x, float y)
310{
311 float m00 = a->m[0][0], m01 = a->m[0][1], m02 = a->m[0][2];
312
313 a->m[0][0] += a->m[1][0] * x;
314 a->m[0][1] += a->m[1][1] * x;
315 a->m[0][2] += a->m[1][2] * x;
316
317 a->m[1][0] += m00 * y;
318 a->m[1][1] += m01 * y;
319 a->m[1][2] += m02 * y;
320}
321
322/*
323 Preconditions:
324
325 angle is in radians.
326
327 Postconditions:
328
329 a is set to rotation_matrix(angle) * a.
330*/
331
332void vg_mat3x3_premul_rotate(VG_MAT3X3_T *a, float angle)
333{
334 float s, c;
335 sin_cos_(&s, &c, angle);
336 vg_mat3x3_premul_rotate_sc(a, s, c);
337}
338
339/*
340 Preconditions:
341
342 there is some angle such that:
343 s = sin(angle)
344 c = cos(angle)
345
346 Postconditions:
347
348 a is set to rotation_matrix(angle) * a.
349*/
350
351void vg_mat3x3_premul_rotate_sc(VG_MAT3X3_T *a, float s, float c)
352{
353 float m00 = a->m[0][0], m01 = a->m[0][1], m02 = a->m[0][2];
354
355 a->m[0][0] = (m00 * c) - (a->m[1][0] * s);
356 a->m[0][1] = (m01 * c) - (a->m[1][1] * s);
357 a->m[0][2] = (m02 * c) - (a->m[1][2] * s);
358
359 a->m[1][0] = (m00 * s) + (a->m[1][0] * c);
360 a->m[1][1] = (m01 * s) + (a->m[1][1] * c);
361 a->m[1][2] = (m02 * s) + (a->m[1][2] * c);
362}
363
364/*
365 Preconditions:
366
367 -
368
369 Postconditions:
370
371 returns true iff a is affine.
372*/
373
374bool vg_mat3x3_is_affine(const VG_MAT3X3_T *a)
375{
376 return (a->m[2][0] == 0.0f) && (a->m[2][1] == 0.0f) && (a->m[2][2] == 1.0f);
377}
378
379/*
380 Preconditions:
381
382 -
383
384 Postconditions:
385
386 returns true iff a is affine or has nans in the bad elements.
387*/
388
389bool vg_mat3x3_is_affine_or_nans(const VG_MAT3X3_T *a)
390{
391 return !nan_ne_(a->m[2][0], 0.0f) && !nan_ne_(a->m[2][1], 0.0f) && !nan_ne_(a->m[2][2], 1.0f);
392}
393
394/*
395 Preconditions:
396
397 -
398
399 Postconditions:
400
401 returns the determinant of a.
402*/
403
404float vg_mat3x3_det(const VG_MAT3X3_T *a)
405{
406 return (a->m[0][0] * ((a->m[2][2] * a->m[1][1]) - (a->m[2][1] * a->m[1][2]))) +
407 (a->m[1][0] * ((a->m[0][2] * a->m[2][1]) - (a->m[0][1] * a->m[2][2]))) +
408 (a->m[2][0] * ((a->m[1][2] * a->m[0][1]) - (a->m[1][1] * a->m[0][2])));
409}
410
411/*
412 Preconditions:
413
414 a must be affine (or have nans in the bad elements).
415
416 Postconditions:
417
418 returns the determinant of a.
419*/
420
421float vg_mat3x3_affine_det(const VG_MAT3X3_T *a)
422{
423 vcos_assert(vg_mat3x3_is_affine_or_nans(a));
424 return (a->m[0][0] * a->m[1][1]) - (a->m[1][0] * a->m[0][1]);
425}
426
427/*
428 Preconditions:
429
430 -
431
432 Postconditions:
433
434 returns false iff a is not invertible (or very close to not being
435 invertible).
436*/
437
438bool vg_mat3x3_is_invertible(const VG_MAT3X3_T *a)
439{
440 return absf_(vg_mat3x3_det(a)) >= EPS;
441}
442
443/*
444 Preconditions:
445
446 a must be affine (or have nans in the bad elements).
447
448 Postconditions:
449
450 returns false iff a is not invertible (or very close to not being
451 invertible).
452*/
453
454bool vg_mat3x3_affine_is_invertible(const VG_MAT3X3_T *a)
455{
456 return absf_(vg_mat3x3_affine_det(a)) >= EPS;
457}
458
459/*
460 Preconditions:
461
462 a must be invertible, according to vg_mat3x3_is_invertible.
463
464 Postconditions:
465
466 a is inverted.
467*/
468
469void vg_mat3x3_invert(VG_MAT3X3_T *a)
470{
471 float oo_det;
472 VG_MAT3X3_T b;
473
474 vcos_assert(vg_mat3x3_is_invertible(a));
475
476 oo_det = recip_(vg_mat3x3_det(a));
477
478 b.m[0][0] = ((a->m[2][2] * a->m[1][1]) - (a->m[2][1] * a->m[1][2])) * oo_det;
479 b.m[0][1] = ((a->m[0][2] * a->m[2][1]) - (a->m[0][1] * a->m[2][2])) * oo_det;
480 b.m[0][2] = ((a->m[1][2] * a->m[0][1]) - (a->m[1][1] * a->m[0][2])) * oo_det;
481
482 b.m[1][0] = ((a->m[2][0] * a->m[1][2]) - (a->m[2][2] * a->m[1][0])) * oo_det;
483 b.m[1][1] = ((a->m[0][0] * a->m[2][2]) - (a->m[0][2] * a->m[2][0])) * oo_det;
484 b.m[1][2] = ((a->m[1][0] * a->m[0][2]) - (a->m[1][2] * a->m[0][0])) * oo_det;
485
486 b.m[2][0] = ((a->m[2][1] * a->m[1][0]) - (a->m[2][0] * a->m[1][1])) * oo_det;
487 b.m[2][1] = ((a->m[0][1] * a->m[2][0]) - (a->m[0][0] * a->m[2][1])) * oo_det;
488 b.m[2][2] = ((a->m[1][1] * a->m[0][0]) - (a->m[1][0] * a->m[0][1])) * oo_det;
489
490 *a = b;
491}
492
493/*
494 Preconditions:
495
496 a must be affine (or have nans in the bad elements).
497 a must be invertible, according to vg_mat3x3_affine_is_invertible.
498
499 Postconditions:
500
501 a is inverted.
502*/
503
504void vg_mat3x3_affine_invert(VG_MAT3X3_T *a)
505{
506 float oo_det;
507 float m00, m01, m02, m10;
508
509 vcos_assert(vg_mat3x3_affine_is_invertible(a));
510
511 oo_det = recip_(vg_mat3x3_affine_det(a));
512 m00 = a->m[0][0];
513 m01 = a->m[0][1];
514 m02 = a->m[0][2];
515 m10 = a->m[1][0];
516
517 a->m[0][0] = a->m[1][1] * oo_det;
518 a->m[0][1] = -m01 * oo_det;
519 a->m[0][2] = ((a->m[1][2] * m01) - (a->m[1][1] * m02)) * oo_det;
520
521 a->m[1][0] = -m10 * oo_det;
522 a->m[1][1] = m00 * oo_det;
523 a->m[1][2] = ((m10 * m02) - (a->m[1][2] * m00)) * oo_det;
524
525 a->m[2][0] = 0.0f;
526 a->m[2][1] = 0.0f;
527 a->m[2][2] = 1.0f;
528}
529
530/*
531 Preconditions:
532
533 -
534
535 Postconditions:
536
537 (x, y, -) is set to a * (x, y, 1).
538*/
539
540void vg_mat3x3_affine_transform(const VG_MAT3X3_T *a, float *x_io, float *y_io)
541{
542 float x = (a->m[0][0] * *x_io) + (a->m[0][1] * *y_io) + a->m[0][2];
543 float y = (a->m[1][0] * *x_io) + (a->m[1][1] * *y_io) + a->m[1][2];
544 *x_io = x;
545 *y_io = y;
546}
547
548/*
549 Preconditions:
550
551 -
552
553 Postconditions:
554
555 (x, y, -) is set to a * (x, y, 0).
556*/
557
558void vg_mat3x3_affine_transform_t(const VG_MAT3X3_T *a, float *x_io, float *y_io)
559{
560 float x = (a->m[0][0] * *x_io) + (a->m[0][1] * *y_io);
561 float y = (a->m[1][0] * *x_io) + (a->m[1][1] * *y_io);
562 *x_io = x;
563 *y_io = y;
564}
565
566/*
567 the top-left 2x2 submatrix of a is decomposed into (R * S * Q), where R is a
568 rotation matrix, S is a scale matrix, and Q is a rotation/flip matrix. note
569 that this decomposition is not unique.
570
571 Preconditions:
572
573 r may be NULL.
574 s0 may be NULL.
575 s1 may be NULL.
576
577 Postconditions:
578
579 if r is non-NULL, it is set to the angle of R (so R = rotation_matrix(r)) in
580 radians.
581 if s0 is non-NULL, it is set to S[0][0].
582 if s1 is non-NULL, it is set to S[1][1].
583 so if we set both, S = scale_matrix(s0, s1).
584*/
585
586void vg_mat3x3_rsq(const VG_MAT3X3_T *a,
587 float *r, float *s0, float *s1)
588{
589 /*
590 a = R * S * Q (svd, R is rotation, S is scale, Q is rotation/flip)
591 a^T = Q^T * S * R^T
592 a * a^T = R * S * Q * Q^T * S * R^T = R * S^2 * R^T
593
594 eigenvalues of a * a^T will give S^2
595 eigenvectors of a * a^T will give R
596 */
597
598 /*
599 ( b c ) = a * a^T
600 ( d e )
601 */
602
603 float b = (a->m[0][0] * a->m[0][0]) + (a->m[0][1] * a->m[0][1]);
604 float c = (a->m[0][0] * a->m[1][0]) + (a->m[0][1] * a->m[1][1]);
605 /* d = c */
606 float e = (a->m[1][0] * a->m[1][0]) + (a->m[1][1] * a->m[1][1]);
607
608 float bpe = b + e;
609 float bme = b - e;
610
611 /*
612 solve:
613
614 bx + cy = sx
615 dx + ey = sy
616
617 cy * dx = (s - b)x * (s - e)y
618 c^2 = (s - b) * (s - e)
619 s^2 - (b + e)s + (be - c^2) = 0
620 s = (b + e +/- sqrt((b + e)^2 - 4(be - c^2))) / 2
621 s = (b + e +/- sqrt(b^2 + e^2 - 2be + 4c^2)) / 2
622 s = (b + e +/- sqrt((b - e)^2 + 4c^2)) / 2
623 */
624
625 float t = sqrt_((bme * bme) + (4.0f * c * c));
626 float v = (bpe + t) * 0.5f; /* first eigenvalue */
627 if (s0) {
628 *s0 = sqrt_(v);
629 }
630 if (s1) {
631 *s1 = sqrt_(
632 /* second eigenvalue */
633 _maxf(bpe - t, 0.0f) * 0.5f);
634 }
635
636 /*
637 angle of eigenvector corresponds to r
638 */
639
640 if (r) {
641 /* first eigenvector is (c, v - b) / (v - e, c) */
642 float x = (v - e) + c;
643 float y = (v - b) + c;
644 *r = ((absf_(x) < EPS) && (absf_(y) < EPS)) ? 0.0f : atan2_(y, x);
645 }
646}
647