move two #defined to quakedef.h, and always include quakedef.h first before any other...
[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_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
366 {
367         // scale rotation matrix vectors to a length of 1
368         // note: this is only designed to undo uniform scaling
369         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]);
370         *out = *in1;
371         Matrix4x4_Scale(out, scale, 1);
372 }
373
374 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
375 {
376         int i;
377         double d;
378         double p[4], p2[4];
379         p[0] = normalx;
380         p[1] = normaly;
381         p[2] = normalz;
382         p[3] = -dist;
383         p2[0] = p[0] * axisscale;
384         p2[1] = p[1] * axisscale;
385         p2[2] = p[2] * axisscale;
386         p2[3] = 0;
387         for (i = 0;i < 4;i++)
388         {
389 #ifdef MATRIX4x4_OPENGLORIENTATION
390                 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];
391                 out->m[i][0] += p2[0] * d;
392                 out->m[i][1] += p2[1] * d;
393                 out->m[i][2] += p2[2] * d;
394 #else
395                 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];
396                 out->m[0][i] += p2[0] * d;
397                 out->m[1][i] += p2[1] * d;
398                 out->m[2][i] += p2[2] * d;
399 #endif
400         }
401 }
402
403 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
404 {
405         out->m[0][0]=1.0f;
406         out->m[0][1]=0.0f;
407         out->m[0][2]=0.0f;
408         out->m[0][3]=0.0f;
409         out->m[1][0]=0.0f;
410         out->m[1][1]=1.0f;
411         out->m[1][2]=0.0f;
412         out->m[1][3]=0.0f;
413         out->m[2][0]=0.0f;
414         out->m[2][1]=0.0f;
415         out->m[2][2]=1.0f;
416         out->m[2][3]=0.0f;
417         out->m[3][0]=0.0f;
418         out->m[3][1]=0.0f;
419         out->m[3][2]=0.0f;
420         out->m[3][3]=1.0f;
421 }
422
423 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
424 {
425 #ifdef MATRIX4x4_OPENGLORIENTATION
426         out->m[0][0]=1.0f;
427         out->m[1][0]=0.0f;
428         out->m[2][0]=0.0f;
429         out->m[3][0]=x;
430         out->m[0][1]=0.0f;
431         out->m[1][1]=1.0f;
432         out->m[2][1]=0.0f;
433         out->m[3][1]=y;
434         out->m[0][2]=0.0f;
435         out->m[1][2]=0.0f;
436         out->m[2][2]=1.0f;
437         out->m[3][2]=z;
438         out->m[0][3]=0.0f;
439         out->m[1][3]=0.0f;
440         out->m[2][3]=0.0f;
441         out->m[3][3]=1.0f;
442 #else
443         out->m[0][0]=1.0f;
444         out->m[0][1]=0.0f;
445         out->m[0][2]=0.0f;
446         out->m[0][3]=x;
447         out->m[1][0]=0.0f;
448         out->m[1][1]=1.0f;
449         out->m[1][2]=0.0f;
450         out->m[1][3]=y;
451         out->m[2][0]=0.0f;
452         out->m[2][1]=0.0f;
453         out->m[2][2]=1.0f;
454         out->m[2][3]=z;
455         out->m[3][0]=0.0f;
456         out->m[3][1]=0.0f;
457         out->m[3][2]=0.0f;
458         out->m[3][3]=1.0f;
459 #endif
460 }
461
462 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
463 {
464         double len, c, s;
465
466         len = x*x+y*y+z*z;
467         if (len != 0.0f)
468                 len = 1.0f / sqrt(len);
469         x *= len;
470         y *= len;
471         z *= len;
472
473         angle *= (-M_PI / 180.0);
474         c = cos(angle);
475         s = sin(angle);
476
477 #ifdef MATRIX4x4_OPENGLORIENTATION
478         out->m[0][0]=x * x + c * (1 - x * x);
479         out->m[1][0]=x * y * (1 - c) + z * s;
480         out->m[2][0]=z * x * (1 - c) - y * s;
481         out->m[3][0]=0.0f;
482         out->m[0][1]=x * y * (1 - c) - z * s;
483         out->m[1][1]=y * y + c * (1 - y * y);
484         out->m[2][1]=y * z * (1 - c) + x * s;
485         out->m[3][1]=0.0f;
486         out->m[0][2]=z * x * (1 - c) + y * s;
487         out->m[1][2]=y * z * (1 - c) - x * s;
488         out->m[2][2]=z * z + c * (1 - z * z);
489         out->m[3][2]=0.0f;
490         out->m[0][3]=0.0f;
491         out->m[1][3]=0.0f;
492         out->m[2][3]=0.0f;
493         out->m[3][3]=1.0f;
494 #else
495         out->m[0][0]=x * x + c * (1 - x * x);
496         out->m[0][1]=x * y * (1 - c) + z * s;
497         out->m[0][2]=z * x * (1 - c) - y * s;
498         out->m[0][3]=0.0f;
499         out->m[1][0]=x * y * (1 - c) - z * s;
500         out->m[1][1]=y * y + c * (1 - y * y);
501         out->m[1][2]=y * z * (1 - c) + x * s;
502         out->m[1][3]=0.0f;
503         out->m[2][0]=z * x * (1 - c) + y * s;
504         out->m[2][1]=y * z * (1 - c) - x * s;
505         out->m[2][2]=z * z + c * (1 - z * z);
506         out->m[2][3]=0.0f;
507         out->m[3][0]=0.0f;
508         out->m[3][1]=0.0f;
509         out->m[3][2]=0.0f;
510         out->m[3][3]=1.0f;
511 #endif
512 }
513
514 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
515 {
516         out->m[0][0]=x;
517         out->m[0][1]=0.0f;
518         out->m[0][2]=0.0f;
519         out->m[0][3]=0.0f;
520         out->m[1][0]=0.0f;
521         out->m[1][1]=x;
522         out->m[1][2]=0.0f;
523         out->m[1][3]=0.0f;
524         out->m[2][0]=0.0f;
525         out->m[2][1]=0.0f;
526         out->m[2][2]=x;
527         out->m[2][3]=0.0f;
528         out->m[3][0]=0.0f;
529         out->m[3][1]=0.0f;
530         out->m[3][2]=0.0f;
531         out->m[3][3]=1.0f;
532 }
533
534 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
535 {
536         out->m[0][0]=x;
537         out->m[0][1]=0.0f;
538         out->m[0][2]=0.0f;
539         out->m[0][3]=0.0f;
540         out->m[1][0]=0.0f;
541         out->m[1][1]=y;
542         out->m[1][2]=0.0f;
543         out->m[1][3]=0.0f;
544         out->m[2][0]=0.0f;
545         out->m[2][1]=0.0f;
546         out->m[2][2]=z;
547         out->m[2][3]=0.0f;
548         out->m[3][0]=0.0f;
549         out->m[3][1]=0.0f;
550         out->m[3][2]=0.0f;
551         out->m[3][3]=1.0f;
552 }
553
554 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
555 {
556         double angle, sr, sp, sy, cr, cp, cy;
557
558         if (roll)
559         {
560                 angle = yaw * (M_PI*2 / 360);
561                 sy = sin(angle);
562                 cy = cos(angle);
563                 angle = pitch * (M_PI*2 / 360);
564                 sp = sin(angle);
565                 cp = cos(angle);
566                 angle = roll * (M_PI*2 / 360);
567                 sr = sin(angle);
568                 cr = cos(angle);
569 #ifdef MATRIX4x4_OPENGLORIENTATION
570                 out->m[0][0] = (cp*cy) * scale;
571                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
572                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
573                 out->m[3][0] = x;
574                 out->m[0][1] = (cp*sy) * scale;
575                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
576                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
577                 out->m[3][1] = y;
578                 out->m[0][2] = (-sp) * scale;
579                 out->m[1][2] = (sr*cp) * scale;
580                 out->m[2][2] = (cr*cp) * scale;
581                 out->m[3][2] = z;
582                 out->m[0][3] = 0;
583                 out->m[1][3] = 0;
584                 out->m[2][3] = 0;
585                 out->m[3][3] = 1;
586 #else
587                 out->m[0][0] = (cp*cy) * scale;
588                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
589                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
590                 out->m[0][3] = x;
591                 out->m[1][0] = (cp*sy) * scale;
592                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
593                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
594                 out->m[1][3] = y;
595                 out->m[2][0] = (-sp) * scale;
596                 out->m[2][1] = (sr*cp) * scale;
597                 out->m[2][2] = (cr*cp) * scale;
598                 out->m[2][3] = z;
599                 out->m[3][0] = 0;
600                 out->m[3][1] = 0;
601                 out->m[3][2] = 0;
602                 out->m[3][3] = 1;
603 #endif
604         }
605         else if (pitch)
606         {
607                 angle = yaw * (M_PI*2 / 360);
608                 sy = sin(angle);
609                 cy = cos(angle);
610                 angle = pitch * (M_PI*2 / 360);
611                 sp = sin(angle);
612                 cp = cos(angle);
613 #ifdef MATRIX4x4_OPENGLORIENTATION
614                 out->m[0][0] = (cp*cy) * scale;
615                 out->m[1][0] = (-sy) * scale;
616                 out->m[2][0] = (sp*cy) * scale;
617                 out->m[3][0] = x;
618                 out->m[0][1] = (cp*sy) * scale;
619                 out->m[1][1] = (cy) * scale;
620                 out->m[2][1] = (sp*sy) * scale;
621                 out->m[3][1] = y;
622                 out->m[0][2] = (-sp) * scale;
623                 out->m[1][2] = 0;
624                 out->m[2][2] = (cp) * scale;
625                 out->m[3][2] = z;
626                 out->m[0][3] = 0;
627                 out->m[1][3] = 0;
628                 out->m[2][3] = 0;
629                 out->m[3][3] = 1;
630 #else
631                 out->m[0][0] = (cp*cy) * scale;
632                 out->m[0][1] = (-sy) * scale;
633                 out->m[0][2] = (sp*cy) * scale;
634                 out->m[0][3] = x;
635                 out->m[1][0] = (cp*sy) * scale;
636                 out->m[1][1] = (cy) * scale;
637                 out->m[1][2] = (sp*sy) * scale;
638                 out->m[1][3] = y;
639                 out->m[2][0] = (-sp) * scale;
640                 out->m[2][1] = 0;
641                 out->m[2][2] = (cp) * scale;
642                 out->m[2][3] = z;
643                 out->m[3][0] = 0;
644                 out->m[3][1] = 0;
645                 out->m[3][2] = 0;
646                 out->m[3][3] = 1;
647 #endif
648         }
649         else if (yaw)
650         {
651                 angle = yaw * (M_PI*2 / 360);
652                 sy = sin(angle);
653                 cy = cos(angle);
654 #ifdef MATRIX4x4_OPENGLORIENTATION
655                 out->m[0][0] = (cy) * scale;
656                 out->m[1][0] = (-sy) * scale;
657                 out->m[2][0] = 0;
658                 out->m[3][0] = x;
659                 out->m[0][1] = (sy) * scale;
660                 out->m[1][1] = (cy) * scale;
661                 out->m[2][1] = 0;
662                 out->m[3][1] = y;
663                 out->m[0][2] = 0;
664                 out->m[1][2] = 0;
665                 out->m[2][2] = scale;
666                 out->m[3][2] = z;
667                 out->m[0][3] = 0;
668                 out->m[1][3] = 0;
669                 out->m[2][3] = 0;
670                 out->m[3][3] = 1;
671 #else
672                 out->m[0][0] = (cy) * scale;
673                 out->m[0][1] = (-sy) * scale;
674                 out->m[0][2] = 0;
675                 out->m[0][3] = x;
676                 out->m[1][0] = (sy) * scale;
677                 out->m[1][1] = (cy) * scale;
678                 out->m[1][2] = 0;
679                 out->m[1][3] = y;
680                 out->m[2][0] = 0;
681                 out->m[2][1] = 0;
682                 out->m[2][2] = scale;
683                 out->m[2][3] = z;
684                 out->m[3][0] = 0;
685                 out->m[3][1] = 0;
686                 out->m[3][2] = 0;
687                 out->m[3][3] = 1;
688 #endif
689         }
690         else
691         {
692 #ifdef MATRIX4x4_OPENGLORIENTATION
693                 out->m[0][0] = scale;
694                 out->m[1][0] = 0;
695                 out->m[2][0] = 0;
696                 out->m[3][0] = x;
697                 out->m[0][1] = 0;
698                 out->m[1][1] = scale;
699                 out->m[2][1] = 0;
700                 out->m[3][1] = y;
701                 out->m[0][2] = 0;
702                 out->m[1][2] = 0;
703                 out->m[2][2] = scale;
704                 out->m[3][2] = z;
705                 out->m[0][3] = 0;
706                 out->m[1][3] = 0;
707                 out->m[2][3] = 0;
708                 out->m[3][3] = 1;
709 #else
710                 out->m[0][0] = scale;
711                 out->m[0][1] = 0;
712                 out->m[0][2] = 0;
713                 out->m[0][3] = x;
714                 out->m[1][0] = 0;
715                 out->m[1][1] = scale;
716                 out->m[1][2] = 0;
717                 out->m[1][3] = y;
718                 out->m[2][0] = 0;
719                 out->m[2][1] = 0;
720                 out->m[2][2] = scale;
721                 out->m[2][3] = z;
722                 out->m[3][0] = 0;
723                 out->m[3][1] = 0;
724                 out->m[3][2] = 0;
725                 out->m[3][3] = 1;
726 #endif
727         }
728 }
729
730 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
731 {
732 #ifdef MATRIX4x4_OPENGLORIENTATION
733         vx[0] = in->m[0][0];
734         vx[1] = in->m[0][1];
735         vx[2] = in->m[0][2];
736         vy[0] = in->m[1][0];
737         vy[1] = in->m[1][1];
738         vy[2] = in->m[1][2];
739         vz[0] = in->m[2][0];
740         vz[1] = in->m[2][1];
741         vz[2] = in->m[2][2];
742         t [0] = in->m[3][0];
743         t [1] = in->m[3][1];
744         t [2] = in->m[3][2];
745 #else
746         vx[0] = in->m[0][0];
747         vx[1] = in->m[1][0];
748         vx[2] = in->m[2][0];
749         vy[0] = in->m[0][1];
750         vy[1] = in->m[1][1];
751         vy[2] = in->m[2][1];
752         vz[0] = in->m[0][2];
753         vz[1] = in->m[1][2];
754         vz[2] = in->m[2][2];
755         t [0] = in->m[0][3];
756         t [1] = in->m[1][3];
757         t [2] = in->m[2][3];
758 #endif
759 }
760
761 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
762 {
763 #ifdef MATRIX4x4_OPENGLORIENTATION
764         out->m[0][0] = vx[0];
765         out->m[1][0] = vy[0];
766         out->m[2][0] = vz[0];
767         out->m[3][0] = t[0];
768         out->m[0][1] = vx[1];
769         out->m[1][1] = vy[1];
770         out->m[2][1] = vz[1];
771         out->m[3][1] = t[1];
772         out->m[0][2] = vx[2];
773         out->m[1][2] = vy[2];
774         out->m[2][2] = vz[2];
775         out->m[3][2] = t[2];
776         out->m[0][3] = 0.0f;
777         out->m[1][3] = 0.0f;
778         out->m[2][3] = 0.0f;
779         out->m[3][3] = 1.0f;
780 #else
781         out->m[0][0] = vx[0];
782         out->m[0][1] = vy[0];
783         out->m[0][2] = vz[0];
784         out->m[0][3] = t[0];
785         out->m[1][0] = vx[1];
786         out->m[1][1] = vy[1];
787         out->m[1][2] = vz[1];
788         out->m[1][3] = t[1];
789         out->m[2][0] = vx[2];
790         out->m[2][1] = vy[2];
791         out->m[2][2] = vz[2];
792         out->m[2][3] = t[2];
793         out->m[3][0] = 0.0f;
794         out->m[3][1] = 0.0f;
795         out->m[3][2] = 0.0f;
796         out->m[3][3] = 1.0f;
797 #endif
798 }
799
800 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
801 {
802 #ifdef MATRIX4x4_OPENGLORIENTATION
803         out[ 0] = in->m[0][0];
804         out[ 1] = in->m[0][1];
805         out[ 2] = in->m[0][2];
806         out[ 3] = in->m[0][3];
807         out[ 4] = in->m[1][0];
808         out[ 5] = in->m[1][1];
809         out[ 6] = in->m[1][2];
810         out[ 7] = in->m[1][3];
811         out[ 8] = in->m[2][0];
812         out[ 9] = in->m[2][1];
813         out[10] = in->m[2][2];
814         out[11] = in->m[2][3];
815         out[12] = in->m[3][0];
816         out[13] = in->m[3][1];
817         out[14] = in->m[3][2];
818         out[15] = in->m[3][3];
819 #else
820         out[ 0] = in->m[0][0];
821         out[ 1] = in->m[1][0];
822         out[ 2] = in->m[2][0];
823         out[ 3] = in->m[3][0];
824         out[ 4] = in->m[0][1];
825         out[ 5] = in->m[1][1];
826         out[ 6] = in->m[2][1];
827         out[ 7] = in->m[3][1];
828         out[ 8] = in->m[0][2];
829         out[ 9] = in->m[1][2];
830         out[10] = in->m[2][2];
831         out[11] = in->m[3][2];
832         out[12] = in->m[0][3];
833         out[13] = in->m[1][3];
834         out[14] = in->m[2][3];
835         out[15] = in->m[3][3];
836 #endif
837 }
838
839 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
840 {
841 #ifdef MATRIX4x4_OPENGLORIENTATION
842         out->m[0][0] = in[0];
843         out->m[0][1] = in[1];
844         out->m[0][2] = in[2];
845         out->m[0][3] = in[3];
846         out->m[1][0] = in[4];
847         out->m[1][1] = in[5];
848         out->m[1][2] = in[6];
849         out->m[1][3] = in[7];
850         out->m[2][0] = in[8];
851         out->m[2][1] = in[9];
852         out->m[2][2] = in[10];
853         out->m[2][3] = in[11];
854         out->m[3][0] = in[12];
855         out->m[3][1] = in[13];
856         out->m[3][2] = in[14];
857         out->m[3][3] = in[15];
858 #else
859         out->m[0][0] = in[0];
860         out->m[1][0] = in[1];
861         out->m[2][0] = in[2];
862         out->m[3][0] = in[3];
863         out->m[0][1] = in[4];
864         out->m[1][1] = in[5];
865         out->m[2][1] = in[6];
866         out->m[3][1] = in[7];
867         out->m[0][2] = in[8];
868         out->m[1][2] = in[9];
869         out->m[2][2] = in[10];
870         out->m[3][2] = in[11];
871         out->m[0][3] = in[12];
872         out->m[1][3] = in[13];
873         out->m[2][3] = in[14];
874         out->m[3][3] = in[15];
875 #endif
876 }
877
878 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
879 {
880 #ifdef MATRIX4x4_OPENGLORIENTATION
881         out[ 0] = in->m[0][0];
882         out[ 1] = in->m[1][0];
883         out[ 2] = in->m[2][0];
884         out[ 3] = in->m[3][0];
885         out[ 4] = in->m[0][1];
886         out[ 5] = in->m[1][1];
887         out[ 6] = in->m[2][1];
888         out[ 7] = in->m[3][1];
889         out[ 8] = in->m[0][2];
890         out[ 9] = in->m[1][2];
891         out[10] = in->m[2][2];
892         out[11] = in->m[3][2];
893         out[12] = in->m[0][3];
894         out[13] = in->m[1][3];
895         out[14] = in->m[2][3];
896         out[15] = in->m[3][3];
897 #else
898         out[ 0] = in->m[0][0];
899         out[ 1] = in->m[0][1];
900         out[ 2] = in->m[0][2];
901         out[ 3] = in->m[0][3];
902         out[ 4] = in->m[1][0];
903         out[ 5] = in->m[1][1];
904         out[ 6] = in->m[1][2];
905         out[ 7] = in->m[1][3];
906         out[ 8] = in->m[2][0];
907         out[ 9] = in->m[2][1];
908         out[10] = in->m[2][2];
909         out[11] = in->m[2][3];
910         out[12] = in->m[3][0];
911         out[13] = in->m[3][1];
912         out[14] = in->m[3][2];
913         out[15] = in->m[3][3];
914 #endif
915 }
916
917 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
918 {
919 #ifdef MATRIX4x4_OPENGLORIENTATION
920         out->m[0][0] = in[0];
921         out->m[1][0] = in[1];
922         out->m[2][0] = in[2];
923         out->m[3][0] = in[3];
924         out->m[0][1] = in[4];
925         out->m[1][1] = in[5];
926         out->m[2][1] = in[6];
927         out->m[3][1] = in[7];
928         out->m[0][2] = in[8];
929         out->m[1][2] = in[9];
930         out->m[2][2] = in[10];
931         out->m[3][2] = in[11];
932         out->m[0][3] = in[12];
933         out->m[1][3] = in[13];
934         out->m[2][3] = in[14];
935         out->m[3][3] = in[15];
936 #else
937         out->m[0][0] = in[0];
938         out->m[0][1] = in[1];
939         out->m[0][2] = in[2];
940         out->m[0][3] = in[3];
941         out->m[1][0] = in[4];
942         out->m[1][1] = in[5];
943         out->m[1][2] = in[6];
944         out->m[1][3] = in[7];
945         out->m[2][0] = in[8];
946         out->m[2][1] = in[9];
947         out->m[2][2] = in[10];
948         out->m[2][3] = in[11];
949         out->m[3][0] = in[12];
950         out->m[3][1] = in[13];
951         out->m[3][2] = in[14];
952         out->m[3][3] = in[15];
953 #endif
954 }
955
956 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
957 {
958 #ifdef MATRIX4x4_OPENGLORIENTATION
959         out[ 0] = in->m[0][0];
960         out[ 1] = in->m[0][1];
961         out[ 2] = in->m[0][2];
962         out[ 3] = in->m[1][0];
963         out[ 4] = in->m[1][1];
964         out[ 5] = in->m[1][2];
965         out[ 6] = in->m[2][0];
966         out[ 7] = in->m[2][1];
967         out[ 8] = in->m[2][2];
968         out[ 9] = in->m[3][0];
969         out[10] = in->m[3][1];
970         out[11] = in->m[3][2];
971 #else
972         out[ 0] = in->m[0][0];
973         out[ 1] = in->m[1][0];
974         out[ 2] = in->m[2][0];
975         out[ 3] = in->m[0][1];
976         out[ 4] = in->m[1][1];
977         out[ 5] = in->m[2][1];
978         out[ 6] = in->m[0][2];
979         out[ 7] = in->m[1][2];
980         out[ 8] = in->m[2][2];
981         out[ 9] = in->m[0][3];
982         out[10] = in->m[1][3];
983         out[11] = in->m[2][3];
984 #endif
985 }
986
987 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
988 {
989 #ifdef MATRIX4x4_OPENGLORIENTATION
990         out->m[0][0] = in[0];
991         out->m[0][1] = in[1];
992         out->m[0][2] = in[2];
993         out->m[0][3] = 0;
994         out->m[1][0] = in[3];
995         out->m[1][1] = in[4];
996         out->m[1][2] = in[5];
997         out->m[1][3] = 0;
998         out->m[2][0] = in[6];
999         out->m[2][1] = in[7];
1000         out->m[2][2] = in[8];
1001         out->m[2][3] = 0;
1002         out->m[3][0] = in[9];
1003         out->m[3][1] = in[10];
1004         out->m[3][2] = in[11];
1005         out->m[3][3] = 1;
1006 #else
1007         out->m[0][0] = in[0];
1008         out->m[1][0] = in[1];
1009         out->m[2][0] = in[2];
1010         out->m[3][0] = 0;
1011         out->m[0][1] = in[3];
1012         out->m[1][1] = in[4];
1013         out->m[2][1] = in[5];
1014         out->m[3][1] = 0;
1015         out->m[0][2] = in[6];
1016         out->m[1][2] = in[7];
1017         out->m[2][2] = in[8];
1018         out->m[3][2] = 0;
1019         out->m[0][3] = in[9];
1020         out->m[1][3] = in[10];
1021         out->m[2][3] = in[11];
1022         out->m[3][3] = 1;
1023 #endif
1024 }
1025
1026 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1027 {
1028 #ifdef MATRIX4x4_OPENGLORIENTATION
1029         out[ 0] = in->m[0][0];
1030         out[ 1] = in->m[1][0];
1031         out[ 2] = in->m[2][0];
1032         out[ 3] = in->m[3][0];
1033         out[ 4] = in->m[0][1];
1034         out[ 5] = in->m[1][1];
1035         out[ 6] = in->m[2][1];
1036         out[ 7] = in->m[3][1];
1037         out[ 8] = in->m[0][2];
1038         out[ 9] = in->m[1][2];
1039         out[10] = in->m[2][2];
1040         out[11] = in->m[3][2];
1041 #else
1042         out[ 0] = in->m[0][0];
1043         out[ 1] = in->m[0][1];
1044         out[ 2] = in->m[0][2];
1045         out[ 3] = in->m[0][3];
1046         out[ 4] = in->m[1][0];
1047         out[ 5] = in->m[1][1];
1048         out[ 6] = in->m[1][2];
1049         out[ 7] = in->m[1][3];
1050         out[ 8] = in->m[2][0];
1051         out[ 9] = in->m[2][1];
1052         out[10] = in->m[2][2];
1053         out[11] = in->m[2][3];
1054 #endif
1055 }
1056
1057 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1058 {
1059 #ifdef MATRIX4x4_OPENGLORIENTATION
1060         out->m[0][0] = in[0];
1061         out->m[1][0] = in[1];
1062         out->m[2][0] = in[2];
1063         out->m[3][0] = in[3];
1064         out->m[0][1] = in[4];
1065         out->m[1][1] = in[5];
1066         out->m[2][1] = in[6];
1067         out->m[3][1] = in[7];
1068         out->m[0][2] = in[8];
1069         out->m[1][2] = in[9];
1070         out->m[2][2] = in[10];
1071         out->m[3][2] = in[11];
1072         out->m[0][3] = 0;
1073         out->m[1][3] = 0;
1074         out->m[2][3] = 0;
1075         out->m[3][3] = 1;
1076 #else
1077         out->m[0][0] = in[0];
1078         out->m[0][1] = in[1];
1079         out->m[0][2] = in[2];
1080         out->m[0][3] = in[3];
1081         out->m[1][0] = in[4];
1082         out->m[1][1] = in[5];
1083         out->m[1][2] = in[6];
1084         out->m[1][3] = in[7];
1085         out->m[2][0] = in[8];
1086         out->m[2][1] = in[9];
1087         out->m[2][2] = in[10];
1088         out->m[2][3] = in[11];
1089         out->m[3][0] = 0;
1090         out->m[3][1] = 0;
1091         out->m[3][2] = 0;
1092         out->m[3][3] = 1;
1093 #endif
1094 }
1095
1096 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1097 {
1098 #ifdef MATRIX4x4_OPENGLORIENTATION
1099         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;
1100         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;
1101         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;
1102         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1103 #else
1104         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;
1105         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;
1106         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;
1107         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1108 #endif
1109 }
1110
1111 // LordHavoc: I got this code from:
1112 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1113 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1114 {
1115         double w = 1.0 - (x*x+y*y+z*z);
1116         w = w > 0.0 ? -sqrt(w) : 0.0;
1117 #ifdef MATRIX4x4_OPENGLORIENTATION
1118         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;
1119         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;
1120         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;
1121         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1122 #else
1123         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;
1124         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;
1125         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;
1126         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1127 #endif
1128 }
1129
1130 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1131 {
1132         double iblend = 1 - blend;
1133         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1134         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1135         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1136         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1137         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1138         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1139         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1140         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1141         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1142         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1143         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1144         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1145         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1146         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1147         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1148         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1149 }
1150
1151
1152 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1153 {
1154 #ifdef MATRIX4x4_OPENGLORIENTATION
1155         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1156         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1157         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1158 #else
1159         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1160         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1161         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1162 #endif
1163 }
1164
1165 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1166 {
1167 #ifdef MATRIX4x4_OPENGLORIENTATION
1168         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];
1169         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];
1170         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];
1171         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];
1172 #else
1173         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];
1174         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];
1175         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];
1176         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];
1177 #endif
1178 }
1179
1180 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1181 {
1182 #ifdef MATRIX4x4_OPENGLORIENTATION
1183         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1184         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1185         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1186 #else
1187         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1188         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1189         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1190 #endif
1191 }
1192
1193 /*
1194 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1195 {
1196         double t[3];
1197 #ifdef MATRIX4x4_OPENGLORIENTATION
1198         t[0] = v[0] - in->m[3][0];
1199         t[1] = v[1] - in->m[3][1];
1200         t[2] = v[2] - in->m[3][2];
1201         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1202         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1203         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1204 #else
1205         t[0] = v[0] - in->m[0][3];
1206         t[1] = v[1] - in->m[1][3];
1207         t[2] = v[2] - in->m[2][3];
1208         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1209         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1210         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1211 #endif
1212 }
1213 */
1214
1215 // FIXME: optimize
1216 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1217 {
1218         matrix4x4_t base, temp;
1219         base = *out;
1220         Matrix4x4_CreateTranslate(&temp, x, y, z);
1221         Matrix4x4_Concat(out, &base, &temp);
1222 }
1223
1224 // FIXME: optimize
1225 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1226 {
1227         matrix4x4_t base, temp;
1228         base = *out;
1229         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1230         Matrix4x4_Concat(out, &base, &temp);
1231 }
1232
1233 // FIXME: optimize
1234 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1235 {
1236         matrix4x4_t base, temp;
1237         base = *out;
1238         Matrix4x4_CreateScale(&temp, x);
1239         Matrix4x4_Concat(out, &base, &temp);
1240 }
1241
1242 // FIXME: optimize
1243 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1244 {
1245         matrix4x4_t base, temp;
1246         base = *out;
1247         Matrix4x4_CreateScale3(&temp, x, y, z);
1248         Matrix4x4_Concat(out, &base, &temp);
1249 }
1250
1251 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1252 {
1253 #ifdef MATRIX4x4_OPENGLORIENTATION
1254         out[0] = in->m[3][0];
1255         out[1] = in->m[3][1];
1256         out[2] = in->m[3][2];
1257 #else
1258         out[0] = in->m[0][3];
1259         out[1] = in->m[1][3];
1260         out[2] = in->m[2][3];
1261 #endif
1262 }
1263
1264 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1265 {
1266         // we only support uniform scaling, so assume the first row is enough
1267         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]);
1268 }
1269
1270 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1271 {
1272 #ifdef MATRIX4x4_OPENGLORIENTATION
1273         out->m[3][0] = x;
1274         out->m[3][1] = y;
1275         out->m[3][2] = z;
1276 #else
1277         out->m[0][3] = x;
1278         out->m[1][3] = y;
1279         out->m[2][3] = z;
1280 #endif
1281 }
1282
1283 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1284 {
1285 #ifdef MATRIX4x4_OPENGLORIENTATION
1286         out->m[3][0] += x;
1287         out->m[3][1] += y;
1288         out->m[3][2] += z;
1289 #else
1290         out->m[0][3] += x;
1291         out->m[1][3] += y;
1292         out->m[2][3] += z;
1293 #endif
1294 }
1295
1296 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1297 {
1298         out->m[0][0] *= rotatescale;
1299         out->m[0][1] *= rotatescale;
1300         out->m[0][2] *= rotatescale;
1301         out->m[1][0] *= rotatescale;
1302         out->m[1][1] *= rotatescale;
1303         out->m[1][2] *= rotatescale;
1304         out->m[2][0] *= rotatescale;
1305         out->m[2][1] *= rotatescale;
1306         out->m[2][2] *= rotatescale;
1307 #ifdef MATRIX4x4_OPENGLORIENTATION
1308         out->m[3][0] *= originscale;
1309         out->m[3][1] *= originscale;
1310         out->m[3][2] *= originscale;
1311 #else
1312         out->m[0][3] *= originscale;
1313         out->m[1][3] *= originscale;
1314         out->m[2][3] *= originscale;
1315 #endif
1316 }