volmetrics

view src/matrix.cc @ 36:1df14c5ffa71

conversion to Qt
author John Tsiombikas <nuclear@member.fsf.org>
date Fri, 06 Feb 2015 22:39:51 +0200
parents
children
line source
1 #include <math.h>
2 #include <stdio.h>
3 #include <string.h>
4 #include "matrix.h"
6 Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
8 Matrix4x4::Matrix4x4()
9 {
10 *this = identity;
11 }
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 }
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];
30 for(int i=0; i<16; i++) {
31 *dest++ = *op1++ + *op2++;
32 }
33 return res;
34 }
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];
42 for(int i=0; i<16; i++) {
43 *dest++ = *op1++ - *op2++;
44 }
45 return res;
46 }
48 Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
49 Matrix4x4 res;
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 }
57 return res;
58 }
60 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
61 {
62 float *op1 = m1.m[0];
63 const float *op2 = m2.m[0];
65 for(int i=0; i<16; i++) {
66 *op1++ += *op2++;
67 }
68 }
70 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
71 {
72 float *op1 = m1.m[0];
73 const float *op2 = m2.m[0];
75 for(int i=0; i<16; i++) {
76 *op1++ -= *op2++;
77 }
78 }
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 }
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];
97 for(int i=0; i<16; i++) {
98 *dptr++ = *mptr++ * scalar;
99 }
100 return res;
101 }
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];
109 for(int i=0; i<16; i++) {
110 *dptr++ = *mptr++ * scalar;
111 }
112 return res;
113 }
115 void operator *=(Matrix4x4 &mat, float scalar)
116 {
117 float *mptr = mat.m[0];
119 for(int i=0; i<16; i++) {
120 *mptr++ *= scalar;
121 }
122 }
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 }
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 }
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);
144 Matrix4x4 xrot = Matrix4x4(
145 1, 0, 0, 0,
146 0, cosx, sinx, 0,
147 0, -sinx, cosx, 0,
148 0, 0, 0, 1);
150 Matrix4x4 yrot = Matrix4x4(
151 cosy, 0, -siny, 0,
152 0, 1, 0, 0,
153 siny, 0, cosy, 0,
154 0, 0, 0, 1);
156 Matrix4x4 zrot = Matrix4x4(
157 cosz, sinz, 0, 0,
158 -sinz, cosz, 0, 0,
159 0, 0, 1, 0,
160 0, 0, 0, 1);
162 *this *= (xrot * yrot * zrot);
163 }
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;
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;
185 *this *= Matrix4x4(xform);
186 }
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;
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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]));
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]));
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]));
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]));
291 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
292 }
295 Matrix4x4 Matrix4x4::adjoint() const
296 {
297 Matrix4x4 coef;
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]));
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]));
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]));
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]));
351 coef.transpose();
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 }
360 return coef;
361 }
363 Matrix4x4 Matrix4x4::inverse() const
364 {
365 Matrix4x4 adj = adjoint();
367 return adj * (1.0f / determinant());
368 }
370 inline float *Matrix4x4::operator [](int index)
371 {
372 return m[index];
373 }
375 inline const float *Matrix4x4::operator [](int index) const
376 {
377 return m[index];
378 }
380 inline void Matrix4x4::reset_identity()
381 {
382 *this = identity;
383 }
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 }