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
9#ifdef BUILD_DEBUG
10Matrix34f::Matrix34f() {
11 a.fill(std::numeric_limits<f32>::signaling_NaN());
12}
13#else
14Matrix34f::Matrix34f() = default;
15#endif
16
17Matrix34f::Matrix34f(f32 _e00, f32 _e01, f32 _e02, f32 _e03, f32 _e10, f32 _e11, f32 _e12, f32 _e13,
18 f32 _e20, f32 _e21, f32 _e22, f32 _e23) {
19 mtx[0][0] = _e00;
20 mtx[0][1] = _e01;
21 mtx[0][2] = _e02;
22 mtx[0][3] = _e03;
23 mtx[1][0] = _e10;
24 mtx[1][1] = _e11;
25 mtx[1][2] = _e12;
26 mtx[1][3] = _e13;
27 mtx[2][0] = _e20;
28 mtx[2][1] = _e21;
29 mtx[2][2] = _e22;
30 mtx[2][3] = _e23;
31}
32
35void Matrix34f::makeQT(const Quatf &q, const Vector3f &t) {
36 f32 yy = 2.0f * q.v.y * q.v.y;
37 f32 zz = 2.0f * q.v.z * q.v.z;
38 f32 xx = 2.0f * q.v.x * q.v.x;
39 f32 xy = 2.0f * q.v.x * q.v.y;
40 f32 xz = 2.0f * q.v.x * q.v.z;
41 f32 yz = 2.0f * q.v.y * q.v.z;
42 f32 wz = 2.0f * q.w * q.v.z;
43 f32 wx = 2.0f * q.w * q.v.x;
44 f32 wy = 2.0f * q.w * q.v.y;
45
46 mtx[0][0] = 1.0f - yy - zz;
47 mtx[0][1] = xy - wz;
48 mtx[0][2] = xz + wy;
49
50 mtx[1][0] = xy + wz;
51 mtx[1][1] = 1.0f - xx - zz;
52 mtx[1][2] = yz - wx;
53
54 mtx[2][0] = xz - wy;
55 mtx[2][1] = yz + wx;
56 mtx[2][2] = 1.0f - xx - yy;
57
58 mtx[0][3] = t.x;
59 mtx[1][3] = t.y;
60 mtx[2][3] = t.z;
61}
62
65void Matrix34f::makeQ(const Quatf &q) {
66 f32 yy = 2.0f * q.v.y * q.v.y;
67 f32 zz = 2.0f * q.v.z * q.v.z;
68 f32 xx = 2.0f * q.v.x * q.v.x;
69 f32 xy = 2.0f * q.v.x * q.v.y;
70 f32 xz = 2.0f * q.v.x * q.v.z;
71 f32 yz = 2.0f * q.v.y * q.v.z;
72 f32 wz = 2.0f * q.w * q.v.z;
73 f32 wx = 2.0f * q.w * q.v.x;
74 f32 wy = 2.0f * q.w * q.v.y;
75
76 mtx[0][0] = 1.0f - yy - zz;
77 mtx[0][1] = xy - wz;
78 mtx[0][2] = xz + wy;
79
80 mtx[1][0] = xy + wz;
81 mtx[1][1] = 1.0f - xx - zz;
82 mtx[1][2] = yz - wx;
83
84 mtx[2][0] = xz - wy;
85 mtx[2][1] = yz + wx;
86 mtx[2][2] = 1.0f - xx - yy;
87
88 mtx[0][3] = 0.0f;
89 mtx[1][3] = 0.0f;
90 mtx[2][3] = 0.0f;
91}
92
95void Matrix34f::makeRT(const Vector3f &r, const Vector3f &t) {
96 EGG::Vector3f s = EGG::Vector3f(sin(r.x), sin(r.y), sin(r.z));
97 EGG::Vector3f c = EGG::Vector3f(cos(r.x), cos(r.y), cos(r.z));
98
99 const f32 c0_c2 = c.x * c.z;
100 const f32 s0_s1 = s.x * s.y;
101 const f32 c0_s2 = c.x * s.z;
102
103 mtx[0][0] = (c.y * c.z);
104 mtx[1][0] = (c.y * s.z);
105 mtx[2][0] = (-s.y);
106
107 mtx[0][1] = (s0_s1 * c.z) - c0_s2;
108 mtx[1][1] = (s0_s1 * s.z) + c0_c2;
109 mtx[2][1] = (s.x * c.y);
110
111 mtx[0][2] = (c0_c2 * s.y) + (s.x * s.z);
112 mtx[1][2] = (c0_s2 * s.y) - (s.x * c.z);
113 mtx[2][2] = (c.x * c.y);
114
115 mtx[0][3] = t.x;
116 mtx[1][3] = t.y;
117 mtx[2][3] = t.z;
118}
119
122void Matrix34f::makeR(const Vector3f &r) {
123 EGG::Vector3f s = EGG::Vector3f(sin(r.x), sin(r.y), sin(r.z));
124 EGG::Vector3f c = EGG::Vector3f(cos(r.x), cos(r.y), cos(r.z));
125
126 const f32 c0_c2 = c.x * c.z;
127 const f32 s0_s1 = s.x * s.y;
128 const f32 c0_s2 = c.x * s.z;
129
130 mtx[0][0] = (c.y * c.z);
131 mtx[1][0] = (c.y * s.z);
132 mtx[2][0] = (-s.y);
133
134 mtx[0][1] = (s0_s1 * c.z) - c0_s2;
135 mtx[1][1] = (s0_s1 * s.z) + c0_c2;
136 mtx[2][1] = (s.x * c.y);
137
138 mtx[0][2] = (c0_c2 * s.y) + (s.x * s.z);
139 mtx[1][2] = (c0_s2 * s.y) - (s.x * c.z);
140 mtx[2][2] = (c.x * c.y);
141
142 mtx[0][3] = 0.0f;
143 mtx[1][3] = 0.0f;
144 mtx[2][3] = 0.0f;
145}
146
148void Matrix34f::makeS(const Vector3f &s) {
149 makeZero();
150 mtx[0][0] = s.x;
151 mtx[1][1] = s.y;
152 mtx[2][2] = s.z;
153}
154
162void Matrix34f::makeOrthonormalBasis(const Vector3f &forward, const Vector3f &up) {
163 Vector3f x = up.cross(forward);
164 x.normalise();
165 Vector3f y = forward.cross(x);
166 y.normalise();
167
168 setBase(0, x);
169 setBase(1, y);
170 setBase(2, forward);
171}
172
175void Matrix34f::setAxisRotation(f32 angle, const EGG::Vector3f &axis) {
176 EGG::Quatf q;
177 q.setAxisRotation(angle, axis);
178 makeQ(q);
179}
180
182void Matrix34f::mulRow33(size_t rowIdx, const Vector3f &row) {
183 mtx[rowIdx][0] *= row.x;
184 mtx[rowIdx][1] *= row.y;
185 mtx[rowIdx][2] *= row.z;
186}
187
189void Matrix34f::setBase(size_t col, const Vector3f &base) {
190 mtx[0][col] = base.x;
191 mtx[1][col] = base.y;
192 mtx[2][col] = base.z;
193}
194
197Matrix34f Matrix34f::multiplyTo(const Matrix34f &rhs) const {
198 Matrix34f mat;
199
200 mat[0, 0] = fma(rhs[2, 0], mtx[0][2], fma(rhs[1, 0], mtx[0][1], rhs[0, 0] * mtx[0][0]));
201 mat[0, 1] = fma(rhs[2, 1], mtx[0][2], fma(rhs[1, 1], mtx[0][1], rhs[0, 1] * mtx[0][0]));
202 mat[1, 0] = fma(rhs[2, 0], mtx[1][2], fma(rhs[1, 0], mtx[1][1], rhs[0, 0] * mtx[1][0]));
203 mat[1, 1] = fma(rhs[2, 1], mtx[1][2], fma(rhs[1, 1], mtx[1][1], rhs[0, 1] * mtx[1][0]));
204 mat[0, 2] = fma(rhs[2, 2], mtx[0][2], fma(rhs[1, 2], mtx[0][1], rhs[0, 2] * mtx[0][0]));
205 mat[0, 3] = fma(1.0f, mtx[0][3],
206 fma(rhs[2, 3], mtx[0][2], fma(rhs[1, 3], mtx[0][1], rhs[0, 3] * mtx[0][0])));
207 mat[1, 2] = fma(rhs[2, 2], mtx[1][2], fma(rhs[1, 2], mtx[1][1], rhs[0, 2] * mtx[1][0]));
208 mat[1, 3] = fma(1.0f, mtx[1][3],
209 fma(rhs[2, 3], mtx[1][2], fma(rhs[1, 3], mtx[1][1], rhs[0, 3] * mtx[1][0])));
210 mat[2, 0] = fma(rhs[2, 0], mtx[2][2], fma(rhs[1, 0], mtx[2][1], rhs[0, 0] * mtx[2][0]));
211 mat[2, 1] = fma(rhs[2, 1], mtx[2][2], fma(rhs[1, 1], mtx[2][1], rhs[0, 1] * mtx[2][0]));
212 mat[2, 2] = fma(rhs[2, 2], mtx[2][2], fma(rhs[1, 2], mtx[2][1], rhs[0, 2] * mtx[2][0]));
213 mat[2, 3] = fma(1.0f, mtx[2][3],
214 fma(rhs[2, 3], mtx[2][2], fma(rhs[1, 3], mtx[2][1], rhs[0, 3] * mtx[2][0])));
215
216 return mat;
217}
218
220Vector3f Matrix34f::multVector(const Vector3f &vec) const {
221 Vector3f ret;
222
223 ret.x = mtx[0][0] * vec.x + mtx[0][3] + mtx[0][1] * vec.y + mtx[0][2] * vec.z;
224 ret.y = mtx[1][0] * vec.x + mtx[1][3] + mtx[1][1] * vec.y + mtx[1][2] * vec.z;
225 ret.z = mtx[2][0] * vec.x + mtx[2][3] + mtx[2][1] * vec.y + mtx[2][2] * vec.z;
226
227 return ret;
228}
229
232Vector3f Matrix34f::ps_multVector(const Vector3f &vec) const {
233 Vector3f ret;
234
235 ret.x = fma(mtx[0][2], vec.z, mtx[0][0] * vec.x) + fma(mtx[0][3], 1.0f, mtx[0][1] * vec.y);
236 ret.y = fma(mtx[1][2], vec.z, mtx[1][0] * vec.x) + fma(mtx[1][3], 1.0f, mtx[1][1] * vec.y);
237 ret.z = fma(mtx[2][2], vec.z, mtx[2][0] * vec.x) + fma(mtx[2][3], 1.0f, mtx[2][1] * vec.y);
238
239 return ret;
240}
241
243Vector3f Matrix34f::multVector33(const Vector3f &vec) const {
244 Vector3f ret;
245
246 ret.x = mtx[0][0] * vec.x + mtx[0][1] * vec.y + mtx[0][2] * vec.z;
247 ret.y = mtx[1][0] * vec.x + mtx[1][1] * vec.y + mtx[1][2] * vec.z;
248 ret.z = mtx[2][0] * vec.x + mtx[2][1] * vec.y + mtx[2][2] * vec.z;
249
250 return ret;
251}
252
255Vector3f Matrix34f::ps_multVector33(const Vector3f &vec) const {
256 Vector3f ret;
257
258 ret.x = fma(mtx[0][2], vec.z, fma(mtx[0][0], vec.x, mtx[0][1] * vec.y));
259 ret.y = fma(mtx[1][2], vec.z, fma(mtx[1][0], vec.x, mtx[1][1] * vec.y));
260 ret.z = fma(mtx[2][2], vec.z, fma(mtx[2][0], vec.x, mtx[2][1] * vec.y));
261
262 return ret;
263}
264
269void Matrix34f::inverseTo33(Matrix34f &out) const {
270 f32 determinant = ((((mtx[2][1] * (mtx[0][2] * mtx[1][0])) +
271 ((mtx[2][2] * (mtx[0][0] * mtx[1][1])) +
272 (mtx[2][0] * (mtx[0][1] * mtx[1][2])))) -
273 (mtx[0][2] * (mtx[2][0] * mtx[1][1]))) -
274 (mtx[2][2] * (mtx[1][0] * mtx[0][1]))) -
275 (mtx[1][2] * (mtx[0][0] * mtx[2][1]));
276
277 if (determinant == 0.0f) {
278 out = Matrix34f::ident;
279 return;
280 }
281
282 f32 invDet = 1.0f / determinant;
283
284 out[0, 2] = (mtx[0][1] * mtx[1][2] - mtx[1][1] * mtx[0][2]) * invDet;
285 out[1, 2] = -(mtx[0][0] * mtx[1][2] - mtx[0][2] * mtx[1][0]) * invDet;
286 out[2, 1] = -(mtx[0][0] * mtx[2][1] - mtx[2][0] * mtx[0][1]) * invDet;
287 out[2, 2] = (mtx[0][0] * mtx[1][1] - mtx[1][0] * mtx[0][1]) * invDet;
288 out[2, 0] = (mtx[1][0] * mtx[2][1] - mtx[2][0] * mtx[1][1]) * invDet;
289 out[0, 0] = (mtx[1][1] * mtx[2][2] - mtx[2][1] * mtx[1][2]) * invDet;
290 out[0, 1] = -(mtx[0][1] * mtx[2][2] - mtx[2][1] * mtx[0][2]) * invDet;
291 out[1, 0] = -(mtx[1][0] * mtx[2][2] - mtx[2][0] * mtx[1][2]) * invDet;
292 out[1, 1] = (mtx[0][0] * mtx[2][2] - mtx[2][0] * mtx[0][2]) * invDet;
293}
294
298bool Matrix34f::ps_inverse(Matrix34f &out) const {
299 f32 fVar14 = fms(mtx[0][1], mtx[1][2], mtx[1][1] * mtx[0][2]);
300 f32 fVar15 = fms(mtx[1][1], mtx[2][2], mtx[2][1] * mtx[1][2]);
301 f32 fVar13 = fms(mtx[2][1], mtx[0][2], mtx[0][1] * mtx[2][2]);
302 f32 determinant = fma(mtx[2][0], fVar14, fma(mtx[1][0], fVar13, mtx[0][0] * fVar15));
303
304 if (determinant == 0.0f) {
305 return false;
306 }
307
308 f32 invDet = 1.0f / determinant;
309 invDet = -fms(determinant, invDet * invDet, invDet + invDet);
310
311 out[0, 0] = fVar15 * invDet;
312 out[0, 1] = fVar13 * invDet;
313 out[1, 0] = fms(mtx[1][2], mtx[2][0], mtx[2][2] * mtx[1][0]) * invDet;
314 out[1, 1] = fms(mtx[2][2], mtx[0][0], mtx[0][2] * mtx[2][0]) * invDet;
315 out[2, 0] = fms(mtx[1][0], mtx[2][1], mtx[1][1] * mtx[2][0]) * invDet;
316 out[2, 1] = fms(mtx[0][1], mtx[2][0], mtx[0][0] * mtx[2][1]) * invDet;
317 out[2, 2] = fms(mtx[0][0], mtx[1][1], mtx[0][1] * mtx[1][0]) * invDet;
318 out[0, 2] = fVar14 * invDet;
319 out[0, 3] = -fma(out[0, 2], mtx[2][3], fma(out[0, 1], mtx[1][3], out[0, 0] * mtx[0][3]));
320 out[1, 2] = fms(mtx[0][2], mtx[1][0], mtx[1][2] * mtx[0][0]) * invDet;
321 out[1, 3] = -fma(out[1, 2], mtx[2][3], fma(out[1, 1], mtx[1][3], out[1, 0] * mtx[0][3]));
322 out[2, 3] = -fma(out[2, 2], mtx[2][3], fma(out[2, 1], mtx[1][3], out[2, 0] * mtx[0][3]));
323
324 return true;
325}
326
328Matrix34f Matrix34f::transpose() const {
329 Matrix34f ret = *this;
330
331 ret[0, 1] = mtx[1][0];
332 ret[0, 2] = mtx[2][0];
333 ret[1, 0] = mtx[0][1];
334 ret[1, 2] = mtx[2][1];
335 ret[2, 0] = mtx[0][2];
336 ret[2, 1] = mtx[1][2];
337
338 return ret;
339}
340
342const Matrix34f Matrix34f::ident(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f,
343 0.0f);
344
345const Matrix34f Matrix34f::zero(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
346 0.0f);
347
348} // 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:162
EGG::Matrix34f::multiplyTo
Matrix34f multiplyTo(const Matrix34f &rhs) const
Multiplies two matrices.
Definition Matrix.cc:197
EGG::Matrix34f::setBase
void setBase(size_t col, const Vector3f &base)
Sets one column of a matrix.
Definition Matrix.cc:189
EGG::Matrix34f::makeZero
void makeZero()
Zeroes every element of the matrix.
Definition Matrix.hh:36
EGG::Matrix34f::setAxisRotation
void setAxisRotation(f32 angle, const Vector3f &axis)
Rotates the matrix about an axis.
Definition Matrix.cc:175
EGG::Matrix34f::makeQ
void makeQ(const Quatf &q)
Sets rotation matrix from quaternion.
Definition Matrix.cc:65
EGG::Matrix34f::multVector33
Vector3f multVector33(const Vector3f &vec) const
Multiplies a 3x3 matrix by a vector.
Definition Matrix.cc:243
EGG::Matrix34f::inverseTo33
void inverseTo33(Matrix34f &out) const
Inverts the 3x3 portion of the 3x4 matrix.
Definition Matrix.cc:269
EGG::Matrix34f::makeR
void makeR(const Vector3f &r)
Sets 3x3 rotation matrix from a vector of Euler angles.
Definition Matrix.cc:122
EGG::Matrix34f::multVector
Vector3f multVector(const Vector3f &vec) const
Multiplies a vector by a matrix.
Definition Matrix.cc:220
EGG::Matrix34f::makeRT
void makeRT(const Vector3f &r, const Vector3f &t)
Sets rotation-translation matrix.
Definition Matrix.cc:95
EGG::Matrix34f::transpose
Matrix34f transpose() const
Transposes the 3x3 portion of the matrix.
Definition Matrix.cc:328
EGG::Matrix34f::ps_multVector
Vector3f ps_multVector(const Vector3f &vec) const
Paired-singles impl. of multVector.
Definition Matrix.cc:232
EGG::Matrix34f::makeQT
void makeQT(const Quatf &q, const Vector3f &t)
Sets matrix from rotation and position.
Definition Matrix.cc:35
EGG::Matrix34f::mulRow33
void mulRow33(size_t rowIdx, const Vector3f &row)
Multiplies one row of a 3x3 matrix by a vector.
Definition Matrix.cc:182
EGG::Matrix34f::ps_inverse
bool ps_inverse(Matrix34f &out) const
Definition Matrix.cc:298
EGG::Matrix34f::ps_multVector33
Vector3f ps_multVector33(const Vector3f &vec) const
Paired-singles impl. of multVector33.
Definition Matrix.cc:255
EGG::Mathf::fma
static f32 fma(f32 x, f32 y, f32 z)
Fused multiply-add operation.
Definition Math.hh:67
EGG::Mathf::sin
static f32 sin(f32 x)
Definition Math.hh:32
EGG::Mathf::cos
static f32 cos(f32 x)
Definition Math.hh:38
EGG::Mathf::fms
static f32 fms(f32 x, f32 y, f32 z)
Fused multiply-subtract operation.
Definition Math.hh:74
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:106
EGG::Vector3f
A 3D float vector.
Definition Vector.hh:83
EGG::Vector3f::normalise
f32 normalise()
Normalizes the vector and returns the original length.
Definition Vector.cc:44