A reimplementation of Mario Kart Wii's physics engine in C++
Loading...
Searching...
No Matches
Matrix.cc
1#include "Matrix.hh"
2
3#include "egg/math/Math.hh"
4
5namespace EGG {
6
7using namespace Mathf;
8
11void Matrix34f::makeQT(const Quatf &q, const Vector3f &t) {
12 f32 yy = 2.0f * q.v.y * q.v.y;
13 f32 zz = 2.0f * q.v.z * q.v.z;
14 f32 xx = 2.0f * q.v.x * q.v.x;
15 f32 xy = 2.0f * q.v.x * q.v.y;
16 f32 xz = 2.0f * q.v.x * q.v.z;
17 f32 yz = 2.0f * q.v.y * q.v.z;
18 f32 wz = 2.0f * q.w * q.v.z;
19 f32 wx = 2.0f * q.w * q.v.x;
20 f32 wy = 2.0f * q.w * q.v.y;
21
22 mtx[0][0] = 1.0f - yy - zz;
23 mtx[0][1] = xy - wz;
24 mtx[0][2] = xz + wy;
25
26 mtx[1][0] = xy + wz;
27 mtx[1][1] = 1.0f - xx - zz;
28 mtx[1][2] = yz - wx;
29
30 mtx[2][0] = xz - wy;
31 mtx[2][1] = yz + wx;
32 mtx[2][2] = 1.0f - xx - yy;
33
34 mtx[0][3] = t.x;
35 mtx[1][3] = t.y;
36 mtx[2][3] = t.z;
37}
38
41void Matrix34f::makeQ(const Quatf &q) {
42 f32 yy = 2.0f * q.v.y * q.v.y;
43 f32 zz = 2.0f * q.v.z * q.v.z;
44 f32 xx = 2.0f * q.v.x * q.v.x;
45 f32 xy = 2.0f * q.v.x * q.v.y;
46 f32 xz = 2.0f * q.v.x * q.v.z;
47 f32 yz = 2.0f * q.v.y * q.v.z;
48 f32 wz = 2.0f * q.w * q.v.z;
49 f32 wx = 2.0f * q.w * q.v.x;
50 f32 wy = 2.0f * q.w * q.v.y;
51
52 mtx[0][0] = 1.0f - yy - zz;
53 mtx[0][1] = xy - wz;
54 mtx[0][2] = xz + wy;
55
56 mtx[1][0] = xy + wz;
57 mtx[1][1] = 1.0f - xx - zz;
58 mtx[1][2] = yz - wx;
59
60 mtx[2][0] = xz - wy;
61 mtx[2][1] = yz + wx;
62 mtx[2][2] = 1.0f - xx - yy;
63
64 mtx[0][3] = 0.0f;
65 mtx[1][3] = 0.0f;
66 mtx[2][3] = 0.0f;
67}
68
71void Matrix34f::makeRT(const Vector3f &r, const Vector3f &t) {
72 EGG::Vector3f s = EGG::Vector3f(sin(r.x), sin(r.y), sin(r.z));
73 EGG::Vector3f c = EGG::Vector3f(cos(r.x), cos(r.y), cos(r.z));
74
75 const f32 c0_c2 = c.x * c.z;
76 const f32 s0_s1 = s.x * s.y;
77 const f32 c0_s2 = c.x * s.z;
78
79 mtx[0][0] = (c.y * c.z);
80 mtx[1][0] = (c.y * s.z);
81 mtx[2][0] = (-s.y);
82
83 mtx[0][1] = (s0_s1 * c.z) - c0_s2;
84 mtx[1][1] = (s0_s1 * s.z) + c0_c2;
85 mtx[2][1] = (s.x * c.y);
86
87 mtx[0][2] = (c0_c2 * s.y) + (s.x * s.z);
88 mtx[1][2] = (c0_s2 * s.y) - (s.x * c.z);
89 mtx[2][2] = (c.x * c.y);
90
91 mtx[0][3] = t.x;
92 mtx[1][3] = t.y;
93 mtx[2][3] = t.z;
94}
95
98void Matrix34f::makeR(const Vector3f &r) {
99 EGG::Vector3f s = EGG::Vector3f(sin(r.x), sin(r.y), sin(r.z));
100 EGG::Vector3f c = EGG::Vector3f(cos(r.x), cos(r.y), cos(r.z));
101
102 const f32 c0_c2 = c.x * c.z;
103 const f32 s0_s1 = s.x * s.y;
104 const f32 c0_s2 = c.x * s.z;
105
106 mtx[0][0] = (c.y * c.z);
107 mtx[1][0] = (c.y * s.z);
108 mtx[2][0] = (-s.y);
109
110 mtx[0][1] = (s0_s1 * c.z) - c0_s2;
111 mtx[1][1] = (s0_s1 * s.z) + c0_c2;
112 mtx[2][1] = (s.x * c.y);
113
114 mtx[0][2] = (c0_c2 * s.y) + (s.x * s.z);
115 mtx[1][2] = (c0_s2 * s.y) - (s.x * c.z);
116 mtx[2][2] = (c.x * c.y);
117
118 mtx[0][3] = 0.0f;
119 mtx[1][3] = 0.0f;
120 mtx[2][3] = 0.0f;
121}
122
124void Matrix34f::makeS(const Vector3f &s) {
125 makeZero();
126 mtx[0][0] = s.x;
127 mtx[1][1] = s.y;
128 mtx[2][2] = s.z;
129}
130
132void Matrix34f::makeT(const Vector3f &t) {
133 mtx[0][0] = 1.0f;
134 mtx[0][1] = 0.0f;
135 mtx[0][2] = 0.0f;
136 mtx[1][0] = 0.0f;
137 mtx[1][1] = 1.0f;
138 mtx[1][2] = 0.0f;
139 mtx[2][0] = 0.0f;
140 mtx[2][1] = 0.0f;
141 mtx[2][2] = 1.0f;
142 mtx[0][3] = t.x;
143 mtx[1][3] = t.y;
144 mtx[2][3] = t.z;
145}
146
154void Matrix34f::makeOrthonormalBasis(const Vector3f &forward, const Vector3f &up) {
155 Vector3f x = up.cross(forward);
156 x.normalise();
157 Vector3f y = forward.cross(x);
158 y.normalise();
159
160 setBase(0, x);
161 setBase(1, y);
162 setBase(2, forward);
163}
164
166void Matrix34f::makeOrthonormalBasisLocal(Vector3f forward, Vector3f up) {
167 forward.normalise2();
168 EGG::Vector3f right = up.cross(forward);
169 right.normalise2();
170 up = forward.cross(right);
171
172 setBase(3, EGG::Vector3f::zero);
173 setBase(0, right);
174 setBase(1, up);
175 setBase(2, forward);
176}
177
180void Matrix34f::setAxisRotation(f32 angle, const EGG::Vector3f &axis) {
181 EGG::Quatf q;
182 q.setAxisRotation(angle, axis);
183 makeQ(q);
184}
185
187void Matrix34f::mulRow33(size_t rowIdx, const Vector3f &row) {
188 mtx[rowIdx][0] *= row.x;
189 mtx[rowIdx][1] *= row.y;
190 mtx[rowIdx][2] *= row.z;
191}
192
194void Matrix34f::setBase(size_t col, const Vector3f &base) {
195 mtx[0][col] = base.x;
196 mtx[1][col] = base.y;
197 mtx[2][col] = base.z;
198}
199
202Matrix34f Matrix34f::multiplyTo(const Matrix34f &rhs) const {
203 Matrix34f mat;
204
205 mat[0, 0] = fma(rhs[2, 0], mtx[0][2], fma(rhs[1, 0], mtx[0][1], rhs[0, 0] * mtx[0][0]));
206 mat[0, 1] = fma(rhs[2, 1], mtx[0][2], fma(rhs[1, 1], mtx[0][1], rhs[0, 1] * mtx[0][0]));
207 mat[1, 0] = fma(rhs[2, 0], mtx[1][2], fma(rhs[1, 0], mtx[1][1], rhs[0, 0] * mtx[1][0]));
208 mat[1, 1] = fma(rhs[2, 1], mtx[1][2], fma(rhs[1, 1], mtx[1][1], rhs[0, 1] * mtx[1][0]));
209 mat[0, 2] = fma(rhs[2, 2], mtx[0][2], fma(rhs[1, 2], mtx[0][1], rhs[0, 2] * mtx[0][0]));
210 mat[0, 3] = fma(1.0f, mtx[0][3],
211 fma(rhs[2, 3], mtx[0][2], fma(rhs[1, 3], mtx[0][1], rhs[0, 3] * mtx[0][0])));
212 mat[1, 2] = fma(rhs[2, 2], mtx[1][2], fma(rhs[1, 2], mtx[1][1], rhs[0, 2] * mtx[1][0]));
213 mat[1, 3] = fma(1.0f, mtx[1][3],
214 fma(rhs[2, 3], mtx[1][2], fma(rhs[1, 3], mtx[1][1], rhs[0, 3] * mtx[1][0])));
215 mat[2, 0] = fma(rhs[2, 0], mtx[2][2], fma(rhs[1, 0], mtx[2][1], rhs[0, 0] * mtx[2][0]));
216 mat[2, 1] = fma(rhs[2, 1], mtx[2][2], fma(rhs[1, 1], mtx[2][1], rhs[0, 1] * mtx[2][0]));
217 mat[2, 2] = fma(rhs[2, 2], mtx[2][2], fma(rhs[1, 2], mtx[2][1], rhs[0, 2] * mtx[2][0]));
218 mat[2, 3] = fma(1.0f, mtx[2][3],
219 fma(rhs[2, 3], mtx[2][2], fma(rhs[1, 3], mtx[2][1], rhs[0, 3] * mtx[2][0])));
220
221 return mat;
222}
223
225Vector3f Matrix34f::multVector(const Vector3f &vec) const {
226 Vector3f ret;
227
228 ret.x = mtx[0][0] * vec.x + mtx[0][3] + mtx[0][1] * vec.y + mtx[0][2] * vec.z;
229 ret.y = mtx[1][0] * vec.x + mtx[1][3] + mtx[1][1] * vec.y + mtx[1][2] * vec.z;
230 ret.z = mtx[2][0] * vec.x + mtx[2][3] + mtx[2][1] * vec.y + mtx[2][2] * vec.z;
231
232 return ret;
233}
234
237Vector3f Matrix34f::ps_multVector(const Vector3f &vec) const {
238 Vector3f ret;
239
240 ret.x = fma(mtx[0][2], vec.z, mtx[0][0] * vec.x) + fma(mtx[0][3], 1.0f, mtx[0][1] * vec.y);
241 ret.y = fma(mtx[1][2], vec.z, mtx[1][0] * vec.x) + fma(mtx[1][3], 1.0f, mtx[1][1] * vec.y);
242 ret.z = fma(mtx[2][2], vec.z, mtx[2][0] * vec.x) + fma(mtx[2][3], 1.0f, mtx[2][1] * vec.y);
243
244 return ret;
245}
246
249Vector3f Matrix34f::multVector33(const Vector3f &vec) const {
250 Vector3f ret;
251
252 ret.x = mtx[0][2] * vec.z + (mtx[0][0] * vec.x + mtx[0][1] * vec.y);
253 ret.y = mtx[1][2] * vec.z + (mtx[1][0] * vec.x + mtx[1][1] * vec.y);
254 ret.z = mtx[2][2] * vec.z + (mtx[2][0] * vec.x + mtx[2][1] * vec.y);
255
256 return ret;
257}
258
261Vector3f Matrix34f::ps_multVector33(const Vector3f &vec) const {
262 Vector3f ret;
263
264 ret.x = fma(mtx[0][2], vec.z, mtx[0][0] * vec.x + mtx[0][1] * vec.y);
265 ret.y = fma(mtx[1][2], vec.z, mtx[1][0] * vec.x + mtx[1][1] * vec.y);
266 ret.z = fma(mtx[2][2], vec.z, mtx[2][0] * vec.x + mtx[2][1] * vec.y);
267
268 return ret;
269}
270
272[[nodiscard]] Vector3f Matrix34f::calcRPY() const {
273 constexpr f32 GIMBAL_LOCK_THRESHOLD = 0.999999f;
274
275 EGG::Vector3f xAxisBasis = base(0);
276 f32 absZ = EGG::Mathf::abs(xAxisBasis.z);
277
278 if (absZ > GIMBAL_LOCK_THRESHOLD) {
279 f32 y = xAxisBasis.z / absZ * -HALF_PI;
280 f32 z = EGG::Mathf::atan2(-mtx[0][1], -xAxisBasis.z * mtx[0][2]);
281 return EGG::Vector3f(0.0f, y, z);
282 }
283
284 f32 x = EGG::Mathf::atan2(mtx[2][1], mtx[2][2]);
285 f32 y = EGG::Mathf::asin(-xAxisBasis.z);
286 f32 z = EGG::Mathf::atan2(xAxisBasis.y, xAxisBasis.x);
287
288 return EGG::Vector3f(x, y, z);
289}
290
295void Matrix34f::inverseTo33(Matrix34f &out) const {
296 f32 determinant = ((((mtx[2][1] * (mtx[0][2] * mtx[1][0])) +
297 ((mtx[2][2] * (mtx[0][0] * mtx[1][1])) +
298 (mtx[2][0] * (mtx[0][1] * mtx[1][2])))) -
299 (mtx[0][2] * (mtx[2][0] * mtx[1][1]))) -
300 (mtx[2][2] * (mtx[1][0] * mtx[0][1]))) -
301 (mtx[1][2] * (mtx[0][0] * mtx[2][1]));
302
303 if (determinant == 0.0f) {
304 out = Matrix34f::ident;
305 return;
306 }
307
308 f32 invDet = 1.0f / determinant;
309
310 out[0, 2] = (mtx[0][1] * mtx[1][2] - mtx[1][1] * mtx[0][2]) * invDet;
311 out[1, 2] = -(mtx[0][0] * mtx[1][2] - mtx[0][2] * mtx[1][0]) * invDet;
312 out[2, 1] = -(mtx[0][0] * mtx[2][1] - mtx[2][0] * mtx[0][1]) * invDet;
313 out[2, 2] = (mtx[0][0] * mtx[1][1] - mtx[1][0] * mtx[0][1]) * invDet;
314 out[2, 0] = (mtx[1][0] * mtx[2][1] - mtx[2][0] * mtx[1][1]) * invDet;
315 out[0, 0] = (mtx[1][1] * mtx[2][2] - mtx[2][1] * mtx[1][2]) * invDet;
316 out[0, 1] = -(mtx[0][1] * mtx[2][2] - mtx[2][1] * mtx[0][2]) * invDet;
317 out[1, 0] = -(mtx[1][0] * mtx[2][2] - mtx[2][0] * mtx[1][2]) * invDet;
318 out[1, 1] = (mtx[0][0] * mtx[2][2] - mtx[2][0] * mtx[0][2]) * invDet;
319}
320
324bool Matrix34f::ps_inverse(Matrix34f &out) const {
325 f32 fVar14 = fms(mtx[0][1], mtx[1][2], mtx[1][1] * mtx[0][2]);
326 f32 fVar15 = fms(mtx[1][1], mtx[2][2], mtx[2][1] * mtx[1][2]);
327 f32 fVar13 = fms(mtx[2][1], mtx[0][2], mtx[0][1] * mtx[2][2]);
328 f32 determinant = fma(mtx[2][0], fVar14, fma(mtx[1][0], fVar13, mtx[0][0] * fVar15));
329
330 if (determinant == 0.0f) {
331 return false;
332 }
333
334 f32 invDet = EGG::Mathf::finv(determinant);
335
336 out[0, 0] = fVar15 * invDet;
337 out[0, 1] = fVar13 * invDet;
338 out[1, 0] = fms(mtx[1][2], mtx[2][0], mtx[2][2] * mtx[1][0]) * invDet;
339 out[1, 1] = fms(mtx[2][2], mtx[0][0], mtx[0][2] * mtx[2][0]) * invDet;
340 out[2, 0] = fms(mtx[1][0], mtx[2][1], mtx[1][1] * mtx[2][0]) * invDet;
341 out[2, 1] = fms(mtx[0][1], mtx[2][0], mtx[0][0] * mtx[2][1]) * invDet;
342 out[2, 2] = fms(mtx[0][0], mtx[1][1], mtx[0][1] * mtx[1][0]) * invDet;
343 out[0, 2] = fVar14 * invDet;
344 out[0, 3] = -fma(out[0, 2], mtx[2][3], fma(out[0, 1], mtx[1][3], out[0, 0] * mtx[0][3]));
345 out[1, 2] = fms(mtx[0][2], mtx[1][0], mtx[1][2] * mtx[0][0]) * invDet;
346 out[1, 3] = -fma(out[1, 2], mtx[2][3], fma(out[1, 1], mtx[1][3], out[1, 0] * mtx[0][3]));
347 out[2, 3] = -fma(out[2, 2], mtx[2][3], fma(out[2, 1], mtx[1][3], out[2, 0] * mtx[0][3]));
348
349 return true;
350}
351
353Matrix34f Matrix34f::transpose() const {
354 Matrix34f ret = *this;
355
356 ret[0, 1] = mtx[1][0];
357 ret[0, 2] = mtx[2][0];
358 ret[1, 0] = mtx[0][1];
359 ret[1, 2] = mtx[2][1];
360 ret[2, 0] = mtx[0][2];
361 ret[2, 1] = mtx[1][2];
362
363 return ret;
364}
365
366} // namespace EGG
EGG::Matrix34f
A 3 x 4 matrix.
Definition Matrix.hh:8
EGG::Matrix34f::makeOrthonormalBasis
void makeOrthonormalBasis(const Vector3f &v0, const Vector3f &v1)
Sets a 3x3 orthonormal basis for a local coordinate system.
Definition Matrix.cc:154
EGG::Matrix34f::multiplyTo
Matrix34f multiplyTo(const Matrix34f &rhs) const
Multiplies two matrices.
Definition Matrix.cc:202
EGG::Matrix34f::setBase
void setBase(size_t col, const Vector3f &base)
Sets one column of a matrix.
Definition Matrix.cc:194
EGG::Matrix34f::base
Vector3f base(size_t col) const
Get a particular column from a matrix.
Definition Matrix.hh:72
EGG::Matrix34f::makeZero
void makeZero()
Zeroes every element of the matrix.
Definition Matrix.hh:46
EGG::Matrix34f::setAxisRotation
void setAxisRotation(f32 angle, const Vector3f &axis)
Rotates the matrix about an axis.
Definition Matrix.cc:180
EGG::Matrix34f::makeQ
void makeQ(const Quatf &q)
Sets rotation matrix from quaternion.
Definition Matrix.cc:41
EGG::Matrix34f::multVector33
Vector3f multVector33(const Vector3f &vec) const
Multiplies a 3x3 matrix by a vector.
Definition Matrix.cc:249
EGG::Matrix34f::inverseTo33
void inverseTo33(Matrix34f &out) const
Inverts the 3x3 portion of the 3x4 matrix.
Definition Matrix.cc:295
EGG::Matrix34f::makeR
void makeR(const Vector3f &r)
Sets 3x3 rotation matrix from a vector of Euler angles.
Definition Matrix.cc:98
EGG::Matrix34f::multVector
Vector3f multVector(const Vector3f &vec) const
Multiplies a vector by a matrix.
Definition Matrix.cc:225
EGG::Matrix34f::makeRT
void makeRT(const Vector3f &r, const Vector3f &t)
Sets rotation-translation matrix.
Definition Matrix.cc:71
EGG::Matrix34f::transpose
Matrix34f transpose() const
Transposes the 3x3 portion of the matrix.
Definition Matrix.cc:353
EGG::Matrix34f::ps_multVector
Vector3f ps_multVector(const Vector3f &vec) const
Paired-singles impl. of multVector.
Definition Matrix.cc:237
EGG::Matrix34f::makeQT
void makeQT(const Quatf &q, const Vector3f &t)
Sets matrix from rotation and position.
Definition Matrix.cc:11
EGG::Matrix34f::mulRow33
void mulRow33(size_t rowIdx, const Vector3f &row)
Multiplies one row of a 3x3 matrix by a vector.
Definition Matrix.cc:187
EGG::Matrix34f::ps_inverse
bool ps_inverse(Matrix34f &out) const
Definition Matrix.cc:324
EGG::Matrix34f::ps_multVector33
Vector3f ps_multVector33(const Vector3f &vec) const
Paired-singles impl. of multVector33.
Definition Matrix.cc:261
EGG::Mathf::fma
static f32 fma(f32 x, f32 y, f32 z)
Fused multiply-add operation.
Definition Math.hh:77
EGG::Mathf::sin
static f32 sin(f32 x)
Definition Math.hh:37
EGG::Mathf::finv
static f32 finv(f32 x)
Fused Newton-Raphson operation.
Definition Math.hh:324
EGG::Mathf::cos
static f32 cos(f32 x)
Definition Math.hh:43
EGG::Mathf::fms
static f32 fms(f32 x, f32 y, f32 z)
Fused multiply-subtract operation.
Definition Math.hh:84
EGG
EGG core library.
Definition Archive.cc:6
EGG::Quatf
A quaternion, used to represent 3D rotation.
Definition Quat.hh:12
EGG::Quatf::setAxisRotation
void setAxisRotation(f32 angle, const Vector3f &axis)
Set the quat given angle and axis.
Definition Quat.cc:111
EGG::Vector3f
A 3D float vector.
Definition Vector.hh:88
EGG::Vector3f::normalise
f32 normalise()
Normalizes the vector and returns the original length.
Definition Vector.cc:52