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. to_ptrmat445 inline __float* to_ptr() const { return (__float *)this; } 46 mat4mat447 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 } mat4mat450 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 identitymat455 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 multiplymat459 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 multiplymat482 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 } frustummat491 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 }; perspectivemat4103 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 }; orthomat4110 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 }; determinantmat4113 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 transposemat4136 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 inverse_mat3mat4140 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 inversemat4168 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 translatemat4229 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 rotateAxismat4235 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 rotatemat4245 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 scalemat4273 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 transformmat4277 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 vec4_multiplymat4293 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 }; lookatmat4303 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 unProjectmat4326 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__) */ 340