created a couple matrix libraries (4x4 and 3x4), hopefully these will be useful
[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         c = cos(angle);
102         s = sin(angle);
103
104         out->m[0][0]=x * x + c * (1 - x * x);
105         out->m[0][1]=x * y * (1 - c) + z * s;
106         out->m[0][2]=z * x * (1 - c) - y * s;
107         out->m[0][3]=0.0f;
108         out->m[1][0]=x * y * (1 - c) - z * s;
109         out->m[1][1]=y * y + c * (1 - y * y);
110         out->m[1][2]=y * z * (1 - c) + x * s;
111         out->m[1][3]=0.0f;
112         out->m[2][0]=z * x * (1 - c) + y * s;
113         out->m[2][1]=y * z * (1 - c) - x * s;
114         out->m[2][2]=z * z + c * (1 - z * z);
115         out->m[2][3]=0.0f;
116         out->m[3][0]=0.0f;
117         out->m[3][1]=0.0f;
118         out->m[3][2]=0.0f;
119         out->m[3][3]=1.0f;
120 }
121
122 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
123 {
124         out->m[0][0]=x;
125         out->m[0][1]=0.0f;
126         out->m[0][2]=0.0f;
127         out->m[0][3]=0.0f;
128         out->m[1][0]=0.0f;
129         out->m[1][1]=x;
130         out->m[1][2]=0.0f;
131         out->m[1][3]=0.0f;
132         out->m[2][0]=0.0f;
133         out->m[2][1]=0.0f;
134         out->m[2][2]=x;
135         out->m[2][3]=0.0f;
136         out->m[3][0]=0.0f;
137         out->m[3][1]=0.0f;
138         out->m[3][2]=0.0f;
139         out->m[3][3]=1.0f;
140 }
141
142 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
143 {
144         out->m[0][0]=x;
145         out->m[0][1]=0.0f;
146         out->m[0][2]=0.0f;
147         out->m[0][3]=0.0f;
148         out->m[1][0]=0.0f;
149         out->m[1][1]=y;
150         out->m[1][2]=0.0f;
151         out->m[1][3]=0.0f;
152         out->m[2][0]=0.0f;
153         out->m[2][1]=0.0f;
154         out->m[2][2]=z;
155         out->m[2][3]=0.0f;
156         out->m[3][0]=0.0f;
157         out->m[3][1]=0.0f;
158         out->m[3][2]=0.0f;
159         out->m[3][3]=1.0f;
160 }
161
162 void Matrix4x4_ToVectors(const matrix4x4_t *in, vec3_t vx, vec3_t vy, vec3_t vz, vec3_t t)
163 {
164         vx[0] = in->m[0][0];
165         vx[1] = in->m[1][0];
166         vx[2] = in->m[2][0];
167         vy[0] = in->m[0][1];
168         vy[1] = in->m[1][1];
169         vy[2] = in->m[2][1];
170         vz[0] = in->m[0][2];
171         vz[1] = in->m[1][2];
172         vz[2] = in->m[2][2];
173         t[0] = in->m[0][3];
174         t[1] = in->m[1][3];
175         t[2] = in->m[2][3];
176 }
177
178 void Matrix4x4_ToVectors4(const matrix4x4_t *in, vec4_t vx, vec4_t vy, vec4_t vz, vec4_t t)
179 {
180         vx[0] = in->m[0][0];
181         vx[1] = in->m[1][0];
182         vx[2] = in->m[2][0];
183         vx[3] = in->m[3][0];
184         vy[0] = in->m[0][1];
185         vy[1] = in->m[1][1];
186         vy[2] = in->m[2][1];
187         vy[3] = in->m[3][1];
188         vz[0] = in->m[0][2];
189         vz[1] = in->m[1][2];
190         vz[2] = in->m[2][2];
191         vz[3] = in->m[3][2];
192         t[0] = in->m[0][3];
193         t[1] = in->m[1][3];
194         t[2] = in->m[2][3];
195         t[3] = in->m[3][3];
196 }
197
198 void Matrix4x4_FromVectors(matrix4x4_t *out, const vec3_t vx, const vec3_t vy, const vec3_t vz, const vec3_t t)
199 {
200         out->m[0][0] = vx[0];
201         out->m[0][1] = vy[0];
202         out->m[0][2] = vz[0];
203         out->m[0][3] = t[0];
204         out->m[1][0] = vx[1];
205         out->m[1][1] = vy[1];
206         out->m[1][2] = vz[1];
207         out->m[1][3] = t[1];
208         out->m[2][0] = vx[2];
209         out->m[2][1] = vy[2];
210         out->m[2][2] = vz[2];
211         out->m[2][3] = t[2];
212         out->m[3][0] = 0.0f;
213         out->m[3][1] = 0.0f;
214         out->m[3][2] = 0.0f;
215         out->m[3][3] = 1.0f;
216 }
217
218 void Matrix4x4_FromVectors4(matrix4x4_t *out, const vec4_t vx, const vec4_t vy, const vec4_t vz, const vec4_t t)
219 {
220         out->m[0][0] = vx[0];
221         out->m[0][1] = vy[0];
222         out->m[0][2] = vz[0];
223         out->m[0][3] = t[0];
224         out->m[1][0] = vx[1];
225         out->m[1][1] = vy[1];
226         out->m[1][2] = vz[1];
227         out->m[1][3] = t[1];
228         out->m[2][0] = vx[2];
229         out->m[2][1] = vy[2];
230         out->m[2][2] = vz[2];
231         out->m[2][3] = t[2];
232         out->m[3][0] = vx[3];
233         out->m[3][1] = vy[3];
234         out->m[3][2] = vz[3];
235         out->m[3][3] = t[3];
236 }
237
238 void Matrix4x4_Transform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
239 {
240         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
241         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
242         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
243 }
244
245 void Matrix4x4_Transform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
246 {
247         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];
248         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];
249         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];
250         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];
251 }
252
253 void Matrix4x4_Untransform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
254 {
255         vec3_t t;
256         t[0] = v[0] - in->m[0][3];
257         t[1] = v[1] - in->m[1][3];
258         t[2] = v[2] - in->m[2][3];
259         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
260         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
261         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
262 }
263
264 void Matrix4x4_Untransform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
265 {
266         vec3_t t;
267         t[0] = v[0] - in->m[0][3];
268         t[1] = v[1] - in->m[1][3];
269         t[2] = v[2] - in->m[2][3];
270         t[3] = v[3] - in->m[3][3];
271         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];
272         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];
273         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];
274         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];
275 }
276
277 // FIXME: optimize
278 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
279 {
280         matrix4x4_t base, temp;
281         Matrix4x4_Copy(out, &base);
282         Matrix4x4_CreateTranslate(&temp, x, y, z);
283         Matrix4x4_Concat(out, &base, &temp);
284 }
285
286 // FIXME: optimize
287 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
288 {
289         matrix4x4_t base, temp;
290         Matrix4x4_Copy(out, &base);
291         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
292         Matrix4x4_Concat(out, &base, &temp);
293 }
294
295 // FIXME: optimize
296 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
297 {
298         matrix4x4_t base, temp;
299         Matrix4x4_Copy(out, &base);
300         Matrix4x4_CreateScale(&temp, x);
301         Matrix4x4_Concat(out, &base, &temp);
302 }
303
304 // FIXME: optimize
305 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
306 {
307         matrix4x4_t base, temp;
308         Matrix4x4_Copy(out, &base);
309         Matrix4x4_CreateScale3(&temp, x, y, z);
310         Matrix4x4_Concat(out, &base, &temp);
311 }
312