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