/* ====================== mat4_create_identity() ====================================================
Will create an 4x4 identity matrix.
Returns pointer to Mat4.
Usage: Mat4 *a = mat4_create_identity()
*/
Mat4 *mat4_create_identity() {
int i;
Mat4 *m = mat4_create(4);
for (i = 0; i < 4; ++i)
mat4_set(m, i, i, 1);
return m;
}
mat4_t mat3_toMat4(mat3_t mat, mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
dest[15] = 1;
dest[14] = 0;
dest[13] = 0;
dest[12] = 0;
dest[11] = 0;
dest[10] = mat[8];
dest[9] = mat[7];
dest[8] = mat[6];
dest[7] = 0;
dest[6] = mat[5];
dest[5] = mat[4];
dest[4] = mat[3];
dest[3] = 0;
dest[2] = mat[2];
dest[1] = mat[1];
dest[0] = mat[0];
return dest;
}
Mat4* mat4_mult(Mat4* tmatrix, Mat4* imatrix) {
Mat4* result = mat4_create(imatrix->cols);
double* trow = malloc(sizeof(double) * 4);
double* icol = malloc(sizeof(double) * 4);
int c = 0, r = 0;
while (r < 4) {
while (c < result->cols) {
trow[0] = mat4_get(tmatrix,r,0);
trow[1] = mat4_get(tmatrix,r,1);
trow[2] = mat4_get(tmatrix,r,2);
trow[3] = mat4_get(tmatrix,r,3);
icol[0] = mat4_get(imatrix,0,c);
icol[1] = mat4_get(imatrix,1,c);
icol[2] = mat4_get(imatrix,2,c);
icol[3] = mat4_get(imatrix,3,c);
mat4_set(result, r, c, dotprod(trow,icol));
c++;
}
r++;
c = 0;
}
return result;
}
void InitGL()
{
glEnable(GL_DEPTH_TEST);
glClearColor(0.2, 0.2 , 0.6, 0.0);
projection = mat4_create(projection);
view = mat4_create(view);
model = mat4_create(model);
model = mat4_identity(model);
scan_data data = load_scan_data(filename, data_width, data_height, data_length, bit_rate);
marching_cubes(threshold, &data, debug, &g);
model = mat4_translate(model, g.center, model);
free(data.data);
}
mat4* mat4_create_id(){
mat4* mat_ptr = mat4_create();
mat_ptr[0] = 1.0;
mat_ptr[5] = 1.0;
mat_ptr[10] = 1.0;
mat_ptr[15] = 1.0;
return mat_ptr;
}
/* ============================== mat4_copy() ============================================================
Create a copy of a matrix
Usage: Mat4 *c = mat4_copy(a);
*/
Mat4 *mat4_copy(Mat4 *original) {
int row, col;
Mat4 *result = mat4_create(original->cols);
if (original->cols > 0)
for (row = 0; row < 4; ++row)
for (col = 0; col < original->cols; ++col)
mat4_set(result,row,col,mat4_get(original,row,col));
return result;
}
mat4_t mat4_identity(mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
dest[0] = 1;
dest[1] = 0;
dest[2] = 0;
dest[3] = 0;
dest[4] = 0;
dest[5] = 1;
dest[6] = 0;
dest[7] = 0;
dest[8] = 0;
dest[9] = 0;
dest[10] = 1;
dest[11] = 0;
dest[12] = 0;
dest[13] = 0;
dest[14] = 0;
dest[15] = 1;
return dest;
}
quat_t quat_toMat4(quat_t quat, mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
float x = quat[0], y = quat[1], z = quat[2], w = quat[3],
x2 = x + x,
y2 = y + y,
z2 = z + z,
xx = x * x2,
xy = x * y2,
xz = x * z2,
yy = y * y2,
yz = y * z2,
zz = z * z2,
wx = w * x2,
wy = w * y2,
wz = w * z2;
dest[0] = 1 - (yy + zz);
dest[1] = xy + wz;
dest[2] = xz - wy;
dest[3] = 0;
dest[4] = xy - wz;
dest[5] = 1 - (xx + zz);
dest[6] = yz + wx;
dest[7] = 0;
dest[8] = xz + wy;
dest[9] = yz - wx;
dest[10] = 1 - (xx + yy);
dest[11] = 0;
dest[12] = 0;
dest[13] = 0;
dest[14] = 0;
dest[15] = 1;
return dest;
}
mat4_t mat4_fromRotationTranslation(quat_t quat, vec3_t vec, mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
// Quaternion math
double x = quat[0], y = quat[1], z = quat[2], w = quat[3],
x2 = x + x,
y2 = y + y,
z2 = z + z,
xx = x * x2,
xy = x * y2,
xz = x * z2,
yy = y * y2,
yz = y * z2,
zz = z * z2,
wx = w * x2,
wy = w * y2,
wz = w * z2;
dest[0] = 1 - (yy + zz);
dest[1] = xy + wz;
dest[2] = xz - wy;
dest[3] = 0;
dest[4] = xy - wz;
dest[5] = 1 - (xx + zz);
dest[6] = yz + wx;
dest[7] = 0;
dest[8] = xz + wy;
dest[9] = yz - wx;
dest[10] = 1 - (xx + yy);
dest[11] = 0;
dest[12] = vec[0];
dest[13] = vec[1];
dest[14] = vec[2];
dest[15] = 1;
return dest;
}
vec3_t vec3_unproject(vec3_t vec, mat4_t view, mat4_t proj, vec4_t viewport, vec3_t dest) {
if (!dest) { dest = vec; }
mat4_t m = mat4_create(NULL);
double *v = malloc(sizeof(double) * 4);
v[0] = (vec[0] - viewport[0]) * 2.0 / viewport[2] - 1.0;
v[1] = (vec[1] - viewport[1]) * 2.0 / viewport[3] - 1.0;
v[2] = 2.0 * vec[2] - 1.0;
v[3] = 1.0;
mat4_multiply(proj, view, m);
if(!mat4_inverse(m, NULL)) { return NULL; }
mat4_multiplyVec4(m, v, NULL);
if(v[3] == 0.0) { return NULL; }
dest[0] = v[0] / v[3];
dest[1] = v[1] / v[3];
dest[2] = v[2] / v[3];
return dest;
}
mat4_t mat4_ortho(double left, double right, double bottom, double top, double near, double far, mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
double rl = (right - left),
tb = (top - bottom),
fn = (far - near);
dest[0] = 2 / rl;
dest[1] = 0;
dest[2] = 0;
dest[3] = 0;
dest[4] = 0;
dest[5] = 2 / tb;
dest[6] = 0;
dest[7] = 0;
dest[8] = 0;
dest[9] = 0;
dest[10] = -2 / fn;
dest[11] = 0;
dest[12] = -(left + right) / rl;
dest[13] = -(top + bottom) / tb;
dest[14] = -(far + near) / fn;
dest[15] = 1;
return dest;
}
mat4_t mat4_frustum(double left, double right, double bottom, double top, double near, double far, mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
double rl = (right - left),
tb = (top - bottom),
fn = (far - near);
dest[0] = (near * 2) / rl;
dest[1] = 0;
dest[2] = 0;
dest[3] = 0;
dest[4] = 0;
dest[5] = (near * 2) / tb;
dest[6] = 0;
dest[7] = 0;
dest[8] = (right + left) / rl;
dest[9] = (top + bottom) / tb;
dest[10] = -(far + near) / fn;
dest[11] = -1;
dest[12] = 0;
dest[13] = 0;
dest[14] = -(far * near * 2) / fn;
dest[15] = 0;
return dest;
}
mat4_t mat4_ortho(float left, float right, float bottom, float top, float near, float far, mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
float rl = (right - left),
tb = (top - bottom),
fn = (far - near);
dest[0] = 2 / rl;
dest[1] = 0;
dest[2] = 0;
dest[3] = 0;
dest[4] = 0;
dest[5] = 2 / tb;
dest[6] = 0;
dest[7] = 0;
dest[8] = 0;
dest[9] = 0;
dest[10] = -2 / fn;
dest[11] = 0;
dest[12] = -(left + right) / rl;
dest[13] = -(top + bottom) / tb;
dest[14] = -(far + near) / fn;
dest[15] = 1;
return dest;
}
mat4_t mat4_frustum(float left, float right, float bottom, float top, float near, float far, mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
float rl = (right - left),
tb = (top - bottom),
fn = (far - near);
dest[0] = (near * 2) / rl;
dest[1] = 0;
dest[2] = 0;
dest[3] = 0;
dest[4] = 0;
dest[5] = (near * 2) / tb;
dest[6] = 0;
dest[7] = 0;
dest[8] = (right + left) / rl;
dest[9] = (top + bottom) / tb;
dest[10] = -(far + near) / fn;
dest[11] = -1;
dest[12] = 0;
dest[13] = 0;
dest[14] = -(far * near * 2) / fn;
dest[15] = 0;
return dest;
}
mat4_t mat4_lookAt(vec3_t eye, vec3_t center, vec3_t up, mat4_t dest) {
if (!dest) { dest = mat4_create(NULL); }
double x0, x1, x2, y0, y1, y2, z0, z1, z2, len,
eyex = eye[0],
eyey = eye[1],
eyez = eye[2],
upx = up[0],
upy = up[1],
upz = up[2],
centerx = center[0],
centery = center[1],
centerz = center[2];
if (eyex == centerx && eyey == centery && eyez == centerz) {
return mat4_identity(dest);
}
//vec3.direction(eye, center, z);
z0 = eyex - centerx;
z1 = eyey - centery;
z2 = eyez - centerz;
// normalize (no check needed for 0 because of early return)
len = 1 / sqrt(z0 * z0 + z1 * z1 + z2 * z2);
z0 *= len;
z1 *= len;
z2 *= len;
//vec3.normalize(vec3.cross(up, z, x));
x0 = upy * z2 - upz * z1;
x1 = upz * z0 - upx * z2;
x2 = upx * z1 - upy * z0;
len = sqrt(x0 * x0 + x1 * x1 + x2 * x2);
if (!len) {
x0 = 0;
x1 = 0;
x2 = 0;
} else {
len = 1 / len;
x0 *= len;
x1 *= len;
x2 *= len;
}
//vec3.normalize(vec3.cross(z, x, y));
y0 = z1 * x2 - z2 * x1;
y1 = z2 * x0 - z0 * x2;
y2 = z0 * x1 - z1 * x0;
len = sqrt(y0 * y0 + y1 * y1 + y2 * y2);
if (!len) {
y0 = 0;
y1 = 0;
y2 = 0;
} else {
len = 1 / len;
y0 *= len;
y1 *= len;
y2 *= len;
}
dest[0] = x0;
dest[1] = y0;
dest[2] = z0;
dest[3] = 0;
dest[4] = x1;
dest[5] = y1;
dest[6] = z1;
dest[7] = 0;
dest[8] = x2;
dest[9] = y2;
dest[10] = z2;
dest[11] = 0;
dest[12] = -(x0 * eyex + x1 * eyey + x2 * eyez);
dest[13] = -(y0 * eyex + y1 * eyey + y2 * eyez);
dest[14] = -(z0 * eyex + z1 * eyey + z2 * eyez);
dest[15] = 1;
return dest;
}