mirror of
https://gitlab.freedesktop.org/wayland/weston.git
synced 2026-05-08 13:38:05 +02:00
libweston: add linalg-3.h
3x3 matrices, a straight-forward copy of libalg-4.h. Useful for color computations and 2-D geometry. Signed-off-by: Pekka Paalanen <pekka.paalanen@collabora.com>
This commit is contained in:
Pekka Paalanen
parent
2bbd2e4364
commit
e8b7ade58b
5 changed files with 315 additions and 0 deletions
158
include/libweston/linalg-3.h
Normal file
158
include/libweston/linalg-3.h
Normal file
|
|
@ -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 <stdbool.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <libweston/linalg-types.h>
|
||||
|
||||
/* ================= 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);
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -26,4 +26,5 @@
|
|||
#pragma once
|
||||
|
||||
#include <libweston/linalg-types.h>
|
||||
#include <libweston/linalg-3.h>
|
||||
#include <libweston/linalg-4.h>
|
||||
|
|
|
|||
121
shared/matrix.c
121
shared/matrix.c
|
|
@ -35,6 +35,7 @@
|
|||
#include <wayland-server.h>
|
||||
#include <libweston/matrix.h>
|
||||
#include <libweston/linalg-4.h>
|
||||
#include <libweston/linalg-3.h>
|
||||
|
||||
/*
|
||||
* 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)
|
||||
{
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue