DevKit7/citro3d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix Mtx_Inverse

Browse Source
This commit is contained in:
Michael Theall 2016-08-03 00:45:42 -05:00
parent d7d5ba28a1
commit 7b511a809d
1 changed files with 106 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

203
source/maths/mtx_inverse.c
View File

@ -2,127 +2,132 @@
bool Mtx_Inverse(C3D_Mtx* out) bool Mtx_Inverse(C3D_Mtx* out)
{ {
float inv[16], det; C3D_Mtx inv;
float det;
int i; int i;
inv[0] = out->m[5] * out->m[10] * out->m[15] - inv.r[0].x = out->r[1].y * out->r[2].z * out->r[3].w -
out->m[5] * out->m[11] * out->m[14] - out->r[1].y * out->r[2].w * out->r[3].z -
out->m[9] * out->m[6] * out->m[15] + out->r[2].y * out->r[1].z * out->r[3].w +
out->m[9] * out->m[7] * out->m[14] + out->r[2].y * out->r[1].w * out->r[3].z +
out->m[13] * out->m[6] * out->m[11] - out->r[3].y * out->r[1].z * out->r[2].w -
out->m[13] * out->m[7] * out->m[10]; out->r[3].y * out->r[1].w * out->r[2].z;
inv[4] = -out->m[4] * out->m[10] * out->m[15] + inv.r[1].x = -out->r[1].x * out->r[2].z * out->r[3].w +
out->m[4] * out->m[11] * out->m[14] + out->r[1].x * out->r[2].w * out->r[3].z +
out->m[8] * out->m[6] * out->m[15] - out->r[2].x * out->r[1].z * out->r[3].w -
out->m[8] * out->m[7] * out->m[14] - out->r[2].x * out->r[1].w * out->r[3].z -
out->m[12] * out->m[6] * out->m[11] + out->r[3].x * out->r[1].z * out->r[2].w +
out->m[12] * out->m[7] * out->m[10]; out->r[3].x * out->r[1].w * out->r[2].z;
inv[8] = out->m[4] * out->m[9] * out->m[15] - inv.r[2].x = out->r[1].x * out->r[2].y * out->r[3].w -
out->m[4] * out->m[11] * out->m[13] - out->r[1].x * out->r[2].w * out->r[3].y -
out->m[8] * out->m[5] * out->m[15] + out->r[2].x * out->r[1].y * out->r[3].w +
out->m[8] * out->m[7] * out->m[13] + out->r[2].x * out->r[1].w * out->r[3].y +
out->m[12] * out->m[5] * out->m[11] - out->r[3].x * out->r[1].y * out->r[2].w -
out->m[12] * out->m[7] * out->m[9]; out->r[3].x * out->r[1].w * out->r[2].y;
inv[12] = -out->m[4] * out->m[9] * out->m[14] + inv.r[3].x = -out->r[1].x * out->r[2].y * out->r[3].z +
out->m[4] * out->m[10] * out->m[13] + out->r[1].x * out->r[2].z * out->r[3].y +
out->m[8] * out->m[5] * out->m[14] - out->r[2].x * out->r[1].y * out->r[3].z -
out->m[8] * out->m[6] * out->m[13] - out->r[2].x * out->r[1].z * out->r[3].y -
out->m[12] * out->m[5] * out->m[10] + out->r[3].x * out->r[1].y * out->r[2].z +
out->m[12] * out->m[6] * out->m[9]; out->r[3].x * out->r[1].z * out->r[2].y;
det = out->r[0].x * inv.r[0].x + out->r[0].y * inv.r[1].x +
out->r[0].z * inv.r[2].x + out->r[0].w * inv.r[3].x;
det = out->m[0] * inv[0] + out->m[1] * inv[4] + out->m[2] * inv[8] + out->m[3] * inv[12];
if (fabsf(det) < FLT_EPSILON) if (fabsf(det) < FLT_EPSILON)
return false; return false;
inv[1] = -out->m[1] * out->m[10] * out->m[15] + inv.r[0].y = -out->r[0].y * out->r[2].z * out->r[3].w +
out->m[1] * out->m[11] * out->m[14] + out->r[0].y * out->r[2].w * out->r[3].z +
out->m[9] * out->m[2] * out->m[15] - out->r[2].y * out->r[0].z * out->r[3].w -
out->m[9] * out->m[3] * out->m[14] - out->r[2].y * out->r[0].w * out->r[3].z -
out->m[13] * out->m[2] * out->m[11] + out->r[3].y * out->r[0].z * out->r[2].w +
out->m[13] * out->m[3] * out->m[10]; out->r[3].y * out->r[0].w * out->r[2].z;
inv[5] = out->m[0] * out->m[10] * out->m[15] - inv.r[1].y = out->r[0].x * out->r[2].z * out->r[3].w -
out->m[0] * out->m[11] * out->m[14] - out->r[0].x * out->r[2].w * out->r[3].z -
out->m[8] * out->m[2] * out->m[15] + out->r[2].x * out->r[0].z * out->r[3].w +
out->m[8] * out->m[3] * out->m[14] + out->r[2].x * out->r[0].w * out->r[3].z +
out->m[12] * out->m[2] * out->m[11] - out->r[3].x * out->r[0].z * out->r[2].w -
out->m[12] * out->m[3] * out->m[10]; out->r[3].x * out->r[0].w * out->r[2].z;
inv[9] = -out->m[0] * out->m[9] * out->m[15] + inv.r[2].y = -out->r[0].x * out->r[2].y * out->r[3].w +
out->m[0] * out->m[11] * out->m[13] + out->r[0].x * out->r[2].w * out->r[3].y +
out->m[8] * out->m[1] * out->m[15] - out->r[2].x * out->r[0].y * out->r[3].w -
out->m[8] * out->m[3] * out->m[13] - out->r[2].x * out->r[0].w * out->r[3].y -
out->m[12] * out->m[1] * out->m[11] + out->r[3].x * out->r[0].y * out->r[2].w +
out->m[12] * out->m[3] * out->m[9]; out->r[3].x * out->r[0].w * out->r[2].y;
inv[13] = out->m[0] * out->m[9] * out->m[14] - inv.r[3].y = out->r[0].x * out->r[2].y * out->r[3].z -
out->m[0] * out->m[10] * out->m[13] - out->r[0].x * out->r[2].z * out->r[3].y -
out->m[8] * out->m[1] * out->m[14] + out->r[2].x * out->r[0].y * out->r[3].z +
out->m[8] * out->m[2] * out->m[13] + out->r[2].x * out->r[0].z * out->r[3].y +
out->m[12] * out->m[1] * out->m[10] - out->r[3].x * out->r[0].y * out->r[2].z -
out->m[12] * out->m[2] * out->m[9]; out->r[3].x * out->r[0].z * out->r[2].y;
inv[2] = out->m[1] * out->m[6] * out->m[15] - inv.r[0].z = out->r[0].y * out->r[1].z * out->r[3].w -
out->m[1] * out->m[7] * out->m[14] - out->r[0].y * out->r[1].w * out->r[3].z -
out->m[5] * out->m[2] * out->m[15] + out->r[1].y * out->r[0].z * out->r[3].w +
out->m[5] * out->m[3] * out->m[14] + out->r[1].y * out->r[0].w * out->r[3].z +
out->m[13] * out->m[2] * out->m[7] - out->r[3].y * out->r[0].z * out->r[1].w -
out->m[13] * out->m[3] * out->m[6]; out->r[3].y * out->r[0].w * out->r[1].z;
inv[6] = -out->m[0] * out->m[6] * out->m[15] + inv.r[1].z = -out->r[0].x * out->r[1].z * out->r[3].w +
out->m[0] * out->m[7] * out->m[14] + out->r[0].x * out->r[1].w * out->r[3].z +
out->m[4] * out->m[2] * out->m[15] - out->r[1].x * out->r[0].z * out->r[3].w -
out->m[4] * out->m[3] * out->m[14] - out->r[1].x * out->r[0].w * out->r[3].z -
out->m[12] * out->m[2] * out->m[7] + out->r[3].x * out->r[0].z * out->r[1].w +
out->m[12] * out->m[3] * out->m[6]; out->r[3].x * out->r[0].w * out->r[1].z;
inv[10] = out->m[0] * out->m[5] * out->m[15] - inv.r[2].z = out->r[0].x * out->r[1].y * out->r[3].w -
out->m[0] * out->m[7] * out->m[13] - out->r[0].x * out->r[1].w * out->r[3].y -
out->m[4] * out->m[1] * out->m[15] + out->r[1].x * out->r[0].y * out->r[3].w +
out->m[4] * out->m[3] * out->m[13] + out->r[1].x * out->r[0].w * out->r[3].y +
out->m[12] * out->m[1] * out->m[7] - out->r[3].x * out->r[0].y * out->r[1].w -
out->m[12] * out->m[3] * out->m[5]; out->r[3].x * out->r[0].w * out->r[1].y;
inv[14] = -out->m[0] * out->m[5] * out->m[14] + inv.r[3].z = -out->r[0].x * out->r[1].y * out->r[3].z +
out->m[0] * out->m[6] * out->m[13] + out->r[0].x * out->r[1].z * out->r[3].y +
out->m[4] * out->m[1] * out->m[14] - out->r[1].x * out->r[0].y * out->r[3].z -
out->m[4] * out->m[2] * out->m[13] - out->r[1].x * out->r[0].z * out->r[3].y -
out->m[12] * out->m[1] * out->m[6] + out->r[3].x * out->r[0].y * out->r[1].z +
out->m[12] * out->m[2] * out->m[5]; out->r[3].x * out->r[0].z * out->r[1].y;
inv[3] = -out->m[1] * out->m[6] * out->m[11] + inv.r[0].w = -out->r[0].y * out->r[1].z * out->r[2].w +
out->m[1] * out->m[7] * out->m[10] + out->r[0].y * out->r[1].w * out->r[2].z +
out->m[5] * out->m[2] * out->m[11] - out->r[1].y * out->r[0].z * out->r[2].w -
out->m[5] * out->m[3] * out->m[10] - out->r[1].y * out->r[0].w * out->r[2].z -
out->m[9] * out->m[2] * out->m[7] + out->r[2].y * out->r[0].z * out->r[1].w +
out->m[9] * out->m[3] * out->m[6]; out->r[2].y * out->r[0].w * out->r[1].z;
inv[7] = out->m[0] * out->m[6] * out->m[11] - inv.r[1].w = out->r[0].x * out->r[1].z * out->r[2].w -
out->m[0] * out->m[7] * out->m[10] - out->r[0].x * out->r[1].w * out->r[2].z -
out->m[4] * out->m[2] * out->m[11] + out->r[1].x * out->r[0].z * out->r[2].w +
out->m[4] * out->m[3] * out->m[10] + out->r[1].x * out->r[0].w * out->r[2].z +
out->m[8] * out->m[2] * out->m[7] - out->r[2].x * out->r[0].z * out->r[1].w -
out->m[8] * out->m[3] * out->m[6]; out->r[2].x * out->r[0].w * out->r[1].z;
inv[11] = -out->m[0] * out->m[5] * out->m[11] + inv.r[2].w = -out->r[0].x * out->r[1].y * out->r[2].w +
out->m[0] * out->m[7] * out->m[9] + out->r[0].x * out->r[1].w * out->r[2].y +
out->m[4] * out->m[1] * out->m[11] - out->r[1].x * out->r[0].y * out->r[2].w -
out->m[4] * out->m[3] * out->m[9] - out->r[1].x * out->r[0].w * out->r[2].y -
out->m[8] * out->m[1] * out->m[7] + out->r[2].x * out->r[0].y * out->r[1].w +
out->m[8] * out->m[3] * out->m[5]; out->r[2].x * out->r[0].w * out->r[1].y;
inv[15] = out->m[0] * out->m[5] * out->m[10] - inv.r[3].w = out->r[0].x * out->r[1].y * out->r[2].z -
out->m[0] * out->m[6] * out->m[9] - out->r[0].x * out->r[1].z * out->r[2].y -
out->m[4] * out->m[1] * out->m[10] + out->r[1].x * out->r[0].y * out->r[2].z +
out->m[4] * out->m[2] * out->m[9] + out->r[1].x * out->r[0].z * out->r[2].y +
out->m[8] * out->m[1] * out->m[6] - out->r[2].x * out->r[0].y * out->r[1].z -
out->m[8] * out->m[2] * out->m[5]; out->r[2].x * out->r[0].z * out->r[1].y;
det = 1.0 / det; det = 1.0 / det;
for (i = 0; i < 16; i++) for (i = 0; i < 16; i++)
out->m[i] = inv[i] * det; out->m[i] = inv.m[i] * det;
return true; return true;
} }