|
|
@@ -3,7 +3,7 @@
|
|
|
// Copyright (C) 2017 Sanjay Madhav. All rights reserved.
|
|
|
//
|
|
|
// Released under the BSD License
|
|
|
-// See LICENSE.txt for full details.
|
|
|
+// See LICENSE in root directory for full details.
|
|
|
// ----------------------------------------------------------------
|
|
|
|
|
|
#include "Math.h"
|
|
|
@@ -99,12 +99,13 @@ Vector3 Vector3::Transform(const Vector3& v, const Quaternion& q)
|
|
|
void Matrix4::Invert()
|
|
|
{
|
|
|
// Thanks slow math
|
|
|
- float tmp[12]; /* temp array for pairs */
|
|
|
- float src[16]; /* array of transpose source matrix */
|
|
|
- float dst[16]; /* storage */
|
|
|
- float det; /* determinant */
|
|
|
- /* transpose matrix */
|
|
|
+ // This is a really janky way to unroll everything...
|
|
|
+ float tmp[12];
|
|
|
+ float src[16];
|
|
|
+ float dst[16];
|
|
|
+ float det;
|
|
|
|
|
|
+ // Transpose matrix
|
|
|
// row 1 to col 1
|
|
|
src[0] = mat[0][0];
|
|
|
src[4] = mat[0][1];
|
|
|
@@ -129,13 +130,7 @@ void Matrix4::Invert()
|
|
|
src[11] = mat[3][2];
|
|
|
src[15] = mat[3][3];
|
|
|
|
|
|
- // for (int i = 0; i < 4; i++) {
|
|
|
- // src[i] = mat[i*4];
|
|
|
- // src[i + 4] = mat[i*4 + 1];
|
|
|
- // src[i + 8] = mat[i*4 + 2];
|
|
|
- // src[i + 12] = mat[i*4 + 3];
|
|
|
- // }
|
|
|
- /* calculate pairs for first 8 elements (cofactors) */
|
|
|
+ // Calculate cofactors
|
|
|
tmp[0] = src[10] * src[15];
|
|
|
tmp[1] = src[11] * src[14];
|
|
|
tmp[2] = src[9] * src[15];
|
|
|
@@ -148,7 +143,7 @@ void Matrix4::Invert()
|
|
|
tmp[9] = src[10] * src[12];
|
|
|
tmp[10] = src[8] * src[13];
|
|
|
tmp[11] = src[9] * src[12];
|
|
|
- /* calculate first 8 elements (cofactors) */
|
|
|
+
|
|
|
dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
|
|
|
dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
|
|
|
dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
|
|
|
@@ -165,7 +160,7 @@ void Matrix4::Invert()
|
|
|
dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
|
|
|
dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
|
|
|
dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];
|
|
|
- /* calculate pairs for second 8 elements (cofactors) */
|
|
|
+
|
|
|
tmp[0] = src[2] * src[7];
|
|
|
tmp[1] = src[3] * src[6];
|
|
|
tmp[2] = src[1] * src[7];
|
|
|
@@ -178,7 +173,7 @@ void Matrix4::Invert()
|
|
|
tmp[9] = src[2] * src[4];
|
|
|
tmp[10] = src[0] * src[5];
|
|
|
tmp[11] = src[1] * src[4];
|
|
|
- /* calculate second 8 elements (cofactors) */
|
|
|
+
|
|
|
dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
|
|
|
dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
|
|
|
dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
|
|
|
@@ -195,12 +190,16 @@ void Matrix4::Invert()
|
|
|
dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
|
|
|
dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
|
|
|
dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];
|
|
|
- /* calculate determinant */
|
|
|
+
|
|
|
+ // Calculate determinant
|
|
|
det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3] * dst[3];
|
|
|
- /* calculate matrix inverse */
|
|
|
+
|
|
|
+ // Inverse of matrix is divided by determinant
|
|
|
det = 1 / det;
|
|
|
for (int j = 0; j < 16; j++)
|
|
|
+ {
|
|
|
dst[j] *= det;
|
|
|
+ }
|
|
|
|
|
|
// Set it back
|
|
|
for (int i = 0; i < 4; i++)
|