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