diff --git a/include/libweston/linalg-3.h b/include/libweston/linalg-3.h new file mode 100644 index 000000000..8a4df7e63 --- /dev/null +++ b/include/libweston/linalg-3.h @@ -0,0 +1,158 @@ +/* + * 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 + +/* ================= 3-vectors and 3x3 matrices ============== */ + +/** Construct a column vector from elements */ +#define WESTON_VEC3F(x, y, z) ((struct weston_vec3f){ .el = { (x), (y), (z) }}) + +/** Construct the [0, 0, 0]^T vector */ +#define WESTON_VEC3F_ZERO ((struct weston_vec3f){ .el = {}}) + +/** Construct matrix from elements a{row}{column} */ +#define WESTON_MAT3F(a00, a01, a02, \ + a10, a11, a12, \ + a20, a21, a22) \ + ((struct weston_mat3f){ .colmaj = { \ + a00, a10, a20, \ + a01, a11, a21, \ + a02, a12, a22, \ + }}) + +/** Construct the identity 3x3 matrix */ +#define WESTON_MAT3F_IDENTITY \ + ((struct weston_mat3f){ .colmaj = { \ + 1.0f, 0.0f, 0.0f, \ + 0.0f, 1.0f, 0.0f, \ + 0.0f, 0.0f, 1.0f, \ + }}) + +/** Copy the top-left 3x3 from 4x4 */ +static inline struct weston_mat3f +weston_m3f_from_m4f_xyz(struct weston_mat4f M) +{ + return WESTON_MAT3F( + M.col[0].el[0], M.col[1].el[0], M.col[2].el[0], + M.col[0].el[1], M.col[1].el[1], M.col[2].el[1], + M.col[0].el[2], M.col[1].el[2], M.col[2].el[2] + ); +} + +/** 3-vector dot product */ +static inline float +weston_v3f_dot_v3f(struct weston_vec3f a, struct weston_vec3f b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +/** + * Matrix infinity-norm + * + * http://www.netlib.org/lapack/lug/node75.html + */ +static inline float +weston_m3f_inf_norm(struct weston_mat3f M) +{ + unsigned row; + double infnorm = -1.0; + + for (row = 0; row < 3; row++) { + unsigned col; + double sum = 0.0; + + for (col = 0; col < 3; col++) + sum += fabsf(M.col[col].el[row]); + + if (infnorm < sum) + infnorm = sum; + } + + return infnorm; +} + +/** Transpose 3x3 matrix */ +static inline struct weston_mat3f +weston_m3f_transpose(struct weston_mat3f M) +{ + struct weston_mat3f R; + unsigned i, j; + + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + R.col[j].el[i] = M.col[i].el[j]; + + return R; +} + +/** Matrix-vector multiplication A * b */ +static inline struct weston_vec3f +weston_m3f_mul_v3f(struct weston_mat3f A, struct weston_vec3f b) +{ + struct weston_vec3f result; + unsigned r; + + for (r = 0; r < 3; r++) { + struct weston_vec3f row = + WESTON_VEC3F(A.col[0].el[r], A.col[1].el[r], A.col[2].el[r]); + result.el[r] = weston_v3f_dot_v3f(row, b); + } + return result; +} + +/** Matrix multiplication A * B */ +static inline struct weston_mat3f +weston_m3f_mul_m3f(struct weston_mat3f A, struct weston_mat3f B) +{ + struct weston_mat3f result; + unsigned c; + + for (c = 0; c < 3; c++) + result.col[c] = weston_m3f_mul_v3f(A, B.col[c]); + + return result; +} + +/** Element-wise matrix subtraction A - B */ +static inline struct weston_mat3f +weston_m3f_sub_m3f(struct weston_mat3f A, struct weston_mat3f B) +{ + struct weston_mat3f R; + unsigned i; + + for (i = 0; i < 3 * 3; i++) + R.colmaj[i] = A.colmaj[i] - B.colmaj[i]; + + return R; +} + +bool +weston_m3f_invert(struct weston_mat3f *out, struct weston_mat3f M); diff --git a/include/libweston/linalg-4.h b/include/libweston/linalg-4.h index ab209100f..9b7db4d6a 100644 --- a/include/libweston/linalg-4.h +++ b/include/libweston/linalg-4.h @@ -96,6 +96,17 @@ weston_m4f_rotation_xy(float cos_th, float sin_th) 0.0f, 0.0f, 0.0f, 1.0f); } +static inline struct weston_mat4f +weston_m4f_from_m3f_v3f(struct weston_mat3f R, struct weston_vec3f t) +{ + return WESTON_MAT4F( + R.col[0].el[0], R.col[1].el[0], R.col[2].el[0], t.el[0], + R.col[0].el[1], R.col[1].el[1], R.col[2].el[1], t.el[1], + R.col[0].el[2], R.col[1].el[2], R.col[2].el[2], t.el[2], + 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) diff --git a/include/libweston/linalg-types.h b/include/libweston/linalg-types.h index accc045ee..c6fb2f1c4 100644 --- a/include/libweston/linalg-types.h +++ b/include/libweston/linalg-types.h @@ -25,6 +25,30 @@ #pragma once +/** Column 3-vector */ +struct weston_vec3f { + union { + float el[3]; + struct { + float x, y, z; + }; + struct { + float r, g, b; + }; + struct { + float Y, Cb, Cr; + }; + }; +}; + +/** 3x3 matrix, column-major */ +struct weston_mat3f { + union { + struct weston_vec3f col[3]; + float colmaj[3 * 3]; + }; +}; + /** Column 4-vector */ struct weston_vec4f { union { diff --git a/include/libweston/linalg.h b/include/libweston/linalg.h index 8ab2663a9..8cd6bdb9d 100644 --- a/include/libweston/linalg.h +++ b/include/libweston/linalg.h @@ -26,4 +26,5 @@ #pragma once #include +#include #include diff --git a/shared/matrix.c b/shared/matrix.c index d38de4b84..b78ff38ae 100644 --- a/shared/matrix.c +++ b/shared/matrix.c @@ -35,6 +35,7 @@ #include #include #include +#include /* * Matrices are stored in column-major order, that is the array indices are: @@ -385,6 +386,126 @@ weston_m4f_invert(struct weston_mat4f *out, struct weston_mat4f M) return true; } +static inline void +swap_rows3(double *restrict a, double *restrict b) +{ + unsigned k; + double tmp; + + for (k = 0; k < 7; k += 3) { + tmp = a[k]; + a[k] = b[k]; + b[k] = tmp; + } +} + +static inline unsigned +find_pivot3(double *column, unsigned k) +{ + unsigned p = k; + for (++k; k < 3; ++k) + if (fabs(column[p]) < fabs(column[k])) + p = k; + + return p; +} + +static inline bool +m3f_LU_decompose(double *restrict LU, unsigned *restrict p, struct weston_mat3f M) +{ + unsigned i, j, k; + unsigned pivot; + double pv; + + for (i = 0; i < 3; ++i) + p[i] = i; + for (i = 9; i--; ) + LU[i] = M.colmaj[i]; + + /* LU decomposition with partial pivoting */ + for (k = 0; k < 3; ++k) { + pivot = find_pivot3(&LU[k * 3], k); + if (pivot != k) { + swap_unsigned(&p[k], &p[pivot]); + swap_rows3(&LU[k], &LU[pivot]); + } + + pv = LU[k * 3 + k]; + if (fabs(pv) < 1e-9) + return false; /* zero pivot, error */ + + for (i = k + 1; i < 3; ++i) { + LU[i + k * 3] /= pv; + + for (j = k + 1; j < 3; ++j) + LU[i + j * 3] -= LU[i + k * 3] * LU[k + j * 3]; + } + } + + return true; +} + +static inline void +m3f_LU_inverse_transform(const double *restrict A, + const unsigned *restrict p, + struct weston_vec3f *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[3]; + 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 * 3]; + b[2] = v->el[p[2]] - b[0] * A[2 + 0 * 3] - b[1] * A[2 + 1 * 3]; + + /* backward substitution, column version, solves U * y = b */ + for (j = 2; j > 0; --j) { + unsigned k; + b[j] /= A[j + j * 3]; + for (k = 0; k < j; ++k) + b[k] -= b[j] * A[k + j * 3]; + } + + b[0] /= A[0 + 0 * 3]; + + /* the result */ + for (j = 0; j < 3; ++j) + v->el[j] = b[j]; +} + +/** Invert 3x3 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_m3f_invert(struct weston_mat3f *out, struct weston_mat3f M) +{ + double LU[9]; /* column-major */ + unsigned perm[3]; /* permutation */ + unsigned c; + + if (!m3f_LU_decompose(LU, perm, M)) + return false; + + *out = WESTON_MAT3F_IDENTITY; + for (c = 0; c < 3; ++c) + m3f_LU_inverse_transform(LU, perm, &out->col[c]); + + return true; +} + static bool near_zero(float a) {