moved all type-specific model fields to respective structures (alias, sprite, brush)
[divverent/darkplaces.git] / matrixlib.c
1
2 #include "matrixlib.h"
3 #include <math.h>
4
5 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
6 {
7         *out = *in;
8 }
9
10 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
11 {
12         out->m[0][0] = in->m[0][0];
13         out->m[0][1] = in->m[0][1];
14         out->m[0][2] = in->m[0][2];
15         out->m[0][3] = 0.0f;
16         out->m[1][0] = in->m[1][0];
17         out->m[1][1] = in->m[1][1];
18         out->m[1][2] = in->m[1][2];
19         out->m[1][3] = 0.0f;
20         out->m[2][0] = in->m[2][0];
21         out->m[2][1] = in->m[2][1];
22         out->m[2][2] = in->m[2][2];
23         out->m[2][3] = 0.0f;
24         out->m[3][0] = 0.0f;
25         out->m[3][1] = 0.0f;
26         out->m[3][2] = 0.0f;
27         out->m[3][3] = 1.0f;
28 }
29
30 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
31 {
32         out->m[0][0] = 0.0f;
33         out->m[0][1] = 0.0f;
34         out->m[0][2] = 0.0f;
35         out->m[0][3] = in->m[0][3];
36         out->m[1][0] = 0.0f;
37         out->m[1][1] = 0.0f;
38         out->m[1][2] = 0.0f;
39         out->m[1][3] = in->m[1][3];
40         out->m[2][0] = 0.0f;
41         out->m[2][1] = 0.0f;
42         out->m[2][2] = 0.0f;
43         out->m[2][3] = in->m[2][3];
44         out->m[3][0] = 0.0f;
45         out->m[3][1] = 0.0f;
46         out->m[3][2] = 0.0f;
47         out->m[3][3] = 1.0f;
48 }
49
50 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
51 {
52         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];
53         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];
54         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];
55         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];
56         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];
57         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];
58         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];
59         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];
60         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];
61         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];
62         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];
63         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];
64         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];
65         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];
66         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];
67         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];
68 }
69
70 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
71 {
72         out->m[0][0] = in1->m[0][0];
73         out->m[0][1] = in1->m[1][0];
74         out->m[0][2] = in1->m[2][0];
75         out->m[0][3] = in1->m[3][0];
76         out->m[1][0] = in1->m[0][1];
77         out->m[1][1] = in1->m[1][1];
78         out->m[1][2] = in1->m[2][1];
79         out->m[1][3] = in1->m[3][1];
80         out->m[2][0] = in1->m[0][2];
81         out->m[2][1] = in1->m[1][2];
82         out->m[2][2] = in1->m[2][2];
83         out->m[2][3] = in1->m[3][2];
84         out->m[3][0] = in1->m[0][3];
85         out->m[3][1] = in1->m[1][3];
86         out->m[3][2] = in1->m[2][3];
87         out->m[3][3] = in1->m[3][3];
88 }
89
90 void Matrix4x4_Transpose3x3 (matrix4x4_t *out, const matrix4x4_t *in1)
91 {
92         out->m[0][0] = in1->m[0][0];
93         out->m[0][1] = in1->m[1][0];
94         out->m[0][2] = in1->m[2][0];
95         out->m[1][0] = in1->m[0][1];
96         out->m[1][1] = in1->m[1][1];
97         out->m[1][2] = in1->m[2][1];
98         out->m[2][0] = in1->m[0][2];
99         out->m[2][1] = in1->m[1][2];
100         out->m[2][2] = in1->m[2][2];
101
102         out->m[0][3] = in1->m[0][3];
103         out->m[1][3] = in1->m[1][3];
104         out->m[2][3] = in1->m[2][3];
105         out->m[3][0] = in1->m[0][3];
106         out->m[3][1] = in1->m[1][3];
107         out->m[3][2] = in1->m[2][3];
108         out->m[3][3] = in1->m[3][3];
109 }
110
111 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
112 {
113         // we only support uniform scaling, so assume the first row is enough
114         // (note the lack of sqrt here, because we're trying to undo the scaling,
115         // this means multiplying by the inverse scale twice - squaring it, which
116         // makes the sqrt a waste of time)
117 #if 1
118         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]);
119 #else
120         double scale = 3.0 / sqrt
121                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
122                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
123                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
124         scale *= scale;
125 #endif
126
127         // invert the rotation by transposing and multiplying by the squared
128         // recipricol of the input matrix scale as described above
129         out->m[0][0] = (float)(in1->m[0][0] * scale);
130         out->m[0][1] = (float)(in1->m[1][0] * scale);
131         out->m[0][2] = (float)(in1->m[2][0] * scale);
132         out->m[1][0] = (float)(in1->m[0][1] * scale);
133         out->m[1][1] = (float)(in1->m[1][1] * scale);
134         out->m[1][2] = (float)(in1->m[2][1] * scale);
135         out->m[2][0] = (float)(in1->m[0][2] * scale);
136         out->m[2][1] = (float)(in1->m[1][2] * scale);
137         out->m[2][2] = (float)(in1->m[2][2] * scale);
138
139         // invert the translate
140         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]);
141         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]);
142         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]);
143
144         // don't know if there's anything worth doing here
145         out->m[3][0] = 0;
146         out->m[3][1] = 0;
147         out->m[3][2] = 0;
148         out->m[3][3] = 1;
149 }
150
151 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
152 {
153         out->m[0][0]=1.0f;
154         out->m[0][1]=0.0f;
155         out->m[0][2]=0.0f;
156         out->m[0][3]=0.0f;
157         out->m[1][0]=0.0f;
158         out->m[1][1]=1.0f;
159         out->m[1][2]=0.0f;
160         out->m[1][3]=0.0f;
161         out->m[2][0]=0.0f;
162         out->m[2][1]=0.0f;
163         out->m[2][2]=1.0f;
164         out->m[2][3]=0.0f;
165         out->m[3][0]=0.0f;
166         out->m[3][1]=0.0f;
167         out->m[3][2]=0.0f;
168         out->m[3][3]=1.0f;
169 }
170
171 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
172 {
173         out->m[0][0]=1.0f;
174         out->m[0][1]=0.0f;
175         out->m[0][2]=0.0f;
176         out->m[0][3]=x;
177         out->m[1][0]=0.0f;
178         out->m[1][1]=1.0f;
179         out->m[1][2]=0.0f;
180         out->m[1][3]=y;
181         out->m[2][0]=0.0f;
182         out->m[2][1]=0.0f;
183         out->m[2][2]=1.0f;
184         out->m[2][3]=z;
185         out->m[3][0]=0.0f;
186         out->m[3][1]=0.0f;
187         out->m[3][2]=0.0f;
188         out->m[3][3]=1.0f;
189 }
190
191 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
192 {
193         float len, c, s;
194
195         len = x*x+y*y+z*z;
196         if (len != 0.0f)
197                 len = 1.0f / (float)sqrt(len);
198         x *= len;
199         y *= len;
200         z *= len;
201
202         angle *= (float)(-M_PI / 180.0);
203         c = (float)cos(angle);
204         s = (float)sin(angle);
205
206         out->m[0][0]=x * x + c * (1 - x * x);
207         out->m[0][1]=x * y * (1 - c) + z * s;
208         out->m[0][2]=z * x * (1 - c) - y * s;
209         out->m[0][3]=0.0f;
210         out->m[1][0]=x * y * (1 - c) - z * s;
211         out->m[1][1]=y * y + c * (1 - y * y);
212         out->m[1][2]=y * z * (1 - c) + x * s;
213         out->m[1][3]=0.0f;
214         out->m[2][0]=z * x * (1 - c) + y * s;
215         out->m[2][1]=y * z * (1 - c) - x * s;
216         out->m[2][2]=z * z + c * (1 - z * z);
217         out->m[2][3]=0.0f;
218         out->m[3][0]=0.0f;
219         out->m[3][1]=0.0f;
220         out->m[3][2]=0.0f;
221         out->m[3][3]=1.0f;
222 }
223
224 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
225 {
226         out->m[0][0]=x;
227         out->m[0][1]=0.0f;
228         out->m[0][2]=0.0f;
229         out->m[0][3]=0.0f;
230         out->m[1][0]=0.0f;
231         out->m[1][1]=x;
232         out->m[1][2]=0.0f;
233         out->m[1][3]=0.0f;
234         out->m[2][0]=0.0f;
235         out->m[2][1]=0.0f;
236         out->m[2][2]=x;
237         out->m[2][3]=0.0f;
238         out->m[3][0]=0.0f;
239         out->m[3][1]=0.0f;
240         out->m[3][2]=0.0f;
241         out->m[3][3]=1.0f;
242 }
243
244 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
245 {
246         out->m[0][0]=x;
247         out->m[0][1]=0.0f;
248         out->m[0][2]=0.0f;
249         out->m[0][3]=0.0f;
250         out->m[1][0]=0.0f;
251         out->m[1][1]=y;
252         out->m[1][2]=0.0f;
253         out->m[1][3]=0.0f;
254         out->m[2][0]=0.0f;
255         out->m[2][1]=0.0f;
256         out->m[2][2]=z;
257         out->m[2][3]=0.0f;
258         out->m[3][0]=0.0f;
259         out->m[3][1]=0.0f;
260         out->m[3][2]=0.0f;
261         out->m[3][3]=1.0f;
262 }
263
264 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
265 {
266         double angle, sr, sp, sy, cr, cp, cy;
267
268         angle = yaw * (M_PI*2 / 360);
269         sy = sin(angle);
270         cy = cos(angle);
271         angle = pitch * (M_PI*2 / 360);
272         sp = sin(angle);
273         cp = cos(angle);
274         angle = roll * (M_PI*2 / 360);
275         sr = sin(angle);
276         cr = cos(angle);
277         out->m[0][0] = (float)((cp*cy) * scale);
278         out->m[0][1] = (float)((sr*sp*cy+cr*-sy) * scale);
279         out->m[0][2] = (float)((cr*sp*cy+-sr*-sy) * scale);
280         out->m[0][3] = x;
281         out->m[1][0] = (float)((cp*sy) * scale);
282         out->m[1][1] = (float)((sr*sp*sy+cr*cy) * scale);
283         out->m[1][2] = (float)((cr*sp*sy+-sr*cy) * scale);
284         out->m[1][3] = y;
285         out->m[2][0] = (float)((-sp) * scale);
286         out->m[2][1] = (float)((sr*cp) * scale);
287         out->m[2][2] = (float)((cr*cp) * scale);
288         out->m[2][3] = z;
289         out->m[3][0] = 0;
290         out->m[3][1] = 0;
291         out->m[3][2] = 0;
292         out->m[3][3] = 1;
293 }
294
295 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
296 {
297         vx[0] = in->m[0][0];
298         vx[1] = in->m[1][0];
299         vx[2] = in->m[2][0];
300         vy[0] = in->m[0][1];
301         vy[1] = in->m[1][1];
302         vy[2] = in->m[2][1];
303         vz[0] = in->m[0][2];
304         vz[1] = in->m[1][2];
305         vz[2] = in->m[2][2];
306         t[0] = in->m[0][3];
307         t[1] = in->m[1][3];
308         t[2] = in->m[2][3];
309 }
310
311 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
312 {
313         out->m[0][0] = vx[0];
314         out->m[0][1] = vy[0];
315         out->m[0][2] = vz[0];
316         out->m[0][3] = t[0];
317         out->m[1][0] = vx[1];
318         out->m[1][1] = vy[1];
319         out->m[1][2] = vz[1];
320         out->m[1][3] = t[1];
321         out->m[2][0] = vx[2];
322         out->m[2][1] = vy[2];
323         out->m[2][2] = vz[2];
324         out->m[2][3] = t[2];
325         out->m[3][0] = 0.0f;
326         out->m[3][1] = 0.0f;
327         out->m[3][2] = 0.0f;
328         out->m[3][3] = 1.0f;
329 }
330
331 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, float blend)
332 {
333         float iblend = 1 - blend;
334         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
335         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
336         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
337         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
338         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
339         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
340         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
341         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
342         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
343         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
344         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
345         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
346         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
347         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
348         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
349         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
350 }
351
352
353 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
354 {
355         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
356         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
357         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
358 }
359
360 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
361 {
362         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];
363         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];
364         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];
365         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];
366 }
367
368 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
369 {
370         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
371         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
372         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
373 }
374
375 /*
376 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
377 {
378         float t[3];
379         t[0] = v[0] - in->m[0][3];
380         t[1] = v[1] - in->m[1][3];
381         t[2] = v[2] - in->m[2][3];
382         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
383         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
384         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
385 }
386 */
387
388 // FIXME: optimize
389 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
390 {
391         matrix4x4_t base, temp;
392         base = *out;
393         Matrix4x4_CreateTranslate(&temp, x, y, z);
394         Matrix4x4_Concat(out, &base, &temp);
395 }
396
397 // FIXME: optimize
398 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
399 {
400         matrix4x4_t base, temp;
401         base = *out;
402         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
403         Matrix4x4_Concat(out, &base, &temp);
404 }
405
406 // FIXME: optimize
407 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
408 {
409         matrix4x4_t base, temp;
410         base = *out;
411         Matrix4x4_CreateScale(&temp, x);
412         Matrix4x4_Concat(out, &base, &temp);
413 }
414
415 // FIXME: optimize
416 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
417 {
418         matrix4x4_t base, temp;
419         base = *out;
420         Matrix4x4_CreateScale3(&temp, x, y, z);
421         Matrix4x4_Concat(out, &base, &temp);
422 }
423
424 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
425 {
426         out[0] = in->m[0][3];
427         out[1] = in->m[1][3];
428         out[2] = in->m[2][3];
429 }
430
431 float Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
432 {
433         // we only support uniform scaling, so assume the first row is enough
434         return (float)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]);
435 }
436