7 #pragma warning(disable : 4244) // LordHavoc: MSVC++ 4 x86, double/float
8 #pragma warning(disable : 4305) // LordHavoc: MSVC++ 6 x86, double/float
11 const matrix4x4_t identitymatrix =
21 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
26 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
28 out->m[0][0] = in->m[0][0];
29 out->m[0][1] = in->m[0][1];
30 out->m[0][2] = in->m[0][2];
32 out->m[1][0] = in->m[1][0];
33 out->m[1][1] = in->m[1][1];
34 out->m[1][2] = in->m[1][2];
36 out->m[2][0] = in->m[2][0];
37 out->m[2][1] = in->m[2][1];
38 out->m[2][2] = in->m[2][2];
46 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
48 #ifdef MATRIX4x4_OPENGLORIENTATION
52 out->m[3][0] = in->m[0][3];
56 out->m[3][1] = in->m[1][3];
60 out->m[3][2] = in->m[2][3];
69 out->m[0][3] = in->m[0][3];
73 out->m[1][3] = in->m[1][3];
77 out->m[2][3] = in->m[2][3];
85 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
87 #ifdef MATRIX4x4_OPENGLORIENTATION
88 out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[1][0] * in2->m[0][1] + in1->m[2][0] * in2->m[0][2] + in1->m[3][0] * in2->m[0][3];
89 out->m[1][0] = in1->m[0][0] * in2->m[1][0] + in1->m[1][0] * in2->m[1][1] + in1->m[2][0] * in2->m[1][2] + in1->m[3][0] * in2->m[1][3];
90 out->m[2][0] = in1->m[0][0] * in2->m[2][0] + in1->m[1][0] * in2->m[2][1] + in1->m[2][0] * in2->m[2][2] + in1->m[3][0] * in2->m[2][3];
91 out->m[3][0] = in1->m[0][0] * in2->m[3][0] + in1->m[1][0] * in2->m[3][1] + in1->m[2][0] * in2->m[3][2] + in1->m[3][0] * in2->m[3][3];
92 out->m[0][1] = in1->m[0][1] * in2->m[0][0] + in1->m[1][1] * in2->m[0][1] + in1->m[2][1] * in2->m[0][2] + in1->m[3][1] * in2->m[0][3];
93 out->m[1][1] = in1->m[0][1] * in2->m[1][0] + in1->m[1][1] * in2->m[1][1] + in1->m[2][1] * in2->m[1][2] + in1->m[3][1] * in2->m[1][3];
94 out->m[2][1] = in1->m[0][1] * in2->m[2][0] + in1->m[1][1] * in2->m[2][1] + in1->m[2][1] * in2->m[2][2] + in1->m[3][1] * in2->m[2][3];
95 out->m[3][1] = in1->m[0][1] * in2->m[3][0] + in1->m[1][1] * in2->m[3][1] + in1->m[2][1] * in2->m[3][2] + in1->m[3][1] * in2->m[3][3];
96 out->m[0][2] = in1->m[0][2] * in2->m[0][0] + in1->m[1][2] * in2->m[0][1] + in1->m[2][2] * in2->m[0][2] + in1->m[3][2] * in2->m[0][3];
97 out->m[1][2] = in1->m[0][2] * in2->m[1][0] + in1->m[1][2] * in2->m[1][1] + in1->m[2][2] * in2->m[1][2] + in1->m[3][2] * in2->m[1][3];
98 out->m[2][2] = in1->m[0][2] * in2->m[2][0] + in1->m[1][2] * in2->m[2][1] + in1->m[2][2] * in2->m[2][2] + in1->m[3][2] * in2->m[2][3];
99 out->m[3][2] = in1->m[0][2] * in2->m[3][0] + in1->m[1][2] * in2->m[3][1] + in1->m[2][2] * in2->m[3][2] + in1->m[3][2] * in2->m[3][3];
100 out->m[0][3] = in1->m[0][3] * in2->m[0][0] + in1->m[1][3] * in2->m[0][1] + in1->m[2][3] * in2->m[0][2] + in1->m[3][3] * in2->m[0][3];
101 out->m[1][3] = in1->m[0][3] * in2->m[1][0] + in1->m[1][3] * in2->m[1][1] + in1->m[2][3] * in2->m[1][2] + in1->m[3][3] * in2->m[1][3];
102 out->m[2][3] = in1->m[0][3] * in2->m[2][0] + in1->m[1][3] * in2->m[2][1] + in1->m[2][3] * in2->m[2][2] + in1->m[3][3] * in2->m[2][3];
103 out->m[3][3] = in1->m[0][3] * in2->m[3][0] + in1->m[1][3] * in2->m[3][1] + in1->m[2][3] * in2->m[3][2] + in1->m[3][3] * in2->m[3][3];
105 out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0] + in1->m[0][3] * in2->m[3][0];
106 out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1] + in1->m[0][3] * in2->m[3][1];
107 out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2] + in1->m[0][3] * in2->m[3][2];
108 out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3] * in2->m[3][3];
109 out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0] + in1->m[1][3] * in2->m[3][0];
110 out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1] + in1->m[1][3] * in2->m[3][1];
111 out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2] + in1->m[1][3] * in2->m[3][2];
112 out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3] * in2->m[3][3];
113 out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0] + in1->m[2][3] * in2->m[3][0];
114 out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1] + in1->m[2][3] * in2->m[3][1];
115 out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2] + in1->m[2][3] * in2->m[3][2];
116 out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3] * in2->m[3][3];
117 out->m[3][0] = in1->m[3][0] * in2->m[0][0] + in1->m[3][1] * in2->m[1][0] + in1->m[3][2] * in2->m[2][0] + in1->m[3][3] * in2->m[3][0];
118 out->m[3][1] = in1->m[3][0] * in2->m[0][1] + in1->m[3][1] * in2->m[1][1] + in1->m[3][2] * in2->m[2][1] + in1->m[3][3] * in2->m[3][1];
119 out->m[3][2] = in1->m[3][0] * in2->m[0][2] + in1->m[3][1] * in2->m[1][2] + in1->m[3][2] * in2->m[2][2] + in1->m[3][3] * in2->m[3][2];
120 out->m[3][3] = in1->m[3][0] * in2->m[0][3] + in1->m[3][1] * in2->m[1][3] + in1->m[3][2] * in2->m[2][3] + in1->m[3][3] * in2->m[3][3];
124 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
126 out->m[0][0] = in1->m[0][0];
127 out->m[0][1] = in1->m[1][0];
128 out->m[0][2] = in1->m[2][0];
129 out->m[0][3] = in1->m[3][0];
130 out->m[1][0] = in1->m[0][1];
131 out->m[1][1] = in1->m[1][1];
132 out->m[1][2] = in1->m[2][1];
133 out->m[1][3] = in1->m[3][1];
134 out->m[2][0] = in1->m[0][2];
135 out->m[2][1] = in1->m[1][2];
136 out->m[2][2] = in1->m[2][2];
137 out->m[2][3] = in1->m[3][2];
138 out->m[3][0] = in1->m[0][3];
139 out->m[3][1] = in1->m[1][3];
140 out->m[3][2] = in1->m[2][3];
141 out->m[3][3] = in1->m[3][3];
144 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
157 #ifdef MATRIX4x4_OPENGLORIENTATION
158 r[0][0] = in1->m[0][0]; r[0][1] = in1->m[1][0]; r[0][2] = in1->m[2][0]; r[0][3] = in1->m[3][0];
159 r[0][4] = 1.0; r[0][5] = r[0][6] = r[0][7] = 0.0;
161 r[1][0] = in1->m[0][1]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[2][1]; r[1][3] = in1->m[3][1];
162 r[1][5] = 1.0; r[1][4] = r[1][6] = r[1][7] = 0.0;
164 r[2][0] = in1->m[0][2]; r[2][1] = in1->m[1][2]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[3][2];
165 r[2][6] = 1.0; r[2][4] = r[2][5] = r[2][7] = 0.0;
167 r[3][0] = in1->m[0][3]; r[3][1] = in1->m[1][3]; r[3][2] = in1->m[2][3]; r[3][3] = in1->m[3][3];
168 r[3][7] = 1.0; r[3][4] = r[3][5] = r[3][6] = 0.0;
170 r[0][0] = in1->m[0][0]; r[0][1] = in1->m[0][1]; r[0][2] = in1->m[0][2]; r[0][3] = in1->m[0][3];
171 r[0][4] = 1.0; r[0][5] = r[0][6] = r[0][7] = 0.0;
173 r[1][0] = in1->m[1][0]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[1][2]; r[1][3] = in1->m[1][3];
174 r[1][5] = 1.0; r[1][4] = r[1][6] = r[1][7] = 0.0;
176 r[2][0] = in1->m[2][0]; r[2][1] = in1->m[2][1]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[2][3];
177 r[2][6] = 1.0; r[2][4] = r[2][5] = r[2][7] = 0.0;
179 r[3][0] = in1->m[3][0]; r[3][1] = in1->m[3][1]; r[3][2] = in1->m[3][2]; r[3][3] = in1->m[3][3];
180 r[3][7] = 1.0; r[3][4] = r[3][5] = r[3][6] = 0.0;
183 if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
184 if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
185 if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; }
189 m[1] = r[1][0] / r[0][0];
190 m[2] = r[2][0] / r[0][0];
191 m[3] = r[3][0] / r[0][0];
193 s = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s;
194 s = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s;
195 s = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s;
197 s = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
198 s = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
199 s = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
200 s = r[0][7]; if (s) { r[1][7] -= m[1] * s; r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
202 if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
203 if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
207 m[2] = r[2][1] / r[1][1];
208 m[3] = r[3][1] / r[1][1];
209 r[2][2] -= m[2] * r[1][2];
210 r[3][2] -= m[3] * r[1][2];
211 r[2][3] -= m[2] * r[1][3];
212 r[3][3] -= m[3] * r[1][3];
214 s = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
215 s = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
216 s = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
217 s = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
219 if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
223 m[3] = r[3][2] / r[2][2];
224 r[3][3] -= m[3] * r[2][3];
225 r[3][4] -= m[3] * r[2][4];
226 r[3][5] -= m[3] * r[2][5];
227 r[3][6] -= m[3] * r[2][6];
228 r[3][7] -= m[3] * r[2][7];
240 r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
241 r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
242 r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
243 r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
246 r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1];
247 r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1];
250 r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0];
251 r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0];
255 r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
256 r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
259 r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0];
260 r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0];
264 r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
265 r[0][6] = s * (r[0][6] - r[1][6] * m[0]), r[0][7] = s * (r[0][7] - r[1][7] * m[0]);
267 #ifdef MATRIX4x4_OPENGLORIENTATION
268 out->m[0][0] = r[0][4];
269 out->m[0][1] = r[1][4];
270 out->m[0][2] = r[2][4];
271 out->m[0][3] = r[3][4];
272 out->m[1][0] = r[0][5];
273 out->m[1][1] = r[1][5];
274 out->m[1][2] = r[2][5];
275 out->m[1][3] = r[3][5];
276 out->m[2][0] = r[0][6];
277 out->m[2][1] = r[1][6];
278 out->m[2][2] = r[2][6];
279 out->m[2][3] = r[3][6];
280 out->m[3][0] = r[0][7];
281 out->m[3][1] = r[1][7];
282 out->m[3][2] = r[2][7];
283 out->m[3][3] = r[3][7];
285 out->m[0][0] = r[0][4];
286 out->m[0][1] = r[0][5];
287 out->m[0][2] = r[0][6];
288 out->m[0][3] = r[0][7];
289 out->m[1][0] = r[1][4];
290 out->m[1][1] = r[1][5];
291 out->m[1][2] = r[1][6];
292 out->m[1][3] = r[1][7];
293 out->m[2][0] = r[2][4];
294 out->m[2][1] = r[2][5];
295 out->m[2][2] = r[2][6];
296 out->m[2][3] = r[2][7];
297 out->m[3][0] = r[3][4];
298 out->m[3][1] = r[3][5];
299 out->m[3][2] = r[3][6];
300 out->m[3][3] = r[3][7];
312 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
314 // we only support uniform scaling, so assume the first row is enough
315 // (note the lack of sqrt here, because we're trying to undo the scaling,
316 // this means multiplying by the inverse scale twice - squaring it, which
317 // makes the sqrt a waste of time)
319 double scale = 1.0 / (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
321 double scale = 3.0 / sqrt
322 (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
323 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
324 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
328 // invert the rotation by transposing and multiplying by the squared
329 // recipricol of the input matrix scale as described above
330 out->m[0][0] = in1->m[0][0] * scale;
331 out->m[0][1] = in1->m[1][0] * scale;
332 out->m[0][2] = in1->m[2][0] * scale;
333 out->m[1][0] = in1->m[0][1] * scale;
334 out->m[1][1] = in1->m[1][1] * scale;
335 out->m[1][2] = in1->m[2][1] * scale;
336 out->m[2][0] = in1->m[0][2] * scale;
337 out->m[2][1] = in1->m[1][2] * scale;
338 out->m[2][2] = in1->m[2][2] * scale;
340 #ifdef MATRIX4x4_OPENGLORIENTATION
341 // invert the translate
342 out->m[3][0] = -(in1->m[3][0] * out->m[0][0] + in1->m[3][1] * out->m[1][0] + in1->m[3][2] * out->m[2][0]);
343 out->m[3][1] = -(in1->m[3][0] * out->m[0][1] + in1->m[3][1] * out->m[1][1] + in1->m[3][2] * out->m[2][1]);
344 out->m[3][2] = -(in1->m[3][0] * out->m[0][2] + in1->m[3][1] * out->m[1][2] + in1->m[3][2] * out->m[2][2]);
346 // don't know if there's anything worth doing here
352 // invert the translate
353 out->m[0][3] = -(in1->m[0][3] * out->m[0][0] + in1->m[1][3] * out->m[0][1] + in1->m[2][3] * out->m[0][2]);
354 out->m[1][3] = -(in1->m[0][3] * out->m[1][0] + in1->m[1][3] * out->m[1][1] + in1->m[2][3] * out->m[1][2]);
355 out->m[2][3] = -(in1->m[0][3] * out->m[2][0] + in1->m[1][3] * out->m[2][1] + in1->m[2][3] * out->m[2][2]);
357 // don't know if there's anything worth doing here
365 void Matrix4x4_Interpolate (matrix4x4_t *out, matrix4x4_t *in1, matrix4x4_t *in2, double frac)
368 for (i = 0;i < 4;i++)
369 for (j = 0;j < 4;j++)
370 out->m[i][j] = in1->m[i][j] + frac * (in2->m[i][j] - in1->m[i][j]);
373 void Matrix4x4_Clear (matrix4x4_t *out)
376 for (i = 0;i < 4;i++)
377 for (j = 0;j < 4;j++)
381 void Matrix4x4_Accumulate (matrix4x4_t *out, matrix4x4_t *in, double weight)
384 for (i = 0;i < 4;i++)
385 for (j = 0;j < 4;j++)
386 out->m[i][j] += in->m[i][j] * weight;
389 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
391 // scale rotation matrix vectors to a length of 1
392 // note: this is only designed to undo uniform scaling
393 double scale = 1.0 / sqrt(in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
395 Matrix4x4_Scale(out, scale, 1);
398 void Matrix4x4_Normalize3 (matrix4x4_t *out, matrix4x4_t *in1)
402 // scale each rotation matrix vector to a length of 1
403 // intended for use after Matrix4x4_Interpolate or Matrix4x4_Accumulate
405 for (i = 0;i < 3;i++)
407 #ifdef MATRIX4x4_OPENGLORIENTATION
408 scale = sqrt(in1->m[i][0] * in1->m[i][0] + in1->m[i][1] * in1->m[i][1] + in1->m[i][2] * in1->m[i][2]);
411 out->m[i][0] *= scale;
412 out->m[i][1] *= scale;
413 out->m[i][2] *= scale;
415 scale = sqrt(in1->m[0][i] * in1->m[0][i] + in1->m[1][i] * in1->m[1][i] + in1->m[2][i] * in1->m[2][i]);
418 out->m[0][i] *= scale;
419 out->m[1][i] *= scale;
420 out->m[2][i] *= scale;
425 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
434 p2[0] = p[0] * axisscale;
435 p2[1] = p[1] * axisscale;
436 p2[2] = p[2] * axisscale;
438 for (i = 0;i < 4;i++)
440 #ifdef MATRIX4x4_OPENGLORIENTATION
441 d = out->m[i][0] * p[0] + out->m[i][1] * p[1] + out->m[i][2] * p[2] + out->m[i][3] * p[3];
442 out->m[i][0] += p2[0] * d;
443 out->m[i][1] += p2[1] * d;
444 out->m[i][2] += p2[2] * d;
446 d = out->m[0][i] * p[0] + out->m[1][i] * p[1] + out->m[2][i] * p[2] + out->m[3][i] * p[3];
447 out->m[0][i] += p2[0] * d;
448 out->m[1][i] += p2[1] * d;
449 out->m[2][i] += p2[2] * d;
454 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
474 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
476 #ifdef MATRIX4x4_OPENGLORIENTATION
513 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
519 len = 1.0f / sqrt(len);
524 angle *= (-M_PI / 180.0);
528 #ifdef MATRIX4x4_OPENGLORIENTATION
529 out->m[0][0]=x * x + c * (1 - x * x);
530 out->m[1][0]=x * y * (1 - c) + z * s;
531 out->m[2][0]=z * x * (1 - c) - y * s;
533 out->m[0][1]=x * y * (1 - c) - z * s;
534 out->m[1][1]=y * y + c * (1 - y * y);
535 out->m[2][1]=y * z * (1 - c) + x * s;
537 out->m[0][2]=z * x * (1 - c) + y * s;
538 out->m[1][2]=y * z * (1 - c) - x * s;
539 out->m[2][2]=z * z + c * (1 - z * z);
546 out->m[0][0]=x * x + c * (1 - x * x);
547 out->m[0][1]=x * y * (1 - c) + z * s;
548 out->m[0][2]=z * x * (1 - c) - y * s;
550 out->m[1][0]=x * y * (1 - c) - z * s;
551 out->m[1][1]=y * y + c * (1 - y * y);
552 out->m[1][2]=y * z * (1 - c) + x * s;
554 out->m[2][0]=z * x * (1 - c) + y * s;
555 out->m[2][1]=y * z * (1 - c) - x * s;
556 out->m[2][2]=z * z + c * (1 - z * z);
565 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
585 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
605 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
607 double angle, sr, sp, sy, cr, cp, cy;
611 angle = yaw * (M_PI*2 / 360);
614 angle = pitch * (M_PI*2 / 360);
617 angle = roll * (M_PI*2 / 360);
620 #ifdef MATRIX4x4_OPENGLORIENTATION
621 out->m[0][0] = (cp*cy) * scale;
622 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
623 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
625 out->m[0][1] = (cp*sy) * scale;
626 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
627 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
629 out->m[0][2] = (-sp) * scale;
630 out->m[1][2] = (sr*cp) * scale;
631 out->m[2][2] = (cr*cp) * scale;
638 out->m[0][0] = (cp*cy) * scale;
639 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
640 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
642 out->m[1][0] = (cp*sy) * scale;
643 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
644 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
646 out->m[2][0] = (-sp) * scale;
647 out->m[2][1] = (sr*cp) * scale;
648 out->m[2][2] = (cr*cp) * scale;
658 angle = yaw * (M_PI*2 / 360);
661 angle = pitch * (M_PI*2 / 360);
664 #ifdef MATRIX4x4_OPENGLORIENTATION
665 out->m[0][0] = (cp*cy) * scale;
666 out->m[1][0] = (-sy) * scale;
667 out->m[2][0] = (sp*cy) * scale;
669 out->m[0][1] = (cp*sy) * scale;
670 out->m[1][1] = (cy) * scale;
671 out->m[2][1] = (sp*sy) * scale;
673 out->m[0][2] = (-sp) * scale;
675 out->m[2][2] = (cp) * scale;
682 out->m[0][0] = (cp*cy) * scale;
683 out->m[0][1] = (-sy) * scale;
684 out->m[0][2] = (sp*cy) * scale;
686 out->m[1][0] = (cp*sy) * scale;
687 out->m[1][1] = (cy) * scale;
688 out->m[1][2] = (sp*sy) * scale;
690 out->m[2][0] = (-sp) * scale;
692 out->m[2][2] = (cp) * scale;
702 angle = yaw * (M_PI*2 / 360);
705 #ifdef MATRIX4x4_OPENGLORIENTATION
706 out->m[0][0] = (cy) * scale;
707 out->m[1][0] = (-sy) * scale;
710 out->m[0][1] = (sy) * scale;
711 out->m[1][1] = (cy) * scale;
716 out->m[2][2] = scale;
723 out->m[0][0] = (cy) * scale;
724 out->m[0][1] = (-sy) * scale;
727 out->m[1][0] = (sy) * scale;
728 out->m[1][1] = (cy) * scale;
733 out->m[2][2] = scale;
743 #ifdef MATRIX4x4_OPENGLORIENTATION
744 out->m[0][0] = scale;
749 out->m[1][1] = scale;
754 out->m[2][2] = scale;
761 out->m[0][0] = scale;
766 out->m[1][1] = scale;
771 out->m[2][2] = scale;
781 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
783 #ifdef MATRIX4x4_OPENGLORIENTATION
812 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
814 #ifdef MATRIX4x4_OPENGLORIENTATION
815 out->m[0][0] = vx[0];
816 out->m[1][0] = vy[0];
817 out->m[2][0] = vz[0];
819 out->m[0][1] = vx[1];
820 out->m[1][1] = vy[1];
821 out->m[2][1] = vz[1];
823 out->m[0][2] = vx[2];
824 out->m[1][2] = vy[2];
825 out->m[2][2] = vz[2];
832 out->m[0][0] = vx[0];
833 out->m[0][1] = vy[0];
834 out->m[0][2] = vz[0];
836 out->m[1][0] = vx[1];
837 out->m[1][1] = vy[1];
838 out->m[1][2] = vz[1];
840 out->m[2][0] = vx[2];
841 out->m[2][1] = vy[2];
842 out->m[2][2] = vz[2];
851 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
853 #ifdef MATRIX4x4_OPENGLORIENTATION
854 out[ 0] = in->m[0][0];
855 out[ 1] = in->m[0][1];
856 out[ 2] = in->m[0][2];
857 out[ 3] = in->m[0][3];
858 out[ 4] = in->m[1][0];
859 out[ 5] = in->m[1][1];
860 out[ 6] = in->m[1][2];
861 out[ 7] = in->m[1][3];
862 out[ 8] = in->m[2][0];
863 out[ 9] = in->m[2][1];
864 out[10] = in->m[2][2];
865 out[11] = in->m[2][3];
866 out[12] = in->m[3][0];
867 out[13] = in->m[3][1];
868 out[14] = in->m[3][2];
869 out[15] = in->m[3][3];
871 out[ 0] = in->m[0][0];
872 out[ 1] = in->m[1][0];
873 out[ 2] = in->m[2][0];
874 out[ 3] = in->m[3][0];
875 out[ 4] = in->m[0][1];
876 out[ 5] = in->m[1][1];
877 out[ 6] = in->m[2][1];
878 out[ 7] = in->m[3][1];
879 out[ 8] = in->m[0][2];
880 out[ 9] = in->m[1][2];
881 out[10] = in->m[2][2];
882 out[11] = in->m[3][2];
883 out[12] = in->m[0][3];
884 out[13] = in->m[1][3];
885 out[14] = in->m[2][3];
886 out[15] = in->m[3][3];
890 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
892 #ifdef MATRIX4x4_OPENGLORIENTATION
893 out->m[0][0] = in[0];
894 out->m[0][1] = in[1];
895 out->m[0][2] = in[2];
896 out->m[0][3] = in[3];
897 out->m[1][0] = in[4];
898 out->m[1][1] = in[5];
899 out->m[1][2] = in[6];
900 out->m[1][3] = in[7];
901 out->m[2][0] = in[8];
902 out->m[2][1] = in[9];
903 out->m[2][2] = in[10];
904 out->m[2][3] = in[11];
905 out->m[3][0] = in[12];
906 out->m[3][1] = in[13];
907 out->m[3][2] = in[14];
908 out->m[3][3] = in[15];
910 out->m[0][0] = in[0];
911 out->m[1][0] = in[1];
912 out->m[2][0] = in[2];
913 out->m[3][0] = in[3];
914 out->m[0][1] = in[4];
915 out->m[1][1] = in[5];
916 out->m[2][1] = in[6];
917 out->m[3][1] = in[7];
918 out->m[0][2] = in[8];
919 out->m[1][2] = in[9];
920 out->m[2][2] = in[10];
921 out->m[3][2] = in[11];
922 out->m[0][3] = in[12];
923 out->m[1][3] = in[13];
924 out->m[2][3] = in[14];
925 out->m[3][3] = in[15];
929 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
931 #ifdef MATRIX4x4_OPENGLORIENTATION
932 out[ 0] = in->m[0][0];
933 out[ 1] = in->m[1][0];
934 out[ 2] = in->m[2][0];
935 out[ 3] = in->m[3][0];
936 out[ 4] = in->m[0][1];
937 out[ 5] = in->m[1][1];
938 out[ 6] = in->m[2][1];
939 out[ 7] = in->m[3][1];
940 out[ 8] = in->m[0][2];
941 out[ 9] = in->m[1][2];
942 out[10] = in->m[2][2];
943 out[11] = in->m[3][2];
944 out[12] = in->m[0][3];
945 out[13] = in->m[1][3];
946 out[14] = in->m[2][3];
947 out[15] = in->m[3][3];
949 out[ 0] = in->m[0][0];
950 out[ 1] = in->m[0][1];
951 out[ 2] = in->m[0][2];
952 out[ 3] = in->m[0][3];
953 out[ 4] = in->m[1][0];
954 out[ 5] = in->m[1][1];
955 out[ 6] = in->m[1][2];
956 out[ 7] = in->m[1][3];
957 out[ 8] = in->m[2][0];
958 out[ 9] = in->m[2][1];
959 out[10] = in->m[2][2];
960 out[11] = in->m[2][3];
961 out[12] = in->m[3][0];
962 out[13] = in->m[3][1];
963 out[14] = in->m[3][2];
964 out[15] = in->m[3][3];
968 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
970 #ifdef MATRIX4x4_OPENGLORIENTATION
971 out->m[0][0] = in[0];
972 out->m[1][0] = in[1];
973 out->m[2][0] = in[2];
974 out->m[3][0] = in[3];
975 out->m[0][1] = in[4];
976 out->m[1][1] = in[5];
977 out->m[2][1] = in[6];
978 out->m[3][1] = in[7];
979 out->m[0][2] = in[8];
980 out->m[1][2] = in[9];
981 out->m[2][2] = in[10];
982 out->m[3][2] = in[11];
983 out->m[0][3] = in[12];
984 out->m[1][3] = in[13];
985 out->m[2][3] = in[14];
986 out->m[3][3] = in[15];
988 out->m[0][0] = in[0];
989 out->m[0][1] = in[1];
990 out->m[0][2] = in[2];
991 out->m[0][3] = in[3];
992 out->m[1][0] = in[4];
993 out->m[1][1] = in[5];
994 out->m[1][2] = in[6];
995 out->m[1][3] = in[7];
996 out->m[2][0] = in[8];
997 out->m[2][1] = in[9];
998 out->m[2][2] = in[10];
999 out->m[2][3] = in[11];
1000 out->m[3][0] = in[12];
1001 out->m[3][1] = in[13];
1002 out->m[3][2] = in[14];
1003 out->m[3][3] = in[15];
1007 void Matrix4x4_ToArrayFloatGL(const matrix4x4_t *in, float out[16])
1009 #ifdef MATRIX4x4_OPENGLORIENTATION
1010 out[ 0] = in->m[0][0];
1011 out[ 1] = in->m[0][1];
1012 out[ 2] = in->m[0][2];
1013 out[ 3] = in->m[0][3];
1014 out[ 4] = in->m[1][0];
1015 out[ 5] = in->m[1][1];
1016 out[ 6] = in->m[1][2];
1017 out[ 7] = in->m[1][3];
1018 out[ 8] = in->m[2][0];
1019 out[ 9] = in->m[2][1];
1020 out[10] = in->m[2][2];
1021 out[11] = in->m[2][3];
1022 out[12] = in->m[3][0];
1023 out[13] = in->m[3][1];
1024 out[14] = in->m[3][2];
1025 out[15] = in->m[3][3];
1027 out[ 0] = in->m[0][0];
1028 out[ 1] = in->m[1][0];
1029 out[ 2] = in->m[2][0];
1030 out[ 3] = in->m[3][0];
1031 out[ 4] = in->m[0][1];
1032 out[ 5] = in->m[1][1];
1033 out[ 6] = in->m[2][1];
1034 out[ 7] = in->m[3][1];
1035 out[ 8] = in->m[0][2];
1036 out[ 9] = in->m[1][2];
1037 out[10] = in->m[2][2];
1038 out[11] = in->m[3][2];
1039 out[12] = in->m[0][3];
1040 out[13] = in->m[1][3];
1041 out[14] = in->m[2][3];
1042 out[15] = in->m[3][3];
1046 void Matrix4x4_FromArrayFloatGL (matrix4x4_t *out, const float in[16])
1048 #ifdef MATRIX4x4_OPENGLORIENTATION
1049 out->m[0][0] = in[0];
1050 out->m[0][1] = in[1];
1051 out->m[0][2] = in[2];
1052 out->m[0][3] = in[3];
1053 out->m[1][0] = in[4];
1054 out->m[1][1] = in[5];
1055 out->m[1][2] = in[6];
1056 out->m[1][3] = in[7];
1057 out->m[2][0] = in[8];
1058 out->m[2][1] = in[9];
1059 out->m[2][2] = in[10];
1060 out->m[2][3] = in[11];
1061 out->m[3][0] = in[12];
1062 out->m[3][1] = in[13];
1063 out->m[3][2] = in[14];
1064 out->m[3][3] = in[15];
1066 out->m[0][0] = in[0];
1067 out->m[1][0] = in[1];
1068 out->m[2][0] = in[2];
1069 out->m[3][0] = in[3];
1070 out->m[0][1] = in[4];
1071 out->m[1][1] = in[5];
1072 out->m[2][1] = in[6];
1073 out->m[3][1] = in[7];
1074 out->m[0][2] = in[8];
1075 out->m[1][2] = in[9];
1076 out->m[2][2] = in[10];
1077 out->m[3][2] = in[11];
1078 out->m[0][3] = in[12];
1079 out->m[1][3] = in[13];
1080 out->m[2][3] = in[14];
1081 out->m[3][3] = in[15];
1085 void Matrix4x4_ToArrayFloatD3D(const matrix4x4_t *in, float out[16])
1087 #ifdef MATRIX4x4_OPENGLORIENTATION
1088 out[ 0] = in->m[0][0];
1089 out[ 1] = in->m[1][0];
1090 out[ 2] = in->m[2][0];
1091 out[ 3] = in->m[3][0];
1092 out[ 4] = in->m[0][1];
1093 out[ 5] = in->m[1][1];
1094 out[ 6] = in->m[2][1];
1095 out[ 7] = in->m[3][1];
1096 out[ 8] = in->m[0][2];
1097 out[ 9] = in->m[1][2];
1098 out[10] = in->m[2][2];
1099 out[11] = in->m[3][2];
1100 out[12] = in->m[0][3];
1101 out[13] = in->m[1][3];
1102 out[14] = in->m[2][3];
1103 out[15] = in->m[3][3];
1105 out[ 0] = in->m[0][0];
1106 out[ 1] = in->m[0][1];
1107 out[ 2] = in->m[0][2];
1108 out[ 3] = in->m[0][3];
1109 out[ 4] = in->m[1][0];
1110 out[ 5] = in->m[1][1];
1111 out[ 6] = in->m[1][2];
1112 out[ 7] = in->m[1][3];
1113 out[ 8] = in->m[2][0];
1114 out[ 9] = in->m[2][1];
1115 out[10] = in->m[2][2];
1116 out[11] = in->m[2][3];
1117 out[12] = in->m[3][0];
1118 out[13] = in->m[3][1];
1119 out[14] = in->m[3][2];
1120 out[15] = in->m[3][3];
1124 void Matrix4x4_FromArrayFloatD3D (matrix4x4_t *out, const float in[16])
1126 #ifdef MATRIX4x4_OPENGLORIENTATION
1127 out->m[0][0] = in[0];
1128 out->m[1][0] = in[1];
1129 out->m[2][0] = in[2];
1130 out->m[3][0] = in[3];
1131 out->m[0][1] = in[4];
1132 out->m[1][1] = in[5];
1133 out->m[2][1] = in[6];
1134 out->m[3][1] = in[7];
1135 out->m[0][2] = in[8];
1136 out->m[1][2] = in[9];
1137 out->m[2][2] = in[10];
1138 out->m[3][2] = in[11];
1139 out->m[0][3] = in[12];
1140 out->m[1][3] = in[13];
1141 out->m[2][3] = in[14];
1142 out->m[3][3] = in[15];
1144 out->m[0][0] = in[0];
1145 out->m[0][1] = in[1];
1146 out->m[0][2] = in[2];
1147 out->m[0][3] = in[3];
1148 out->m[1][0] = in[4];
1149 out->m[1][1] = in[5];
1150 out->m[1][2] = in[6];
1151 out->m[1][3] = in[7];
1152 out->m[2][0] = in[8];
1153 out->m[2][1] = in[9];
1154 out->m[2][2] = in[10];
1155 out->m[2][3] = in[11];
1156 out->m[3][0] = in[12];
1157 out->m[3][1] = in[13];
1158 out->m[3][2] = in[14];
1159 out->m[3][3] = in[15];
1163 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
1165 #ifdef MATRIX4x4_OPENGLORIENTATION
1166 out[ 0] = in->m[0][0];
1167 out[ 1] = in->m[0][1];
1168 out[ 2] = in->m[0][2];
1169 out[ 3] = in->m[1][0];
1170 out[ 4] = in->m[1][1];
1171 out[ 5] = in->m[1][2];
1172 out[ 6] = in->m[2][0];
1173 out[ 7] = in->m[2][1];
1174 out[ 8] = in->m[2][2];
1175 out[ 9] = in->m[3][0];
1176 out[10] = in->m[3][1];
1177 out[11] = in->m[3][2];
1179 out[ 0] = in->m[0][0];
1180 out[ 1] = in->m[1][0];
1181 out[ 2] = in->m[2][0];
1182 out[ 3] = in->m[0][1];
1183 out[ 4] = in->m[1][1];
1184 out[ 5] = in->m[2][1];
1185 out[ 6] = in->m[0][2];
1186 out[ 7] = in->m[1][2];
1187 out[ 8] = in->m[2][2];
1188 out[ 9] = in->m[0][3];
1189 out[10] = in->m[1][3];
1190 out[11] = in->m[2][3];
1194 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
1196 #ifdef MATRIX4x4_OPENGLORIENTATION
1197 out->m[0][0] = in[0];
1198 out->m[0][1] = in[1];
1199 out->m[0][2] = in[2];
1201 out->m[1][0] = in[3];
1202 out->m[1][1] = in[4];
1203 out->m[1][2] = in[5];
1205 out->m[2][0] = in[6];
1206 out->m[2][1] = in[7];
1207 out->m[2][2] = in[8];
1209 out->m[3][0] = in[9];
1210 out->m[3][1] = in[10];
1211 out->m[3][2] = in[11];
1214 out->m[0][0] = in[0];
1215 out->m[1][0] = in[1];
1216 out->m[2][0] = in[2];
1218 out->m[0][1] = in[3];
1219 out->m[1][1] = in[4];
1220 out->m[2][1] = in[5];
1222 out->m[0][2] = in[6];
1223 out->m[1][2] = in[7];
1224 out->m[2][2] = in[8];
1226 out->m[0][3] = in[9];
1227 out->m[1][3] = in[10];
1228 out->m[2][3] = in[11];
1233 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1235 #ifdef MATRIX4x4_OPENGLORIENTATION
1236 out[ 0] = in->m[0][0];
1237 out[ 1] = in->m[1][0];
1238 out[ 2] = in->m[2][0];
1239 out[ 3] = in->m[3][0];
1240 out[ 4] = in->m[0][1];
1241 out[ 5] = in->m[1][1];
1242 out[ 6] = in->m[2][1];
1243 out[ 7] = in->m[3][1];
1244 out[ 8] = in->m[0][2];
1245 out[ 9] = in->m[1][2];
1246 out[10] = in->m[2][2];
1247 out[11] = in->m[3][2];
1249 out[ 0] = in->m[0][0];
1250 out[ 1] = in->m[0][1];
1251 out[ 2] = in->m[0][2];
1252 out[ 3] = in->m[0][3];
1253 out[ 4] = in->m[1][0];
1254 out[ 5] = in->m[1][1];
1255 out[ 6] = in->m[1][2];
1256 out[ 7] = in->m[1][3];
1257 out[ 8] = in->m[2][0];
1258 out[ 9] = in->m[2][1];
1259 out[10] = in->m[2][2];
1260 out[11] = in->m[2][3];
1264 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1266 #ifdef MATRIX4x4_OPENGLORIENTATION
1267 out->m[0][0] = in[0];
1268 out->m[1][0] = in[1];
1269 out->m[2][0] = in[2];
1270 out->m[3][0] = in[3];
1271 out->m[0][1] = in[4];
1272 out->m[1][1] = in[5];
1273 out->m[2][1] = in[6];
1274 out->m[3][1] = in[7];
1275 out->m[0][2] = in[8];
1276 out->m[1][2] = in[9];
1277 out->m[2][2] = in[10];
1278 out->m[3][2] = in[11];
1284 out->m[0][0] = in[0];
1285 out->m[0][1] = in[1];
1286 out->m[0][2] = in[2];
1287 out->m[0][3] = in[3];
1288 out->m[1][0] = in[4];
1289 out->m[1][1] = in[5];
1290 out->m[1][2] = in[6];
1291 out->m[1][3] = in[7];
1292 out->m[2][0] = in[8];
1293 out->m[2][1] = in[9];
1294 out->m[2][2] = in[10];
1295 out->m[2][3] = in[11];
1303 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1305 #ifdef MATRIX4x4_OPENGLORIENTATION
1306 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;
1307 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;
1308 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;
1309 m->m[0][3]= 0 ;m->m[1][3]= 0 ;m->m[2][3]= 0 ;m->m[3][3]=1;
1311 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;
1312 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;
1313 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;
1314 m->m[3][0]= 0 ;m->m[3][1]= 0 ;m->m[3][2]= 0 ;m->m[3][3]=1;
1318 // LordHavoc: I got this code from:
1319 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1320 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1322 double w = 1.0 - (x*x+y*y+z*z);
1323 w = w > 0.0 ? -sqrt(w) : 0.0;
1324 #ifdef MATRIX4x4_OPENGLORIENTATION
1325 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;
1326 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;
1327 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;
1328 m->m[0][3]= 0 ;m->m[1][3]= 0 ;m->m[2][3]= 0 ;m->m[3][3]=1;
1330 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;
1331 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;
1332 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;
1333 m->m[3][0]= 0 ;m->m[3][1]= 0 ;m->m[3][2]= 0 ;m->m[3][3]=1;
1337 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1339 double iblend = 1 - blend;
1340 out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1341 out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1342 out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1343 out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1344 out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1345 out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1346 out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1347 out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1348 out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1349 out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1350 out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1351 out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1352 out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1353 out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1354 out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1355 out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1359 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1361 #ifdef MATRIX4x4_OPENGLORIENTATION
1362 out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1363 out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1364 out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1366 out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1367 out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1368 out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1372 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1374 #ifdef MATRIX4x4_OPENGLORIENTATION
1375 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];
1376 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];
1377 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];
1378 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];
1380 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];
1381 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];
1382 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];
1383 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];
1387 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1389 #ifdef MATRIX4x4_OPENGLORIENTATION
1390 out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1391 out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1392 out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1394 out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1395 out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1396 out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1400 void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1402 #ifdef MATRIX4x4_OPENGLORIENTATION
1403 o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
1404 o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
1405 o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
1406 o[3] = d + (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
1408 o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
1409 o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
1410 o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
1411 o[3] = d + (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
1415 void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1417 #ifdef MATRIX4x4_OPENGLORIENTATION
1418 o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
1419 o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
1420 o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
1421 o[3] = d - (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
1423 o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
1424 o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
1425 o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
1426 o[3] = d - (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
1431 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1434 #ifdef MATRIX4x4_OPENGLORIENTATION
1435 t[0] = v[0] - in->m[3][0];
1436 t[1] = v[1] - in->m[3][1];
1437 t[2] = v[2] - in->m[3][2];
1438 out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1439 out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1440 out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1442 t[0] = v[0] - in->m[0][3];
1443 t[1] = v[1] - in->m[1][3];
1444 t[2] = v[2] - in->m[2][3];
1445 out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1446 out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1447 out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1453 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1455 matrix4x4_t base, temp;
1457 Matrix4x4_CreateTranslate(&temp, x, y, z);
1458 Matrix4x4_Concat(out, &base, &temp);
1462 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1464 matrix4x4_t base, temp;
1466 Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1467 Matrix4x4_Concat(out, &base, &temp);
1471 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1473 matrix4x4_t base, temp;
1475 Matrix4x4_CreateScale(&temp, x);
1476 Matrix4x4_Concat(out, &base, &temp);
1480 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1482 matrix4x4_t base, temp;
1484 Matrix4x4_CreateScale3(&temp, x, y, z);
1485 Matrix4x4_Concat(out, &base, &temp);
1488 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1490 #ifdef MATRIX4x4_OPENGLORIENTATION
1491 out[0] = in->m[3][0];
1492 out[1] = in->m[3][1];
1493 out[2] = in->m[3][2];
1495 out[0] = in->m[0][3];
1496 out[1] = in->m[1][3];
1497 out[2] = in->m[2][3];
1501 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1503 // we only support uniform scaling, so assume the first row is enough
1504 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]);
1507 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1509 #ifdef MATRIX4x4_OPENGLORIENTATION
1520 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1522 #ifdef MATRIX4x4_OPENGLORIENTATION
1533 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1535 out->m[0][0] *= rotatescale;
1536 out->m[0][1] *= rotatescale;
1537 out->m[0][2] *= rotatescale;
1538 out->m[1][0] *= rotatescale;
1539 out->m[1][1] *= rotatescale;
1540 out->m[1][2] *= rotatescale;
1541 out->m[2][0] *= rotatescale;
1542 out->m[2][1] *= rotatescale;
1543 out->m[2][2] *= rotatescale;
1544 #ifdef MATRIX4x4_OPENGLORIENTATION
1545 out->m[3][0] *= originscale;
1546 out->m[3][1] *= originscale;
1547 out->m[3][2] *= originscale;
1549 out->m[0][3] *= originscale;
1550 out->m[1][3] *= originscale;
1551 out->m[2][3] *= originscale;