1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#include "Math/BsMatrix4.h"
4
5#include "Math/BsVector3.h"
6#include "Math/BsMatrix3.h"
7#include "Math/BsQuaternion.h"
8
9namespace bs
10{
11 const Matrix4 Matrix4::ZERO{BS_ZERO()};
12 const Matrix4 Matrix4::IDENTITY{BS_IDENTITY()};
13
14 static float MINOR(const Matrix4& m, const UINT32 r0, const UINT32 r1, const UINT32 r2,
15 const UINT32 c0, const UINT32 c1, const UINT32 c2)
16 {
17 return m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) -
18 m[r0][c1] * (m[r1][c0] * m[r2][c2] - m[r2][c0] * m[r1][c2]) +
19 m[r0][c2] * (m[r1][c0] * m[r2][c1] - m[r2][c0] * m[r1][c1]);
20 }
21
22 Matrix4 Matrix4::adjoint() const
23 {
24 return Matrix4( MINOR(*this, 1, 2, 3, 1, 2, 3),
25 -MINOR(*this, 0, 2, 3, 1, 2, 3),
26 MINOR(*this, 0, 1, 3, 1, 2, 3),
27 -MINOR(*this, 0, 1, 2, 1, 2, 3),
28
29 -MINOR(*this, 1, 2, 3, 0, 2, 3),
30 MINOR(*this, 0, 2, 3, 0, 2, 3),
31 -MINOR(*this, 0, 1, 3, 0, 2, 3),
32 MINOR(*this, 0, 1, 2, 0, 2, 3),
33
34 MINOR(*this, 1, 2, 3, 0, 1, 3),
35 -MINOR(*this, 0, 2, 3, 0, 1, 3),
36 MINOR(*this, 0, 1, 3, 0, 1, 3),
37 -MINOR(*this, 0, 1, 2, 0, 1, 3),
38
39 -MINOR(*this, 1, 2, 3, 0, 1, 2),
40 MINOR(*this, 0, 2, 3, 0, 1, 2),
41 -MINOR(*this, 0, 1, 3, 0, 1, 2),
42 MINOR(*this, 0, 1, 2, 0, 1, 2));
43 }
44
45 float Matrix4::determinant() const
46 {
47 return m[0][0] * MINOR(*this, 1, 2, 3, 1, 2, 3) -
48 m[0][1] * MINOR(*this, 1, 2, 3, 0, 2, 3) +
49 m[0][2] * MINOR(*this, 1, 2, 3, 0, 1, 3) -
50 m[0][3] * MINOR(*this, 1, 2, 3, 0, 1, 2);
51 }
52
53 float Matrix4::determinant3x3() const
54 {
55 float cofactor00 = m[1][1] * m[2][2] - m[1][2] * m[2][1];
56 float cofactor10 = m[1][2] * m[2][0] - m[1][0] * m[2][2];
57 float cofactor20 = m[1][0] * m[2][1] - m[1][1] * m[2][0];
58
59 float det = m[0][0] * cofactor00 + m[0][1] * cofactor10 + m[0][2] * cofactor20;
60
61 return det;
62 }
63
64 Matrix4 Matrix4::inverse() const
65 {
66 float m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3];
67 float m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3];
68 float m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3];
69 float m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3];
70
71 float v0 = m20 * m31 - m21 * m30;
72 float v1 = m20 * m32 - m22 * m30;
73 float v2 = m20 * m33 - m23 * m30;
74 float v3 = m21 * m32 - m22 * m31;
75 float v4 = m21 * m33 - m23 * m31;
76 float v5 = m22 * m33 - m23 * m32;
77
78 float t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
79 float t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
80 float t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
81 float t30 = - (v3 * m10 - v1 * m11 + v0 * m12);
82
83 float invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);
84
85 float d00 = t00 * invDet;
86 float d10 = t10 * invDet;
87 float d20 = t20 * invDet;
88 float d30 = t30 * invDet;
89
90 float d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
91 float d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
92 float d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
93 float d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
94
95 v0 = m10 * m31 - m11 * m30;
96 v1 = m10 * m32 - m12 * m30;
97 v2 = m10 * m33 - m13 * m30;
98 v3 = m11 * m32 - m12 * m31;
99 v4 = m11 * m33 - m13 * m31;
100 v5 = m12 * m33 - m13 * m32;
101
102 float d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
103 float d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
104 float d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
105 float d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
106
107 v0 = m21 * m10 - m20 * m11;
108 v1 = m22 * m10 - m20 * m12;
109 v2 = m23 * m10 - m20 * m13;
110 v3 = m22 * m11 - m21 * m12;
111 v4 = m23 * m11 - m21 * m13;
112 v5 = m23 * m12 - m22 * m13;
113
114 float d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
115 float d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
116 float d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
117 float d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
118
119 return Matrix4(
120 d00, d01, d02, d03,
121 d10, d11, d12, d13,
122 d20, d21, d22, d23,
123 d30, d31, d32, d33);
124 }
125
126 Matrix4 Matrix4::inverseAffine() const
127 {
128 float m10 = m[1][0], m11 = m[1][1], m12 = m[1][2];
129 float m20 = m[2][0], m21 = m[2][1], m22 = m[2][2];
130
131 float t00 = m22 * m11 - m21 * m12;
132 float t10 = m20 * m12 - m22 * m10;
133 float t20 = m21 * m10 - m20 * m11;
134
135 float m00 = m[0][0], m01 = m[0][1], m02 = m[0][2];
136
137 float invDet = 1 / (m00 * t00 + m01 * t10 + m02 * t20);
138
139 t00 *= invDet; t10 *= invDet; t20 *= invDet;
140
141 m00 *= invDet; m01 *= invDet; m02 *= invDet;
142
143 float r00 = t00;
144 float r01 = m02 * m21 - m01 * m22;
145 float r02 = m01 * m12 - m02 * m11;
146
147 float r10 = t10;
148 float r11 = m00 * m22 - m02 * m20;
149 float r12 = m02 * m10 - m00 * m12;
150
151 float r20 = t20;
152 float r21 = m01 * m20 - m00 * m21;
153 float r22 = m00 * m11 - m01 * m10;
154
155 float m03 = m[0][3], m13 = m[1][3], m23 = m[2][3];
156
157 float r03 = - (r00 * m03 + r01 * m13 + r02 * m23);
158 float r13 = - (r10 * m03 + r11 * m13 + r12 * m23);
159 float r23 = - (r20 * m03 + r21 * m13 + r22 * m23);
160
161 return Matrix4(
162 r00, r01, r02, r03,
163 r10, r11, r12, r13,
164 r20, r21, r22, r23,
165 0, 0, 0, 1);
166 }
167
168 void Matrix4::setTRS(const Vector3& translation, const Quaternion& rotation, const Vector3& scale)
169 {
170 Matrix3 rot3x3;
171 rotation.toRotationMatrix(rot3x3);
172
173 m[0][0] = scale.x * rot3x3[0][0]; m[0][1] = scale.y * rot3x3[0][1]; m[0][2] = scale.z * rot3x3[0][2]; m[0][3] = translation.x;
174 m[1][0] = scale.x * rot3x3[1][0]; m[1][1] = scale.y * rot3x3[1][1]; m[1][2] = scale.z * rot3x3[1][2]; m[1][3] = translation.y;
175 m[2][0] = scale.x * rot3x3[2][0]; m[2][1] = scale.y * rot3x3[2][1]; m[2][2] = scale.z * rot3x3[2][2]; m[2][3] = translation.z;
176
177 // No projection term
178 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
179 }
180
181 void Matrix4::setInverseTRS(const Vector3& translation, const Quaternion& rotation, const Vector3& scale)
182 {
183 // Invert the parameters
184 Vector3 invTranslate = -translation;
185 Vector3 invScale(1 / scale.x, 1 / scale.y, 1 / scale.z);
186 Quaternion invRot = rotation.inverse();
187
188 // Because we're inverting, order is translation, rotation, scale
189 // So make translation relative to scale & rotation
190 invTranslate = invRot.rotate(invTranslate);
191 invTranslate *= invScale;
192
193 // Next, make a 3x3 rotation matrix
194 Matrix3 rot3x3;
195 invRot.toRotationMatrix(rot3x3);
196
197 // Set up final matrix with scale, rotation and translation
198 m[0][0] = invScale.x * rot3x3[0][0]; m[0][1] = invScale.x * rot3x3[0][1]; m[0][2] = invScale.x * rot3x3[0][2]; m[0][3] = invTranslate.x;
199 m[1][0] = invScale.y * rot3x3[1][0]; m[1][1] = invScale.y * rot3x3[1][1]; m[1][2] = invScale.y * rot3x3[1][2]; m[1][3] = invTranslate.y;
200 m[2][0] = invScale.z * rot3x3[2][0]; m[2][1] = invScale.z * rot3x3[2][1]; m[2][2] = invScale.z * rot3x3[2][2]; m[2][3] = invTranslate.z;
201
202 // No projection term
203 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
204 }
205
206 void Matrix4::decomposition(Vector3& position, Quaternion& rotation, Vector3& scale) const
207 {
208 Matrix3 m3x3 = get3x3();
209
210 Matrix3 matQ;
211 Vector3 vecU;
212 m3x3.QDUDecomposition(matQ, scale, vecU);
213
214 rotation = Quaternion(matQ);
215 position = Vector3(m[0][3], m[1][3], m[2][3]);
216 }
217
218 void Matrix4::makeView(const Vector3& position, const Quaternion& orientation)
219 {
220 // View matrix is:
221 //
222 // [ Lx Uy Dz Tx ]
223 // [ Lx Uy Dz Ty ]
224 // [ Lx Uy Dz Tz ]
225 // [ 0 0 0 1 ]
226 //
227 // Where T = -(Transposed(Rot) * Pos)
228
229 // This is most efficiently done using 3x3 Matrices
230 Matrix3 rot;
231 orientation.toRotationMatrix(rot);
232
233 // Make the translation relative to new axes
234 Matrix3 rotT = rot.transpose();
235 Vector3 trans = (-rotT).multiply(position);
236
237 // Make final matrix
238 *this = Matrix4(rotT);
239 m[0][3] = trans.x;
240 m[1][3] = trans.y;
241 m[2][3] = trans.z;
242 }
243
244 void Matrix4::makeProjectionOrtho(float left, float right, float top,
245 float bottom, float near, float far)
246 {
247 // Create a matrix that transforms coordinate to normalized device coordinate in range:
248 // Left -1 - Right 1
249 // Bottom -1 - Top 1
250 // Near -1 - Far 1
251
252 float deltaX = right - left;
253 float deltaY = bottom - top;
254 float deltaZ = far - near;
255
256 m[0][0] = 2.0F / deltaX;
257 m[0][1] = 0.0f;
258 m[0][2] = 0.0f;
259 m[0][3] = -(right + left) / deltaX;
260
261 m[1][0] = 0.0f;
262 m[1][1] = -2.0F / deltaY;
263 m[1][2] = 0.0f;
264 m[1][3] = (top + bottom) / deltaY;
265
266 m[2][0] = 0.0f;
267 m[2][1] = 0.0f;
268
269 if (far == 0.0f)
270 {
271 m[2][2] = 1.0f;
272 m[2][3] = 0.0f;
273 }
274 else
275 {
276 m[2][2] = -2.0F / deltaZ;
277 m[2][3] = -(far + near) / deltaZ;
278 }
279
280 m[3][0] = 0.0f;
281 m[3][1] = 0.0f;
282 m[3][2] = 0.0f;
283 m[3][3] = 1.0f;
284 }
285
286 Matrix4 Matrix4::translation(const Vector3& translation)
287 {
288 Matrix4 mat;
289
290 mat[0][0] = 1.0f; mat[0][1] = 0.0f; mat[0][2] = 0.0f; mat[0][3] = translation.x;
291 mat[1][0] = 0.0f; mat[1][1] = 1.0f; mat[1][2] = 0.0f; mat[1][3] = translation.y;
292 mat[2][0] = 0.0f; mat[2][1] = 0.0f; mat[2][2] = 1.0f; mat[2][3] = translation.z;
293 mat[3][0] = 0.0f; mat[3][1] = 0.0f; mat[3][2] = 0.0f; mat[3][3] = 1.0f;
294
295 return mat;
296 }
297
298 Matrix4 Matrix4::scaling(const Vector3& scale)
299 {
300 Matrix4 mat;
301
302 mat[0][0] = scale.x; mat[0][1] = 0.0f; mat[0][2] = 0.0f; mat[0][3] = 0.0f;
303 mat[1][0] = 0.0f; mat[1][1] = scale.y; mat[1][2] = 0.0f; mat[1][3] = 0.0f;
304 mat[2][0] = 0.0f; mat[2][1] = 0.0f; mat[2][2] = scale.z; mat[2][3] = 0.0f;
305 mat[3][0] = 0.0f; mat[3][1] = 0.0f; mat[3][2] = 0.0f; mat[3][3] = 1.0f;
306
307 return mat;
308 }
309
310 Matrix4 Matrix4::scaling(float scale)
311 {
312 Matrix4 mat;
313
314 mat[0][0] = scale; mat[0][1] = 0.0f; mat[0][2] = 0.0f; mat[0][3] = 0.0f;
315 mat[1][0] = 0.0f; mat[1][1] = scale; mat[1][2] = 0.0f; mat[1][3] = 0.0f;
316 mat[2][0] = 0.0f; mat[2][1] = 0.0f; mat[2][2] = scale; mat[2][3] = 0.0f;
317 mat[3][0] = 0.0f; mat[3][1] = 0.0f; mat[3][2] = 0.0f; mat[3][3] = 1.0f;
318
319 return mat;
320 }
321
322 Matrix4 Matrix4::rotation(const Quaternion& rotation)
323 {
324 Matrix3 mat;
325 rotation.toRotationMatrix(mat);
326
327 return Matrix4(mat);
328 }
329
330 Matrix4 Matrix4::projectionPerspective(const Degree& horzFOV, float aspect, float near, float far, bool positiveZ)
331 {
332 // Note: Duplicate code in Camera, bring it all here eventually
333 static constexpr float INFINITE_FAR_PLANE_ADJUST = 0.00001f;
334
335 Radian thetaX(horzFOV * 0.5f);
336 float tanThetaX = Math::tan(thetaX);
337 float tanThetaY = tanThetaX / aspect;
338
339 float half_w = tanThetaX * near;
340 float half_h = tanThetaY * near;
341
342 float left = -half_w;
343 float right = half_w;
344 float bottom = -half_h;
345 float top = half_h;
346
347 float inv_w = 1 / (right - left);
348 float inv_h = 1 / (top - bottom);
349 float inv_d = 1 / (far - near);
350
351 float A = 2 * near * inv_w;
352 float B = 2 * near * inv_h;
353 float C = (right + left) * inv_w;
354 float D = (top + bottom) * inv_h;
355 float q, qn;
356 float sign = positiveZ ? 1.0f : -1.0f;
357
358 if (far == 0)
359 {
360 // Infinite far plane
361 q = INFINITE_FAR_PLANE_ADJUST - 1;
362 qn = near * (INFINITE_FAR_PLANE_ADJUST - 2);
363 }
364 else
365 {
366 q = sign * (far + near) * inv_d;
367 qn = -2.0f * (far * near) * inv_d;
368 }
369
370 Matrix4 mat;
371 mat[0][0] = A; mat[0][1] = 0.0f; mat[0][2] = C; mat[0][3] = 0.0f;
372 mat[1][0] = 0.0f; mat[1][1] = B; mat[1][2] = D; mat[1][3] = 0.0f;
373 mat[2][0] = 0.0f; mat[2][1] = 0.0f; mat[2][2] = q; mat[2][3] = qn;
374 mat[3][0] = 0.0f; mat[3][1] = 0.0f; mat[3][2] = sign; mat[3][3] = 0.0f;
375
376 return mat;
377 }
378
379 Matrix4 Matrix4::projectionOrthographic(float left, float right, float top, float bottom, float near, float far)
380 {
381 Matrix4 output;
382 output.makeProjectionOrtho(left, right, top, bottom, near, far);
383
384 return output;
385 }
386
387 Matrix4 Matrix4::view(const Vector3& position, const Quaternion& orientation)
388 {
389 Matrix4 mat;
390 mat.makeView(position, orientation);
391
392 return mat;
393 }
394
395 Matrix4 Matrix4::TRS(const Vector3& translation, const Quaternion& rotation, const Vector3& scale)
396 {
397 Matrix4 mat;
398 mat.setTRS(translation, rotation, scale);
399
400 return mat;
401 }
402
403 Matrix4 Matrix4::inverseTRS(const Vector3& translation, const Quaternion& rotation, const Vector3& scale)
404 {
405 Matrix4 mat;
406 mat.setInverseTRS(translation, rotation, scale);
407
408 return mat;
409 }
410}
411