OXYGENHUB
◆ main openmsx / src / video / gl_mat.hh
9.9 KiB · C++ · last changed chore(): initial commit 1 month ago source blame history raw
1 
#ifndef GL_MAT_HH
2 
#define GL_MAT_HH
3
4 
// This code implements a mathematical matrix, comparable in functionality
5 
// and syntax to the matrix types in GLSL.
6 
//
7 
// The code was tuned for 4x4 matrixes, but when it didn't complicate the
8 
// code, other sizes (even non-square) are supported as well.
9 
//
10 
// The functionality of this code is focused on openGL(ES). So only the
11 
// operation 'matrix x column-vector' is supported and not e.g. 'row-vector x
12 
// matrix'. The internal layout is also compatible with openGL(ES).
13 
//
14 
// In the past we had (some) manual SSE optimizations in this code. Though for
15 
// the functions that matter (matrix-vector and matrix-matrix multiplication),
16 
// the compiler's auto-vectorization has become as good as the manually
17 
// vectorized code.
18
19 
#include "gl_vec.hh"
20 
#include "xrange.hh"
21 
#include <cassert>
22
23 
namespace gl {
24
25 
// Matrix with M rows and N columns (M by N), elements have type T.
26 
// Internally elements are stored in column-major order. This is the same
27 
// convention as openGL (and Fortran), but different from the typical row-major
28 
// order in C++. This allows to directly pass these matrices to openGL(ES).
29 
template<int M, int N, typename T> class matMxN
30 
{
31 
public:
32 
	// Default copy-constructor and assignment operator.
33
34 
	// Construct identity matrix.
35 
	constexpr matMxN()
36 
	{
37 
		for (auto i : xrange(N)) {
38 
			for (auto j : xrange(M)) {
39 
				c[i][j] = (i == j) ? T(1) : T(0);
40 
			}
41 
		}
42 
	}
43
44 
	// Construct diagonal matrix.
45 
	constexpr explicit matMxN(const vecN<(M < N ? M : N), T>& d)
46 
	{
47 
		for (auto i : xrange(N)) {
48 
			for (auto j : xrange(M)) {
49 
				c[i][j] = (i == j) ? d[i] : T(0);
50 
			}
51 
		}
52 
	}
53
54 
	// Construct from larger matrix (higher order elements are dropped).
55 
	template<int M2, int N2> constexpr explicit matMxN(const matMxN<M2, N2, T>& x)
56 
	{
57 
		static_assert(((M2 > M) && (N2 >= N)) || ((M2 >= M) && (N2 > N)),
58 
		              "matrix must have strictly larger dimensions");
59 
		for (auto i : xrange(N)) c[i] = vecN<M, T>(x[i]);
60 
	}
61
62 
	// Construct matrix from 2 given columns (only valid when N == 2).
63 
	constexpr matMxN(const vecN<M, T>& x, const vecN<M, T>& y)
64 
	{
65 
		static_assert(N == 2, "wrong #constructor arguments");
66 
		c[0] = x; c[1] = y;
67 
	}
68
69 
	// Construct matrix from 3 given columns (only valid when N == 3).
70 
	constexpr matMxN(const vecN<M, T>& x, const vecN<M, T>& y, const vecN<M, T>& z)
71 
	{
72 
		static_assert(N == 3, "wrong #constructor arguments");
73 
		c[0] = x; c[1] = y; c[2] = z;
74 
	}
75
76 
	// Construct matrix from 4 given columns (only valid when N == 4).
77 
	constexpr matMxN(const vecN<M, T>& x, const vecN<M, T>& y,
78 
	                 const vecN<M, T>& z, const vecN<M, T>& w)
79 
	{
80 
		static_assert(N == 4, "wrong #constructor arguments");
81 
		c[0] = x; c[1] = y; c[2] = z; c[3] = w;
82 
	}
83
84 
	// Access the i-the column of the matrix.
85 
	// Vectors are also indexable, so 'A[i][j]' returns the element
86 
	// at the i-th column, j-th row.
87 
	[[nodiscard]] constexpr const vecN<M, T>& operator[](unsigned i) const {
88 
		#ifdef DEBUG
89 
		assert(i < N);
90 
		#endif
91 
		return c[i];
92 
	}
93 
	[[nodiscard]] constexpr vecN<M, T>& operator[](unsigned i) {
94 
		#ifdef DEBUG
95 
		assert(i < N);
96 
		#endif
97 
		return c[i];
98 
	}
99
100 
	[[nodiscard]] constexpr const T* data() const { return c[0].data(); }
101 
	[[nodiscard]] constexpr       T* data()       { return c[0].data(); }
102
103
104 
	// Assignment version of the +,-,* operations defined below.
105 
	constexpr matMxN& operator+=(const matMxN& x) { *this = *this + x; return *this; }
106 
	constexpr matMxN& operator-=(const matMxN& x) { *this = *this - x; return *this; }
107 
	constexpr matMxN& operator*=(T             x) { *this = *this * x; return *this; }
108 
	constexpr matMxN& operator*=(const matMxN<N, N, T>& x) { *this = *this * x; return *this; }
109
110 
	[[nodiscard]] constexpr bool operator==(const matMxN&) const = default;
111
112 
	// matrix + matrix
113 
	[[nodiscard]] constexpr friend matMxN operator+(const matMxN& A, const matMxN& B) {
114 
		matMxN<M, N, T> R;
115 
		for (auto i : xrange(N)) R[i] = A[i] + B[i];
116 
		return R;
117 
	}
118
119 
	// matrix - matrix
120 
	[[nodiscard]] constexpr friend matMxN operator-(const matMxN& A, const matMxN& B) {
121 
		matMxN<M, N, T> R;
122 
		for (auto i : xrange(N)) R[i] = A[i] - B[i];
123 
		return R;
124 
	}
125
126 
	// scalar * matrix
127 
	[[nodiscard]] constexpr friend matMxN operator*(T x, const matMxN& A) {
128 
		matMxN<M, N, T> R;
129 
		for (auto i : xrange(N)) R[i] = x * A[i];
130 
		return R;
131 
	}
132
133 
	// matrix * scalar
134 
	[[nodiscard]] constexpr friend matMxN operator*(const matMxN& A, T x) {
135 
		matMxN<M, N, T> R;
136 
		for (auto i : xrange(N)) R[i] = A[i] * x;
137 
		return R;
138 
	}
139
140 
	// matrix * column-vector
141 
	[[nodiscard]] constexpr friend vecN<M, T> operator*(const matMxN& A, const vecN<N, T>& x) {
142 
		vecN<M, T> r;
143 
		for (auto i : xrange(N)) r += A[i] * x[i];
144 
		return r;
145 
	}
146
147 
	// matrix * matrix
148 
	template<int O>
149 
	[[nodiscard]] constexpr friend matMxN<M, O, T> operator*(const matMxN& A, const matMxN<N, O, T>& B) {
150 
		matMxN<M, O, T> R;
151 
		for (auto i : xrange(O)) R[i] = A * B[i];
152 
		return R;
153 
	}
154
155 
	// Textual representation. (Only) used to debug unittest.
156 
	friend std::ostream& operator<<(std::ostream& os, const matMxN& A) {
157 
		for (auto j : xrange(M)) {
158 
			for (auto i : xrange(N)) {
159 
				os << A[i][j] << ' ';
160 
			}
161 
			os << '\n';
162 
		}
163 
		return os;
164 
	}
165
166 
private:
167 
	std::array<vecN<M, T>, N> c;
168 
};
169
170
171 
// Convenience typedefs (same names as used by GLSL).
172 
using mat2 = matMxN<2, 2, float>;
173 
using mat3 = matMxN<3, 3, float>;
174 
using mat4 = matMxN<4, 4, float>;
175
176
177 
// -- Matrix functions --
178
179 
// matrix negation
180 
template<int M, int N, typename T>
181 
[[nodiscard]] constexpr matMxN<M, N, T> operator-(const matMxN<M, N, T>& A)
182 
{
183 
	return matMxN<M, N, T>(vecN<(M < N ? M : N), T>()) - A;
184 
}
185
186 
// matrix transpose
187 
template<int M, int N, typename T>
188 
[[nodiscard]] constexpr matMxN<N, M, T> transpose(const matMxN<M, N, T>& A)
189 
{
190 
	matMxN<N, M, T> R;
191 
	for (auto i : xrange(N)) {
192 
		for (auto j : xrange(M)) {
193 
			R[j][i] = A[i][j];
194 
		}
195 
	}
196 
	return R;
197 
}
198
199 
// determinant of a 2x2 matrix
200 
template<typename T>
201 
[[nodiscard]] constexpr T determinant(const matMxN<2, 2, T>& A)
202 
{
203 
	return A[0][0] * A[1][1] - A[0][1] * A[1][0];
204 
}
205
206 
// determinant of a 3x3 matrix
207 
template<typename T>
208 
[[nodiscard]] constexpr T determinant(const matMxN<3, 3, T>& A)
209 
{
210 
	return A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1])
211 
	     - A[1][0] * (A[0][1] * A[2][2] - A[0][2] * A[2][1])
212 
	     + A[2][0] * (A[0][1] * A[1][2] - A[0][2] * A[1][1]);
213 
}
214
215 
// determinant of a 4x4 matrix
216 
template<typename T>
217 
[[nodiscard]] constexpr T determinant(const matMxN<4, 4, T>& A)
218 
{
219 
	// Implementation based on glm:  http://glm.g-truc.net
220 
	T f0 = A[2][2] * A[3][3] - A[3][2] * A[2][3];
221 
	T f1 = A[2][1] * A[3][3] - A[3][1] * A[2][3];
222 
	T f2 = A[2][1] * A[3][2] - A[3][1] * A[2][2];
223 
	T f3 = A[2][0] * A[3][3] - A[3][0] * A[2][3];
224 
	T f4 = A[2][0] * A[3][2] - A[3][0] * A[2][2];
225 
	T f5 = A[2][0] * A[3][1] - A[3][0] * A[2][1];
226 
	vecN<4, T> c((A[1][1] * f0 - A[1][2] * f1 + A[1][3] * f2),
227 
	             (A[1][2] * f3 - A[1][3] * f4 - A[1][0] * f0),
228 
	             (A[1][0] * f1 - A[1][1] * f3 + A[1][3] * f5),
229 
	             (A[1][1] * f4 - A[1][2] * f5 - A[1][0] * f2));
230 
	return dot(A[0], c);
231 
}
232
233 
// inverse of a 2x2 matrix
234 
template<typename T>
235 
[[nodiscard]] constexpr matMxN<2, 2, T> inverse(const matMxN<2, 2, T>& A)
236 
{
237 
	T d = T(1) / determinant(A);
238 
	return d * matMxN<2, 2, T>(vecN<2, T>( A[1][1], -A[0][1]),
239 
	                           vecN<2, T>(-A[1][0],  A[0][0]));
240 
}
241
242 
// inverse of a 3x3 matrix
243 
template<typename T>
244 
[[nodiscard]] constexpr matMxN<3, 3, T> inverse(const matMxN<3, 3, T>& A)
245 
{
246 
	T d = T(1) / determinant(A);
247 
	return d * matMxN<3, 3, T>(
248 
		vecN<3, T>(A[1][1] * A[2][2] - A[1][2] * A[2][1],
249 
		           A[0][2] * A[2][1] - A[0][1] * A[2][2],
250 
		           A[0][1] * A[1][2] - A[0][2] * A[1][1]),
251 
		vecN<3, T>(A[1][2] * A[2][0] - A[1][0] * A[2][2],
252 
		           A[0][0] * A[2][2] - A[0][2] * A[2][0],
253 
		           A[0][2] * A[1][0] - A[0][0] * A[1][2]),
254 
		vecN<3, T>(A[1][0] * A[2][1] - A[1][1] * A[2][0],
255 
		           A[0][1] * A[2][0] - A[0][0] * A[2][1],
256 
		           A[0][0] * A[1][1] - A[0][1] * A[1][0]));
257 
}
258
259 
// inverse of a 4x4 matrix
260 
template<typename T>
261 
[[nodiscard]] constexpr matMxN<4, 4, T> inverse(const matMxN<4, 4, T>& A)
262 
{
263 
	// Implementation based on glm:  http://glm.g-truc.net
264
265 
	T c00 = A[2][2] * A[3][3] - A[3][2] * A[2][3];
266 
	T c02 = A[1][2] * A[3][3] - A[3][2] * A[1][3];
267 
	T c03 = A[1][2] * A[2][3] - A[2][2] * A[1][3];
268
269 
	T c04 = A[2][1] * A[3][3] - A[3][1] * A[2][3];
270 
	T c06 = A[1][1] * A[3][3] - A[3][1] * A[1][3];
271 
	T c07 = A[1][1] * A[2][3] - A[2][1] * A[1][3];
272
273 
	T c08 = A[2][1] * A[3][2] - A[3][1] * A[2][2];
274 
	T c10 = A[1][1] * A[3][2] - A[3][1] * A[1][2];
275 
	T c11 = A[1][1] * A[2][2] - A[2][1] * A[1][2];
276
277 
	T c12 = A[2][0] * A[3][3] - A[3][0] * A[2][3];
278 
	T c14 = A[1][0] * A[3][3] - A[3][0] * A[1][3];
279 
	T c15 = A[1][0] * A[2][3] - A[2][0] * A[1][3];
280
281 
	T c16 = A[2][0] * A[3][2] - A[3][0] * A[2][2];
282 
	T c18 = A[1][0] * A[3][2] - A[3][0] * A[1][2];
283 
	T c19 = A[1][0] * A[2][2] - A[2][0] * A[1][2];
284
285 
	T c20 = A[2][0] * A[3][1] - A[3][0] * A[2][1];
286 
	T c22 = A[1][0] * A[3][1] - A[3][0] * A[1][1];
287 
	T c23 = A[1][0] * A[2][1] - A[2][0] * A[1][1];
288
289 
	vecN<4, T> f0(c00, c00, c02, c03);
290 
	vecN<4, T> f1(c04, c04, c06, c07);
291 
	vecN<4, T> f2(c08, c08, c10, c11);
292 
	vecN<4, T> f3(c12, c12, c14, c15);
293 
	vecN<4, T> f4(c16, c16, c18, c19);
294 
	vecN<4, T> f5(c20, c20, c22, c23);
295
296 
	vecN<4, T> v0(A[1][0], A[0][0], A[0][0], A[0][0]);
297 
	vecN<4, T> v1(A[1][1], A[0][1], A[0][1], A[0][1]);
298 
	vecN<4, T> v2(A[1][2], A[0][2], A[0][2], A[0][2]);
299 
	vecN<4, T> v3(A[1][3], A[0][3], A[0][3], A[0][3]);
300
301 
	vecN<4, T> i0(v1 * f0 - v2 * f1 + v3 * f2);
302 
	vecN<4, T> i1(v0 * f0 - v2 * f3 + v3 * f4);
303 
	vecN<4, T> i2(v0 * f1 - v1 * f3 + v3 * f5);
304 
	vecN<4, T> i3(v0 * f2 - v1 * f4 + v2 * f5);
305
306 
	vecN<4, T> sa(+1, -1, +1, -1);
307 
	vecN<4, T> sb(-1, +1, -1, +1);
308 
	matMxN<4, 4, T> inverse(i0 * sa, i1 * sb, i2 * sa, i3 * sb);
309
310 
	vecN<4, T> row0(inverse[0][0], inverse[1][0], inverse[2][0], inverse[3][0]);
311 
	T OneOverDeterminant = static_cast<T>(1) / dot(A[0], row0);
312
313 
	return inverse * OneOverDeterminant;
314 
}
315
316 
// norm-2 squared
317 
template<int M, int N, typename T>
318 
[[nodiscard]] constexpr T norm2_2(const matMxN<M, N, T>& A)
319 
{
320 
	vecN<M, T> t;
321 
	for (auto i : xrange(N)) t += A[i] * A[i];
322 
	return sum(t);
323 
}
324
325 
} // namespace gl
326
327 
#endif