| 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__) */ |