volmetrics

view src/matrix.cc @ 21:c22866ecb7ae

find changesets by author, revision, files, or words in the commit message
frame :p
author Eleni Maria Stea <elene.mst@gmail.com>
date Tue, 25 Mar 2014 00:17:39 +0200
parents
children
line source
1 #include <math.h>
2 #include <stdio.h>
3 #include <string.h>
4 #include "matrix.h"
5
6 Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
7
8 Matrix4x4::Matrix4x4()
9 {
10 *this = identity;
11 }
12
13 Matrix4x4::Matrix4x4( float m11, float m12, float m13, float m14,
14 float m21, float m22, float m23, float m24,
15 float m31, float m32, float m33, float m34,
16 float m41, float m42, float m43, float m44)
17 {
18 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
19 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
20 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
21 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
22 }
23
24 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
25 {
26 Matrix4x4 res;
27 const float *op1 = m1.m[0], *op2 = m2.m[0];
28 float *dest = res.m[0];
29
30 for(int i=0; i<16; i++) {
31 *dest++ = *op1++ + *op2++;
32 }
33 return res;
34 }
35
36 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
37 {
38 Matrix4x4 res;
39 const float *op1 = m1.m[0], *op2 = m2.m[0];
40 float *dest = res.m[0];
41
42 for(int i=0; i<16; i++) {
43 *dest++ = *op1++ - *op2++;
44 }
45 return res;
46 }
47
48 Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
49 Matrix4x4 res;
50
51 for(int i=0; i<4; i++) {
52 for(int j=0; j<4; j++) {
53 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
54 }
55 }
56
57 return res;
58 }
59
60 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
61 {
62 float *op1 = m1.m[0];
63 const float *op2 = m2.m[0];
64
65 for(int i=0; i<16; i++) {
66 *op1++ += *op2++;
67 }
68 }
69
70 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
71 {
72 float *op1 = m1.m[0];
73 const float *op2 = m2.m[0];
74
75 for(int i=0; i<16; i++) {
76 *op1++ -= *op2++;
77 }
78 }
79
80 void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2)
81 {
82 Matrix4x4 res;
83 for(int i=0; i<4; i++) {
84 for(int j=0; j<4; j++) {
85 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
86 }
87 }
88 memcpy(m1.m, res.m, 16 * sizeof(float));
89 }
90
91 Matrix4x4 operator *(const Matrix4x4 &mat, float scalar)
92 {
93 Matrix4x4 res;
94 const float *mptr = mat.m[0];
95 float *dptr = res.m[0];
96
97 for(int i=0; i<16; i++) {
98 *dptr++ = *mptr++ * scalar;
99 }
100 return res;
101 }
102
103 Matrix4x4 operator *(float scalar, const Matrix4x4 &mat)
104 {
105 Matrix4x4 res;
106 const float *mptr = mat.m[0];
107 float *dptr = res.m[0];
108
109 for(int i=0; i<16; i++) {
110 *dptr++ = *mptr++ * scalar;
111 }
112 return res;
113 }
114
115 void operator *=(Matrix4x4 &mat, float scalar)
116 {
117 float *mptr = mat.m[0];
118
119 for(int i=0; i<16; i++) {
120 *mptr++ *= scalar;
121 }
122 }
123
124 void Matrix4x4::translate(const Vector3 &trans)
125 {
126 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
127 *this *= tmat;
128 }
129
130 void Matrix4x4::set_translation(const Vector3 &trans)
131 {
132 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
133 }
134
135 void Matrix4x4::rotate(const Vector3 &euler)
136 {
137 float sinx = sin(euler.x);
138 float cosx = cos(euler.x);
139 float siny = sin(euler.y);
140 float cosy = cos(euler.y);
141 float sinz = sin(euler.z);
142 float cosz = cos(euler.z);
143
144 Matrix4x4 xrot = Matrix4x4(
145 1, 0, 0, 0,
146 0, cosx, sinx, 0,
147 0, -sinx, cosx, 0,
148 0, 0, 0, 1);
149
150 Matrix4x4 yrot = Matrix4x4(
151 cosy, 0, -siny, 0,
152 0, 1, 0, 0,
153 siny, 0, cosy, 0,
154 0, 0, 0, 1);
155
156 Matrix4x4 zrot = Matrix4x4(
157 cosz, sinz, 0, 0,
158 -sinz, cosz, 0, 0,
159 0, 0, 1, 0,
160 0, 0, 0, 1);
161
162 *this *= (xrot * yrot * zrot);
163 }
164
165 void Matrix4x4::rotate(const Vector3 &axis, float angle)
166 {
167 float sina = (float)sin(angle);
168 float cosa = (float)cos(angle);
169 float invcosa = 1-cosa;
170 float nxsq = axis.x * axis.x;
171 float nysq = axis.y * axis.y;
172 float nzsq = axis.z * axis.z;
173
174 Matrix4x4 xform;
175 xform[0][0] = nxsq + (1-nxsq) * cosa;
176 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
177 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
178 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
179 xform[1][1] = nysq + (1-nysq) * cosa;
180 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
181 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
182 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
183 xform[2][2] = nzsq + (1-nzsq) * cosa;
184
185 *this *= Matrix4x4(xform);
186 }
187
188 void Matrix4x4::set_rotation(const Vector3 &axis, float angle)
189 {
190 float sina = (float)sin(angle);
191 float cosa = (float)cos(angle);
192 float invcosa = 1-cosa;
193 float nxsq = axis.x * axis.x;
194 float nysq = axis.y * axis.y;
195 float nzsq = axis.z * axis.z;
196
197 reset_identity();
198 m[0][0] = nxsq + (1-nxsq) * cosa;
199 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
200 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
201 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
202 m[1][1] = nysq + (1-nysq) * cosa;
203 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
204 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
205 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
206 m[2][2] = nzsq + (1-nzsq) * cosa;
207 }
208
209 void Matrix4x4::scale(const Vector3 &scale_vec)
210 {
211 Matrix4x4 smat( scale_vec.x, 0, 0, 0,
212 0, scale_vec.y, 0, 0,
213 0, 0, scale_vec.z, 0,
214 0, 0, 0, 1.0);
215 *this *= smat;
216 }
217
218 void Matrix4x4::set_scaling(const Vector3 &scale_vec)
219 {
220 *this = Matrix4x4( scale_vec.x, 0, 0, 0,
221 0, scale_vec.y, 0, 0,
222 0, 0, scale_vec.z, 0,
223 0, 0, 0, 1);
224 }
225
226 void Matrix4x4::set_column_vector(const Vector3 &vec, unsigned int col_index)
227 {
228 m[0][col_index] = vec.x;
229 m[1][col_index] = vec.y;
230 m[2][col_index] = vec.z;
231 m[3][col_index] = 1;
232 }
233
234 void Matrix4x4::set_row_vector(const Vector3 &vec, unsigned int row_index)
235 {
236 m[row_index][0] = vec.x;
237 m[row_index][1] = vec.y;
238 m[row_index][2] = vec.z;
239 m[row_index][3] = 1;
240 }
241
242 Vector3 Matrix4x4::get_column_vector(unsigned int col_index) const
243 {
244 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
245 }
246
247 Vector3 Matrix4x4::get_row_vector(unsigned int row_index) const
248 {
249 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
250 }
251
252 void Matrix4x4::transpose()
253 {
254 Matrix4x4 tmp = *this;
255 for(int i=0; i<4; i++) {
256 for(int j=0; j<4; j++) {
257 m[i][j] = tmp[j][i];
258 }
259 }
260 }
261
262 Matrix4x4 Matrix4x4::transposed() const
263 {
264 Matrix4x4 res;
265 for(int i=0; i<4; i++) {
266 for(int j=0; j<4; j++) {
267 res[i][j] = m[j][i];
268 }
269 }
270 return res;
271 }
272
273 float Matrix4x4::determinant() const
274 {
275 float det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
276 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
277 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
278
279 float det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
280 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
281 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
282
283 float det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
284 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
285 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
286
287 float det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
288 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
289 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
290
291 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
292 }
293
294
295 Matrix4x4 Matrix4x4::adjoint() const
296 {
297 Matrix4x4 coef;
298
299 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
300 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
301 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
302 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
303 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
304 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
305 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
306 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
307 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
308 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
309 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
310 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
311
312 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
313 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
314 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
315 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
316 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
317 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
318 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
319 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
320 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
321 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
322 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
323 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
324
325 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
326 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
327 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
328 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
329 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
330 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
331 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
332 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
333 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
334 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
335 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
336 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
337
338 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
339 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
340 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
341 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
342 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
343 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
344 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
345 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
346 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
347 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
348 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
349 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
350
351 coef.transpose();
352
353 for(int i=0; i<4; i++) {
354 for(int j=0; j<4; j++) {
355 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
356 if(i%2) coef.m[i][j] = -coef.m[i][j];
357 }
358 }
359
360 return coef;
361 }
362
363 Matrix4x4 Matrix4x4::inverse() const
364 {
365 Matrix4x4 adj = adjoint();
366
367 return adj * (1.0f / determinant());
368 }
369
370 inline float *Matrix4x4::operator [](int index)
371 {
372 return m[index];
373 }
374
375 inline const float *Matrix4x4::operator [](int index) const
376 {
377 return m[index];
378 }
379
380 inline void Matrix4x4::reset_identity()
381 {
382 *this = identity;
383 }
384
385 std::ostream &operator <<(std::ostream &out, const Matrix4x4 &mat)
386 {
387 for(int i=0; i<4; i++) {
388 char str[100];
389 sprintf(str, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2], (float)mat.m[i][3]);
390 out << str;
391 }
392 return out;
393 }