2 * Copyright © 2011 Intel Corporation
3 * Copyright © 2012 Collabora, Ltd.
5 * Permission to use, copy, modify, distribute, and sell this software and
6 * its documentation for any purpose is hereby granted without fee, provided
7 * that the above copyright notice appear in all copies and that both that
8 * copyright notice and this permission notice appear in supporting
9 * documentation, and that the name of the copyright holders not be used in
10 * advertising or publicity pertaining to distribution of the software
11 * without specific, written prior permission. The copyright holders make
12 * no representations about the suitability of this software for any
13 * purpose. It is provided "as is" without express or implied warranty.
15 * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS
16 * SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
17 * FITNESS, IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
18 * SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
19 * RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
20 * CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
21 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
30 #include <wayland-server.h>
39 * Matrices are stored in column-major order, that is the array indices are:
47 weston_matrix_init(struct weston_matrix *matrix)
49 static const struct weston_matrix identity = {
50 .d = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
54 memcpy(matrix, &identity, sizeof identity);
57 /* m <- n * m, that is, m is multiplied on the LEFT. */
59 weston_matrix_multiply(struct weston_matrix *m, const struct weston_matrix *n)
61 struct weston_matrix tmp;
62 const float *row, *column;
66 for (i = 0; i < 16; i++) {
69 row = m->d + d.quot * 4;
70 column = n->d + d.rem;
71 for (j = 0; j < 4; j++)
72 tmp.d[i] += row[j] * column[j * 4];
74 tmp.type = m->type | n->type;
75 memcpy(m, &tmp, sizeof tmp);
79 weston_matrix_translate(struct weston_matrix *matrix, float x, float y, float z)
81 struct weston_matrix translate = {
82 .d = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, x, y, z, 1 },
83 .type = WESTON_MATRIX_TRANSFORM_TRANSLATE,
86 weston_matrix_multiply(matrix, &translate);
90 weston_matrix_scale(struct weston_matrix *matrix, float x, float y,float z)
92 struct weston_matrix scale = {
93 .d = { x, 0, 0, 0, 0, y, 0, 0, 0, 0, z, 0, 0, 0, 0, 1 },
94 .type = WESTON_MATRIX_TRANSFORM_SCALE,
97 weston_matrix_multiply(matrix, &scale);
101 weston_matrix_rotate_xy(struct weston_matrix *matrix, float cos, float sin)
103 struct weston_matrix translate = {
104 .d = { cos, sin, 0, 0, -sin, cos, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
105 .type = WESTON_MATRIX_TRANSFORM_ROTATE,
108 weston_matrix_multiply(matrix, &translate);
113 weston_matrix_transform(struct weston_matrix *matrix, struct weston_vector *v)
116 struct weston_vector t;
118 for (i = 0; i < 4; i++) {
120 for (j = 0; j < 4; j++)
121 t.f[i] += v->f[j] * matrix->d[i + j * 4];
128 swap_rows(double *a, double *b)
133 for (k = 0; k < 13; k += 4) {
141 swap_unsigned(unsigned *a, unsigned *b)
150 static inline unsigned
151 find_pivot(double *column, unsigned k)
154 for (++k; k < 4; ++k)
155 if (fabs(column[p]) < fabs(column[k]))
162 * reference: Gene H. Golub and Charles F. van Loan. Matrix computations.
163 * 3rd ed. The Johns Hopkins University Press. 1996.
164 * LU decomposition, forward and back substitution: Chapter 3.
167 MATRIX_TEST_EXPORT inline int
168 matrix_invert(double *A, unsigned *p, const struct weston_matrix *matrix)
174 for (i = 0; i < 4; ++i)
179 /* LU decomposition with partial pivoting */
180 for (k = 0; k < 4; ++k) {
181 pivot = find_pivot(&A[k * 4], k);
183 swap_unsigned(&p[k], &p[pivot]);
184 swap_rows(&A[k], &A[pivot]);
189 return -1; /* zero pivot, not invertible */
191 for (i = k + 1; i < 4; ++i) {
194 for (j = k + 1; j < 4; ++j)
195 A[i + j * 4] -= A[i + k * 4] * A[k + j * 4];
202 MATRIX_TEST_EXPORT inline void
203 inverse_transform(const double *LU, const unsigned *p, float *v)
205 /* Solve A * x = v, when we have P * A = L * U.
206 * P * A * x = P * v => L * U * x = P * v
207 * Let U * x = b, then L * b = P * v.
212 /* Forward substitution, column version, solves L * b = P * v */
213 /* The diagonal of L is all ones, and not explicitly stored. */
215 b[1] = (double)v[p[1]] - b[0] * LU[1 + 0 * 4];
216 b[2] = (double)v[p[2]] - b[0] * LU[2 + 0 * 4];
217 b[3] = (double)v[p[3]] - b[0] * LU[3 + 0 * 4];
218 b[2] -= b[1] * LU[2 + 1 * 4];
219 b[3] -= b[1] * LU[3 + 1 * 4];
220 b[3] -= b[2] * LU[3 + 2 * 4];
222 /* backward substitution, column version, solves U * y = b */
224 /* hand-unrolled, 25% faster for whole function */
225 b[3] /= LU[3 + 3 * 4];
226 b[0] -= b[3] * LU[0 + 3 * 4];
227 b[1] -= b[3] * LU[1 + 3 * 4];
228 b[2] -= b[3] * LU[2 + 3 * 4];
230 b[2] /= LU[2 + 2 * 4];
231 b[0] -= b[2] * LU[0 + 2 * 4];
232 b[1] -= b[2] * LU[1 + 2 * 4];
234 b[1] /= LU[1 + 1 * 4];
235 b[0] -= b[1] * LU[0 + 1 * 4];
237 b[0] /= LU[0 + 0 * 4];
239 for (j = 3; j > 0; --j) {
241 b[j] /= LU[j + j * 4];
242 for (k = 0; k < j; ++k)
243 b[k] -= b[j] * LU[k + j * 4];
246 b[0] /= LU[0 + 0 * 4];
250 for (j = 0; j < 4; ++j)
255 weston_matrix_invert(struct weston_matrix *inverse,
256 const struct weston_matrix *matrix)
258 double LU[16]; /* column-major */
259 unsigned perm[4]; /* permutation */
262 if (matrix_invert(LU, perm, matrix) < 0)
265 weston_matrix_init(inverse);
266 for (c = 0; c < 4; ++c)
267 inverse_transform(LU, perm, &inverse->d[c * 4]);
268 inverse->type = matrix->type;