put parentheses around parameters to min/max/bound macros
[divverent/darkplaces.git] / matrix4x4.c
1
2 #include "matrix4x4.h"
3 #include <math.h>
4
5 void Matrix4x4_Copy (matrix4x4_t *out, matrix4x4_t *in)
6 {
7         *out = *in;
8 }
9
10 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
11 {
12         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];
13         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];
14         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];
15         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];
16         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];
17         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];
18         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];
19         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];
20         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];
21         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];
22         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];
23         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];
24         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];
25         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];
26         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];
27         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];
28 }
29
30 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
31 {
32         out->m[0][0] = in1->m[0][0];
33         out->m[0][1] = in1->m[1][0];
34         out->m[0][2] = in1->m[2][0];
35         out->m[0][3] = in1->m[3][0];
36         out->m[1][0] = in1->m[0][1];
37         out->m[1][1] = in1->m[1][1];
38         out->m[1][2] = in1->m[2][1];
39         out->m[1][3] = in1->m[3][1];
40         out->m[2][0] = in1->m[0][2];
41         out->m[2][1] = in1->m[1][2];
42         out->m[2][2] = in1->m[2][2];
43         out->m[2][3] = in1->m[3][2];
44         out->m[3][0] = in1->m[0][3];
45         out->m[3][1] = in1->m[1][3];
46         out->m[3][2] = in1->m[2][3];
47         out->m[3][3] = in1->m[3][3];
48 }
49
50 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
51 {
52         out->m[0][0]=1.0f;
53         out->m[0][1]=0.0f;
54         out->m[0][2]=0.0f;
55         out->m[0][3]=0.0f;
56         out->m[1][0]=0.0f;
57         out->m[1][1]=1.0f;
58         out->m[1][2]=0.0f;
59         out->m[1][3]=0.0f;
60         out->m[2][0]=0.0f;
61         out->m[2][1]=0.0f;
62         out->m[2][2]=1.0f;
63         out->m[2][3]=0.0f;
64         out->m[3][0]=0.0f;
65         out->m[3][1]=0.0f;
66         out->m[3][2]=0.0f;
67         out->m[3][3]=1.0f;
68 }
69
70 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
71 {
72         out->m[0][0]=1.0f;
73         out->m[0][1]=0.0f;
74         out->m[0][2]=0.0f;
75         out->m[0][3]=x;
76         out->m[1][0]=0.0f;
77         out->m[1][1]=1.0f;
78         out->m[1][2]=0.0f;
79         out->m[1][3]=y;
80         out->m[2][0]=0.0f;
81         out->m[2][1]=0.0f;
82         out->m[2][2]=1.0f;
83         out->m[2][3]=z;
84         out->m[3][0]=0.0f;
85         out->m[3][1]=0.0f;
86         out->m[3][2]=0.0f;
87         out->m[3][3]=1.0f;
88 }
89
90 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
91 {
92         float len, c, s;
93
94         len = x*x+y*y+z*z;
95         if (len != 0.0f)
96                 len = 1.0f / sqrt(len);
97         x *= len;
98         y *= len;
99         z *= len;
100
101         angle *= M_PI / 180.0;
102         c = cos(angle);
103         s = sin(angle);
104
105         out->m[0][0]=x * x + c * (1 - x * x);
106         out->m[0][1]=x * y * (1 - c) + z * s;
107         out->m[0][2]=z * x * (1 - c) - y * s;
108         out->m[0][3]=0.0f;
109         out->m[1][0]=x * y * (1 - c) - z * s;
110         out->m[1][1]=y * y + c * (1 - y * y);
111         out->m[1][2]=y * z * (1 - c) + x * s;
112         out->m[1][3]=0.0f;
113         out->m[2][0]=z * x * (1 - c) + y * s;
114         out->m[2][1]=y * z * (1 - c) - x * s;
115         out->m[2][2]=z * z + c * (1 - z * z);
116         out->m[2][3]=0.0f;
117         out->m[3][0]=0.0f;
118         out->m[3][1]=0.0f;
119         out->m[3][2]=0.0f;
120         out->m[3][3]=1.0f;
121 }
122
123 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
124 {
125         out->m[0][0]=x;
126         out->m[0][1]=0.0f;
127         out->m[0][2]=0.0f;
128         out->m[0][3]=0.0f;
129         out->m[1][0]=0.0f;
130         out->m[1][1]=x;
131         out->m[1][2]=0.0f;
132         out->m[1][3]=0.0f;
133         out->m[2][0]=0.0f;
134         out->m[2][1]=0.0f;
135         out->m[2][2]=x;
136         out->m[2][3]=0.0f;
137         out->m[3][0]=0.0f;
138         out->m[3][1]=0.0f;
139         out->m[3][2]=0.0f;
140         out->m[3][3]=1.0f;
141 }
142
143 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
144 {
145         out->m[0][0]=x;
146         out->m[0][1]=0.0f;
147         out->m[0][2]=0.0f;
148         out->m[0][3]=0.0f;
149         out->m[1][0]=0.0f;
150         out->m[1][1]=y;
151         out->m[1][2]=0.0f;
152         out->m[1][3]=0.0f;
153         out->m[2][0]=0.0f;
154         out->m[2][1]=0.0f;
155         out->m[2][2]=z;
156         out->m[2][3]=0.0f;
157         out->m[3][0]=0.0f;
158         out->m[3][1]=0.0f;
159         out->m[3][2]=0.0f;
160         out->m[3][3]=1.0f;
161 }
162
163 void Matrix4x4_ToVectors(const matrix4x4_t *in, vec3_t vx, vec3_t vy, vec3_t vz, vec3_t t)
164 {
165         vx[0] = in->m[0][0];
166         vx[1] = in->m[1][0];
167         vx[2] = in->m[2][0];
168         vy[0] = in->m[0][1];
169         vy[1] = in->m[1][1];
170         vy[2] = in->m[2][1];
171         vz[0] = in->m[0][2];
172         vz[1] = in->m[1][2];
173         vz[2] = in->m[2][2];
174         t[0] = in->m[0][3];
175         t[1] = in->m[1][3];
176         t[2] = in->m[2][3];
177 }
178
179 void Matrix4x4_ToVectors4(const matrix4x4_t *in, vec4_t vx, vec4_t vy, vec4_t vz, vec4_t t)
180 {
181         vx[0] = in->m[0][0];
182         vx[1] = in->m[1][0];
183         vx[2] = in->m[2][0];
184         vx[3] = in->m[3][0];
185         vy[0] = in->m[0][1];
186         vy[1] = in->m[1][1];
187         vy[2] = in->m[2][1];
188         vy[3] = in->m[3][1];
189         vz[0] = in->m[0][2];
190         vz[1] = in->m[1][2];
191         vz[2] = in->m[2][2];
192         vz[3] = in->m[3][2];
193         t[0] = in->m[0][3];
194         t[1] = in->m[1][3];
195         t[2] = in->m[2][3];
196         t[3] = in->m[3][3];
197 }
198
199 void Matrix4x4_FromVectors(matrix4x4_t *out, const vec3_t vx, const vec3_t vy, const vec3_t vz, const vec3_t t)
200 {
201         out->m[0][0] = vx[0];
202         out->m[0][1] = vy[0];
203         out->m[0][2] = vz[0];
204         out->m[0][3] = t[0];
205         out->m[1][0] = vx[1];
206         out->m[1][1] = vy[1];
207         out->m[1][2] = vz[1];
208         out->m[1][3] = t[1];
209         out->m[2][0] = vx[2];
210         out->m[2][1] = vy[2];
211         out->m[2][2] = vz[2];
212         out->m[2][3] = t[2];
213         out->m[3][0] = 0.0f;
214         out->m[3][1] = 0.0f;
215         out->m[3][2] = 0.0f;
216         out->m[3][3] = 1.0f;
217 }
218
219 void Matrix4x4_FromVectors4(matrix4x4_t *out, const vec4_t vx, const vec4_t vy, const vec4_t vz, const vec4_t t)
220 {
221         out->m[0][0] = vx[0];
222         out->m[0][1] = vy[0];
223         out->m[0][2] = vz[0];
224         out->m[0][3] = t[0];
225         out->m[1][0] = vx[1];
226         out->m[1][1] = vy[1];
227         out->m[1][2] = vz[1];
228         out->m[1][3] = t[1];
229         out->m[2][0] = vx[2];
230         out->m[2][1] = vy[2];
231         out->m[2][2] = vz[2];
232         out->m[2][3] = t[2];
233         out->m[3][0] = vx[3];
234         out->m[3][1] = vy[3];
235         out->m[3][2] = vz[3];
236         out->m[3][3] = t[3];
237 }
238
239 void Matrix4x4_Transform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
240 {
241         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
242         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
243         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
244 }
245
246 void Matrix4x4_Transform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
247 {
248         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];
249         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];
250         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];
251         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];
252 }
253
254 void Matrix4x4_Untransform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
255 {
256         vec3_t t;
257         t[0] = v[0] - in->m[0][3];
258         t[1] = v[1] - in->m[1][3];
259         t[2] = v[2] - in->m[2][3];
260         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
261         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
262         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
263 }
264
265 void Matrix4x4_Untransform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
266 {
267         vec3_t t;
268         t[0] = v[0] - in->m[0][3];
269         t[1] = v[1] - in->m[1][3];
270         t[2] = v[2] - in->m[2][3];
271         t[3] = v[3] - in->m[3][3];
272         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0] + t[3] * in->m[3][0];
273         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1] + t[3] * in->m[3][1];
274         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2] + t[3] * in->m[3][2];
275         out[3] = t[0] * in->m[0][3] + t[1] * in->m[1][3] + t[2] * in->m[2][3] + t[3] * in->m[3][3];
276 }
277
278 // FIXME: optimize
279 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
280 {
281         matrix4x4_t base, temp;
282         Matrix4x4_Copy(out, &base);
283         Matrix4x4_CreateTranslate(&temp, x, y, z);
284         Matrix4x4_Concat(out, &base, &temp);
285 }
286
287 // FIXME: optimize
288 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
289 {
290         matrix4x4_t base, temp;
291         Matrix4x4_Copy(out, &base);
292         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
293         Matrix4x4_Concat(out, &base, &temp);
294 }
295
296 // FIXME: optimize
297 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
298 {
299         matrix4x4_t base, temp;
300         Matrix4x4_Copy(out, &base);
301         Matrix4x4_CreateScale(&temp, x);
302         Matrix4x4_Concat(out, &base, &temp);
303 }
304
305 // FIXME: optimize
306 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
307 {
308         matrix4x4_t base, temp;
309         Matrix4x4_Copy(out, &base);
310         Matrix4x4_CreateScale3(&temp, x, y, z);
311         Matrix4x4_Concat(out, &base, &temp);
312 }
313