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
138void Matrix34f::makeOrthonormalBasis(const Vector3f &forward, const Vector3f &up) {
139 Vector3f x = up.cross(forward);
140 x.normalise();
141 Vector3f y = forward.cross(x);
142 y.normalise();
143
144 setBase(0, x);
145 setBase(1, y);
146 setBase(2, forward);
147}
148
151void Matrix34f::setAxisRotation(f32 angle, const EGG::Vector3f &axis) {
152 EGG::Quatf q;
153 q.setAxisRotation(angle, axis);
154 makeQ(q);
155}
156
158void Matrix34f::mulRow33(size_t rowIdx, const Vector3f &row) {
159 mtx[rowIdx][0] *= row.x;
160 mtx[rowIdx][1] *= row.y;
161 mtx[rowIdx][2] *= row.z;
162}
163
165void Matrix34f::setBase(size_t col, const Vector3f &base) {
166 mtx[0][col] = base.x;
167 mtx[1][col] = base.y;
168 mtx[2][col] = base.z;
169}
170
173Matrix34f Matrix34f::multiplyTo(const Matrix34f &rhs) const {
174 Matrix34f mat;
175
176 mat[0, 0] = fma(rhs[2, 0], mtx[0][2], fma(rhs[1, 0], mtx[0][1], rhs[0, 0] * mtx[0][0]));
177 mat[0, 1] = fma(rhs[2, 1], mtx[0][2], fma(rhs[1, 1], mtx[0][1], rhs[0, 1] * mtx[0][0]));
178 mat[1, 0] = fma(rhs[2, 0], mtx[1][2], fma(rhs[1, 0], mtx[1][1], rhs[0, 0] * mtx[1][0]));
179 mat[1, 1] = fma(rhs[2, 1], mtx[1][2], fma(rhs[1, 1], mtx[1][1], rhs[0, 1] * mtx[1][0]));
180 mat[0, 2] = fma(rhs[2, 2], mtx[0][2], fma(rhs[1, 2], mtx[0][1], rhs[0, 2] * mtx[0][0]));
181 mat[0, 3] = fma(1.0f, mtx[0][3],
182 fma(rhs[2, 3], mtx[0][2], fma(rhs[1, 3], mtx[0][1], rhs[0, 3] * mtx[0][0])));
183 mat[1, 2] = fma(rhs[2, 2], mtx[1][2], fma(rhs[1, 2], mtx[1][1], rhs[0, 2] * mtx[1][0]));
184 mat[1, 3] = fma(1.0f, mtx[1][3],
185 fma(rhs[2, 3], mtx[1][2], fma(rhs[1, 3], mtx[1][1], rhs[0, 3] * mtx[1][0])));
186 mat[2, 0] = fma(rhs[2, 0], mtx[2][2], fma(rhs[1, 0], mtx[2][1], rhs[0, 0] * mtx[2][0]));
187 mat[2, 1] = fma(rhs[2, 1], mtx[2][2], fma(rhs[1, 1], mtx[2][1], rhs[0, 1] * mtx[2][0]));
188 mat[2, 2] = fma(rhs[2, 2], mtx[2][2], fma(rhs[1, 2], mtx[2][1], rhs[0, 2] * mtx[2][0]));
189 mat[2, 3] = fma(1.0f, mtx[2][3],
190 fma(rhs[2, 3], mtx[2][2], fma(rhs[1, 3], mtx[2][1], rhs[0, 3] * mtx[2][0])));
191
192 return mat;
193}
194
196Vector3f Matrix34f::multVector(const Vector3f &vec) const {
197 Vector3f ret;
198
199 ret.x = mtx[0][0] * vec.x + mtx[0][3] + mtx[0][1] * vec.y + mtx[0][2] * vec.z;
200 ret.y = mtx[1][0] * vec.x + mtx[1][3] + mtx[1][1] * vec.y + mtx[1][2] * vec.z;
201 ret.z = mtx[2][0] * vec.x + mtx[2][3] + mtx[2][1] * vec.y + mtx[2][2] * vec.z;
202
203 return ret;
204}
205
208Vector3f Matrix34f::ps_multVector(const Vector3f &vec) const {
209 Vector3f ret;
210
211 ret.x = fma(mtx[0][2], vec.z, mtx[0][0] * vec.x) + fma(mtx[0][3], 1.0f, mtx[0][1] * vec.y);
212 ret.y = fma(mtx[1][2], vec.z, mtx[1][0] * vec.x) + fma(mtx[1][3], 1.0f, mtx[1][1] * vec.y);
213 ret.z = fma(mtx[2][2], vec.z, mtx[2][0] * vec.x) + fma(mtx[2][3], 1.0f, mtx[2][1] * vec.y);
214
215 return ret;
216}
217
219Vector3f Matrix34f::multVector33(const Vector3f &vec) const {
220 Vector3f ret;
221
222 ret.x = mtx[0][0] * vec.x + mtx[0][1] * vec.y + mtx[0][2] * vec.z;
223 ret.y = mtx[1][0] * vec.x + mtx[1][1] * vec.y + mtx[1][2] * vec.z;
224 ret.z = mtx[2][0] * vec.x + mtx[2][1] * vec.y + mtx[2][2] * vec.z;
225
226 return ret;
227}
228
231Vector3f Matrix34f::ps_multVector33(const Vector3f &vec) const {
232 Vector3f ret;
233
234 ret.x = fma(mtx[0][2], vec.z, fma(mtx[0][0], vec.x, mtx[0][1] * vec.y));
235 ret.y = fma(mtx[1][2], vec.z, fma(mtx[1][0], vec.x, mtx[1][1] * vec.y));
236 ret.z = fma(mtx[2][2], vec.z, fma(mtx[2][0], vec.x, mtx[2][1] * vec.y));
237
238 return ret;
239}
240
245void Matrix34f::inverseTo33(Matrix34f &out) const {
246 f32 determinant = ((((mtx[2][1] * (mtx[0][2] * mtx[1][0])) +
247 ((mtx[2][2] * (mtx[0][0] * mtx[1][1])) +
248 (mtx[2][0] * (mtx[0][1] * mtx[1][2])))) -
249 (mtx[0][2] * (mtx[2][0] * mtx[1][1]))) -
250 (mtx[2][2] * (mtx[1][0] * mtx[0][1]))) -
251 (mtx[1][2] * (mtx[0][0] * mtx[2][1]));
252
253 if (determinant == 0.0f) {
254 out = Matrix34f::ident;
255 return;
256 }
257
258 f32 invDet = 1.0f / determinant;
259
260 out[0, 2] = (mtx[0][1] * mtx[1][2] - mtx[1][1] * mtx[0][2]) * invDet;
261 out[1, 2] = -(mtx[0][0] * mtx[1][2] - mtx[0][2] * mtx[1][0]) * invDet;
262 out[2, 1] = -(mtx[0][0] * mtx[2][1] - mtx[2][0] * mtx[0][1]) * invDet;
263 out[2, 2] = (mtx[0][0] * mtx[1][1] - mtx[1][0] * mtx[0][1]) * invDet;
264 out[2, 0] = (mtx[1][0] * mtx[2][1] - mtx[2][0] * mtx[1][1]) * invDet;
265 out[0, 0] = (mtx[1][1] * mtx[2][2] - mtx[2][1] * mtx[1][2]) * invDet;
266 out[0, 1] = -(mtx[0][1] * mtx[2][2] - mtx[2][1] * mtx[0][2]) * invDet;
267 out[1, 0] = -(mtx[1][0] * mtx[2][2] - mtx[2][0] * mtx[1][2]) * invDet;
268 out[1, 1] = (mtx[0][0] * mtx[2][2] - mtx[2][0] * mtx[0][2]) * invDet;
269}
270
274bool Matrix34f::ps_inverse(Matrix34f &out) const {
275 f32 fVar14 = fms(mtx[0][1], mtx[1][2], mtx[1][1] * mtx[0][2]);
276 f32 fVar15 = fms(mtx[1][1], mtx[2][2], mtx[2][1] * mtx[1][2]);
277 f32 fVar13 = fms(mtx[2][1], mtx[0][2], mtx[0][1] * mtx[2][2]);
278 f32 determinant = fma(mtx[2][0], fVar14, fma(mtx[1][0], fVar13, mtx[0][0] * fVar15));
279
280 if (determinant == 0.0f) {
281 return false;
282 }
283
284 f32 invDet = 1.0f / determinant;
285 invDet = -fms(determinant, invDet * invDet, invDet + invDet);
286
287 out[0, 0] = fVar15 * invDet;
288 out[0, 1] = fVar13 * invDet;
289 out[1, 0] = fms(mtx[1][2], mtx[2][0], mtx[2][2] * mtx[1][0]) * invDet;
290 out[1, 1] = fms(mtx[2][2], mtx[0][0], mtx[0][2] * mtx[2][0]) * invDet;
291 out[2, 0] = fms(mtx[1][0], mtx[2][1], mtx[1][1] * mtx[2][0]) * invDet;
292 out[2, 1] = fms(mtx[0][1], mtx[2][0], mtx[0][0] * mtx[2][1]) * invDet;
293 out[2, 2] = fms(mtx[0][0], mtx[1][1], mtx[0][1] * mtx[1][0]) * invDet;
294 out[0, 2] = fVar14 * invDet;
295 out[0, 3] = -fma(out[0, 2], mtx[2][3], fma(out[0, 1], mtx[1][3], out[0, 0] * mtx[0][3]));
296 out[1, 2] = fms(mtx[0][2], mtx[1][0], mtx[1][2] * mtx[0][0]) * invDet;
297 out[1, 3] = -fma(out[1, 2], mtx[2][3], fma(out[1, 1], mtx[1][3], out[1, 0] * mtx[0][3]));
298 out[2, 3] = -fma(out[2, 2], mtx[2][3], fma(out[2, 1], mtx[1][3], out[2, 0] * mtx[0][3]));
299
300 return true;
301}
302
304Matrix34f Matrix34f::transpose() const {
305 Matrix34f ret = *this;
306
307 ret[0, 1] = mtx[1][0];
308 ret[0, 2] = mtx[2][0];
309 ret[1, 0] = mtx[0][1];
310 ret[1, 2] = mtx[2][1];
311 ret[2, 0] = mtx[0][2];
312 ret[2, 1] = mtx[1][2];
313
314 return ret;
315}
316
317} // 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:138
EGG::Matrix34f::multiplyTo
Matrix34f multiplyTo(const Matrix34f &rhs) const
Multiplies two matrices.
Definition Matrix.cc:173
EGG::Matrix34f::setBase
void setBase(size_t col, const Vector3f &base)
Sets one column of a matrix.
Definition Matrix.cc:165
EGG::Matrix34f::base
Vector3f base(size_t col) const
Get a particular column from a matrix.
Definition Matrix.hh:69
EGG::Matrix34f::makeZero
void makeZero()
Zeroes every element of the matrix.
Definition Matrix.hh:45
EGG::Matrix34f::setAxisRotation
void setAxisRotation(f32 angle, const Vector3f &axis)
Rotates the matrix about an axis.
Definition Matrix.cc:151
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:219
EGG::Matrix34f::inverseTo33
void inverseTo33(Matrix34f &out) const
Inverts the 3x3 portion of the 3x4 matrix.
Definition Matrix.cc:245
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:196
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:304
EGG::Matrix34f::ps_multVector
Vector3f ps_multVector(const Vector3f &vec) const
Paired-singles impl. of multVector.
Definition Matrix.cc:208
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:158
EGG::Matrix34f::ps_inverse
bool ps_inverse(Matrix34f &out) const
Definition Matrix.cc:274
EGG::Matrix34f::ps_multVector33
Vector3f ps_multVector33(const Vector3f &vec) const
Paired-singles impl. of multVector33.
Definition Matrix.cc:231
EGG::Mathf::fma
static f32 fma(f32 x, f32 y, f32 z)
Fused multiply-add operation.
Definition Math.hh:68
EGG::Mathf::sin
static f32 sin(f32 x)
Definition Math.hh:33
EGG::Mathf::cos
static f32 cos(f32 x)
Definition Math.hh:39
EGG::Mathf::fms
static f32 fms(f32 x, f32 y, f32 z)
Fused multiply-subtract operation.
Definition Math.hh:75
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:87
EGG::Vector3f::normalise
f32 normalise()
Normalizes the vector and returns the original length.
Definition Vector.cc:44