A reimplementation of Mario Kart Wii's physics engine in C++
Loading...
Searching...
No Matches
Matrix.hh
1#pragma once
2
3#include "egg/math/Quat.hh"
4
5namespace Kinoko::EGG {
6
7using namespace Mathf;
8
10class Matrix34f {
11public:
12#ifdef BUILD_DEBUG
13 constexpr Matrix34f() {
14 a.fill(std::numeric_limits<f32>::signaling_NaN());
15 }
16#else
17 constexpr Matrix34f() = default;
18#endif
19
20 constexpr Matrix34f(f32 _e00, f32 _e01, f32 _e02, f32 _e03, f32 _e10, f32 _e11, f32 _e12,
21 f32 _e13, f32 _e20, f32 _e21, f32 _e22, f32 _e23)
22 : a{{_e00, _e01, _e02, _e03, _e10, _e11, _e12, _e13, _e20, _e21, _e22, _e23}} {}
23
24 constexpr ~Matrix34f() = default;
25
26 constexpr bool operator==(const Matrix34f &rhs) const {
27 return mtx == rhs.mtx;
28 }
29
31 [[nodiscard]] constexpr f32 &operator[](size_t row, size_t col) {
32 return mtx[row][col];
33 }
34
36 [[nodiscard]] constexpr f32 operator[](size_t row, size_t col) const {
37 return mtx[row][col];
38 }
39
42 constexpr void makeQT(const Quatf &q, const Vector3f &t) {
43 f32 yy = 2.0f * q.v.y * q.v.y;
44 f32 zz = 2.0f * q.v.z * q.v.z;
45 f32 xx = 2.0f * q.v.x * q.v.x;
46 f32 xy = 2.0f * q.v.x * q.v.y;
47 f32 xz = 2.0f * q.v.x * q.v.z;
48 f32 yz = 2.0f * q.v.y * q.v.z;
49 f32 wz = 2.0f * q.w * q.v.z;
50 f32 wx = 2.0f * q.w * q.v.x;
51 f32 wy = 2.0f * q.w * q.v.y;
52
53 mtx[0][0] = 1.0f - yy - zz;
54 mtx[0][1] = xy - wz;
55 mtx[0][2] = xz + wy;
56
57 mtx[1][0] = xy + wz;
58 mtx[1][1] = 1.0f - xx - zz;
59 mtx[1][2] = yz - wx;
60
61 mtx[2][0] = xz - wy;
62 mtx[2][1] = yz + wx;
63 mtx[2][2] = 1.0f - xx - yy;
64
65 mtx[0][3] = t.x;
66 mtx[1][3] = t.y;
67 mtx[2][3] = t.z;
68 }
69
72 constexpr void makeQ(const Quatf &q) {
73 f32 yy = 2.0f * q.v.y * q.v.y;
74 f32 zz = 2.0f * q.v.z * q.v.z;
75 f32 xx = 2.0f * q.v.x * q.v.x;
76 f32 xy = 2.0f * q.v.x * q.v.y;
77 f32 xz = 2.0f * q.v.x * q.v.z;
78 f32 yz = 2.0f * q.v.y * q.v.z;
79 f32 wz = 2.0f * q.w * q.v.z;
80 f32 wx = 2.0f * q.w * q.v.x;
81 f32 wy = 2.0f * q.w * q.v.y;
82
83 mtx[0][0] = 1.0f - yy - zz;
84 mtx[0][1] = xy - wz;
85 mtx[0][2] = xz + wy;
86
87 mtx[1][0] = xy + wz;
88 mtx[1][1] = 1.0f - xx - zz;
89 mtx[1][2] = yz - wx;
90
91 mtx[2][0] = xz - wy;
92 mtx[2][1] = yz + wx;
93 mtx[2][2] = 1.0f - xx - yy;
94
95 mtx[0][3] = 0.0f;
96 mtx[1][3] = 0.0f;
97 mtx[2][3] = 0.0f;
98 }
99
102 constexpr void makeRT(const Vector3f &r, const Vector3f &t) {
103 EGG::Vector3f s = EGG::Vector3f(sin(r.x), sin(r.y), sin(r.z));
104 EGG::Vector3f c = EGG::Vector3f(cos(r.x), cos(r.y), cos(r.z));
105
106 const f32 c0_c2 = c.x * c.z;
107 const f32 s0_s1 = s.x * s.y;
108 const f32 c0_s2 = c.x * s.z;
109
110 mtx[0][0] = (c.y * c.z);
111 mtx[1][0] = (c.y * s.z);
112 mtx[2][0] = (-s.y);
113
114 mtx[0][1] = (s0_s1 * c.z) - c0_s2;
115 mtx[1][1] = (s0_s1 * s.z) + c0_c2;
116 mtx[2][1] = (s.x * c.y);
117
118 mtx[0][2] = (c0_c2 * s.y) + (s.x * s.z);
119 mtx[1][2] = (c0_s2 * s.y) - (s.x * c.z);
120 mtx[2][2] = (c.x * c.y);
121
122 mtx[0][3] = t.x;
123 mtx[1][3] = t.y;
124 mtx[2][3] = t.z;
125 }
126
129 constexpr void makeR(const Vector3f &r) {
130 EGG::Vector3f s = EGG::Vector3f(sin(r.x), sin(r.y), sin(r.z));
131 EGG::Vector3f c = EGG::Vector3f(cos(r.x), cos(r.y), cos(r.z));
132
133 const f32 c0_c2 = c.x * c.z;
134 const f32 s0_s1 = s.x * s.y;
135 const f32 c0_s2 = c.x * s.z;
136
137 mtx[0][0] = (c.y * c.z);
138 mtx[1][0] = (c.y * s.z);
139 mtx[2][0] = (-s.y);
140
141 mtx[0][1] = (s0_s1 * c.z) - c0_s2;
142 mtx[1][1] = (s0_s1 * s.z) + c0_c2;
143 mtx[2][1] = (s.x * c.y);
144
145 mtx[0][2] = (c0_c2 * s.y) + (s.x * s.z);
146 mtx[1][2] = (c0_s2 * s.y) - (s.x * c.z);
147 mtx[2][2] = (c.x * c.y);
148
149 mtx[0][3] = 0.0f;
150 mtx[1][3] = 0.0f;
151 mtx[2][3] = 0.0f;
152 }
153
155 constexpr void makeS(const Vector3f &s) {
156 makeZero();
157 mtx[0][0] = s.x;
158 mtx[1][1] = s.y;
159 mtx[2][2] = s.z;
160 }
161
163 constexpr void makeT(const Vector3f &t) {
164 mtx[0][0] = 1.0f;
165 mtx[0][1] = 0.0f;
166 mtx[0][2] = 0.0f;
167 mtx[1][0] = 0.0f;
168 mtx[1][1] = 1.0f;
169 mtx[1][2] = 0.0f;
170 mtx[2][0] = 0.0f;
171 mtx[2][1] = 0.0f;
172 mtx[2][2] = 1.0f;
173 mtx[0][3] = t.x;
174 mtx[1][3] = t.y;
175 mtx[2][3] = t.z;
176 }
177
179 constexpr void makeZero() {
180 *this = zero;
181 }
182
190 constexpr void makeOrthonormalBasis(const Vector3f &forward, const Vector3f &up) {
191 Vector3f x = up.cross(forward);
192 x.normalise();
193 Vector3f y = forward.cross(x);
194 y.normalise();
195
196 setBase(0, x);
197 setBase(1, y);
198 setBase(2, forward);
199 }
200
202 constexpr void makeOrthonormalBasisLocal(Vector3f forward, Vector3f up) {
203 forward.normalise2();
204 EGG::Vector3f right = up.cross(forward);
205 right.normalise2();
206 up = forward.cross(right);
207
208 setBase(3, EGG::Vector3f::zero);
209 setBase(0, right);
210 setBase(1, up);
211 setBase(2, forward);
212 }
213
216 constexpr void setAxisRotation(f32 angle, const EGG::Vector3f &axis) {
217 EGG::Quatf q;
218 q.setAxisRotation(angle, axis);
219 makeQ(q);
220 }
221
223 constexpr void mulRow33(size_t rowIdx, const Vector3f &row) {
224 mtx[rowIdx][0] *= row.x;
225 mtx[rowIdx][1] *= row.y;
226 mtx[rowIdx][2] *= row.z;
227 }
228
230 constexpr void setBase(size_t col, const Vector3f &base) {
231 mtx[0][col] = base.x;
232 mtx[1][col] = base.y;
233 mtx[2][col] = base.z;
234 }
235
238 [[nodiscard]] constexpr Matrix34f multiplyTo(const Matrix34f &rhs) const {
239 Matrix34f mat;
240
241 mat[0, 0] = fma(rhs[2, 0], mtx[0][2], fma(rhs[1, 0], mtx[0][1], rhs[0, 0] * mtx[0][0]));
242 mat[0, 1] = fma(rhs[2, 1], mtx[0][2], fma(rhs[1, 1], mtx[0][1], rhs[0, 1] * mtx[0][0]));
243 mat[1, 0] = fma(rhs[2, 0], mtx[1][2], fma(rhs[1, 0], mtx[1][1], rhs[0, 0] * mtx[1][0]));
244 mat[1, 1] = fma(rhs[2, 1], mtx[1][2], fma(rhs[1, 1], mtx[1][1], rhs[0, 1] * mtx[1][0]));
245 mat[0, 2] = fma(rhs[2, 2], mtx[0][2], fma(rhs[1, 2], mtx[0][1], rhs[0, 2] * mtx[0][0]));
246 mat[0, 3] = fma(1.0f, mtx[0][3],
247 fma(rhs[2, 3], mtx[0][2], fma(rhs[1, 3], mtx[0][1], rhs[0, 3] * mtx[0][0])));
248 mat[1, 2] = fma(rhs[2, 2], mtx[1][2], fma(rhs[1, 2], mtx[1][1], rhs[0, 2] * mtx[1][0]));
249 mat[1, 3] = fma(1.0f, mtx[1][3],
250 fma(rhs[2, 3], mtx[1][2], fma(rhs[1, 3], mtx[1][1], rhs[0, 3] * mtx[1][0])));
251 mat[2, 0] = fma(rhs[2, 0], mtx[2][2], fma(rhs[1, 0], mtx[2][1], rhs[0, 0] * mtx[2][0]));
252 mat[2, 1] = fma(rhs[2, 1], mtx[2][2], fma(rhs[1, 1], mtx[2][1], rhs[0, 1] * mtx[2][0]));
253 mat[2, 2] = fma(rhs[2, 2], mtx[2][2], fma(rhs[1, 2], mtx[2][1], rhs[0, 2] * mtx[2][0]));
254 mat[2, 3] = fma(1.0f, mtx[2][3],
255 fma(rhs[2, 3], mtx[2][2], fma(rhs[1, 3], mtx[2][1], rhs[0, 3] * mtx[2][0])));
256
257 return mat;
258 }
259
261 [[nodiscard]] constexpr Vector3f multVector(const Vector3f &vec) const {
262 Vector3f ret;
263
264 ret.x = mtx[0][0] * vec.x + mtx[0][3] + mtx[0][1] * vec.y + mtx[0][2] * vec.z;
265 ret.y = mtx[1][0] * vec.x + mtx[1][3] + mtx[1][1] * vec.y + mtx[1][2] * vec.z;
266 ret.z = mtx[2][0] * vec.x + mtx[2][3] + mtx[2][1] * vec.y + mtx[2][2] * vec.z;
267
268 return ret;
269 }
270
273 [[nodiscard]] constexpr Vector3f ps_multVector(const Vector3f &vec) const {
274 Vector3f ret;
275
276 ret.x = fma(mtx[0][2], vec.z, mtx[0][0] * vec.x) + fma(mtx[0][3], 1.0f, mtx[0][1] * vec.y);
277 ret.y = fma(mtx[1][2], vec.z, mtx[1][0] * vec.x) + fma(mtx[1][3], 1.0f, mtx[1][1] * vec.y);
278 ret.z = fma(mtx[2][2], vec.z, mtx[2][0] * vec.x) + fma(mtx[2][3], 1.0f, mtx[2][1] * vec.y);
279
280 return ret;
281 }
282
285 [[nodiscard]] constexpr Vector3f multVector33(const Vector3f &vec) const {
286 Vector3f ret;
287
288 ret.x = mtx[0][2] * vec.z + (mtx[0][0] * vec.x + mtx[0][1] * vec.y);
289 ret.y = mtx[1][2] * vec.z + (mtx[1][0] * vec.x + mtx[1][1] * vec.y);
290 ret.z = mtx[2][2] * vec.z + (mtx[2][0] * vec.x + mtx[2][1] * vec.y);
291
292 return ret;
293 }
294
296 [[nodiscard]] constexpr Vector3f calcRPY() const {
297 constexpr f32 GIMBAL_LOCK_THRESHOLD = 0.999999f;
298
299 EGG::Vector3f xAxisBasis = base(0);
300 f32 absZ = EGG::Mathf::abs(xAxisBasis.z);
301
302 if (absZ > GIMBAL_LOCK_THRESHOLD) {
303 f32 y = xAxisBasis.z / absZ * -HALF_PI;
304 f32 z = EGG::Mathf::atan2(-mtx[0][1], -xAxisBasis.z * mtx[0][2]);
305 return EGG::Vector3f(0.0f, y, z);
306 }
307
308 f32 x = EGG::Mathf::atan2(mtx[2][1], mtx[2][2]);
309 f32 y = EGG::Mathf::asin(-xAxisBasis.z);
310 f32 z = EGG::Mathf::atan2(xAxisBasis.y, xAxisBasis.x);
311
312 return EGG::Vector3f(x, y, z);
313 }
314
317 [[nodiscard]] constexpr Vector3f ps_multVector33(const Vector3f &vec) const {
318 Vector3f ret;
319
320 ret.x = fma(mtx[0][2], vec.z, mtx[0][0] * vec.x + mtx[0][1] * vec.y);
321 ret.y = fma(mtx[1][2], vec.z, mtx[1][0] * vec.x + mtx[1][1] * vec.y);
322 ret.z = fma(mtx[2][2], vec.z, mtx[2][0] * vec.x + mtx[2][1] * vec.y);
323
324 return ret;
325 }
326
331 constexpr void inverseTo33(Matrix34f &out) const {
332 f32 determinant = ((((mtx[2][1] * (mtx[0][2] * mtx[1][0])) +
333 ((mtx[2][2] * (mtx[0][0] * mtx[1][1])) +
334 (mtx[2][0] * (mtx[0][1] * mtx[1][2])))) -
335 (mtx[0][2] * (mtx[2][0] * mtx[1][1]))) -
336 (mtx[2][2] * (mtx[1][0] * mtx[0][1]))) -
337 (mtx[1][2] * (mtx[0][0] * mtx[2][1]));
338
339 if (determinant == 0.0f) {
340 out = ident;
341 return;
342 }
343
344 f32 invDet = 1.0f / determinant;
345
346 out[0, 2] = (mtx[0][1] * mtx[1][2] - mtx[1][1] * mtx[0][2]) * invDet;
347 out[1, 2] = -(mtx[0][0] * mtx[1][2] - mtx[0][2] * mtx[1][0]) * invDet;
348 out[2, 1] = -(mtx[0][0] * mtx[2][1] - mtx[2][0] * mtx[0][1]) * invDet;
349 out[2, 2] = (mtx[0][0] * mtx[1][1] - mtx[1][0] * mtx[0][1]) * invDet;
350 out[2, 0] = (mtx[1][0] * mtx[2][1] - mtx[2][0] * mtx[1][1]) * invDet;
351 out[0, 0] = (mtx[1][1] * mtx[2][2] - mtx[2][1] * mtx[1][2]) * invDet;
352 out[0, 1] = -(mtx[0][1] * mtx[2][2] - mtx[2][1] * mtx[0][2]) * invDet;
353 out[1, 0] = -(mtx[1][0] * mtx[2][2] - mtx[2][0] * mtx[1][2]) * invDet;
354 out[1, 1] = (mtx[0][0] * mtx[2][2] - mtx[2][0] * mtx[0][2]) * invDet;
355 }
356
360 constexpr bool ps_inverse(Matrix34f &out) const {
361 f32 fVar14 = fms(mtx[0][1], mtx[1][2], mtx[1][1] * mtx[0][2]);
362 f32 fVar15 = fms(mtx[1][1], mtx[2][2], mtx[2][1] * mtx[1][2]);
363 f32 fVar13 = fms(mtx[2][1], mtx[0][2], mtx[0][1] * mtx[2][2]);
364 f32 determinant = fma(mtx[2][0], fVar14, fma(mtx[1][0], fVar13, mtx[0][0] * fVar15));
365
366 if (determinant == 0.0f) {
367 return false;
368 }
369
370 f32 invDet = EGG::Mathf::finv(determinant);
371
372 out[0, 0] = fVar15 * invDet;
373 out[0, 1] = fVar13 * invDet;
374 out[1, 0] = fms(mtx[1][2], mtx[2][0], mtx[2][2] * mtx[1][0]) * invDet;
375 out[1, 1] = fms(mtx[2][2], mtx[0][0], mtx[0][2] * mtx[2][0]) * invDet;
376 out[2, 0] = fms(mtx[1][0], mtx[2][1], mtx[1][1] * mtx[2][0]) * invDet;
377 out[2, 1] = fms(mtx[0][1], mtx[2][0], mtx[0][0] * mtx[2][1]) * invDet;
378 out[2, 2] = fms(mtx[0][0], mtx[1][1], mtx[0][1] * mtx[1][0]) * invDet;
379 out[0, 2] = fVar14 * invDet;
380 out[0, 3] = -fma(out[0, 2], mtx[2][3], fma(out[0, 1], mtx[1][3], out[0, 0] * mtx[0][3]));
381 out[1, 2] = fms(mtx[0][2], mtx[1][0], mtx[1][2] * mtx[0][0]) * invDet;
382 out[1, 3] = -fma(out[1, 2], mtx[2][3], fma(out[1, 1], mtx[1][3], out[1, 0] * mtx[0][3]));
383 out[2, 3] = -fma(out[2, 2], mtx[2][3], fma(out[2, 1], mtx[1][3], out[2, 0] * mtx[0][3]));
384
385 return true;
386 }
387
389 [[nodiscard]] constexpr Matrix34f transpose() const {
390 Matrix34f ret = *this;
391
392 ret[0, 1] = mtx[1][0];
393 ret[0, 2] = mtx[2][0];
394 ret[1, 0] = mtx[0][1];
395 ret[1, 2] = mtx[2][1];
396 ret[2, 0] = mtx[0][2];
397 ret[2, 1] = mtx[1][2];
398
399 return ret;
400 }
401
402 [[nodiscard]] constexpr Vector3f translation() const {
403 return Vector3f(mtx[0][3], mtx[1][3], mtx[2][3]);
404 }
405
408 [[nodiscard]] constexpr Vector3f base(size_t col) const {
409 return Vector3f(mtx[0][col], mtx[1][col], mtx[2][col]);
410 }
411
412 static const Matrix34f ident;
413 static const Matrix34f zero;
414
415private:
416 union {
417 std::array<std::array<f32, 4>, 3> mtx;
418 std::array<f32, 12> a;
419 };
420};
421
423inline constexpr Matrix34f Matrix34f::ident(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
424 0.0f, 1.0f, 0.0f);
425
426inline constexpr Matrix34f Matrix34f::zero(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
427 0.0f, 0.0f, 0.0f);
428
429} // namespace Kinoko::EGG
Kinoko::EGG::Matrix34f
A 3 x 4 matrix.
Definition Matrix.hh:10
Kinoko::EGG::Matrix34f::ps_multVector33
constexpr Vector3f ps_multVector33(const Vector3f &vec) const
Paired-singles impl. of multVector33.
Definition Matrix.hh:317
Kinoko::EGG::Matrix34f::makeZero
constexpr void makeZero()
Zeroes every element of the matrix.
Definition Matrix.hh:179
Kinoko::EGG::Matrix34f::makeR
constexpr void makeR(const Vector3f &r)
Sets 3x3 rotation matrix from a vector of Euler angles.
Definition Matrix.hh:129
Kinoko::EGG::Matrix34f::makeQT
constexpr void makeQT(const Quatf &q, const Vector3f &t)
Sets matrix from rotation and position.
Definition Matrix.hh:42
Kinoko::EGG::Matrix34f::setAxisRotation
constexpr void setAxisRotation(f32 angle, const EGG::Vector3f &axis)
Rotates the matrix about an axis.
Definition Matrix.hh:216
Kinoko::EGG::Matrix34f::makeQ
constexpr void makeQ(const Quatf &q)
Sets rotation matrix from quaternion.
Definition Matrix.hh:72
Kinoko::EGG::Matrix34f::makeOrthonormalBasis
constexpr void makeOrthonormalBasis(const Vector3f &forward, const Vector3f &up)
Sets a 3x3 orthonormal basis for a local coordinate system.
Definition Matrix.hh:190
Kinoko::EGG::Matrix34f::transpose
constexpr Matrix34f transpose() const
Transposes the 3x3 portion of the matrix.
Definition Matrix.hh:389
Kinoko::EGG::Matrix34f::ps_multVector
constexpr Vector3f ps_multVector(const Vector3f &vec) const
Paired-singles impl. of multVector.
Definition Matrix.hh:273
Kinoko::EGG::Matrix34f::multVector
constexpr Vector3f multVector(const Vector3f &vec) const
Multiplies a vector by a matrix.
Definition Matrix.hh:261
Kinoko::EGG::Matrix34f::operator[]
constexpr f32 & operator[](size_t row, size_t col)
Accesses the matrix element at the specified row and column.
Definition Matrix.hh:31
Kinoko::EGG::Matrix34f::base
constexpr Vector3f base(size_t col) const
Get a particular column from a matrix.
Definition Matrix.hh:408
Kinoko::EGG::Matrix34f::ps_inverse
constexpr bool ps_inverse(Matrix34f &out) const
Definition Matrix.hh:360
Kinoko::EGG::Matrix34f::operator[]
constexpr f32 operator[](size_t row, size_t col) const
Accesses the matrix element at the specified row and column.
Definition Matrix.hh:36
Kinoko::EGG::Matrix34f::mulRow33
constexpr void mulRow33(size_t rowIdx, const Vector3f &row)
Multiplies one row of a 3x3 matrix by a vector.
Definition Matrix.hh:223
Kinoko::EGG::Matrix34f::setBase
constexpr void setBase(size_t col, const Vector3f &base)
Sets one column of a matrix.
Definition Matrix.hh:230
Kinoko::EGG::Matrix34f::inverseTo33
constexpr void inverseTo33(Matrix34f &out) const
Inverts the 3x3 portion of the 3x4 matrix.
Definition Matrix.hh:331
Kinoko::EGG::Matrix34f::multVector33
constexpr Vector3f multVector33(const Vector3f &vec) const
Multiplies a 3x3 matrix by a vector.
Definition Matrix.hh:285
Kinoko::EGG::Matrix34f::multiplyTo
constexpr Matrix34f multiplyTo(const Matrix34f &rhs) const
Multiplies two matrices.
Definition Matrix.hh:238
Kinoko::EGG::Matrix34f::makeRT
constexpr void makeRT(const Vector3f &r, const Vector3f &t)
Sets rotation-translation matrix.
Definition Matrix.hh:102
Kinoko::EGG::Mathf::cos
static constexpr f32 cos(f32 x)
Definition Math.hh:457
Kinoko::EGG::Mathf::fms
static constexpr f32 fms(f32 x, f32 y, f32 z)
Fused multiply-subtract operation.
Definition Math.hh:40
Kinoko::EGG::Mathf::finv
static constexpr f32 finv(f32 x)
Fused Newton-Raphson operation.
Definition Math.hh:775
Kinoko::EGG::Mathf::sin
static constexpr f32 sin(f32 x)
Definition Math.hh:451
Kinoko::EGG::Mathf::fma
static constexpr f32 fma(f32 x, f32 y, f32 z)
Fused multiply-add operation.
Definition Math.hh:33
Kinoko::EGG
EGG core library.
Definition Archive.cc:6
Kinoko::EGG::Quatf
A quaternion, used to represent 3D rotation.
Definition Quat.hh:12
Kinoko::EGG::Quatf::setAxisRotation
constexpr void setAxisRotation(f32 angle, const EGG::Vector3f &axis)
Set the quat given angle and axis.
Definition Quat.hh:208
Kinoko::EGG::Vector3f
A 3D float vector.
Definition Vector.hh:107
Kinoko::EGG::Vector3f::normalise
constexpr f32 normalise()
Normalizes the vector and returns the original length.
Definition Vector.hh:278