From f0567654d65d571c622536e173da8b556b32e3ca Mon Sep 17 00:00:00 2001 From: Pekka Paalanen Date: Tue, 18 Mar 2025 16:00:56 +0200 Subject: [PATCH] libweston: introduce linalg.h linalg-types.h can be used from e.g. libweston.h so that structures can embed linalg types but without forcing every user of libweston.h to compile all the inline functions. weston_mat4f implements most of weston_matrix, and some more. This is just for starters. Implementing weston_mat3fwill be essentially a copy of all this. The matrix inversion is a copy from weston_matrix with a little bit of clean-up. The goal is to create a more intuitive libear algebra API, that is easy to extend to 3x3 matrices and 3-vectors, eventually superseding all existing matrix code: weston_matrix, calls to LittleCMS matrix routines (plugin API), and ad hoc matrix code in tests/. Signed-off-by: Pekka Paalanen --- include/libweston/linalg-4.h | 187 +++++++++++++++++++++++++++++++ include/libweston/linalg-types.h | 44 ++++++++ include/libweston/linalg.h | 29 +++++ include/libweston/meson.build | 3 + shared/matrix.c | 99 +++++++++++++++- 5 files changed, 361 insertions(+), 1 deletion(-) create mode 100644 include/libweston/linalg-4.h create mode 100644 include/libweston/linalg-types.h create mode 100644 include/libweston/linalg.h diff --git a/include/libweston/linalg-4.h b/include/libweston/linalg-4.h new file mode 100644 index 000000000..ab209100f --- /dev/null +++ b/include/libweston/linalg-4.h @@ -0,0 +1,187 @@ +/* + * Copyright 2025 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. + */ + +#pragma once + +#include +#include + +#include + +/* ================= 4-vectors and 4x4 matrices ============== */ + +/** Construct a column vector from elements */ +#define WESTON_VEC4F(x, y, z, w) ((struct weston_vec4f){ .el = { (x), (y), (z), (w) }}) + +/** Construct the [0, 0, 0, 0]^T vector */ +#define WESTON_VEC4F_ZERO ((struct weston_vec4f){ .el = {}}) + +/** Construct matrix from elements a{row}{column} */ +#define WESTON_MAT4F(a00, a01, a02, a03, \ + a10, a11, a12, a13, \ + a20, a21, a22, a23, \ + a30, a31, a32, a33) \ + ((struct weston_mat4f){ .colmaj = { \ + a00, a10, a20, a30, \ + a01, a11, a21, a31, \ + a02, a12, a22, a32, \ + a03, a13, a23, a33, \ + }}) + +/** Construct the identity 4x4 matrix */ +#define WESTON_MAT4F_IDENTITY \ + ((struct weston_mat4f){ .colmaj = { \ + 1.0f, 0.0f, 0.0f, 0.0f, \ + 0.0f, 1.0f, 0.0f, 0.0f, \ + 0.0f, 0.0f, 1.0f, 0.0f, \ + 0.0f, 0.0f, 0.0f, 1.0f, \ + }}) + +/** Construct a translation matrix */ +static inline struct weston_mat4f +weston_m4f_translation(float tx, float ty, float tz) +{ + return WESTON_MAT4F( + 1.0f, 0.0f, 0.0f, tx, + 0.0f, 1.0f, 0.0f, ty, + 0.0f, 0.0f, 1.0f, tz, + 0.0f, 0.0f, 0.0f, 1.0f); +} + +/** Construct a scaling matrix */ +static inline struct weston_mat4f +weston_m4f_scaling(float sx, float sy, float sz) +{ + return WESTON_MAT4F( + sx, 0.0f, 0.0f, 0.0f, + 0.0f, sy, 0.0f, 0.0f, + 0.0f, 0.0f, sz, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); +} + +/** Construct a 2D x-y rotation matrix + * + * \param cos_th Cosine of the counter-clockwise angle. + * \param sin_th Sine of the counter-clockwise angle. + */ +static inline struct weston_mat4f +weston_m4f_rotation_xy(float cos_th, float sin_th) +{ + return WESTON_MAT4F( + cos_th, -sin_th, 0.0f, 0.0f, + sin_th, cos_th, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); +} + +/** 4-vector dot product */ +static inline float +weston_v4f_dot_v4f(struct weston_vec4f a, struct weston_vec4f b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + +/** + * Matrix infinity-norm + * + * http://www.netlib.org/lapack/lug/node75.html + */ +static inline float +weston_m4f_inf_norm(struct weston_mat4f M) +{ + unsigned row; + double infnorm = -1.0; + + for (row = 0; row < 4; row++) { + unsigned col; + double sum = 0.0; + + for (col = 0; col < 4; col++) + sum += fabsf(M.col[col].el[row]); + + if (infnorm < sum) + infnorm = sum; + } + + return infnorm; +} + +/** Transpose 4x4 matrix */ +static inline struct weston_mat4f +weston_m4f_transpose(struct weston_mat4f M) +{ + struct weston_mat4f R; + unsigned i, j; + + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) + R.col[j].el[i] = M.col[i].el[j]; + + return R; +} + +/** Matrix-vector multiplication A * b */ +static inline struct weston_vec4f +weston_m4f_mul_v4f(struct weston_mat4f A, struct weston_vec4f b) +{ + struct weston_vec4f result; + unsigned r; + + for (r = 0; r < 4; r++) { + struct weston_vec4f row = + WESTON_VEC4F(A.col[0].el[r], A.col[1].el[r], A.col[2].el[r], A.col[3].el[r]); + result.el[r] = weston_v4f_dot_v4f(row, b); + } + return result; +} + +/** Matrix multiplication A * B */ +static inline struct weston_mat4f +weston_m4f_mul_m4f(struct weston_mat4f A, struct weston_mat4f B) +{ + struct weston_mat4f result; + unsigned c; + + for (c = 0; c < 4; c++) + result.col[c] = weston_m4f_mul_v4f(A, B.col[c]); + + return result; +} + +/** Element-wise matrix subtraction A - B */ +static inline struct weston_mat4f +weston_m4f_sub_m4f(struct weston_mat4f A, struct weston_mat4f B) +{ + struct weston_mat4f R; + unsigned i; + + for (i = 0; i < 4 * 4; i++) + R.colmaj[i] = A.colmaj[i] - B.colmaj[i]; + + return R; +} + +bool +weston_m4f_invert(struct weston_mat4f *out, struct weston_mat4f M); diff --git a/include/libweston/linalg-types.h b/include/libweston/linalg-types.h new file mode 100644 index 000000000..accc045ee --- /dev/null +++ b/include/libweston/linalg-types.h @@ -0,0 +1,44 @@ +/* + * Copyright 2025 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. + */ + +#pragma once + +/** Column 4-vector */ +struct weston_vec4f { + union { + float el[4]; + struct { + float x, y, z, w; + }; + }; +}; + +/** 4x4 matrix, column-major */ +struct weston_mat4f { + union { + struct weston_vec4f col[4]; + float colmaj[4 * 4]; + }; +}; diff --git a/include/libweston/linalg.h b/include/libweston/linalg.h new file mode 100644 index 000000000..8ab2663a9 --- /dev/null +++ b/include/libweston/linalg.h @@ -0,0 +1,29 @@ +/* + * Copyright 2025 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. + */ + +#pragma once + +#include +#include diff --git a/include/libweston/meson.build b/include/libweston/meson.build index fc1070c5f..4623abc1b 100644 --- a/include/libweston/meson.build +++ b/include/libweston/meson.build @@ -1,6 +1,9 @@ install_headers( 'config-parser.h', 'libweston.h', + 'linalg.h', + 'linalg-4.h', + 'linalg-types.h', 'desktop.h', 'matrix.h', 'plugin-registry.h', diff --git a/shared/matrix.c b/shared/matrix.c index 472f299a0..d38de4b84 100644 --- a/shared/matrix.c +++ b/shared/matrix.c @@ -34,7 +34,7 @@ #include #include - +#include /* * Matrices are stored in column-major order, that is the array indices are: @@ -288,6 +288,103 @@ weston_matrix_invert(struct weston_matrix *inverse, return 0; } +static bool +m4f_LU_decompose(double *restrict LU, unsigned *restrict p, struct weston_mat4f M) +{ + unsigned i, j, k; + unsigned pivot; + double pv; + + for (i = 0; i < 4; ++i) + p[i] = i; + for (i = 16; i--; ) + LU[i] = M.colmaj[i]; + + /* LU decomposition with partial pivoting */ + for (k = 0; k < 4; ++k) { + pivot = find_pivot(&LU[k * 4], k); + if (pivot != k) { + swap_unsigned(&p[k], &p[pivot]); + swap_rows(&LU[k], &LU[pivot]); + } + + pv = LU[k * 4 + k]; + if (fabs(pv) < 1e-9) + return false; /* zero pivot, error */ + + for (i = k + 1; i < 4; ++i) { + LU[i + k * 4] /= pv; + + for (j = k + 1; j < 4; ++j) + LU[i + j * 4] -= LU[i + k * 4] * LU[k + j * 4]; + } + } + + return true; +} + +static inline void +m4f_LU_inverse_transform(const double *restrict A, + const unsigned *restrict p, + struct weston_vec4f *restrict 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->el[p[0]]; + b[1] = v->el[p[1]] - b[0] * A[1 + 0 * 4]; + b[2] = v->el[p[2]] - b[0] * A[2 + 0 * 4] - b[1] * A[2 + 1 * 4]; + b[3] = v->el[p[3]] - b[0] * A[3 + 0 * 4] - b[1] * A[3 + 1 * 4] - b[2] * A[3 + 2 * 4]; + + /* backward substitution, column version, solves U * y = b */ + for (j = 3; j > 0; --j) { + unsigned k; + b[j] /= A[j + j * 4]; + for (k = 0; k < j; ++k) + b[k] -= b[j] * A[k + j * 4]; + } + + b[0] /= A[0 + 0 * 4]; + + /* the result */ + for (j = 0; j < 4; ++j) + v->el[j] = b[j]; +} + +/** Invert 4x4 matrix + * + * 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. + * + * \param[out] out Destination to save the inverted matrix. + * \param M The matrix to invert. + * \return True for success, false for failure. On failure, + * \c *out remains unchanged. + */ +WL_EXPORT bool +weston_m4f_invert(struct weston_mat4f *out, struct weston_mat4f M) +{ + double LU[16]; /* column-major */ + unsigned perm[4]; /* permutation */ + unsigned c; + + if (!m4f_LU_decompose(LU, perm, M)) + return false; + + *out = WESTON_MAT4F_IDENTITY; + for (c = 0; c < 4; ++c) + m4f_LU_inverse_transform(LU, perm, &out->col[c]); + + return true; +} + static bool near_zero(float a) {