PS2GL
OpenGL*-like API for the PS2
Loading...
Searching...
No Matches
inverse.cpp
1// I grabbed this from somewhere that I've now forgotten...
2// it badly needs to be replaced, in any case
3
4/************************************************************
5 *
6 * input:
7 * mat - pointer to array of 16 floats (source matrix)
8 * output:
9 * dst - pointer to array of 16 floats (invert matrix)
10 * *************************************************************/
11
12void Invert2(float* mat, float* dst)
13{
14 float tmp[12]; /* temp array for pairs */
15 float src[16]; /* array of transpose source matrix */
16 float det; /* determinant*/
17
18 /* transpose matrix */
19 for (int i = 0; i < 4; i++) {
20 src[i] = mat[i * 4];
21 src[i + 4] = mat[i * 4 + 1];
22 src[i + 8] = mat[i * 4 + 2];
23 src[i + 12] = mat[i * 4 + 3];
24 }
25
26 /* calculate pairs for first 8 elements (cofactors) */
27 tmp[0] = src[10] * src[15];
28 tmp[1] = src[11] * src[14];
29 tmp[2] = src[9] * src[15];
30 tmp[3] = src[11] * src[13];
31 tmp[4] = src[9] * src[14];
32 tmp[5] = src[10] * src[13];
33 tmp[6] = src[8] * src[15];
34 tmp[7] = src[11] * src[12];
35 tmp[8] = src[8] * src[14];
36 tmp[9] = src[10] * src[12];
37 tmp[10] = src[8] * src[13];
38 tmp[11] = src[9] * src[12];
39
40 /* calculate first 8 elements (cofactors) */
41 dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
42 dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
43 dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
44 dst[1] -= tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7];
45 dst[2] = tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7];
46 dst[2] -= tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7];
47 dst[3] = tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6];
48 dst[3] -= tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6];
49 dst[4] = tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3];
50 dst[4] -= tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3];
51 dst[5] = tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3];
52 dst[5] -= tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3];
53 dst[6] = tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3];
54 dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
55 dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
56 dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];
57
58 /* calculate pairs for second 8 elements (cofactors) */
59 tmp[0] = src[2] * src[7];
60 tmp[1] = src[3] * src[6];
61 tmp[2] = src[1] * src[7];
62 tmp[3] = src[3] * src[5];
63 tmp[4] = src[1] * src[6];
64 tmp[5] = src[2] * src[5];
65 tmp[6] = src[0] * src[7];
66 tmp[7] = src[3] * src[4];
67 tmp[8] = src[0] * src[6];
68 tmp[9] = src[2] * src[4];
69 tmp[10] = src[0] * src[5];
70 tmp[11] = src[1] * src[4];
71
72 /* calculate second 8 elements (cofactors) */
73 dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
74 dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
75 dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
76 dst[9] -= tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15];
77 dst[10] = tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15];
78 dst[10] -= tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15];
79 dst[11] = tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14];
80 dst[11] -= tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14];
81 dst[12] = tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9];
82 dst[12] -= tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10];
83 dst[13] = tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10];
84 dst[13] -= tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8];
85 dst[14] = tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8];
86 dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
87 dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
88 dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];
89
90 /* calculate determinant */
91 det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3] * dst[3];
92
93 /* calculate matrix inverse */
94 det = 1 / det;
95 for (int j = 0; j < 16; j++)
96 dst[j] *= det;
97}