a23ce29506
Remove IN_WESTON in favour of the already defined UNIT_TEST which is used to modify a compilation unit to expose more functions for unit tests to prod at. Originally IN_WESTON meant that compilation unit was being compiled for use in the Weston compositor, but it probably never really did anything more than change what WL_EXPORT means in matrix.c. This patch not only simplifies the logic, but it fixes the Meson build of test-matrix: IN_WESTON was defined there even when matrix.c was being built outside of Weston, which caused it to depend on libwayland headers, which were not included in the Meson build of test-matrix. Test-matrix has no reason to depend in libwayland in any way, so this patch fixes that. Reported-by: Greg V <greg@unrelenting.technology> Signed-off-by: Pekka Paalanen <pq@iki.fi>
277 lines
6.3 KiB
C
277 lines
6.3 KiB
C
/*
|
|
* Copyright © 2011 Intel Corporation
|
|
* Copyright © 2012 Collabora, Ltd.
|
|
*
|
|
* 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:
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#include "config.h"
|
|
|
|
#include <float.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
#ifdef UNIT_TEST
|
|
#define WL_EXPORT
|
|
#else
|
|
#include <wayland-server.h>
|
|
#endif
|
|
|
|
#include "matrix.h"
|
|
|
|
|
|
/*
|
|
* 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 = {
|
|
.d = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
|
|
.type = 0,
|
|
};
|
|
|
|
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;
|
|
const float *row, *column;
|
|
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];
|
|
}
|
|
tmp.type = m->type | n->type;
|
|
memcpy(m, &tmp, sizeof tmp);
|
|
}
|
|
|
|
WL_EXPORT void
|
|
weston_matrix_translate(struct weston_matrix *matrix, float x, float y, float z)
|
|
{
|
|
struct weston_matrix translate = {
|
|
.d = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, x, y, z, 1 },
|
|
.type = WESTON_MATRIX_TRANSFORM_TRANSLATE,
|
|
};
|
|
|
|
weston_matrix_multiply(matrix, &translate);
|
|
}
|
|
|
|
WL_EXPORT void
|
|
weston_matrix_scale(struct weston_matrix *matrix, float x, float y,float z)
|
|
{
|
|
struct weston_matrix scale = {
|
|
.d = { x, 0, 0, 0, 0, y, 0, 0, 0, 0, z, 0, 0, 0, 0, 1 },
|
|
.type = WESTON_MATRIX_TRANSFORM_SCALE,
|
|
};
|
|
|
|
weston_matrix_multiply(matrix, &scale);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
/* 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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
static inline void
|
|
swap_unsigned(unsigned *a, unsigned *b)
|
|
{
|
|
unsigned tmp;
|
|
|
|
tmp = *a;
|
|
*a = *b;
|
|
*b = tmp;
|
|
}
|
|
|
|
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.
|
|
*/
|
|
|
|
MATRIX_TEST_EXPORT inline int
|
|
matrix_invert(double *A, unsigned *p, const struct weston_matrix *matrix)
|
|
{
|
|
unsigned i, j, k;
|
|
unsigned pivot;
|
|
double pv;
|
|
|
|
for (i = 0; i < 4; ++i)
|
|
p[i] = i;
|
|
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) {
|
|
swap_unsigned(&p[k], &p[pivot]);
|
|
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;
|
|
}
|
|
|
|
MATRIX_TEST_EXPORT inline void
|
|
inverse_transform(const double *LU, const unsigned *p, float *v)
|
|
{
|
|
/* 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];
|
|
unsigned j;
|
|
|
|
/* Forward substitution, column version, solves L * b = P * v */
|
|
/* The diagonal of L is all ones, and not explicitly stored. */
|
|
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];
|
|
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) {
|
|
unsigned k;
|
|
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)
|
|
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]);
|
|
inverse->type = matrix->type;
|
|
|
|
return 0;
|
|
}
|