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
167void Matrix34f::setAxisRotation(f32 angle, const EGG::Vector3f &axis) {
168 EGG::Quatf q;
169 q.setAxisRotation(angle, axis);
170 makeQ(q);
171}
172
174void Matrix34f::mulRow33(size_t rowIdx, const Vector3f &row) {
175 mtx[rowIdx][0] *= row.x;
176 mtx[rowIdx][1] *= row.y;
177 mtx[rowIdx][2] *= row.z;
178}
179
181void Matrix34f::setBase(size_t col, const Vector3f &base) {
182 mtx[0][col] = base.x;
183 mtx[1][col] = base.y;
184 mtx[2][col] = base.z;
185}
186
189Matrix34f Matrix34f::multiplyTo(const Matrix34f &rhs) const {
190 Matrix34f mat;
191
192 mat[0, 0] = fma(rhs[2, 0], mtx[0][2], fma(rhs[1, 0], mtx[0][1], rhs[0, 0] * mtx[0][0]));
193 mat[0, 1] = fma(rhs[2, 1], mtx[0][2], fma(rhs[1, 1], mtx[0][1], rhs[0, 1] * mtx[0][0]));
194 mat[1, 0] = fma(rhs[2, 0], mtx[1][2], fma(rhs[1, 0], mtx[1][1], rhs[0, 0] * mtx[1][0]));
195 mat[1, 1] = fma(rhs[2, 1], mtx[1][2], fma(rhs[1, 1], mtx[1][1], rhs[0, 1] * mtx[1][0]));
196 mat[0, 2] = fma(rhs[2, 2], mtx[0][2], fma(rhs[1, 2], mtx[0][1], rhs[0, 2] * mtx[0][0]));
197 mat[0, 3] = fma(1.0f, mtx[0][3],
198 fma(rhs[2, 3], mtx[0][2], fma(rhs[1, 3], mtx[0][1], rhs[0, 3] * mtx[0][0])));
199 mat[1, 2] = fma(rhs[2, 2], mtx[1][2], fma(rhs[1, 2], mtx[1][1], rhs[0, 2] * mtx[1][0]));
200 mat[1, 3] = fma(1.0f, mtx[1][3],
201 fma(rhs[2, 3], mtx[1][2], fma(rhs[1, 3], mtx[1][1], rhs[0, 3] * mtx[1][0])));
202 mat[2, 0] = fma(rhs[2, 0], mtx[2][2], fma(rhs[1, 0], mtx[2][1], rhs[0, 0] * mtx[2][0]));
203 mat[2, 1] = fma(rhs[2, 1], mtx[2][2], fma(rhs[1, 1], mtx[2][1], rhs[0, 1] * mtx[2][0]));
204 mat[2, 2] = fma(rhs[2, 2], mtx[2][2], fma(rhs[1, 2], mtx[2][1], rhs[0, 2] * mtx[2][0]));
205 mat[2, 3] = fma(1.0f, mtx[2][3],
206 fma(rhs[2, 3], mtx[2][2], fma(rhs[1, 3], mtx[2][1], rhs[0, 3] * mtx[2][0])));
207
208 return mat;
209}
210
212Vector3f Matrix34f::multVector(const Vector3f &vec) const {
213 Vector3f ret;
214
215 ret.x = mtx[0][0] * vec.x + mtx[0][3] + mtx[0][1] * vec.y + mtx[0][2] * vec.z;
216 ret.y = mtx[1][0] * vec.x + mtx[1][3] + mtx[1][1] * vec.y + mtx[1][2] * vec.z;
217 ret.z = mtx[2][0] * vec.x + mtx[2][3] + mtx[2][1] * vec.y + mtx[2][2] * vec.z;
218
219 return ret;
220}
221
224Vector3f Matrix34f::ps_multVector(const Vector3f &vec) const {
225 Vector3f ret;
226
227 ret.x = fma(mtx[0][2], vec.z, mtx[0][0] * vec.x) + fma(mtx[0][3], 1.0f, mtx[0][1] * vec.y);
228 ret.y = fma(mtx[1][2], vec.z, mtx[1][0] * vec.x) + fma(mtx[1][3], 1.0f, mtx[1][1] * vec.y);
229 ret.z = fma(mtx[2][2], vec.z, mtx[2][0] * vec.x) + fma(mtx[2][3], 1.0f, mtx[2][1] * vec.y);
230
231 return ret;
232}
233
236Vector3f Matrix34f::multVector33(const Vector3f &vec) const {
237 Vector3f ret;
238
239 ret.x = mtx[0][0] * vec.x + mtx[0][1] * vec.y + mtx[0][2] * vec.z;
240 ret.y = mtx[1][0] * vec.x + mtx[1][1] * vec.y + mtx[1][2] * vec.z;
241 ret.z = mtx[2][0] * vec.x + mtx[2][1] * vec.y + mtx[2][2] * vec.z;
242
243 return ret;
244}
245
248Vector3f Matrix34f::ps_multVector33(const Vector3f &vec) const {
249 Vector3f ret;
250
251 ret.x = fma(mtx[0][2], vec.z, fma(mtx[0][0], vec.x, mtx[0][1] * vec.y));
252 ret.y = fma(mtx[1][2], vec.z, fma(mtx[1][0], vec.x, mtx[1][1] * vec.y));
253 ret.z = fma(mtx[2][2], vec.z, fma(mtx[2][0], vec.x, mtx[2][1] * vec.y));
254
255 return ret;
256}
257
262void Matrix34f::inverseTo33(Matrix34f &out) const {
263 f32 determinant = ((((mtx[2][1] * (mtx[0][2] * mtx[1][0])) +
264 ((mtx[2][2] * (mtx[0][0] * mtx[1][1])) +
265 (mtx[2][0] * (mtx[0][1] * mtx[1][2])))) -
266 (mtx[0][2] * (mtx[2][0] * mtx[1][1]))) -
267 (mtx[2][2] * (mtx[1][0] * mtx[0][1]))) -
268 (mtx[1][2] * (mtx[0][0] * mtx[2][1]));
269
270 if (determinant == 0.0f) {
271 out = Matrix34f::ident;
272 return;
273 }
274
275 f32 invDet = 1.0f / determinant;
276
277 out[0, 2] = (mtx[0][1] * mtx[1][2] - mtx[1][1] * mtx[0][2]) * invDet;
278 out[1, 2] = -(mtx[0][0] * mtx[1][2] - mtx[0][2] * mtx[1][0]) * invDet;
279 out[2, 1] = -(mtx[0][0] * mtx[2][1] - mtx[2][0] * mtx[0][1]) * invDet;
280 out[2, 2] = (mtx[0][0] * mtx[1][1] - mtx[1][0] * mtx[0][1]) * invDet;
281 out[2, 0] = (mtx[1][0] * mtx[2][1] - mtx[2][0] * mtx[1][1]) * invDet;
282 out[0, 0] = (mtx[1][1] * mtx[2][2] - mtx[2][1] * mtx[1][2]) * invDet;
283 out[0, 1] = -(mtx[0][1] * mtx[2][2] - mtx[2][1] * mtx[0][2]) * invDet;
284 out[1, 0] = -(mtx[1][0] * mtx[2][2] - mtx[2][0] * mtx[1][2]) * invDet;
285 out[1, 1] = (mtx[0][0] * mtx[2][2] - mtx[2][0] * mtx[0][2]) * invDet;
286}
287
291bool Matrix34f::ps_inverse(Matrix34f &out) const {
292 f32 fVar14 = fms(mtx[0][1], mtx[1][2], mtx[1][1] * mtx[0][2]);
293 f32 fVar15 = fms(mtx[1][1], mtx[2][2], mtx[2][1] * mtx[1][2]);
294 f32 fVar13 = fms(mtx[2][1], mtx[0][2], mtx[0][1] * mtx[2][2]);
295 f32 determinant = fma(mtx[2][0], fVar14, fma(mtx[1][0], fVar13, mtx[0][0] * fVar15));
296
297 if (determinant == 0.0f) {
298 return false;
299 }
300
301 f32 invDet = 1.0f / determinant;
302 invDet = -fms(determinant, invDet * invDet, invDet + invDet);
303
304 out[0, 0] = fVar15 * invDet;
305 out[0, 1] = fVar13 * invDet;
306 out[1, 0] = fms(mtx[1][2], mtx[2][0], mtx[2][2] * mtx[1][0]) * invDet;
307 out[1, 1] = fms(mtx[2][2], mtx[0][0], mtx[0][2] * mtx[2][0]) * invDet;
308 out[2, 0] = fms(mtx[1][0], mtx[2][1], mtx[1][1] * mtx[2][0]) * invDet;
309 out[2, 1] = fms(mtx[0][1], mtx[2][0], mtx[0][0] * mtx[2][1]) * invDet;
310 out[2, 2] = fms(mtx[0][0], mtx[1][1], mtx[0][1] * mtx[1][0]) * invDet;
311 out[0, 2] = fVar14 * invDet;
312 out[0, 3] = -fma(out[0, 2], mtx[2][3], fma(out[0, 1], mtx[1][3], out[0, 0] * mtx[0][3]));
313 out[1, 2] = fms(mtx[0][2], mtx[1][0], mtx[1][2] * mtx[0][0]) * invDet;
314 out[1, 3] = -fma(out[1, 2], mtx[2][3], fma(out[1, 1], mtx[1][3], out[1, 0] * mtx[0][3]));
315 out[2, 3] = -fma(out[2, 2], mtx[2][3], fma(out[2, 1], mtx[1][3], out[2, 0] * mtx[0][3]));
316
317 return true;
318}
319
321Matrix34f Matrix34f::transpose() const {
322 Matrix34f ret = *this;
323
324 ret[0, 1] = mtx[1][0];
325 ret[0, 2] = mtx[2][0];
326 ret[1, 0] = mtx[0][1];
327 ret[1, 2] = mtx[2][1];
328 ret[2, 0] = mtx[0][2];
329 ret[2, 1] = mtx[1][2];
330
331 return ret;
332}
333
334} // 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:189
EGG::Matrix34f::setBase
void setBase(size_t col, const Vector3f &base)
Sets one column of a matrix.
Definition Matrix.cc:181
EGG::Matrix34f::base
Vector3f base(size_t col) const
Get a particular column from a matrix.
Definition Matrix.hh:70
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:167
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:236
EGG::Matrix34f::inverseTo33
void inverseTo33(Matrix34f &out) const
Inverts the 3x3 portion of the 3x4 matrix.
Definition Matrix.cc:262
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:212
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:321
EGG::Matrix34f::ps_multVector
Vector3f ps_multVector(const Vector3f &vec) const
Paired-singles impl. of multVector.
Definition Matrix.cc:224
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:174
EGG::Matrix34f::ps_inverse
bool ps_inverse(Matrix34f &out) const
Definition Matrix.cc:291
EGG::Matrix34f::ps_multVector33
Vector3f ps_multVector33(const Vector3f &vec) const
Paired-singles impl. of multVector33.
Definition Matrix.cc:248
EGG::Mathf::fma
static f32 fma(f32 x, f32 y, f32 z)
Fused multiply-add operation.
Definition Math.hh:69
EGG::Mathf::sin
static f32 sin(f32 x)
Definition Math.hh:34
EGG::Mathf::cos
static f32 cos(f32 x)
Definition Math.hh:40
EGG::Mathf::fms
static f32 fms(f32 x, f32 y, f32 z)
Fused multiply-subtract operation.
Definition Math.hh:76
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:87
EGG::Vector3f::normalise
f32 normalise()
Normalizes the vector and returns the original length.
Definition Vector.cc:44