Debian Codegraph @

external/cubicvr2/math/mat4.h

raw
1 //
2 // mat4.h
3 // CubicVR2
4 //
5 // Created by Charles J. Cliffe on 2013-02-21.
6 // Copyright (c) 2013 Charles J. Cliffe. All rights reserved.
7 //
8
9 #ifndef __CubicVR2__mat4__
10 #define __CubicVR2__mat4__
11
12 #include <iostream>
13 #include "cubic_types.h"
14 #include "vec3.h"
15 #include "vec4.h"
16 #include "mat3.h"
17 #include <cmath>
18 #include <stddef.h>
19
20 namespace CubicVR {
21 using namespace std;
22 #define mat4SG(c,x,y) \
23 mat4 COMBINE(get,x)() { return y; } \
24 c & COMBINE(set,x)(mat4 value) { y = value; return *this; }
25
26 struct mat4 {
27
28 //16 elements
29 __float a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p;
30
31 //access as-array:
32 inline __float& operator [] (size_t i) {
33 __float* as_array = (__float*)this;
34 return (as_array[i]);
35 }
36
37 inline const __float& operator [] (size_t i) const {
38 __float* as_array = (__float*)this;
39 return (as_array[i]);
40 }
41
42 //To be accessed by GL API directly, accessed by pointer.
43 //operator* overloading is way too dangerous, especially in ptr != NULL
44 //tests.
45 inline __float* to_ptr() const { return (__float *)this; }
46
47 mat4(__float ai,__float bi,__float ci,__float di,__float ei,__float fi,__float gi,__float hi,__float ii,__float ji,__float ki,__float li,__float mi,__float ni,__float oi,__float pi) {
48 a = ai; b = bi; c = ci; d = di; e = ei; f = fi; g = gi; h = hi; i = ii; j = ji; k = ki; l = li; m = mi; n = ni; o = oi; p = pi;
49 }
50 mat4() { memset(this,0,sizeof(mat4)); }
51 mat4 operator* (mat4 m) { return mat4::multiply(*this, m, true); };
52 void operator*= (mat4 m) { *this = mat4::multiply(*this, m, true); };
53 // mat4 &operator= (const mat4 &m) { memcpy(this,(__float *)m,sizeof(__float)*16); return *this; };
54
55 static mat4 identity() {
56 return mat4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
57 }
58
59 static mat4 multiply(mat4 mLeft, mat4 mRight, bool /* updated */) {
60 mat4 mOut;
61
62 mOut[0] = mLeft[0] * mRight[0] + mLeft[4] * mRight[1] + mLeft[8] * mRight[2] + mLeft[12] * mRight[3];
63 mOut[1] = mLeft[1] * mRight[0] + mLeft[5] * mRight[1] + mLeft[9] * mRight[2] + mLeft[13] * mRight[3];
64 mOut[2] = mLeft[2] * mRight[0] + mLeft[6] * mRight[1] + mLeft[10] * mRight[2] + mLeft[14] * mRight[3];
65 mOut[3] = mLeft[3] * mRight[0] + mLeft[7] * mRight[1] + mLeft[11] * mRight[2] + mLeft[15] * mRight[3];
66 mOut[4] = mLeft[0] * mRight[4] + mLeft[4] * mRight[5] + mLeft[8] * mRight[6] + mLeft[12] * mRight[7];
67 mOut[5] = mLeft[1] * mRight[4] + mLeft[5] * mRight[5] + mLeft[9] * mRight[6] + mLeft[13] * mRight[7];
68 mOut[6] = mLeft[2] * mRight[4] + mLeft[6] * mRight[5] + mLeft[10] * mRight[6] + mLeft[14] * mRight[7];
69 mOut[7] = mLeft[3] * mRight[4] + mLeft[7] * mRight[5] + mLeft[11] * mRight[6] + mLeft[15] * mRight[7];
70 mOut[8] = mLeft[0] * mRight[8] + mLeft[4] * mRight[9] + mLeft[8] * mRight[10] + mLeft[12] * mRight[11];
71 mOut[9] = mLeft[1] * mRight[8] + mLeft[5] * mRight[9] + mLeft[9] * mRight[10] + mLeft[13] * mRight[11];
72 mOut[10] = mLeft[2] * mRight[8] + mLeft[6] * mRight[9] + mLeft[10] * mRight[10] + mLeft[14] * mRight[11];
73 mOut[11] = mLeft[3] * mRight[8] + mLeft[7] * mRight[9] + mLeft[11] * mRight[10] + mLeft[15] * mRight[11];
74 mOut[12] = mLeft[0] * mRight[12] + mLeft[4] * mRight[13] + mLeft[8] * mRight[14] + mLeft[12] * mRight[15];
75 mOut[13] = mLeft[1] * mRight[12] + mLeft[5] * mRight[13] + mLeft[9] * mRight[14] + mLeft[13] * mRight[15];
76 mOut[14] = mLeft[2] * mRight[12] + mLeft[6] * mRight[13] + mLeft[10] * mRight[14] + mLeft[14] * mRight[15];
77 mOut[15] = mLeft[3] * mRight[12] + mLeft[7] * mRight[13] + mLeft[11] * mRight[14] + mLeft[15] * mRight[15];
78
79 return mOut;
80 };
81
82 static vec3 multiply(mat4 m1, vec3 m2, bool /* updated */) {
83 vec3 mOut;
84
85 mOut[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12];
86 mOut[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13];
87 mOut[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14];
88
89 return mOut;
90 }
91 static mat4 frustum(__float left, __float right, __float bottom, __float top, __float zNear, __float zFar) {
92 __float A = (right + left) / (right - left);
93 __float B = (top + bottom) / (top - bottom);
94 __float C = - (zFar + zNear) / (zFar - zNear);
95 __float D = - (-2.0f * zFar * zNear) / (zFar - zNear);
96
97
98 return mat4((2.0f * zNear) / (right - left), 0, A, 0,
99 0, (2.0f * zNear) / (top - bottom), B, 0,
100 0, 0, C, D,
101 0, 0, -1, 0);
102 };
103 static mat4 perspective(__float fovy, __float aspect, __float zNear, __float zFar) {
104 __float yFac = tan(fovy * (float)M_PI / 360.0f);
105 __float xFac = yFac * aspect;
106
107 return mat4::frustum(-xFac, xFac, -yFac, yFac, zNear, zFar);
108
109 };
110 static mat4 ortho(__float left,__float right,__float bottom,__float top,__float znear,__float zfar) {
111 return mat4(2.0f / (right - left), 0, 0, 0, 0, 2.0f / (top - bottom), 0, 0, 0, 0, -2.0f / (zfar - znear), 0, -(left + right) / (right - left), -(top + bottom) / (top - bottom), -(zfar + znear) / (zfar - znear), 1);
112 };
113 static __float determinant(mat4 m) {
114
115 __float a0 = m[0] * m[5] - m[1] * m[4];
116 __float a1 = m[0] * m[6] - m[2] * m[4];
117 __float a2 = m[0] * m[7] - m[3] * m[4];
118 __float a3 = m[1] * m[6] - m[2] * m[5];
119 __float a4 = m[1] * m[7] - m[3] * m[5];
120 __float a5 = m[2] * m[7] - m[3] * m[6];
121 __float b0 = m[8] * m[13] - m[9] * m[12];
122 __float b1 = m[8] * m[14] - m[10] * m[12];
123 __float b2 = m[8] * m[15] - m[11] * m[12];
124 __float b3 = m[9] * m[14] - m[10] * m[13];
125 __float b4 = m[9] * m[15] - m[11] * m[13];
126 __float b5 = m[10] * m[15] - m[11] * m[14];
127
128 __float det = a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
129
130 return det;
131 };
132 // coFactor: function (m, n, out) {
133 // // .. todo..
134 // },
135
136 static mat4 transpose(mat4 m) {
137 return mat4(m[0], m[4], m[8], m[12], m[1], m[5], m[9], m[13], m[2], m[6], m[10], m[14], m[3], m[7], m[11], m[15]);
138 };
139
140 static mat3 inverse_mat3(mat4 mat) {
141 mat3 dest;
142
143 __float a00 = mat[0], a01 = mat[1], a02 = mat[2],
144 a10 = mat[4], a11 = mat[5], a12 = mat[6],
145 a20 = mat[8], a21 = mat[9], a22 = mat[10];
146
147 __float b01 = a22*a11-a12*a21,
148 b11 = -a22*a10+a12*a20,
149 b21 = a21*a10-a11*a20;
150
151 __float d = a00*b01 + a01*b11 + a02*b21;
152 if (!d) { return dest; }
153 __float id = 1/d;
154
155 dest[0] = b01*id;
156 dest[1] = (-a22*a01 + a02*a21)*id;
157 dest[2] = (a12*a01 - a02*a11)*id;
158 dest[3] = b11*id;
159 dest[4] = (a22*a00 - a02*a20)*id;
160 dest[5] = (-a12*a00 + a02*a10)*id;
161 dest[6] = b21*id;
162 dest[7] = (-a21*a00 + a01*a20)*id;
163 dest[8] = (a11*a00 - a01*a10)*id;
164
165 return dest;
166 };
167
168 static mat4 inverse(mat4 m) {
169 mat4 m_inv;
170
171 __float a0 = m[0] * m[5] - m[1] * m[4];
172 __float a1 = m[0] * m[6] - m[2] * m[4];
173 __float a2 = m[0] * m[7] - m[3] * m[4];
174 __float a3 = m[1] * m[6] - m[2] * m[5];
175 __float a4 = m[1] * m[7] - m[3] * m[5];
176 __float a5 = m[2] * m[7] - m[3] * m[6];
177 __float b0 = m[8] * m[13] - m[9] * m[12];
178 __float b1 = m[8] * m[14] - m[10] * m[12];
179 __float b2 = m[8] * m[15] - m[11] * m[12];
180 __float b3 = m[9] * m[14] - m[10] * m[13];
181 __float b4 = m[9] * m[15] - m[11] * m[13];
182 __float b5 = m[10] * m[15] - m[11] * m[14];
183
184 __float determinant = a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
185
186 if (determinant != 0) {
187 m_inv[0] = 0 + m[5] * b5 - m[6] * b4 + m[7] * b3;
188 m_inv[4] = 0 - m[4] * b5 + m[6] * b2 - m[7] * b1;
189 m_inv[8] = 0 + m[4] * b4 - m[5] * b2 + m[7] * b0;
190 m_inv[12] = 0 - m[4] * b3 + m[5] * b1 - m[6] * b0;
191 m_inv[1] = 0 - m[1] * b5 + m[2] * b4 - m[3] * b3;
192 m_inv[5] = 0 + m[0] * b5 - m[2] * b2 + m[3] * b1;
193 m_inv[9] = 0 - m[0] * b4 + m[1] * b2 - m[3] * b0;
194 m_inv[13] = 0 + m[0] * b3 - m[1] * b1 + m[2] * b0;
195 m_inv[2] = 0 + m[13] * a5 - m[14] * a4 + m[15] * a3;
196 m_inv[6] = 0 - m[12] * a5 + m[14] * a2 - m[15] * a1;
197 m_inv[10] = 0 + m[12] * a4 - m[13] * a2 + m[15] * a0;
198 m_inv[14] = 0 - m[12] * a3 + m[13] * a1 - m[14] * a0;
199 m_inv[3] = 0 - m[9] * a5 + m[10] * a4 - m[11] * a3;
200 m_inv[7] = 0 + m[8] * a5 - m[10] * a2 + m[11] * a1;
201 m_inv[11] = 0 - m[8] * a4 + m[9] * a2 - m[11] * a0;
202 m_inv[15] = 0 + m[8] * a3 - m[9] * a1 + m[10] * a0;
203
204 __float inverse_det = 1.0f / determinant;
205
206 m_inv[0] *= inverse_det;
207 m_inv[1] *= inverse_det;
208 m_inv[2] *= inverse_det;
209 m_inv[3] *= inverse_det;
210 m_inv[4] *= inverse_det;
211 m_inv[5] *= inverse_det;
212 m_inv[6] *= inverse_det;
213 m_inv[7] *= inverse_det;
214 m_inv[8] *= inverse_det;
215 m_inv[9] *= inverse_det;
216 m_inv[10] *= inverse_det;
217 m_inv[11] *= inverse_det;
218 m_inv[12] *= inverse_det;
219 m_inv[13] *= inverse_det;
220 m_inv[14] *= inverse_det;
221 m_inv[15] *= inverse_det;
222
223 return m_inv;
224 }
225
226 return mat4::identity();
227 };
228
229 static mat4 translate(__float x, __float y, __float z) {
230 mat4 m = mat4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, x, y, z, 1.0f);
231
232 return m;
233 };
234
235 static mat4 rotateAxis(__float r, __float x, __float y, __float z) { // rotate r about axis x,y,z
236 __float sAng = sinf(r*((float)M_PI/180.0f));
237 __float cAng = cosf(r*((float)M_PI/180.0f));
238
239 return mat4( cAng+(x*x)*(1.0f-cAng), x*y*(1.0f-cAng) - z*sAng, x*z*(1.0f-cAng) + y*sAng, 0,
240 y*x*(1.0f-cAng)+z*sAng, cAng + y*y*(1.0f-cAng), y*z*(1.0f-cAng)-x*sAng, 0,
241 z*x*(1.0f-cAng)-y*sAng, z*y*(1.0f-cAng)+x*sAng, cAng+(z*z)*(1.0f-cAng), 0,
242 0, 0, 0, 1 );
243 };
244
245 static mat4 rotate(__float x, __float y, __float z) { // rotate each axis, angles x, y, z in turn
246 __float sAng,cAng;
247 mat4 mOut = mat4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
248
249 if (z!=0) {
250 sAng = sinf(z*((float)M_PI/180.0f));
251 cAng = cosf(z*((float)M_PI/180.0f));
252
253 mOut *= mat4(cAng, sAng, 0.0f, 0.0f, -sAng, cAng, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
254 }
255
256 if (y!=0) {
257 sAng = sinf(y*((float)M_PI/180.0f));
258 cAng = cosf(y*((float)M_PI/180.0f));
259
260 mOut *= mat4(cAng, 0.0f, -sAng, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, sAng, 0.0f, cAng, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
261 }
262
263 if (x!=0) {
264 sAng = sinf(x*((float)M_PI/180.0f));
265 cAng = cosf(x*((float)M_PI/180.0f));
266
267 mOut *= mat4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, cAng, sAng, 0.0f, 0.0f, -sAng, cAng, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
268 }
269
270 return mOut;
271 };
272
273 static mat4 scale(__float x, __float y, __float z) {
274 return mat4(x, 0.0f, 0.0f, 0.0f, 0.0f, y, 0.0f, 0.0f, 0.0f, 0.0f, z, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
275 };
276
277 static mat4 transform(vec3 position, vec3 rotation, vec3 scale) {
278 mat4 m = mat4::identity();
279
280 m *= mat4::translate(position[0],position[1],position[2]);
281
282 if (!(rotation[0] == 0 && rotation[1] == 0 && rotation[2] == 0)) {
283 m *= mat4::rotate(rotation[0], rotation[1], rotation[2]);
284 }
285
286 if (!(scale[0] == 1 && scale[1] == 1 && scale[2] == 1)) {
287 m *= mat4::scale(scale[0],scale[1],scale[2]);
288 }
289
290 return m;
291 };
292
293 static vec4 vec4_multiply(vec4 m1, mat4 m2) {
294 vec4 mOut;
295
296 mOut[0] = m2[0] * m1[0] + m2[4] * m1[1] + m2[8] * m1[2] + m2[12] * m1[3];
297 mOut[1] = m2[1] * m1[0] + m2[5] * m1[1] + m2[9] * m1[2] + m2[13] * m1[3];
298 mOut[2] = m2[2] * m1[0] + m2[6] * m1[1] + m2[10] * m1[2] + m2[14] * m1[3];
299 mOut[3] = m2[3] * m1[0] + m2[7] * m1[1] + m2[11] * m1[2] + m2[15] * m1[3];
300
301 return mOut;
302 };
303 static mat4 lookat(__float eyex, __float eyey, __float eyez, __float centerx, __float centery, __float centerz, __float upx, __float upy, __float upz) {
304 vec3 forward, side, up;
305
306 forward[0] = centerx - eyex;
307 forward[1] = centery - eyey;
308 forward[2] = centerz - eyez;
309
310 up[0] = upx;
311 up[1] = upy;
312 up[2] = upz;
313
314 forward = vec3::normalize(forward);
315
316 /* Side = forward x up */
317 side = vec3::cross(forward, up);
318 side = vec3::normalize(side);
319
320 /* Recompute up as: up = side x forward */
321 up = vec3::cross(side, forward);
322
323 return mat4::translate(-eyex,-eyey,-eyez) * mat4( side[0], up[0], -forward[0], 0, side[1], up[1], -forward[1], 0, side[2], up[2], -forward[2], 0, 0, 0, 0, 1);
324 };
325
326 static vec3 unProject(mat4 pMatrix, mat4 mvMatrix, float width, float height, float winx, float winy, float /* winz */) {
327 vec4 p(((winx / width) * 2.0f) - 1.0f, -(((winy / height) * 2.0f) - 1.0f), 1.0f, 1.0f);
328
329 vec4 invp = mat4::vec4_multiply(mat4::vec4_multiply(p, mat4::inverse(pMatrix)), mat4::inverse(mvMatrix));
330
331 vec3 result(invp[0] / invp[3], invp[1] / invp[3], invp[2] / invp[3]);
332
333 return result;
334 };
335 };
336 }
337
338
339 #endif /* defined(__CubicVR2__mat4__) */