implemented occlusion query support on corona rendering, this enables
[divverent/darkplaces.git] / matrixlib.c
1 #include "quakedef.h"
2
3 #include <math.h>
4 #include "matrixlib.h"
5
6 #ifdef _MSC_VER
7 #pragma warning(disable : 4244)     // LordHavoc: MSVC++ 4 x86, double/float
8 #pragma warning(disable : 4305)         // LordHavoc: MSVC++ 6 x86, double/float
9 #endif
10
11 const matrix4x4_t identitymatrix =
12 {
13         {
14                 {1, 0, 0, 0},
15                 {0, 1, 0, 0},
16                 {0, 0, 1, 0},
17                 {0, 0, 0, 1}
18         }
19 };
20
21 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
22 {
23         *out = *in;
24 }
25
26 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
27 {
28         out->m[0][0] = in->m[0][0];
29         out->m[0][1] = in->m[0][1];
30         out->m[0][2] = in->m[0][2];
31         out->m[0][3] = 0.0f;
32         out->m[1][0] = in->m[1][0];
33         out->m[1][1] = in->m[1][1];
34         out->m[1][2] = in->m[1][2];
35         out->m[1][3] = 0.0f;
36         out->m[2][0] = in->m[2][0];
37         out->m[2][1] = in->m[2][1];
38         out->m[2][2] = in->m[2][2];
39         out->m[2][3] = 0.0f;
40         out->m[3][0] = 0.0f;
41         out->m[3][1] = 0.0f;
42         out->m[3][2] = 0.0f;
43         out->m[3][3] = 1.0f;
44 }
45
46 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
47 {
48 #ifdef MATRIX4x4_OPENGLORIENTATION
49         out->m[0][0] = 1.0f;
50         out->m[1][0] = 0.0f;
51         out->m[2][0] = 0.0f;
52         out->m[3][0] = in->m[0][3];
53         out->m[0][1] = 0.0f;
54         out->m[1][1] = 1.0f;
55         out->m[2][1] = 0.0f;
56         out->m[3][1] = in->m[1][3];
57         out->m[0][2] = 0.0f;
58         out->m[1][2] = 0.0f;
59         out->m[2][2] = 1.0f;
60         out->m[3][2] = in->m[2][3];
61         out->m[0][3] = 0.0f;
62         out->m[1][3] = 0.0f;
63         out->m[2][3] = 0.0f;
64         out->m[3][3] = 1.0f;
65 #else
66         out->m[0][0] = 1.0f;
67         out->m[0][1] = 0.0f;
68         out->m[0][2] = 0.0f;
69         out->m[0][3] = in->m[0][3];
70         out->m[1][0] = 0.0f;
71         out->m[1][1] = 1.0f;
72         out->m[1][2] = 0.0f;
73         out->m[1][3] = in->m[1][3];
74         out->m[2][0] = 0.0f;
75         out->m[2][1] = 0.0f;
76         out->m[2][2] = 1.0f;
77         out->m[2][3] = in->m[2][3];
78         out->m[3][0] = 0.0f;
79         out->m[3][1] = 0.0f;
80         out->m[3][2] = 0.0f;
81         out->m[3][3] = 1.0f;
82 #endif
83 }
84
85 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
86 {
87 #ifdef MATRIX4x4_OPENGLORIENTATION
88         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[1][0] * in2->m[0][1] + in1->m[2][0] * in2->m[0][2] + in1->m[3][0] * in2->m[0][3];
89         out->m[1][0] = in1->m[0][0] * in2->m[1][0] + in1->m[1][0] * in2->m[1][1] + in1->m[2][0] * in2->m[1][2] + in1->m[3][0] * in2->m[1][3];
90         out->m[2][0] = in1->m[0][0] * in2->m[2][0] + in1->m[1][0] * in2->m[2][1] + in1->m[2][0] * in2->m[2][2] + in1->m[3][0] * in2->m[2][3];
91         out->m[3][0] = in1->m[0][0] * in2->m[3][0] + in1->m[1][0] * in2->m[3][1] + in1->m[2][0] * in2->m[3][2] + in1->m[3][0] * in2->m[3][3];
92         out->m[0][1] = in1->m[0][1] * in2->m[0][0] + in1->m[1][1] * in2->m[0][1] + in1->m[2][1] * in2->m[0][2] + in1->m[3][1] * in2->m[0][3];
93         out->m[1][1] = in1->m[0][1] * in2->m[1][0] + in1->m[1][1] * in2->m[1][1] + in1->m[2][1] * in2->m[1][2] + in1->m[3][1] * in2->m[1][3];
94         out->m[2][1] = in1->m[0][1] * in2->m[2][0] + in1->m[1][1] * in2->m[2][1] + in1->m[2][1] * in2->m[2][2] + in1->m[3][1] * in2->m[2][3];
95         out->m[3][1] = in1->m[0][1] * in2->m[3][0] + in1->m[1][1] * in2->m[3][1] + in1->m[2][1] * in2->m[3][2] + in1->m[3][1] * in2->m[3][3];
96         out->m[0][2] = in1->m[0][2] * in2->m[0][0] + in1->m[1][2] * in2->m[0][1] + in1->m[2][2] * in2->m[0][2] + in1->m[3][2] * in2->m[0][3];
97         out->m[1][2] = in1->m[0][2] * in2->m[1][0] + in1->m[1][2] * in2->m[1][1] + in1->m[2][2] * in2->m[1][2] + in1->m[3][2] * in2->m[1][3];
98         out->m[2][2] = in1->m[0][2] * in2->m[2][0] + in1->m[1][2] * in2->m[2][1] + in1->m[2][2] * in2->m[2][2] + in1->m[3][2] * in2->m[2][3];
99         out->m[3][2] = in1->m[0][2] * in2->m[3][0] + in1->m[1][2] * in2->m[3][1] + in1->m[2][2] * in2->m[3][2] + in1->m[3][2] * in2->m[3][3];
100         out->m[0][3] = in1->m[0][3] * in2->m[0][0] + in1->m[1][3] * in2->m[0][1] + in1->m[2][3] * in2->m[0][2] + in1->m[3][3] * in2->m[0][3];
101         out->m[1][3] = in1->m[0][3] * in2->m[1][0] + in1->m[1][3] * in2->m[1][1] + in1->m[2][3] * in2->m[1][2] + in1->m[3][3] * in2->m[1][3];
102         out->m[2][3] = in1->m[0][3] * in2->m[2][0] + in1->m[1][3] * in2->m[2][1] + in1->m[2][3] * in2->m[2][2] + in1->m[3][3] * in2->m[2][3];
103         out->m[3][3] = in1->m[0][3] * in2->m[3][0] + in1->m[1][3] * in2->m[3][1] + in1->m[2][3] * in2->m[3][2] + in1->m[3][3] * in2->m[3][3];
104 #else
105         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0] + in1->m[0][3] * in2->m[3][0];
106         out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1] + in1->m[0][3] * in2->m[3][1];
107         out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2] + in1->m[0][3] * in2->m[3][2];
108         out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3] * in2->m[3][3];
109         out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0] + in1->m[1][3] * in2->m[3][0];
110         out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1] + in1->m[1][3] * in2->m[3][1];
111         out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2] + in1->m[1][3] * in2->m[3][2];
112         out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3] * in2->m[3][3];
113         out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0] + in1->m[2][3] * in2->m[3][0];
114         out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1] + in1->m[2][3] * in2->m[3][1];
115         out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2] + in1->m[2][3] * in2->m[3][2];
116         out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3] * in2->m[3][3];
117         out->m[3][0] = in1->m[3][0] * in2->m[0][0] + in1->m[3][1] * in2->m[1][0] + in1->m[3][2] * in2->m[2][0] + in1->m[3][3] * in2->m[3][0];
118         out->m[3][1] = in1->m[3][0] * in2->m[0][1] + in1->m[3][1] * in2->m[1][1] + in1->m[3][2] * in2->m[2][1] + in1->m[3][3] * in2->m[3][1];
119         out->m[3][2] = in1->m[3][0] * in2->m[0][2] + in1->m[3][1] * in2->m[1][2] + in1->m[3][2] * in2->m[2][2] + in1->m[3][3] * in2->m[3][2];
120         out->m[3][3] = in1->m[3][0] * in2->m[0][3] + in1->m[3][1] * in2->m[1][3] + in1->m[3][2] * in2->m[2][3] + in1->m[3][3] * in2->m[3][3];
121 #endif
122 }
123
124 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
125 {
126         out->m[0][0] = in1->m[0][0];
127         out->m[0][1] = in1->m[1][0];
128         out->m[0][2] = in1->m[2][0];
129         out->m[0][3] = in1->m[3][0];
130         out->m[1][0] = in1->m[0][1];
131         out->m[1][1] = in1->m[1][1];
132         out->m[1][2] = in1->m[2][1];
133         out->m[1][3] = in1->m[3][1];
134         out->m[2][0] = in1->m[0][2];
135         out->m[2][1] = in1->m[1][2];
136         out->m[2][2] = in1->m[2][2];
137         out->m[2][3] = in1->m[3][2];
138         out->m[3][0] = in1->m[0][3];
139         out->m[3][1] = in1->m[1][3];
140         out->m[3][2] = in1->m[2][3];
141         out->m[3][3] = in1->m[3][3];
142 }
143
144 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
145 {
146         double  *temp;
147         double  *r[4];
148         double  rtemp[4][8];
149         double  m[4];
150         double  s;
151
152         r[0]    = rtemp[0];
153         r[1]    = rtemp[1];
154         r[2]    = rtemp[2];
155         r[3]    = rtemp[3];
156
157 #ifdef MATRIX4x4_OPENGLORIENTATION
158         r[0][0] = in1->m[0][0]; r[0][1] = in1->m[1][0]; r[0][2] = in1->m[2][0]; r[0][3] = in1->m[3][0];
159         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
160
161         r[1][0] = in1->m[0][1]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[2][1]; r[1][3] = in1->m[3][1];
162         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
163
164         r[2][0] = in1->m[0][2]; r[2][1] = in1->m[1][2]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[3][2];
165         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
166
167         r[3][0] = in1->m[0][3]; r[3][1] = in1->m[1][3]; r[3][2] = in1->m[2][3]; r[3][3] = in1->m[3][3];
168         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
169 #else
170         r[0][0] = in1->m[0][0]; r[0][1] = in1->m[0][1]; r[0][2] = in1->m[0][2]; r[0][3] = in1->m[0][3];
171         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
172
173         r[1][0] = in1->m[1][0]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[1][2]; r[1][3] = in1->m[1][3];
174         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
175
176         r[2][0] = in1->m[2][0]; r[2][1] = in1->m[2][1]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[2][3];
177         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
178
179         r[3][0] = in1->m[3][0]; r[3][1] = in1->m[3][1]; r[3][2] = in1->m[3][2]; r[3][3] = in1->m[3][3];
180         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
181 #endif
182
183         if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
184         if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
185         if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; }
186
187         if (r[0][0])
188         {
189                 m[1]    = r[1][0] / r[0][0];
190                 m[2]    = r[2][0] / r[0][0];
191                 m[3]    = r[3][0] / r[0][0];
192
193                 s       = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s;
194                 s       = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s;
195                 s       = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s;
196
197                 s       = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
198                 s       = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
199                 s       = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
200                 s       = r[0][7]; if (s) { r[1][7] -= m[1] * s; r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
201
202                 if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
203                 if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
204
205                 if (r[1][1])
206                 {
207                         m[2]            = r[2][1] / r[1][1];
208                         m[3]            = r[3][1] / r[1][1];
209                         r[2][2] -= m[2] * r[1][2];
210                         r[3][2] -= m[3] * r[1][2];
211                         r[2][3] -= m[2] * r[1][3];
212                         r[3][3] -= m[3] * r[1][3];
213
214                         s       = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
215                         s       = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
216                         s       = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
217                         s       = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
218
219                         if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
220
221                         if (r[2][2])
222                         {
223                                 m[3]            = r[3][2] / r[2][2];
224                                 r[3][3] -= m[3] * r[2][3];
225                                 r[3][4] -= m[3] * r[2][4];
226                                 r[3][5] -= m[3] * r[2][5];
227                                 r[3][6] -= m[3] * r[2][6];
228                                 r[3][7] -= m[3] * r[2][7];
229
230                                 if (r[3][3])
231                                 {
232                                         s                       = 1.0 / r[3][3];
233                                         r[3][4] *= s;
234                                         r[3][5] *= s;
235                                         r[3][6] *= s;
236                                         r[3][7] *= s;
237
238                                         m[2]            = r[2][3];
239                                         s                       = 1.0 / r[2][2];
240                                         r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
241                                         r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
242                                         r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
243                                         r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
244
245                                         m[1]            = r[1][3];
246                                         r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1];
247                                         r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1];
248
249                                         m[0]            = r[0][3];
250                                         r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0];
251                                         r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0];
252
253                                         m[1]            = r[1][2];
254                                         s                       = 1.0 / r[1][1];
255                                         r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
256                                         r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
257
258                                         m[0]            = r[0][2];
259                                         r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0];
260                                         r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0];
261
262                                         m[0]            = r[0][1];
263                                         s                       = 1.0 / r[0][0];
264                                         r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
265                                         r[0][6] = s * (r[0][6] - r[1][6] * m[0]), r[0][7] = s * (r[0][7] - r[1][7] * m[0]);
266
267 #ifdef MATRIX4x4_OPENGLORIENTATION
268                                         out->m[0][0]    = r[0][4];
269                                         out->m[0][1]    = r[1][4];
270                                         out->m[0][2]    = r[2][4];
271                                         out->m[0][3]    = r[3][4];
272                                         out->m[1][0]    = r[0][5];
273                                         out->m[1][1]    = r[1][5];
274                                         out->m[1][2]    = r[2][5];
275                                         out->m[1][3]    = r[3][5];
276                                         out->m[2][0]    = r[0][6];
277                                         out->m[2][1]    = r[1][6];
278                                         out->m[2][2]    = r[2][6];
279                                         out->m[2][3]    = r[3][6];
280                                         out->m[3][0]    = r[0][7];
281                                         out->m[3][1]    = r[1][7];
282                                         out->m[3][2]    = r[2][7];
283                                         out->m[3][3]    = r[3][7];
284 #else
285                                         out->m[0][0]    = r[0][4];
286                                         out->m[0][1]    = r[0][5];
287                                         out->m[0][2]    = r[0][6];
288                                         out->m[0][3]    = r[0][7];
289                                         out->m[1][0]    = r[1][4];
290                                         out->m[1][1]    = r[1][5];
291                                         out->m[1][2]    = r[1][6];
292                                         out->m[1][3]    = r[1][7];
293                                         out->m[2][0]    = r[2][4];
294                                         out->m[2][1]    = r[2][5];
295                                         out->m[2][2]    = r[2][6];
296                                         out->m[2][3]    = r[2][7];
297                                         out->m[3][0]    = r[3][4];
298                                         out->m[3][1]    = r[3][5];
299                                         out->m[3][2]    = r[3][6];
300                                         out->m[3][3]    = r[3][7];
301 #endif
302
303                                         return 1;
304                                 }
305                         }
306                 }
307         }
308
309         return 0;
310 }
311
312 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
313 {
314         // we only support uniform scaling, so assume the first row is enough
315         // (note the lack of sqrt here, because we're trying to undo the scaling,
316         // this means multiplying by the inverse scale twice - squaring it, which
317         // makes the sqrt a waste of time)
318 #if 1
319         double scale = 1.0 / (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
320 #else
321         double scale = 3.0 / sqrt
322                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
323                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
324                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
325         scale *= scale;
326 #endif
327
328         // invert the rotation by transposing and multiplying by the squared
329         // recipricol of the input matrix scale as described above
330         out->m[0][0] = in1->m[0][0] * scale;
331         out->m[0][1] = in1->m[1][0] * scale;
332         out->m[0][2] = in1->m[2][0] * scale;
333         out->m[1][0] = in1->m[0][1] * scale;
334         out->m[1][1] = in1->m[1][1] * scale;
335         out->m[1][2] = in1->m[2][1] * scale;
336         out->m[2][0] = in1->m[0][2] * scale;
337         out->m[2][1] = in1->m[1][2] * scale;
338         out->m[2][2] = in1->m[2][2] * scale;
339
340 #ifdef MATRIX4x4_OPENGLORIENTATION
341         // invert the translate
342         out->m[3][0] = -(in1->m[3][0] * out->m[0][0] + in1->m[3][1] * out->m[1][0] + in1->m[3][2] * out->m[2][0]);
343         out->m[3][1] = -(in1->m[3][0] * out->m[0][1] + in1->m[3][1] * out->m[1][1] + in1->m[3][2] * out->m[2][1]);
344         out->m[3][2] = -(in1->m[3][0] * out->m[0][2] + in1->m[3][1] * out->m[1][2] + in1->m[3][2] * out->m[2][2]);
345
346         // don't know if there's anything worth doing here
347         out->m[0][3] = 0;
348         out->m[1][3] = 0;
349         out->m[2][3] = 0;
350         out->m[3][3] = 1;
351 #else
352         // invert the translate
353         out->m[0][3] = -(in1->m[0][3] * out->m[0][0] + in1->m[1][3] * out->m[0][1] + in1->m[2][3] * out->m[0][2]);
354         out->m[1][3] = -(in1->m[0][3] * out->m[1][0] + in1->m[1][3] * out->m[1][1] + in1->m[2][3] * out->m[1][2]);
355         out->m[2][3] = -(in1->m[0][3] * out->m[2][0] + in1->m[1][3] * out->m[2][1] + in1->m[2][3] * out->m[2][2]);
356
357         // don't know if there's anything worth doing here
358         out->m[3][0] = 0;
359         out->m[3][1] = 0;
360         out->m[3][2] = 0;
361         out->m[3][3] = 1;
362 #endif
363 }
364
365 void Matrix4x4_Interpolate (matrix4x4_t *out, matrix4x4_t *in1, matrix4x4_t *in2, double frac)
366 {
367         int i, j;
368         for (i = 0;i < 4;i++)
369                 for (j = 0;j < 4;j++)
370                         out->m[i][j] = in1->m[i][j] + frac * (in2->m[i][j] - in1->m[i][j]);
371 }
372
373 void Matrix4x4_Clear (matrix4x4_t *out)
374 {
375         int i, j;
376         for (i = 0;i < 4;i++)
377                 for (j = 0;j < 4;j++)
378                         out->m[i][j] = 0;
379 }
380
381 void Matrix4x4_Accumulate (matrix4x4_t *out, matrix4x4_t *in, double weight)
382 {
383         int i, j;
384         for (i = 0;i < 4;i++)
385                 for (j = 0;j < 4;j++)
386                         out->m[i][j] += in->m[i][j] * weight;
387 }
388
389 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
390 {
391         // scale rotation matrix vectors to a length of 1
392         // note: this is only designed to undo uniform scaling
393         double scale = 1.0 / sqrt(in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
394         *out = *in1;
395         Matrix4x4_Scale(out, scale, 1);
396 }
397
398 void Matrix4x4_Normalize3 (matrix4x4_t *out, matrix4x4_t *in1)
399 {
400         int i;
401         double scale;
402         // scale each rotation matrix vector to a length of 1
403         // intended for use after Matrix4x4_Interpolate or Matrix4x4_Accumulate
404         *out = *in1;
405         for (i = 0;i < 3;i++)
406         {
407 #ifdef MATRIX4x4_OPENGLORIENTATION
408                 scale = sqrt(in1->m[i][0] * in1->m[i][0] + in1->m[i][1] * in1->m[i][1] + in1->m[i][2] * in1->m[i][2]);
409                 if (scale)
410                         scale = 1.0 / scale;
411                 out->m[i][0] *= scale;
412                 out->m[i][1] *= scale;
413                 out->m[i][2] *= scale;
414 #else
415                 scale = sqrt(in1->m[0][i] * in1->m[0][i] + in1->m[1][i] * in1->m[1][i] + in1->m[2][i] * in1->m[2][i]);
416                 if (scale)
417                         scale = 1.0 / scale;
418                 out->m[0][i] *= scale;
419                 out->m[1][i] *= scale;
420                 out->m[2][i] *= scale;
421 #endif
422         }
423 }
424
425 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
426 {
427         int i;
428         double d;
429         double p[4], p2[4];
430         p[0] = normalx;
431         p[1] = normaly;
432         p[2] = normalz;
433         p[3] = -dist;
434         p2[0] = p[0] * axisscale;
435         p2[1] = p[1] * axisscale;
436         p2[2] = p[2] * axisscale;
437         p2[3] = 0;
438         for (i = 0;i < 4;i++)
439         {
440 #ifdef MATRIX4x4_OPENGLORIENTATION
441                 d = out->m[i][0] * p[0] + out->m[i][1] * p[1] + out->m[i][2] * p[2] + out->m[i][3] * p[3];
442                 out->m[i][0] += p2[0] * d;
443                 out->m[i][1] += p2[1] * d;
444                 out->m[i][2] += p2[2] * d;
445 #else
446                 d = out->m[0][i] * p[0] + out->m[1][i] * p[1] + out->m[2][i] * p[2] + out->m[3][i] * p[3];
447                 out->m[0][i] += p2[0] * d;
448                 out->m[1][i] += p2[1] * d;
449                 out->m[2][i] += p2[2] * d;
450 #endif
451         }
452 }
453
454 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
455 {
456         out->m[0][0]=1.0f;
457         out->m[0][1]=0.0f;
458         out->m[0][2]=0.0f;
459         out->m[0][3]=0.0f;
460         out->m[1][0]=0.0f;
461         out->m[1][1]=1.0f;
462         out->m[1][2]=0.0f;
463         out->m[1][3]=0.0f;
464         out->m[2][0]=0.0f;
465         out->m[2][1]=0.0f;
466         out->m[2][2]=1.0f;
467         out->m[2][3]=0.0f;
468         out->m[3][0]=0.0f;
469         out->m[3][1]=0.0f;
470         out->m[3][2]=0.0f;
471         out->m[3][3]=1.0f;
472 }
473
474 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
475 {
476 #ifdef MATRIX4x4_OPENGLORIENTATION
477         out->m[0][0]=1.0f;
478         out->m[1][0]=0.0f;
479         out->m[2][0]=0.0f;
480         out->m[3][0]=x;
481         out->m[0][1]=0.0f;
482         out->m[1][1]=1.0f;
483         out->m[2][1]=0.0f;
484         out->m[3][1]=y;
485         out->m[0][2]=0.0f;
486         out->m[1][2]=0.0f;
487         out->m[2][2]=1.0f;
488         out->m[3][2]=z;
489         out->m[0][3]=0.0f;
490         out->m[1][3]=0.0f;
491         out->m[2][3]=0.0f;
492         out->m[3][3]=1.0f;
493 #else
494         out->m[0][0]=1.0f;
495         out->m[0][1]=0.0f;
496         out->m[0][2]=0.0f;
497         out->m[0][3]=x;
498         out->m[1][0]=0.0f;
499         out->m[1][1]=1.0f;
500         out->m[1][2]=0.0f;
501         out->m[1][3]=y;
502         out->m[2][0]=0.0f;
503         out->m[2][1]=0.0f;
504         out->m[2][2]=1.0f;
505         out->m[2][3]=z;
506         out->m[3][0]=0.0f;
507         out->m[3][1]=0.0f;
508         out->m[3][2]=0.0f;
509         out->m[3][3]=1.0f;
510 #endif
511 }
512
513 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
514 {
515         double len, c, s;
516
517         len = x*x+y*y+z*z;
518         if (len != 0.0f)
519                 len = 1.0f / sqrt(len);
520         x *= len;
521         y *= len;
522         z *= len;
523
524         angle *= (-M_PI / 180.0);
525         c = cos(angle);
526         s = sin(angle);
527
528 #ifdef MATRIX4x4_OPENGLORIENTATION
529         out->m[0][0]=x * x + c * (1 - x * x);
530         out->m[1][0]=x * y * (1 - c) + z * s;
531         out->m[2][0]=z * x * (1 - c) - y * s;
532         out->m[3][0]=0.0f;
533         out->m[0][1]=x * y * (1 - c) - z * s;
534         out->m[1][1]=y * y + c * (1 - y * y);
535         out->m[2][1]=y * z * (1 - c) + x * s;
536         out->m[3][1]=0.0f;
537         out->m[0][2]=z * x * (1 - c) + y * s;
538         out->m[1][2]=y * z * (1 - c) - x * s;
539         out->m[2][2]=z * z + c * (1 - z * z);
540         out->m[3][2]=0.0f;
541         out->m[0][3]=0.0f;
542         out->m[1][3]=0.0f;
543         out->m[2][3]=0.0f;
544         out->m[3][3]=1.0f;
545 #else
546         out->m[0][0]=x * x + c * (1 - x * x);
547         out->m[0][1]=x * y * (1 - c) + z * s;
548         out->m[0][2]=z * x * (1 - c) - y * s;
549         out->m[0][3]=0.0f;
550         out->m[1][0]=x * y * (1 - c) - z * s;
551         out->m[1][1]=y * y + c * (1 - y * y);
552         out->m[1][2]=y * z * (1 - c) + x * s;
553         out->m[1][3]=0.0f;
554         out->m[2][0]=z * x * (1 - c) + y * s;
555         out->m[2][1]=y * z * (1 - c) - x * s;
556         out->m[2][2]=z * z + c * (1 - z * z);
557         out->m[2][3]=0.0f;
558         out->m[3][0]=0.0f;
559         out->m[3][1]=0.0f;
560         out->m[3][2]=0.0f;
561         out->m[3][3]=1.0f;
562 #endif
563 }
564
565 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
566 {
567         out->m[0][0]=x;
568         out->m[0][1]=0.0f;
569         out->m[0][2]=0.0f;
570         out->m[0][3]=0.0f;
571         out->m[1][0]=0.0f;
572         out->m[1][1]=x;
573         out->m[1][2]=0.0f;
574         out->m[1][3]=0.0f;
575         out->m[2][0]=0.0f;
576         out->m[2][1]=0.0f;
577         out->m[2][2]=x;
578         out->m[2][3]=0.0f;
579         out->m[3][0]=0.0f;
580         out->m[3][1]=0.0f;
581         out->m[3][2]=0.0f;
582         out->m[3][3]=1.0f;
583 }
584
585 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
586 {
587         out->m[0][0]=x;
588         out->m[0][1]=0.0f;
589         out->m[0][2]=0.0f;
590         out->m[0][3]=0.0f;
591         out->m[1][0]=0.0f;
592         out->m[1][1]=y;
593         out->m[1][2]=0.0f;
594         out->m[1][3]=0.0f;
595         out->m[2][0]=0.0f;
596         out->m[2][1]=0.0f;
597         out->m[2][2]=z;
598         out->m[2][3]=0.0f;
599         out->m[3][0]=0.0f;
600         out->m[3][1]=0.0f;
601         out->m[3][2]=0.0f;
602         out->m[3][3]=1.0f;
603 }
604
605 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
606 {
607         double angle, sr, sp, sy, cr, cp, cy;
608
609         if (roll)
610         {
611                 angle = yaw * (M_PI*2 / 360);
612                 sy = sin(angle);
613                 cy = cos(angle);
614                 angle = pitch * (M_PI*2 / 360);
615                 sp = sin(angle);
616                 cp = cos(angle);
617                 angle = roll * (M_PI*2 / 360);
618                 sr = sin(angle);
619                 cr = cos(angle);
620 #ifdef MATRIX4x4_OPENGLORIENTATION
621                 out->m[0][0] = (cp*cy) * scale;
622                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
623                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
624                 out->m[3][0] = x;
625                 out->m[0][1] = (cp*sy) * scale;
626                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
627                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
628                 out->m[3][1] = y;
629                 out->m[0][2] = (-sp) * scale;
630                 out->m[1][2] = (sr*cp) * scale;
631                 out->m[2][2] = (cr*cp) * scale;
632                 out->m[3][2] = z;
633                 out->m[0][3] = 0;
634                 out->m[1][3] = 0;
635                 out->m[2][3] = 0;
636                 out->m[3][3] = 1;
637 #else
638                 out->m[0][0] = (cp*cy) * scale;
639                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
640                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
641                 out->m[0][3] = x;
642                 out->m[1][0] = (cp*sy) * scale;
643                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
644                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
645                 out->m[1][3] = y;
646                 out->m[2][0] = (-sp) * scale;
647                 out->m[2][1] = (sr*cp) * scale;
648                 out->m[2][2] = (cr*cp) * scale;
649                 out->m[2][3] = z;
650                 out->m[3][0] = 0;
651                 out->m[3][1] = 0;
652                 out->m[3][2] = 0;
653                 out->m[3][3] = 1;
654 #endif
655         }
656         else if (pitch)
657         {
658                 angle = yaw * (M_PI*2 / 360);
659                 sy = sin(angle);
660                 cy = cos(angle);
661                 angle = pitch * (M_PI*2 / 360);
662                 sp = sin(angle);
663                 cp = cos(angle);
664 #ifdef MATRIX4x4_OPENGLORIENTATION
665                 out->m[0][0] = (cp*cy) * scale;
666                 out->m[1][0] = (-sy) * scale;
667                 out->m[2][0] = (sp*cy) * scale;
668                 out->m[3][0] = x;
669                 out->m[0][1] = (cp*sy) * scale;
670                 out->m[1][1] = (cy) * scale;
671                 out->m[2][1] = (sp*sy) * scale;
672                 out->m[3][1] = y;
673                 out->m[0][2] = (-sp) * scale;
674                 out->m[1][2] = 0;
675                 out->m[2][2] = (cp) * scale;
676                 out->m[3][2] = z;
677                 out->m[0][3] = 0;
678                 out->m[1][3] = 0;
679                 out->m[2][3] = 0;
680                 out->m[3][3] = 1;
681 #else
682                 out->m[0][0] = (cp*cy) * scale;
683                 out->m[0][1] = (-sy) * scale;
684                 out->m[0][2] = (sp*cy) * scale;
685                 out->m[0][3] = x;
686                 out->m[1][0] = (cp*sy) * scale;
687                 out->m[1][1] = (cy) * scale;
688                 out->m[1][2] = (sp*sy) * scale;
689                 out->m[1][3] = y;
690                 out->m[2][0] = (-sp) * scale;
691                 out->m[2][1] = 0;
692                 out->m[2][2] = (cp) * scale;
693                 out->m[2][3] = z;
694                 out->m[3][0] = 0;
695                 out->m[3][1] = 0;
696                 out->m[3][2] = 0;
697                 out->m[3][3] = 1;
698 #endif
699         }
700         else if (yaw)
701         {
702                 angle = yaw * (M_PI*2 / 360);
703                 sy = sin(angle);
704                 cy = cos(angle);
705 #ifdef MATRIX4x4_OPENGLORIENTATION
706                 out->m[0][0] = (cy) * scale;
707                 out->m[1][0] = (-sy) * scale;
708                 out->m[2][0] = 0;
709                 out->m[3][0] = x;
710                 out->m[0][1] = (sy) * scale;
711                 out->m[1][1] = (cy) * scale;
712                 out->m[2][1] = 0;
713                 out->m[3][1] = y;
714                 out->m[0][2] = 0;
715                 out->m[1][2] = 0;
716                 out->m[2][2] = scale;
717                 out->m[3][2] = z;
718                 out->m[0][3] = 0;
719                 out->m[1][3] = 0;
720                 out->m[2][3] = 0;
721                 out->m[3][3] = 1;
722 #else
723                 out->m[0][0] = (cy) * scale;
724                 out->m[0][1] = (-sy) * scale;
725                 out->m[0][2] = 0;
726                 out->m[0][3] = x;
727                 out->m[1][0] = (sy) * scale;
728                 out->m[1][1] = (cy) * scale;
729                 out->m[1][2] = 0;
730                 out->m[1][3] = y;
731                 out->m[2][0] = 0;
732                 out->m[2][1] = 0;
733                 out->m[2][2] = scale;
734                 out->m[2][3] = z;
735                 out->m[3][0] = 0;
736                 out->m[3][1] = 0;
737                 out->m[3][2] = 0;
738                 out->m[3][3] = 1;
739 #endif
740         }
741         else
742         {
743 #ifdef MATRIX4x4_OPENGLORIENTATION
744                 out->m[0][0] = scale;
745                 out->m[1][0] = 0;
746                 out->m[2][0] = 0;
747                 out->m[3][0] = x;
748                 out->m[0][1] = 0;
749                 out->m[1][1] = scale;
750                 out->m[2][1] = 0;
751                 out->m[3][1] = y;
752                 out->m[0][2] = 0;
753                 out->m[1][2] = 0;
754                 out->m[2][2] = scale;
755                 out->m[3][2] = z;
756                 out->m[0][3] = 0;
757                 out->m[1][3] = 0;
758                 out->m[2][3] = 0;
759                 out->m[3][3] = 1;
760 #else
761                 out->m[0][0] = scale;
762                 out->m[0][1] = 0;
763                 out->m[0][2] = 0;
764                 out->m[0][3] = x;
765                 out->m[1][0] = 0;
766                 out->m[1][1] = scale;
767                 out->m[1][2] = 0;
768                 out->m[1][3] = y;
769                 out->m[2][0] = 0;
770                 out->m[2][1] = 0;
771                 out->m[2][2] = scale;
772                 out->m[2][3] = z;
773                 out->m[3][0] = 0;
774                 out->m[3][1] = 0;
775                 out->m[3][2] = 0;
776                 out->m[3][3] = 1;
777 #endif
778         }
779 }
780
781 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
782 {
783 #ifdef MATRIX4x4_OPENGLORIENTATION
784         vx[0] = in->m[0][0];
785         vx[1] = in->m[0][1];
786         vx[2] = in->m[0][2];
787         vy[0] = in->m[1][0];
788         vy[1] = in->m[1][1];
789         vy[2] = in->m[1][2];
790         vz[0] = in->m[2][0];
791         vz[1] = in->m[2][1];
792         vz[2] = in->m[2][2];
793         t [0] = in->m[3][0];
794         t [1] = in->m[3][1];
795         t [2] = in->m[3][2];
796 #else
797         vx[0] = in->m[0][0];
798         vx[1] = in->m[1][0];
799         vx[2] = in->m[2][0];
800         vy[0] = in->m[0][1];
801         vy[1] = in->m[1][1];
802         vy[2] = in->m[2][1];
803         vz[0] = in->m[0][2];
804         vz[1] = in->m[1][2];
805         vz[2] = in->m[2][2];
806         t [0] = in->m[0][3];
807         t [1] = in->m[1][3];
808         t [2] = in->m[2][3];
809 #endif
810 }
811
812 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
813 {
814 #ifdef MATRIX4x4_OPENGLORIENTATION
815         out->m[0][0] = vx[0];
816         out->m[1][0] = vy[0];
817         out->m[2][0] = vz[0];
818         out->m[3][0] = t[0];
819         out->m[0][1] = vx[1];
820         out->m[1][1] = vy[1];
821         out->m[2][1] = vz[1];
822         out->m[3][1] = t[1];
823         out->m[0][2] = vx[2];
824         out->m[1][2] = vy[2];
825         out->m[2][2] = vz[2];
826         out->m[3][2] = t[2];
827         out->m[0][3] = 0.0f;
828         out->m[1][3] = 0.0f;
829         out->m[2][3] = 0.0f;
830         out->m[3][3] = 1.0f;
831 #else
832         out->m[0][0] = vx[0];
833         out->m[0][1] = vy[0];
834         out->m[0][2] = vz[0];
835         out->m[0][3] = t[0];
836         out->m[1][0] = vx[1];
837         out->m[1][1] = vy[1];
838         out->m[1][2] = vz[1];
839         out->m[1][3] = t[1];
840         out->m[2][0] = vx[2];
841         out->m[2][1] = vy[2];
842         out->m[2][2] = vz[2];
843         out->m[2][3] = t[2];
844         out->m[3][0] = 0.0f;
845         out->m[3][1] = 0.0f;
846         out->m[3][2] = 0.0f;
847         out->m[3][3] = 1.0f;
848 #endif
849 }
850
851 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
852 {
853 #ifdef MATRIX4x4_OPENGLORIENTATION
854         out[ 0] = in->m[0][0];
855         out[ 1] = in->m[0][1];
856         out[ 2] = in->m[0][2];
857         out[ 3] = in->m[0][3];
858         out[ 4] = in->m[1][0];
859         out[ 5] = in->m[1][1];
860         out[ 6] = in->m[1][2];
861         out[ 7] = in->m[1][3];
862         out[ 8] = in->m[2][0];
863         out[ 9] = in->m[2][1];
864         out[10] = in->m[2][2];
865         out[11] = in->m[2][3];
866         out[12] = in->m[3][0];
867         out[13] = in->m[3][1];
868         out[14] = in->m[3][2];
869         out[15] = in->m[3][3];
870 #else
871         out[ 0] = in->m[0][0];
872         out[ 1] = in->m[1][0];
873         out[ 2] = in->m[2][0];
874         out[ 3] = in->m[3][0];
875         out[ 4] = in->m[0][1];
876         out[ 5] = in->m[1][1];
877         out[ 6] = in->m[2][1];
878         out[ 7] = in->m[3][1];
879         out[ 8] = in->m[0][2];
880         out[ 9] = in->m[1][2];
881         out[10] = in->m[2][2];
882         out[11] = in->m[3][2];
883         out[12] = in->m[0][3];
884         out[13] = in->m[1][3];
885         out[14] = in->m[2][3];
886         out[15] = in->m[3][3];
887 #endif
888 }
889
890 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
891 {
892 #ifdef MATRIX4x4_OPENGLORIENTATION
893         out->m[0][0] = in[0];
894         out->m[0][1] = in[1];
895         out->m[0][2] = in[2];
896         out->m[0][3] = in[3];
897         out->m[1][0] = in[4];
898         out->m[1][1] = in[5];
899         out->m[1][2] = in[6];
900         out->m[1][3] = in[7];
901         out->m[2][0] = in[8];
902         out->m[2][1] = in[9];
903         out->m[2][2] = in[10];
904         out->m[2][3] = in[11];
905         out->m[3][0] = in[12];
906         out->m[3][1] = in[13];
907         out->m[3][2] = in[14];
908         out->m[3][3] = in[15];
909 #else
910         out->m[0][0] = in[0];
911         out->m[1][0] = in[1];
912         out->m[2][0] = in[2];
913         out->m[3][0] = in[3];
914         out->m[0][1] = in[4];
915         out->m[1][1] = in[5];
916         out->m[2][1] = in[6];
917         out->m[3][1] = in[7];
918         out->m[0][2] = in[8];
919         out->m[1][2] = in[9];
920         out->m[2][2] = in[10];
921         out->m[3][2] = in[11];
922         out->m[0][3] = in[12];
923         out->m[1][3] = in[13];
924         out->m[2][3] = in[14];
925         out->m[3][3] = in[15];
926 #endif
927 }
928
929 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
930 {
931 #ifdef MATRIX4x4_OPENGLORIENTATION
932         out[ 0] = in->m[0][0];
933         out[ 1] = in->m[1][0];
934         out[ 2] = in->m[2][0];
935         out[ 3] = in->m[3][0];
936         out[ 4] = in->m[0][1];
937         out[ 5] = in->m[1][1];
938         out[ 6] = in->m[2][1];
939         out[ 7] = in->m[3][1];
940         out[ 8] = in->m[0][2];
941         out[ 9] = in->m[1][2];
942         out[10] = in->m[2][2];
943         out[11] = in->m[3][2];
944         out[12] = in->m[0][3];
945         out[13] = in->m[1][3];
946         out[14] = in->m[2][3];
947         out[15] = in->m[3][3];
948 #else
949         out[ 0] = in->m[0][0];
950         out[ 1] = in->m[0][1];
951         out[ 2] = in->m[0][2];
952         out[ 3] = in->m[0][3];
953         out[ 4] = in->m[1][0];
954         out[ 5] = in->m[1][1];
955         out[ 6] = in->m[1][2];
956         out[ 7] = in->m[1][3];
957         out[ 8] = in->m[2][0];
958         out[ 9] = in->m[2][1];
959         out[10] = in->m[2][2];
960         out[11] = in->m[2][3];
961         out[12] = in->m[3][0];
962         out[13] = in->m[3][1];
963         out[14] = in->m[3][2];
964         out[15] = in->m[3][3];
965 #endif
966 }
967
968 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
969 {
970 #ifdef MATRIX4x4_OPENGLORIENTATION
971         out->m[0][0] = in[0];
972         out->m[1][0] = in[1];
973         out->m[2][0] = in[2];
974         out->m[3][0] = in[3];
975         out->m[0][1] = in[4];
976         out->m[1][1] = in[5];
977         out->m[2][1] = in[6];
978         out->m[3][1] = in[7];
979         out->m[0][2] = in[8];
980         out->m[1][2] = in[9];
981         out->m[2][2] = in[10];
982         out->m[3][2] = in[11];
983         out->m[0][3] = in[12];
984         out->m[1][3] = in[13];
985         out->m[2][3] = in[14];
986         out->m[3][3] = in[15];
987 #else
988         out->m[0][0] = in[0];
989         out->m[0][1] = in[1];
990         out->m[0][2] = in[2];
991         out->m[0][3] = in[3];
992         out->m[1][0] = in[4];
993         out->m[1][1] = in[5];
994         out->m[1][2] = in[6];
995         out->m[1][3] = in[7];
996         out->m[2][0] = in[8];
997         out->m[2][1] = in[9];
998         out->m[2][2] = in[10];
999         out->m[2][3] = in[11];
1000         out->m[3][0] = in[12];
1001         out->m[3][1] = in[13];
1002         out->m[3][2] = in[14];
1003         out->m[3][3] = in[15];
1004 #endif
1005 }
1006
1007 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
1008 {
1009 #ifdef MATRIX4x4_OPENGLORIENTATION
1010         out[ 0] = in->m[0][0];
1011         out[ 1] = in->m[0][1];
1012         out[ 2] = in->m[0][2];
1013         out[ 3] = in->m[1][0];
1014         out[ 4] = in->m[1][1];
1015         out[ 5] = in->m[1][2];
1016         out[ 6] = in->m[2][0];
1017         out[ 7] = in->m[2][1];
1018         out[ 8] = in->m[2][2];
1019         out[ 9] = in->m[3][0];
1020         out[10] = in->m[3][1];
1021         out[11] = in->m[3][2];
1022 #else
1023         out[ 0] = in->m[0][0];
1024         out[ 1] = in->m[1][0];
1025         out[ 2] = in->m[2][0];
1026         out[ 3] = in->m[0][1];
1027         out[ 4] = in->m[1][1];
1028         out[ 5] = in->m[2][1];
1029         out[ 6] = in->m[0][2];
1030         out[ 7] = in->m[1][2];
1031         out[ 8] = in->m[2][2];
1032         out[ 9] = in->m[0][3];
1033         out[10] = in->m[1][3];
1034         out[11] = in->m[2][3];
1035 #endif
1036 }
1037
1038 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
1039 {
1040 #ifdef MATRIX4x4_OPENGLORIENTATION
1041         out->m[0][0] = in[0];
1042         out->m[0][1] = in[1];
1043         out->m[0][2] = in[2];
1044         out->m[0][3] = 0;
1045         out->m[1][0] = in[3];
1046         out->m[1][1] = in[4];
1047         out->m[1][2] = in[5];
1048         out->m[1][3] = 0;
1049         out->m[2][0] = in[6];
1050         out->m[2][1] = in[7];
1051         out->m[2][2] = in[8];
1052         out->m[2][3] = 0;
1053         out->m[3][0] = in[9];
1054         out->m[3][1] = in[10];
1055         out->m[3][2] = in[11];
1056         out->m[3][3] = 1;
1057 #else
1058         out->m[0][0] = in[0];
1059         out->m[1][0] = in[1];
1060         out->m[2][0] = in[2];
1061         out->m[3][0] = 0;
1062         out->m[0][1] = in[3];
1063         out->m[1][1] = in[4];
1064         out->m[2][1] = in[5];
1065         out->m[3][1] = 0;
1066         out->m[0][2] = in[6];
1067         out->m[1][2] = in[7];
1068         out->m[2][2] = in[8];
1069         out->m[3][2] = 0;
1070         out->m[0][3] = in[9];
1071         out->m[1][3] = in[10];
1072         out->m[2][3] = in[11];
1073         out->m[3][3] = 1;
1074 #endif
1075 }
1076
1077 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1078 {
1079 #ifdef MATRIX4x4_OPENGLORIENTATION
1080         out[ 0] = in->m[0][0];
1081         out[ 1] = in->m[1][0];
1082         out[ 2] = in->m[2][0];
1083         out[ 3] = in->m[3][0];
1084         out[ 4] = in->m[0][1];
1085         out[ 5] = in->m[1][1];
1086         out[ 6] = in->m[2][1];
1087         out[ 7] = in->m[3][1];
1088         out[ 8] = in->m[0][2];
1089         out[ 9] = in->m[1][2];
1090         out[10] = in->m[2][2];
1091         out[11] = in->m[3][2];
1092 #else
1093         out[ 0] = in->m[0][0];
1094         out[ 1] = in->m[0][1];
1095         out[ 2] = in->m[0][2];
1096         out[ 3] = in->m[0][3];
1097         out[ 4] = in->m[1][0];
1098         out[ 5] = in->m[1][1];
1099         out[ 6] = in->m[1][2];
1100         out[ 7] = in->m[1][3];
1101         out[ 8] = in->m[2][0];
1102         out[ 9] = in->m[2][1];
1103         out[10] = in->m[2][2];
1104         out[11] = in->m[2][3];
1105 #endif
1106 }
1107
1108 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1109 {
1110 #ifdef MATRIX4x4_OPENGLORIENTATION
1111         out->m[0][0] = in[0];
1112         out->m[1][0] = in[1];
1113         out->m[2][0] = in[2];
1114         out->m[3][0] = in[3];
1115         out->m[0][1] = in[4];
1116         out->m[1][1] = in[5];
1117         out->m[2][1] = in[6];
1118         out->m[3][1] = in[7];
1119         out->m[0][2] = in[8];
1120         out->m[1][2] = in[9];
1121         out->m[2][2] = in[10];
1122         out->m[3][2] = in[11];
1123         out->m[0][3] = 0;
1124         out->m[1][3] = 0;
1125         out->m[2][3] = 0;
1126         out->m[3][3] = 1;
1127 #else
1128         out->m[0][0] = in[0];
1129         out->m[0][1] = in[1];
1130         out->m[0][2] = in[2];
1131         out->m[0][3] = in[3];
1132         out->m[1][0] = in[4];
1133         out->m[1][1] = in[5];
1134         out->m[1][2] = in[6];
1135         out->m[1][3] = in[7];
1136         out->m[2][0] = in[8];
1137         out->m[2][1] = in[9];
1138         out->m[2][2] = in[10];
1139         out->m[2][3] = in[11];
1140         out->m[3][0] = 0;
1141         out->m[3][1] = 0;
1142         out->m[3][2] = 0;
1143         out->m[3][3] = 1;
1144 #endif
1145 }
1146
1147 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1148 {
1149 #ifdef MATRIX4x4_OPENGLORIENTATION
1150         m->m[0][0]=1-2*(y*y+z*z);m->m[1][0]=  2*(x*y-z*w);m->m[2][0]=  2*(x*z+y*w);m->m[3][0]=ox;
1151         m->m[0][1]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[2][1]=  2*(y*z-x*w);m->m[3][1]=oy;
1152         m->m[0][2]=  2*(x*z-y*w);m->m[1][2]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[3][2]=oz;
1153         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1154 #else
1155         m->m[0][0]=1-2*(y*y+z*z);m->m[0][1]=  2*(x*y-z*w);m->m[0][2]=  2*(x*z+y*w);m->m[0][3]=ox;
1156         m->m[1][0]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[1][2]=  2*(y*z-x*w);m->m[1][3]=oy;
1157         m->m[2][0]=  2*(x*z-y*w);m->m[2][1]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[2][3]=oz;
1158         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1159 #endif
1160 }
1161
1162 // LordHavoc: I got this code from:
1163 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1164 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1165 {
1166         double w = 1.0 - (x*x+y*y+z*z);
1167         w = w > 0.0 ? -sqrt(w) : 0.0;
1168 #ifdef MATRIX4x4_OPENGLORIENTATION
1169         m->m[0][0]=1-2*(y*y+z*z);m->m[1][0]=  2*(x*y-z*w);m->m[2][0]=  2*(x*z+y*w);m->m[3][0]=ox;
1170         m->m[0][1]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[2][1]=  2*(y*z-x*w);m->m[3][1]=oy;
1171         m->m[0][2]=  2*(x*z-y*w);m->m[1][2]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[3][2]=oz;
1172         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1173 #else
1174         m->m[0][0]=1-2*(y*y+z*z);m->m[0][1]=  2*(x*y-z*w);m->m[0][2]=  2*(x*z+y*w);m->m[0][3]=ox;
1175         m->m[1][0]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[1][2]=  2*(y*z-x*w);m->m[1][3]=oy;
1176         m->m[2][0]=  2*(x*z-y*w);m->m[2][1]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[2][3]=oz;
1177         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1178 #endif
1179 }
1180
1181 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1182 {
1183         double iblend = 1 - blend;
1184         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1185         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1186         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1187         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1188         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1189         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1190         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1191         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1192         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1193         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1194         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1195         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1196         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1197         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1198         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1199         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1200 }
1201
1202
1203 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1204 {
1205 #ifdef MATRIX4x4_OPENGLORIENTATION
1206         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1207         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1208         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1209 #else
1210         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1211         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1212         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1213 #endif
1214 }
1215
1216 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1217 {
1218 #ifdef MATRIX4x4_OPENGLORIENTATION
1219         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + v[3] * in->m[3][0];
1220         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + v[3] * in->m[3][1];
1221         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + v[3] * in->m[3][2];
1222         out[3] = v[0] * in->m[0][3] + v[1] * in->m[1][3] + v[2] * in->m[2][3] + v[3] * in->m[3][3];
1223 #else
1224         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + v[3] * in->m[0][3];
1225         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + v[3] * in->m[1][3];
1226         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + v[3] * in->m[2][3];
1227         out[3] = v[0] * in->m[3][0] + v[1] * in->m[3][1] + v[2] * in->m[3][2] + v[3] * in->m[3][3];
1228 #endif
1229 }
1230
1231 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1232 {
1233 #ifdef MATRIX4x4_OPENGLORIENTATION
1234         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1235         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1236         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1237 #else
1238         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1239         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1240         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1241 #endif
1242 }
1243
1244 /*
1245 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1246 {
1247         double t[3];
1248 #ifdef MATRIX4x4_OPENGLORIENTATION
1249         t[0] = v[0] - in->m[3][0];
1250         t[1] = v[1] - in->m[3][1];
1251         t[2] = v[2] - in->m[3][2];
1252         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1253         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1254         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1255 #else
1256         t[0] = v[0] - in->m[0][3];
1257         t[1] = v[1] - in->m[1][3];
1258         t[2] = v[2] - in->m[2][3];
1259         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1260         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1261         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1262 #endif
1263 }
1264 */
1265
1266 // FIXME: optimize
1267 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1268 {
1269         matrix4x4_t base, temp;
1270         base = *out;
1271         Matrix4x4_CreateTranslate(&temp, x, y, z);
1272         Matrix4x4_Concat(out, &base, &temp);
1273 }
1274
1275 // FIXME: optimize
1276 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1277 {
1278         matrix4x4_t base, temp;
1279         base = *out;
1280         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1281         Matrix4x4_Concat(out, &base, &temp);
1282 }
1283
1284 // FIXME: optimize
1285 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1286 {
1287         matrix4x4_t base, temp;
1288         base = *out;
1289         Matrix4x4_CreateScale(&temp, x);
1290         Matrix4x4_Concat(out, &base, &temp);
1291 }
1292
1293 // FIXME: optimize
1294 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1295 {
1296         matrix4x4_t base, temp;
1297         base = *out;
1298         Matrix4x4_CreateScale3(&temp, x, y, z);
1299         Matrix4x4_Concat(out, &base, &temp);
1300 }
1301
1302 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1303 {
1304 #ifdef MATRIX4x4_OPENGLORIENTATION
1305         out[0] = in->m[3][0];
1306         out[1] = in->m[3][1];
1307         out[2] = in->m[3][2];
1308 #else
1309         out[0] = in->m[0][3];
1310         out[1] = in->m[1][3];
1311         out[2] = in->m[2][3];
1312 #endif
1313 }
1314
1315 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1316 {
1317         // we only support uniform scaling, so assume the first row is enough
1318         return sqrt(in->m[0][0] * in->m[0][0] + in->m[0][1] * in->m[0][1] + in->m[0][2] * in->m[0][2]);
1319 }
1320
1321 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1322 {
1323 #ifdef MATRIX4x4_OPENGLORIENTATION
1324         out->m[3][0] = x;
1325         out->m[3][1] = y;
1326         out->m[3][2] = z;
1327 #else
1328         out->m[0][3] = x;
1329         out->m[1][3] = y;
1330         out->m[2][3] = z;
1331 #endif
1332 }
1333
1334 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1335 {
1336 #ifdef MATRIX4x4_OPENGLORIENTATION
1337         out->m[3][0] += x;
1338         out->m[3][1] += y;
1339         out->m[3][2] += z;
1340 #else
1341         out->m[0][3] += x;
1342         out->m[1][3] += y;
1343         out->m[2][3] += z;
1344 #endif
1345 }
1346
1347 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1348 {
1349         out->m[0][0] *= rotatescale;
1350         out->m[0][1] *= rotatescale;
1351         out->m[0][2] *= rotatescale;
1352         out->m[1][0] *= rotatescale;
1353         out->m[1][1] *= rotatescale;
1354         out->m[1][2] *= rotatescale;
1355         out->m[2][0] *= rotatescale;
1356         out->m[2][1] *= rotatescale;
1357         out->m[2][2] *= rotatescale;
1358 #ifdef MATRIX4x4_OPENGLORIENTATION
1359         out->m[3][0] *= originscale;
1360         out->m[3][1] *= originscale;
1361         out->m[3][2] *= originscale;
1362 #else
1363         out->m[0][3] *= originscale;
1364         out->m[1][3] *= originscale;
1365         out->m[2][3] *= originscale;
1366 #endif
1367 }