#include "matrix4x4.h"
#include "../util/util.h"
#include <SDL2/SDL.h>
void mat4_identity(Mat4 *a)
{
int i;
for(i = 0; i < 4*4; i++)
{
a->data[i] = 0.0f;
}
a->data[0 + 0 * 4] = 1.0f;
a->data[1 + 1 * 4] = 1.0f;
a->data[2 + 2 * 4] = 1.0f;
a->data[3 + 3 * 4] = 1.0f;
}
/** TODO: Preprocess these operation (1 + 1 * 4) -> 5 **/
Mat4 mat4_inverse(const Mat4 *a)
{
Mat4 inv;
int i;
/** Remember that inverted matrix is (1/det) * cofactor(transposed) **/
inv.data[0 + 0 * 4] = (a->data[1 + 1 * 4] * a->data[2 + 2 * 4] * a->data[3 + 3 * 4] +
a->data[2 + 1 * 4] * a->data[3 + 2 * 4] * a->data[1 + 3 * 4] +
a->data[3 + 1 * 4] * a->data[1 + 2 * 4] * a->data[2 + 3 * 4] -
a->data[1 + 3 * 4] * a->data[2 + 2 * 4] * a->data[3 + 1 * 4] -
a->data[2 + 3 * 4] * a->data[3 + 2 * 4] * a->data[1 + 1 * 4] -
a->data[3 + 3 * 4] * a->data[1 + 2 * 4] * a->data[2 + 1 * 4]);
inv.data[0 + 1 * 4] = -(a->data[0 + 1 * 4] * a->data[2 + 2 * 4] * a->data[3 + 3 * 4] +
a->data[2 + 1 * 4] * a->data[3 + 2 * 4] * a->data[0 + 3 * 4] +
a->data[3 + 1 * 4] * a->data[0 + 2 * 4] * a->data[2 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[2 + 2 * 4] * a->data[3 + 1 * 4] -
a->data[2 + 3 * 4] * a->data[3 + 2 * 4] * a->data[0 + 1 * 4] -
a->data[3 + 3 * 4] * a->data[0 + 2 * 4] * a->data[2 + 1 * 4]);
inv.data[0 + 2 * 4] = (a->data[0 + 1 * 4] * a->data[1 + 2 * 4] * a->data[3 + 3 * 4] +
a->data[1 + 1 * 4] * a->data[3 + 2 * 4] * a->data[0 + 3 * 4] +
a->data[3 + 1 * 4] * a->data[0 + 2 * 4] * a->data[1 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[1 + 2 * 4] * a->data[3 + 1 * 4] -
a->data[1 + 3 * 4] * a->data[3 + 2 * 4] * a->data[0 + 1 * 4] -
a->data[3 + 3 * 4] * a->data[0 + 2 * 4] * a->data[1 + 1 * 4]);
inv.data[0 + 3 * 4] = -(a->data[0 + 1 * 4] * a->data[1 + 2 * 4] * a->data[2 + 3 * 4] +
a->data[1 + 1 * 4] * a->data[2 + 2 * 4] * a->data[0 + 3 * 4] +
a->data[2 + 1 * 4] * a->data[0 + 2 * 4] * a->data[1 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[1 + 2 * 4] * a->data[2 + 1 * 4] -
a->data[1 + 3 * 4] * a->data[2 + 2 * 4] * a->data[0 + 1 * 4] -
a->data[2 + 3 * 4] * a->data[0 + 2 * 4] * a->data[1 + 1 * 4]);
inv.data[1 + 0 * 4] = -(a->data[1 + 0 * 4] * a->data[2 + 2 * 4] * a->data[3 + 3 * 4] +
a->data[2 + 0 * 4] * a->data[3 + 2 * 4] * a->data[1 + 3 * 4] +
a->data[3 + 0 * 4] * a->data[1 + 2 * 4] * a->data[2 + 3 * 4] -
a->data[1 + 3 * 4] * a->data[2 + 2 * 4] * a->data[3 + 0 * 4] -
a->data[2 + 3 * 4] * a->data[3 + 2 * 4] * a->data[1 + 0 * 4] -
a->data[3 + 3 * 4] * a->data[1 + 2 * 4] * a->data[2 + 0 * 4]);
inv.data[1 + 1 * 4] = (a->data[0 + 0 * 4] * a->data[2 + 2 * 4] * a->data[3 + 3 * 4] +
a->data[2 + 0 * 4] * a->data[3 + 2 * 4] * a->data[0 + 3 * 4] +
a->data[3 + 0 * 4] * a->data[0 + 2 * 4] * a->data[2 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[2 + 2 * 4] * a->data[3 + 0 * 4] -
a->data[2 + 3 * 4] * a->data[3 + 2 * 4] * a->data[0 + 0 * 4] -
a->data[3 + 3 * 4] * a->data[0 + 2 * 4] * a->data[2 + 0 * 4]);
inv.data[1 + 2 * 4] = -(a->data[0 + 0 * 4] * a->data[1 + 2 * 4] * a->data[3 + 3 * 4] +
a->data[1 + 0 * 4] * a->data[3 + 2 * 4] * a->data[0 + 3 * 4] +
a->data[3 + 0 * 4] * a->data[0 + 2 * 4] * a->data[1 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[1 + 2 * 4] * a->data[3 + 0 * 4] -
a->data[1 + 3 * 4] * a->data[3 + 2 * 4] * a->data[0 + 0 * 4] -
a->data[3 + 3 + 4] * a->data[0 + 2 * 4] * a->data[1 + 0 * 4]);
inv.data[1 + 3 * 4] = (a->data[0 + 0 * 4] * a->data[1 + 2 * 4] * a->data[2 + 3 * 4] +
a->data[1 + 0 * 4] * a->data[2 + 2 * 4] * a->data[0 + 3 * 4] +
a->data[2 + 0 * 4] * a->data[0 + 2 * 4] * a->data[1 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[1 + 2 * 4] * a->data[2 + 0 * 4] -
a->data[1 + 3 * 4] * a->data[2 + 2 * 4] * a->data[0 + 0 * 4] -
a->data[2 + 3 * 4] * a->data[0 + 2 * 4] * a->data[1 + 0 * 4]);
inv.data[2 + 0 * 4] = (a->data[1 + 0 * 4] * a->data[2 + 1 * 4] * a->data[3 + 3 * 4] +
a->data[2 + 0 * 4] * a->data[3 + 1 * 4] * a->data[1 + 3 * 4] +
a->data[3 + 0 * 4] * a->data[1 + 1 * 4] * a->data[2 + 3 * 4] -
a->data[1 + 3 * 4] * a->data[2 + 1 * 4] * a->data[3 + 0 * 4] -
a->data[2 + 3 * 4] * a->data[3 + 1 * 4] * a->data[1 + 0 * 4] -
a->data[3 + 3 * 4] * a->data[1 + 1 * 4] * a->data[2 + 0 * 4]);
inv.data[2 + 1 * 4] = -(a->data[0 + 0 * 4] * a->data[2 + 1 * 4] * a->data[3 + 3 * 4] +
a->data[2 + 0 * 4] * a->data[3 + 1 * 4] * a->data[0 + 3 * 4] +
a->data[3 + 0 * 4] * a->data[0 + 1 * 4] * a->data[2 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[2 + 1 * 4] * a->data[3 + 0 * 4] -
a->data[2 + 3 * 4] * a->data[3 + 1 * 4] * a->data[0 + 0 * 4] -
a->data[3 + 3 * 4] * a->data[0 + 1 * 4] * a->data[2 + 0 * 4]);
inv.data[2 + 2 * 4] = (a->data[0 + 0 * 4] * a->data[1 + 1 * 4] * a->data[3 + 3 * 4] +
a->data[1 + 0 * 4] * a->data[3 + 1 * 4] * a->data[0 + 3 * 4] +
a->data[3 + 0 * 4] * a->data[0 + 1 * 4] * a->data[1 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[1 + 1 * 4] * a->data[3 + 0 * 4] -
a->data[1 + 3 * 4] * a->data[3 + 1 * 4] * a->data[0 + 0 * 4] -
a->data[3 + 3 * 4] * a->data[0 + 1 * 4] * a->data[1 + 0 * 4]);
inv.data[2 + 3 * 4] = -(a->data[0 + 0 * 4] * a->data[1 + 1 * 4] * a->data[2 + 3 * 4] +
a->data[1 + 0 * 4] * a->data[2 + 1 * 4] * a->data[0 + 3 * 4] +
a->data[2 + 0 * 4] * a->data[0 + 1 * 4] * a->data[1 + 3 * 4] -
a->data[0 + 3 * 4] * a->data[1 + 1 * 4] * a->data[2 + 0 * 4] -
a->data[1 + 3 * 4] * a->data[2 + 1 * 4] * a->data[0 + 0 * 4] -
a->data[2 + 3 * 4] * a->data[0 + 1 * 4] * a->data[1 + 0 * 4]);
inv.data[3 + 0 * 4] = -(a->data[1 + 0 * 4] * a->data[2 + 1 * 4] * a->data[3 + 2 * 4] +
a->data[2 + 0 * 4] * a->data[3 + 1 * 4] * a->data[1 + 2 * 4] +
a->data[3 + 0 * 4] * a->data[1 + 1 * 4] * a->data[2 + 2 * 4] -
a->data[1 + 2 * 4] * a->data[2 + 1 * 4] * a->data[3 + 0 * 4] -
a->data[2 + 2 * 4] * a->data[3 + 1 * 4] * a->data[1 + 0 * 4] -
a->data[3 + 2 * 4] * a->data[1 + 1 * 4] * a->data[2 + 0 * 4]);
inv.data[3 + 1 * 4] = (a->data[0 + 0 * 4] * a->data[2 + 1 * 4] * a->data[3 + 2 * 4] +
a->data[2 + 0 * 4] * a->data[3 + 1 * 4] * a->data[0 + 2 * 4] +
a->data[3 + 0 * 4] * a->data[0 + 1 * 4] * a->data[2 + 2 * 4] -
a->data[0 + 2 * 4] * a->data[2 + 1 * 4] * a->data[3 + 0 * 4] -
a->data[2 + 2 * 4] * a->data[3 + 1 * 4] * a->data[0 + 0 * 4] -
a->data[3 + 2 * 4] * a->data[0 + 1 * 4] * a->data[2 + 0 * 4]);
inv.data[3 + 2 * 4] = -(a->data[0 + 0 * 4] * a->data[1 + 1 * 4] * a->data[3 + 2 * 4] +
a->data[1 + 0 * 4] * a->data[3 + 1 * 4] * a->data[0 + 2 * 4] +
a->data[3 + 0 * 4] * a->data[0 + 1 * 4] * a->data[1 + 2 * 4] -
a->data[0 + 2 * 4] * a->data[1 + 1 * 4] * a->data[3 + 0 * 4] -
a->data[1 + 2 * 4] * a->data[3 + 1 * 4] * a->data[0 + 0 * 4] -
a->data[3 + 2 * 4] * a->data[0 + 1 * 4] * a->data[1 + 0 * 4]);
inv.data[3 + 3 * 4] = (a->data[0 + 0 * 4] * a->data[1 + 1 * 4] * a->data[2 + 2 * 4] +
a->data[1 + 0 * 4] * a->data[2 + 1 * 4] * a->data[0 + 2 * 4] +
a->data[2 + 0 * 4] * a->data[0 + 1 * 4] * a->data[1 + 2 * 4] -
a->data[0 + 2 * 4] * a->data[1 + 1 * 4] * a->data[2 + 0 * 4] -
a->data[1 + 2 * 4] * a->data[2 + 1 * 4] * a->data[0 + 0 * 4] -
a->data[2 + 2 * 4] * a->data[0 + 1 * 4] * a->data[1 + 0 * 4]);
/** Remember that our matrix is already transposed and we also got the minors(inside each "inv" entry),
so we just use them. We use a->data[0 + 1 * 4] * inv.data[1 + 0 * 4] becouse inv.data is transposed **/
float det = a->data[0 + 0 * 4] * inv.data[0 + 0 * 4] + a->data[0 + 1 * 4] * inv.data[1 + 0 * 4] +
a->data[0 + 2 * 4] * inv.data[2 + 0 * 4] + a->data[0 + 3 * 4] * inv.data[3 + 0 * 4];
if(det == 0.0f)
return *a;
det = 1.0f / det;
for(i = 0; i < 16; i++)
inv.data[i] *= det;
return inv;
}
Mat4 mat4_mul(const Mat4 *a, const Mat4 *b)
{
int i, j, k;
GLfloat sum = 0.0f;
Mat4 c;
mat4_identity(&c);
for(i = 0; i < 4; i++)
for(j = 0; j < 4; j++){
for(k = 0; k < 4; k++){
sum += a->data[i + k * 4] * b->data[k + j * 4];
}
c.data[i + j * 4] = sum;
sum = 0.0f;
}
return c;
}
Mat4 mat4_translate(const Vec3 *a)
{
Mat4 b;
mat4_identity(&b);
b.data[0 + 3 * 4] = a->x;
b.data[1 + 3 * 4] = a->y;
b.data[2 + 3 * 4] = a->z;
return b;
}
Mat4 mat4_scale(GLfloat x, GLfloat y, GLfloat z)
{
Mat4 b;
mat4_identity(&b);
b.data[0 + 0 * 4] = x;
b.data[1 + 1 * 4] = y;
b.data[2 + 2 * 4] = z;
return b;
}
Mat4 mat4_rotate(GLfloat degrees, const Vec3 *a)
{
Mat4 b;
mat4_identity(&b);
GLfloat c = SDL_cosf(toRadians(degrees)), s = SDL_sinf(toRadians(degrees)), o = 1-c;
b.data[0 + 0 * 4] = a->x * a->x * o + c;
b.data[0 + 1 * 4] = a->x * a->y * o - a->z * s;
b.data[0 + 2 * 4] = a->x * a->z * o + a->y * s;
b.data[1 + 0 * 4] = a->x * a->y * o + a->z * s;
b.data[1 + 1 * 4] = a->y * a->y * o + c;
b.data[1 + 2 * 4] = a->y * a->z * o - a->x * s;
b.data[2 + 0 * 4] = a->x * a->z * o - a->y * s;
b.data[2 + 1 * 4] = a->y * a->z * o + a->x * s;
b.data[2 + 2 * 4] = a->z * a->z * o + c;
return b;
}
Mat4 mat4_rotate_x(GLfloat degrees)
{
Mat4 a;
mat4_identity(&a);
GLfloat c = SDL_cosf(toRadians(degrees)), s = SDL_sinf(toRadians(degrees));
a.data[1 + 1 * 4] = c;
a.data[2 + 1 * 4] = s;
a.data[1 + 2 * 4] = -s;
a.data[2 + 2 * 4] = c;
return a;
}
Mat4 mat4_rotate_y(GLfloat degrees)
{
Mat4 a;
mat4_identity(&a);
GLfloat c = SDL_cosf(toRadians(degrees)), s = SDL_sinf(toRadians(degrees));
a.data[0 + 0 * 4] = c;
a.data[2 + 0 * 4] = -s;
a.data[0 + 2 * 4] = s;
a.data[2 + 2 * 4] = c;
return a;
}
Mat4 mat4_rotate_z(GLfloat degrees)
{
Mat4 a;
mat4_identity(&a);
GLfloat c = SDL_cosf(toRadians(degrees)), s = SDL_sinf(toRadians(degrees));
a.data[0 + 0 * 4] = c;
a.data[1 + 0 * 4] = s;
a.data[0 + 1 * 4] = -s;
a.data[1 + 1 * 4] = c;
return a;
}
Mat4 mat4_perspective(GLfloat fov, GLfloat aspect, GLfloat zNear, GLfloat zFar)
{
Mat4 a;
GLubyte i, j;
for(i = 0; i < 4; i++)
for(j = 0; j < 4; j++)
a.data[i + j * 4] = 0.0f;
fov = toRadians(fov); // To radians
a.data[0 + 0 * 4] = ( (1.0f / SDL_tanf( fov/2.0f )) / aspect);
a.data[1 + 1 * 4] = (1.0f / SDL_tanf( fov/2.0f ));
a.data[2 + 2 * 4] = -( (zFar + zNear) / (zFar - zNear) );
a.data[2 + 3 * 4] = -( ( 2.0f * zFar * zNear) / (zFar - zNear) );
a.data[3 + 2 * 4] = -1.0f;
return a;
}
Mat4
mat4_orthographic(GLfloat left, GLfloat right, GLfloat bottom, GLfloat top, GLfloat near, GLfloat far)
{
Mat4 a;
mat4_identity(&a);
a.data[0 + 0 * 4] = 2.0f / (right - left);
a.data[1 + 1 * 4] = 2.0f / (top - bottom);
a.data[2 + 2 * 4] = 2.0f / (near - far);
a.data[0 + 3 * 4] = (left + right) / (left - right);
a.data[1 + 3 * 4] = (bottom + top) / (bottom - top);
a.data[2 + 3 * 4] = (far + near) / (far - near);
return a;
}
Mat4 mat4_lookAt(Vec3 *eye, Vec3 *center, Vec3 *up)
{
Vec3 temp = vec3_sub(center, eye);
Vec3 f = vec3_normalize(&temp);
Vec3 u = vec3_normalize(up);
temp = vec3_cross_mul(&f, &u);
Vec3 s = vec3_normalize(&temp);
u = vec3_cross_mul(&s, &f);
Mat4 a;
mat4_identity(&a);
a.data[0 + 0 * 4] = s.x;
a.data[0 + 1 * 4] = s.y;
a.data[0 + 2 * 4] = s.z;
a.data[1 + 0 * 4] = u.x;
a.data[1 + 1 * 4] = u.y;
a.data[1 + 2 * 4] = u.z;
a.data[2 + 0 * 4] =-f.x;
a.data[2 + 1 * 4] =-f.y;
a.data[2 + 2 * 4] =-f.z;
a.data[0 + 3 * 4] =-vec3_dot_mul(&s, eye);
a.data[1 + 3 * 4] =-vec3_dot_mul(&u, eye);
a.data[2 + 3 * 4] = vec3_dot_mul(&f, eye);
return a;
}
Vec3 mat4_mul_vec3(const Mat4 *a, const Vec3 *b)
{
Vec3 c;
c.x = a->data[0 + 0 * 4] * b->x + a->data[0 + 1 * 4] * b->y + a->data[0 + 2 * 4] * b->z + a->data[0 + 3 * 4];
c.y = a->data[1 + 0 * 4] * b->x + a->data[1 + 1 * 4] * b->y + a->data[1 + 2 * 4] * b->z + a->data[1 + 3 * 4];
c.z = a->data[2 + 0 * 4] * b->x + a->data[2 + 1 * 4] * b->y + a->data[2 + 2 * 4] * b->z + a->data[2 + 3 * 4];
return c;
}
Vec4 mat4_mul_vec4(const Mat4 *a, const Vec4 *b)
{
Vec4 c;
c.x = a->data[0 + 0 * 4] * b->x + a->data[0 + 1 * 4] * b->y + a->data[0 + 2 * 4] * b->z + a->data[0 + 3 * 4] * b->w;
c.y = a->data[1 + 0 * 4] * b->x + a->data[1 + 1 * 4] * b->y + a->data[1 + 2 * 4] * b->z + a->data[1 + 3 * 4] * b->w;
c.z = a->data[2 + 0 * 4] * b->x + a->data[2 + 1 * 4] * b->y + a->data[2 + 2 * 4] * b->z + a->data[2 + 3 * 4] * b->w;
c.w = a->data[3 + 0 * 4] * b->x + a->data[3 + 1 * 4] * b->y + a->data[3 + 2 * 4] * b->z + a->data[3 + 3 * 4] * b->w;
return c;
}
