/*
 *   Sweet is a small library for basic math and small matrix operations.
 *   Copyright 2014 Luc Girod.
 *
 * This library is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation, either
 * version 3 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <string.h>
#include <stdio.h>
#include <math.h>
#include "sweet_macro.h"
#include "sweet_matrix.h"
#include "sweet_math.h"
#include "sweet_types.h"

mat2 mat2_new_m3 (mat3 m)
{
    mat2 n;
    n.v[0] = m.v[0];
    n.v[1] = m.v[1];
    n.v[2] = m.v[3];
    n.v[3] = m.v[4];
    return n;
}

mat2 mat2_new_2v (vec2 e1, vec2 e2)
{
    mat2 m;
    m.v[0] = e1.x;
    m.v[1] = e1.y;
    m.v[2] = e2.x;
    m.v[3] = e2.y;
    return m;
}

mat2 mat2_new_3f (float a, float b, float c)
{
    return mat2_new_4f (a, b, c, 0);
}

mat2 mat2_new_4f (float f0, float f1, float f2, float f3)
{
    mat2 n;
    n.v[0] = f0;
    n.v[1] = f1;
    n.v[2] = f2;
    n.v[3] = f3;
    return n;
}

mat3 mat3_new_m4 (mat4 m)
{
    mat3 n;
    n.v[0] = m.v[0];
    n.v[1] = m.v[1];
    n.v[2] = m.v[2];

    n.v[3] = m.v[4];
    n.v[4] = m.v[5];
    n.v[5] = m.v[6];

    n.v[6] = m.v[8];
    n.v[7] = m.v[9];
    n.v[8] = m.v[10];
    return n;
}

mat3 mat3_new_2v (vec2 e1, vec2 e2)
{
    mat3 m;
    m.v[0] = e1.x;
    m.v[1] = e1.y;
    m.v[2] = 0;

    m.v[3] = e2.x;
    m.v[4] = e2.y;
    m.v[5] = 0;

    m.v[6] = 0;
    m.v[7] = 0;
    m.v[8] = 1;
    return m;
}

mat3 mat3_new_3v (vec3 e1, vec3 e2, vec3 e3)
{
    mat3 m;
    m.v[0] = e1.x;
    m.v[1] = e1.y;
    m.v[2] = e1.z;

    m.v[3] = e2.x;
    m.v[4] = e2.y;
    m.v[5] = e2.z;

    m.v[6] = e3.x;
    m.v[7] = e3.y;
    m.v[8] = e3.z;
    return m;
}

mat3 mat3_new_9f(float a0, float a1, float a2, float a3, float a4, float a5, float a6, float a7, float a8)
{
    mat3 m;
    m.v[0] = a0;
    m.v[1] = a1;
    m.v[2] = a2;
    m.v[3] = a3;
    m.v[4] = a4;
    m.v[5] = a5;
    m.v[6] = a6;
    m.v[7] = a7;
    m.v[8] = a8;
    return m;
}

mat4 mat4_new_m4 (mat4 m)
{
    return m;
}

mat4 mat4_new_2v (vec2 e1, vec2 e2)
{
    mat4 m;
    m.v[0] = e1.x;
    m.v[1] = e1.y;
    m.v[2] = 0;
    m.v[3] = 0;

    m.v[4] = e2.x;
    m.v[5] = e2.y;
    m.v[6] = 0;
    m.v[7] = 0;

    m.v[8] = 0;
    m.v[9] = 0;
    m.v[10] = 1;
    m.v[11] = 0;

    m.v[12] = 0;
    m.v[13] = 0;
    m.v[14] = 0;
    m.v[15] = 1;

    return m;
}

mat4 mat4_new_3v (vec3 e1, vec3 e2, vec3 e3)
{
    mat4 m;

    m.v[0] = e1.x;
    m.v[1] = e1.y;
    m.v[2] = e1.z;
    m.v[3] = 0;

    m.v[4] = e2.x;
    m.v[5] = e2.y;
    m.v[6] = e2.z;
    m.v[7] = 0;

    m.v[8] = e3.x;
    m.v[9] = e3.y;
    m.v[10] = e3.z;
    m.v[11] = 0;

    m.v[12] = 0;
    m.v[13] = 0;
    m.v[14] = 0;
    m.v[15] = 1;
    return m;
}

mat4 mat4_new_4v (vec4 e1, vec4 e2, vec4 e3, vec4 e4)
{
    mat4 m;

    m.v[0] = e1.x;
    m.v[1] = e1.y;
    m.v[2] = e1.z;
    m.v[3] = e1.w;

    m.v[4] = e2.x;
    m.v[5] = e2.y;
    m.v[6] = e2.z;
    m.v[7] = e2.w;

    m.v[8] = e3.x;
    m.v[9] = e3.y;
    m.v[10] = e3.z;
    m.v[11] = e3.w;

    m.v[12] = e4.x;
    m.v[13] = e4.y;
    m.v[14] = e4.z;
    m.v[15] = e4.w;

    return m;
}

void mat2_to_str(mat2 * m, char * str, int n)
{
    float *v = m->v;
    snprintf(str, n,
             "%f %f\n"
             "%f %f\n",
             v[0], v[2],
             v[1], v[3]);
}

void mat3_to_str(mat3 * m, char * str, int n)
{
    float *v = m->v;
    snprintf(str, n,
             "%f %f %f\n"
             "%f %f %f\n"
             "%f %f %f\n",
             v[0], v[3], v[6],
             v[1], v[4], v[7],
             v[2], v[5], v[8]);
}

void mat4_to_str(mat4 * m, char * str, int n)
{
    float *v = m->v;
    snprintf(str, n,
             "%f %f %f %f\n"
             "%f %f %f %f\n"
             "%f %f %f %f\n"
             "%f %f %f %f\n",
             v[0], v[4], v[8 ], v[12],
             v[1], v[5], v[9 ], v[13],
             v[2], v[6], v[10], v[14],
             v[3], v[7], v[11], v[15]);
}


mat2
mat2_null (void)
{
    mat2 m;

    m.v[0] = 0.0;
    m.v[1] = 0.0;

    m.v[2] = 0.0;
    m.v[3] = 0.0;

    return m;
}

mat3
mat3_null (void)
{
    mat3 m;

    m.v[0] = 0.0;
    m.v[1] = 0.0;
    m.v[2] = 0.0;

    m.v[3] = 0.0;
    m.v[4] = 0.0;
    m.v[5] = 0.0;

    m.v[6] = 0.0;
    m.v[7] = 0.0;
    m.v[8] = 0.0;

    return m;
}

mat4
mat4_null (void)
{
    mat4 m;

    m.v[0]  = 0.0;
    m.v[1]  = 0.0;
    m.v[2]  = 0.0;
    m.v[3]  = 0.0;

    m.v[4]  = 0.0;
    m.v[5]  = 0.0;
    m.v[6]  = 0.0;
    m.v[7]  = 0.0;

    m.v[8]  = 0.0;
    m.v[9]  = 0.0;
    m.v[10] = 0.0;
    m.v[11] = 0.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = 0.0;
    m.v[15] = 0.0;

    return m;
}

mat2
mat2_identity (void)
{
    mat2 m;

    m.v[0] = 1.0;
    m.v[1] = 0.0;

    m.v[2] = 0.0;
    m.v[3] = 1.0;

    return m;
}

mat3
mat3_identity (void)
{
    mat3 m;

    m.v[0] = 1.0;
    m.v[1] = 0.0;
    m.v[2] = 0.0;

    m.v[3] = 0.0;
    m.v[4] = 1.0;
    m.v[5] = 0.0;

    m.v[6] = 0.0;
    m.v[7] = 0.0;
    m.v[8] = 1.0;

    return m;
}

mat4
mat4_identity (void)
{
    mat4 m;

    m.v[0]  = 1.0;
    m.v[1]  = 0.0;
    m.v[2]  = 0.0;
    m.v[3]  = 0.0;

    m.v[4]  = 0.0;
    m.v[5]  = 1.0;
    m.v[6]  = 0.0;
    m.v[7]  = 0.0;

    m.v[8]  = 0.0;
    m.v[9]  = 0.0;
    m.v[10] = 1.0;
    m.v[11] = 0.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = 0.0;
    m.v[15] = 1.0;

    return m;
}

mat2 
mat2_ortho_basis (vec2 u, vec2 v)
{
    mat2 m;
    m.v[0] = u.x;
    m.v[1] = v.x;

    m.v[2] = u.y;
    m.v[3] = v.y;

    return m;
}

mat3 
mat2h_ortho_basis (vec2 p, vec2 u, vec2 v)
{
    mat3 m;
    m.v[0] = u.x;
    m.v[1] = v.x;
    m.v[2] = 0;

    m.v[3] = u.y;
    m.v[4] = v.y;
    m.v[5] = 0;

    m.v[6] = - p.x * u.x - p.y * u.y;
    m.v[7] = - p.x * v.x - p.y * v.y;
    m.v[8] = 1;

    return m;
}

mat3 
mat3_ortho_basis (vec3 u, vec3 v, vec3 w)
{
    mat3 m;
    m.v[0] = u.x;
    m.v[1] = v.x;
    m.v[2] = w.x;

    m.v[3] = u.y;
    m.v[4] = v.y;
    m.v[5] = w.y;

    m.v[6] = u.z;
    m.v[7] = v.z;
    m.v[8] = w.z;

    return m;
}

mat4
mat3h_ortho_basis (vec3 p, vec3 u, vec3 v, vec3 w)
{
    mat4 m;
    m.v[0] = u.x;
    m.v[1] = v.x;
    m.v[2] = w.x;
    m.v[3] = 0;

    m.v[4] = u.y;
    m.v[5] = v.y;
    m.v[6] = w.y;
    m.v[7] = 0;

    m.v[8] = u.z;
    m.v[9] = v.z;
    m.v[10] = w.z;
    m.v[11] = 0;

    m.v[12] = -p.x * u.x - p.y * u.y - p.z * u.z;
    m.v[13] = -p.x * v.x - p.y * v.y - p.z * v.z;
    m.v[14] = -p.x * w.x - p.y * w.y - p.z * w.z;
    m.v[15] = 1;

    return m;
}

mat4 
mat4_ortho_basis (vec4 u, vec4 v, vec4 w, vec4 t)
{
    mat4 m;
    m.v[0] = u.x;
    m.v[1] = v.x;
    m.v[2] = w.x;
    m.v[3] = t.x;

    m.v[4] = u.y;
    m.v[5] = v.y;
    m.v[6] = w.y;
    m.v[7] = t.y;

    m.v[8]  = u.z;
    m.v[9]  = v.z;
    m.v[10] = w.z;
    m.v[11] = t.z;

    m.v[12] = u.w;
    m.v[13] = v.w;
    m.v[14] = w.w;
    m.v[15] = t.w;

    return m;
}

mat4
matrix_perspective (double fovy, double aspect, double z_near, double z_far)
{
    mat4 m;
    float f;

    fovy = degree_2_radian (fovy);
    f = 1.0 / tan (fovy / 2.0);

    m.v[0] = f / aspect;
    m.v[1] = 0.0;
    m.v[2] = 0.0;
    m.v[3] = 0.0;

    m.v[4] = 0.0;
    m.v[5] = f;
    m.v[6] = 0.0;
    m.v[7] = 0.0;

    m.v[8] = 0.0;
    m.v[9] = 0.0;
    m.v[10] = (z_far + z_near) / (z_near - z_far);
    m.v[11] = -1.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = (2.0 * z_far * z_near) / (z_near - z_far);
    m.v[15] = 0.0;

    return m;
}

mat4
matrix_perspective_infinite (double fovy, double aspect, double z_near)
{
    mat4 m;
    float f;
    float epsilon = 0.0001;

    fovy = degree_2_radian (fovy);
    f = 1.0 / tan (fovy / 2.0);

    m.v[0] = f / aspect;
    m.v[1] = 0.0;
    m.v[2] = 0.0;
    m.v[3] = 0.0;

    m.v[4] = 0.0;
    m.v[5] = f;
    m.v[6] = 0.0;
    m.v[7] = 0.0;

    m.v[8] = 0.0;
    m.v[9] = 0.0;
    m.v[10] = (epsilon - 1.0);
    m.v[11] = -1.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = (epsilon - 2.0) * z_near;
    m.v[15] = 0.0;

    return m;
}

mat4
matrix_frustum (float left,   float right,
                float bottom, float top,
                float z_near, float z_far)
{
    mat4 m;

    m.v[0]  = 2.0 * z_near / (right - left);
    m.v[1]  = 0.0;
    m.v[2]  = 0.0;
    m.v[3]  = 0.0;

    m.v[4]  = 0.0;
    m.v[5]  = 2.0 * z_near / (top - bottom);
    m.v[6]  = 0.0;
    m.v[7]  = 0.0;

    m.v[8]  = (right + left) / (right - left);
    m.v[9]  = (top + bottom) / (top - bottom);
    m.v[10] = (z_far + z_near) / (z_near - z_far);
    m.v[11] = -1.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = (2 * z_far * z_near) / (z_near - z_far);
    m.v[15] = 0.0;

    return m;
}

mat4
matrix_ortho (float left, float right,
              float bottom, float top,
              float zNear, float zFar)
{
    mat4 m;

    m.v[0] = 2/(right - left);
    m.v[1] = 0;
    m.v[2] = 0;
    m.v[3] = 0;
    m.v[4] = 0;
    m.v[5] = 2/(top - bottom);
    m.v[6] = 0;
    m.v[7] = 0;
    m.v[8] = 0;
    m.v[9] = 0;
    m.v[10] = -2 / (zFar - zNear);
    m.v[11] = 0;
    m.v[12] = - (right + left) / (right - left);
    m.v[13] = - (top + bottom) / (top - bottom);
    m.v[14] = - (zFar + zNear) / (zFar - zNear);
    m.v[15] = 1;

    return m;
}

mat4
matrix_look_at (vec3 pos, vec3 dir, vec3 up)
{
    mat4 m;
    vec3 side;
    side = cross (dir, up);
    side = vec3_normalize (side);
    up = cross (side, dir);
    up = vec3_normalize (up);

    m.v[0] = side.x;
    m.v[1] = up.x;
    m.v[2] = -dir.x;
    m.v[3] = 0;

    m.v[4] = side.y;
    m.v[5] = up.y;
    m.v[6] = -dir.y;
    m.v[7] = 0;

    m.v[8] = side.z;
    m.v[9] = up.z;
    m.v[10] = -dir.z;
    m.v[11] = 0;

    m.v[12] = -pos.x * side.x - pos.y * side.y - pos.z * side.z;
    m.v[13] = -pos.x * up.x - pos.y * up.y - pos.z * up.z;
    m.v[14] = pos.x * dir.x + pos.y * dir.y + pos.z * dir.z;
    m.v[15] = 1;

    return m;
}

mat4
matrix_look_at_ortho (vec3 pos, vec3 dir, vec3 up, vec3 right)
{
    dir = vec3_inverse (dir);
    return mat3h_ortho_basis (pos, right, up, dir);
}

quat
mat3_to_quaternion (mat3 * m)
{
    quat q;
    q.w = 0.5 * sqrt(1 + m->v[0] * m->v[0] + m->v[4] * m->v[4] + m->v[8] * m->v[8]);
    q.x = (m->v[7] - m->v[5]) / (4 * q.w);
    q.y = (m->v[2] - m->v[6]) / (4 * q.w);
    q.z = (m->v[3] - m->v[1]) / (4 * q.w);
    return q;
}

mat3
mat3_quat_rotation (quat q)
{
    mat3 m;

    m.v[0] = - 2 * q.y * q.y - 2 * q.z * q.z + 1;
    m.v[1] =   2 * q.x * q.y + 2 * q.w * q.z;
    m.v[2] =   2 * q.x * q.z - 2 * q.w * q.y;

    m.v[3] =   2 * q.x * q.y - 2 * q.w * q.z;
    m.v[4] = - 2 * q.x * q.x - 2 * q.z * q.z + 1;
    m.v[5] =   2 * q.y * q.z + 2 * q.w * q.x;

    m.v[6] =   2 * q.x * q.z + 2 * q.w * q.y;
    m.v[7] =   2 * q.y * q.z - 2 * q.w * q.x;
    m.v[8] = - 2 * q.x * q.x - 2 * q.y * q.y + 1;

    return m;
}

mat4
mat3h_quat_rotation (quat q)
{
    mat4 m;

    m.v[0]  = - 2 * q.y * q.y - 2 * q.z * q.z + 1;
    m.v[1]  =   2 * q.x * q.y + 2 * q.w * q.z;
    m.v[2]  =   2 * q.x * q.z - 2 * q.w * q.y;
    m.v[3]  =   0;

    m.v[4]  =   2 * q.x * q.y - 2 * q.w * q.z;
    m.v[5]  = - 2 * q.x * q.x - 2 * q.z * q.z + 1;
    m.v[6]  =   2 * q.y * q.z + 2 * q.w * q.x;
    m.v[7]  =   0;

    m.v[8]  =   2 * q.x * q.z + 2 * q.w * q.y;
    m.v[9]  =   2 * q.y * q.z - 2 * q.w * q.x;
    m.v[10] = - 2 * q.x * q.x - 2 * q.y * q.y + 1;
    m.v[11] =   0;

    m.v[12] =   0;
    m.v[13] =   0;
    m.v[14] =   0;
    m.v[15] =   1;

    return m;
}


mat2
mat2_rotation (float angle)
{
    mat2 m;

    m.v[0] = cos (angle);
    m.v[1] = sin (angle);
    m.v[2] = -sin (angle);
    m.v[3] = cos (angle);

    return m;
}

mat3
mat2h_rotation (float angle)
{
    mat3 m;

    m.v[0] = cos (angle);
    m.v[1] = sin (angle);
    m.v[2] = 0.0;
    m.v[3] = -sin (angle);
    m.v[4] = cos (angle);
    m.v[5] = 0.0;
    m.v[6] = 0.0;
    m.v[7] = 0.0;
    m.v[8] = 1.0;

    return m;
}

mat3
mat3_rotation (float angle, float x, float y, float z)
{
    mat3 m;

    m.v[0] = x * x * (1 - cos (angle)) + cos (angle);
    m.v[1] = x * y * (1 - cos (angle)) + z * sin (angle);
    m.v[2] = x * z * (1 - cos (angle)) - y * sin (angle);

    m.v[3] = x * y * (1 - cos (angle)) - z * sin (angle);
    m.v[4] = y * y * (1 - cos (angle)) + cos (angle);
    m.v[5] = y * z * (1 - cos (angle)) + x * sin (angle);

    m.v[6] = x * z * (1 - cos (angle)) + y * sin (angle);
    m.v[7] = y * z * (1 - cos (angle)) - x * sin (angle);
    m.v[8] = z * z * (1 - cos (angle)) + cos (angle);

    return m;
}

mat4
mat3h_rotation (float angle, float x, float y, float z)
{
    mat4 m;

    m.v[0] = x * x * (1 - cos (angle)) + cos (angle);
    m.v[1] = x * y * (1 - cos (angle)) + z * sin (angle);
    m.v[2] = x * z * (1 - cos (angle)) - y * sin (angle);
    m.v[3] = 0;

    m.v[4] = x * y * (1 - cos (angle)) - z * sin (angle);
    m.v[5] = y * y * (1 - cos (angle)) + cos (angle);
    m.v[6] = y * z * (1 - cos (angle)) + x * sin (angle);
    m.v[7] = 0;

    m.v[8] = x * z * (1 - cos (angle)) + y * sin (angle);
    m.v[9] = y * z * (1 - cos (angle)) - x * sin (angle);
    m.v[10] = z * z * (1 - cos (angle)) + cos (angle);
    m.v[11] = 0;

    m.v[12] = 0;
    m.v[13] = 0;
    m.v[14] = 0;
    m.v[15] = 1;

    return m;
}

mat3
mat2h_translation (vec2 v)
{
    mat3 m;

    m.v[0] = 1.0;
    m.v[1] = 0.0;
    m.v[2] = 0.0;

    m.v[3] = 0.0;
    m.v[4] = 1.0;
    m.v[5] = 0.0;

    m.v[6] = v.x;
    m.v[7] = v.y;
    m.v[8] = 1.0;

    return m;
}

mat4
mat3h_translation (vec3 v)
{
    mat4 m;

    m.v[0] = 1.0;
    m.v[1] = 0.0;
    m.v[2] = 0.0;
    m.v[3] = 0.0;

    m.v[4] = 0.0;
    m.v[5] = 1.0;
    m.v[6] = 0.0;
    m.v[7] = 0.0;

    m.v[8] = 0.0;
    m.v[9] = 0.0;
    m.v[10] = 1.0;
    m.v[11] = 0.0;

    m.v[12] = v.x;
    m.v[13] = v.y;
    m.v[14] = v.z;
    m.v[15] = 1.0;

    return m;
}

mat2
mat2_scale (float x, float y)
{
    mat2 m;

    m.v[0] = x;
    m.v[1] = 0;

    m.v[2] = 0;
    m.v[3] = y;

    return m;
}

mat3
mat3_scale (float x, float y, float z)
{
    mat3 m;

    m.v[0] = x;
    m.v[1] = 0;
    m.v[2] = 0;

    m.v[3] = 0;
    m.v[4] = y;
    m.v[5] = 0;

    m.v[6] = 0;
    m.v[7] = 0;
    m.v[8] = z;

    return m;
}

mat4
mat4_scale (float x, float y, float z, float w)
{
    mat4 m;

    m.v[0] = x;
    m.v[1] = 0;
    m.v[2] = 0;
    m.v[3] = 0;

    m.v[4] = 0;
    m.v[5] = y;
    m.v[6] = 0;
    m.v[7] = 0;

    m.v[8] = 0;
    m.v[9] = 0;
    m.v[10] = z;
    m.v[11] = 0;

    m.v[12] = 0;
    m.v[13] = 0;
    m.v[14] = 0;
    m.v[15] = w;

    return m;
}

mat4
matrix_texture_bias (void)
{
    mat4 m;
    m.v[0] = 0.5;
    m.v[1] = 0.0;
    m.v[2] = 0.0;
    m.v[3] = 0.0;

    m.v[4] = 0.0;
    m.v[5] = 0.5;
    m.v[6] = 0.0;
    m.v[7] = 0.0;

    m.v[8] = 0.0;
    m.v[9] = 0.0;
    m.v[10] = 0.5;
    m.v[11] = 0.0;

    m.v[12] = 0.5;
    m.v[13] = 0.5;
    m.v[14] = 0.5;
    m.v[15] = 1.0;

    return m;
}

void
mat2_transpose (mat2 * mt, mat2 * m)
{
    float mv1 = m->v[1];
    mt->v[0] = m->v[0];
    mt->v[1] = m->v[2];
    mt->v[2] = mv1;
    mt->v[3] = m->v[3];
}

void
mat3_transpose (mat3 * mt, mat3 * m)
{
    float mv1 = m->v[1];
    float mv2 = m->v[2];
    float mv5 = m->v[5];
    mt->v[0] = m->v[0];
    mt->v[1] = m->v[3];
    mt->v[2] = m->v[6];
    mt->v[3] = mv1;
    mt->v[4] = m->v[4];
    mt->v[5] = m->v[7];
    mt->v[6] = mv2;
    mt->v[7] = mv5;
    mt->v[8] = m->v[8];
}

void
mat4_transpose (mat4 * mt, mat4 * m)
{
    float mv2 = m->v[2];
    float mv3 = m->v[3];
    float mv6 = m->v[6];
    float mv7 = m->v[7];
    float mv11 = m->v[11];

    mt->v[0] = m->v[0];
    mt->v[1] = m->v[4];
    mt->v[2] = m->v[8];
    mt->v[3] = m->v[12];
    mt->v[4] = mv2;
    mt->v[5] = m->v[5];
    mt->v[6] = m->v[9];
    mt->v[7] = m->v[13];
    mt->v[8] = mv2;
    mt->v[9] = mv6;
    mt->v[10] = m->v[10];
    mt->v[11] = m->v[14];
    mt->v[12] = mv3;
    mt->v[13] = mv7;
    mt->v[14] = mv11;
    mt->v[15] = m->v[15];
}

float
mat2_det (mat2 * m)
{
    return m->v[0] * m->v[3] - m->v[1] * m->v[2];
}

float
mat3_det (mat3 * m)
{
    float det =
        m->v[0] * (m->v[4] * m->v[8] - m->v[5] * m->v[7]) -
        m->v[1] * (m->v[3] * m->v[8] - m->v[5] * m->v[6]) +
        m->v[2] * (m->v[3] * m->v[7] - m->v[4] * m->v[6]);

    return det;
}

float
mat4_det (mat4 * m)
{
    float det =
         m->v[0] * (m->v[ 5] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
                    m->v[ 6] * (m->v[ 9] * m->v[15] - m->v[11] * m->v[13]) +
                    m->v[ 7] * (m->v[ 9] * m->v[14] - m->v[10] * m->v[13]))
        -m->v[1] * (m->v[ 4] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
                    m->v[ 6] * (m->v[ 8] * m->v[15] - m->v[11] * m->v[12]) +
                    m->v[ 7] * (m->v[ 8] * m->v[14] - m->v[10] * m->v[12]))
        +m->v[2] * (m->v[ 4] * (m->v[ 9] * m->v[15] - m->v[11] * m->v[13]) -
                    m->v[ 5] * (m->v[ 8] * m->v[15] - m->v[11] * m->v[12]) +
                    m->v[ 7] * (m->v[ 8] * m->v[13] - m->v[ 9] * m->v[12]))
        -m->v[3] * (m->v[ 4] * (m->v[ 9] * m->v[14] - m->v[10] * m->v[13]) -
                    m->v[ 5] * (m->v[ 8] * m->v[14] - m->v[10] * m->v[12]) +
                    m->v[ 6] * (m->v[ 8] * m->v[13] - m->v[ 9] * m->v[12]));

    return det;
}

void
mat2_inverse (mat2 * inv, mat2 * m)
{
    float invdet = 1.0 / mat2_det (m);

    float mv0 = m->v[0];
    inv->v[0] = m->v[3] * invdet;
    inv->v[1] = m->v[1] * (-invdet);
    inv->v[2] = m->v[2] * (-invdet);
    inv->v[3] = mv0 * invdet;
}

void
mat3_inverse (mat3 * inv, mat3 * m)
{
    float mv0 = m->v[0];
    float mv1 = m->v[1];
    float mv2 = m->v[2];
    float mv3 = m->v[3];
    float mv4 = m->v[4];
    float mv6 = m->v[6];
    float invdet;

    invdet = 1.0 / mat3_det (m);

    inv->v[0] = ((m->v[4] * m->v[8]) - (m->v[5] * m->v[7])) * invdet;
    inv->v[1] = (-(m->v[1] * m->v[8]) + (m->v[2] * m->v[7])) * invdet;
    inv->v[2] = ((mv1 * m->v[5]) - (m->v[2] * m->v[4])) * invdet;

    inv->v[3] = (-(m->v[3] * m->v[8]) + (m->v[5] * m->v[6])) * invdet;
    inv->v[4] = ((mv0 * m->v[8]) - (mv2 * m->v[6])) * invdet;
    inv->v[5] = (-(mv0 * m->v[5]) + (mv2 * mv3)) * invdet;

    inv->v[6] = ((mv3 * m->v[7]) - (mv4 * m->v[6])) * invdet;
    inv->v[7] = (-(mv0 * m->v[7]) + (mv1 * mv6)) * invdet;
    inv->v[8] = ((mv0 * mv4) - (mv1 * mv3)) * invdet;
}

static void
matrix_inverse4 (mat4 * inv, mat4 * m)
{
    float mv0 = m->v[0];
    float mv1 = m->v[1];
    float mv2 = m->v[2];
    float mv3 = m->v[3];
    float mv4 = m->v[4];
    float mv5 = m->v[5];
    float mv6 = m->v[6];
    float mv7 = m->v[7];
    float mv8 = m->v[8];
    float mv9 = m->v[9];
    float mv10 = m->v[10];
    float mv12 = m->v[12];
    float mv13 = m->v[13];
    float invdet = 1.0 / mat4_det (m);

    inv->v[0] = m->v[5] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
              m->v[6] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) +
              m->v[7] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]);

    inv->v[1] = -m->v[1] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) +
              m->v[2] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) -
              m->v[3] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]);

    inv->v[2] = mv1 * (m->v[6] * m->v[15] - m->v[7] * m->v[14]) -
              m->v[2] * (m->v[5] * m->v[15] - m->v[7] * m->v[13]) +
              m->v[3] * (m->v[5] * m->v[14] - m->v[6] * m->v[13]);

    inv->v[3] = -mv1 * (m->v[6] * m->v[11] - m->v[7] * m->v[10]) +
              mv2 * (m->v[5] * m->v[11] - m->v[7] * m->v[9]) -
              m->v[3] * (m->v[5] * m->v[10] - m->v[6] * m->v[9]);

    inv->v[4] = -m->v[4] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) +
              m->v[6] * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) -
              m->v[7] * (m->v[8] * m->v[14] - m->v[10] * m->v[12]);

    inv->v[5] = mv0 * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
              mv2 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) +
              mv3 * (m->v[8] * m->v[14] - m->v[10] * m->v[12]);

    inv->v[6] = -mv0 * (m->v[6] * m->v[15] - m->v[7] * m->v[14]) +
              mv2 * (mv4 * m->v[15] - m->v[7] * m->v[12]) -
              mv3 * (mv4 * m->v[14] - m->v[6] * m->v[12]);

    inv->v[7] = mv0 * (mv6 * m->v[11] - m->v[7] * m->v[10]) -
              mv2 * (mv4 * m->v[11] - m->v[7] * m->v[8]) +
              mv3 * (mv4 * m->v[10] - mv6 * m->v[8]);

    inv->v[8] = mv4 * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) -
              mv5 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) +
              mv7 * (m->v[8] * m->v[13] - m->v[9] * m->v[12]);

    inv->v[9] = -mv0 * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) +
              mv1 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) -
              mv3 * (m->v[8] * m->v[13] - m->v[9] * m->v[12]);

    inv->v[10] = mv0 * (mv5 * m->v[15] - mv7 * m->v[13]) -
               mv1 * (mv4 * m->v[15] - mv7 * m->v[12]) +
               mv3 * (mv4 * m->v[13] - mv5 * m->v[12]);

    inv->v[11] = -mv0 * (mv5 * m->v[11] - mv7 * mv9) +
               mv1 * (mv4 * m->v[11] - mv7 * mv8) -
               mv3 * (mv4 * mv9 - mv5 * mv8);

    inv->v[12] = -mv4 * (mv9 * m->v[14] - mv10 * m->v[13]) +
               mv5 * (mv8 * m->v[14] - mv10 * m->v[12]) -
               mv6 * (mv8 * m->v[13] - mv9 * m->v[12]);

    inv->v[13] = mv0 * (mv9 * m->v[14] - mv10 * m->v[13]) -
               mv1 * (mv8 * m->v[14] - mv10 * mv12) +
               mv2 * (mv8 * m->v[13] - mv9 * mv12);

    inv->v[14] = -mv0 * (mv5 * m->v[14] - mv6 * mv13) +
               mv1 * (mv4 * m->v[14] - mv6 * mv12) -
               mv2 * (mv4 * mv13 - mv5 * mv12);

    inv->v[15] = mv0 * (mv5 * mv10 - mv6 * mv9) -
               mv1 * (mv4 * mv10 - mv6 * mv8) +
               mv2 * (mv4 * mv9 - mv5 * mv8);

    inv->v[ 0] *= invdet;
    inv->v[ 1] *= invdet;
    inv->v[ 2] *= invdet;
    inv->v[ 3] *= invdet;
    inv->v[ 4] *= invdet;
    inv->v[ 5] *= invdet;
    inv->v[ 6] *= invdet;
    inv->v[ 7] *= invdet;
    inv->v[ 8] *= invdet;
    inv->v[ 9] *= invdet;
    inv->v[10] *= invdet;
    inv->v[11] *= invdet;
    inv->v[12] *= invdet;
    inv->v[13] *= invdet;
    inv->v[14] *= invdet;
    inv->v[15] *= invdet;
}

void
mat4_inverse (mat4 * inv, mat4 * m)
{
    float detp;
    float invdet;
    mat2 rp;
    mat2 pq;
    mat2 rpq;
    mat2 s;
    mat2 p;

    detp = m->v[0] * m->v[5] - m->v[1] * m->v[4];

    if (float_approx_zero (detp, 0.0000001))
    {
        /* In this case the standard algorithm is used */
        matrix_inverse4 (inv, m);
        return;
    }

    /* In this case we can use a better approach */
    invdet = 1.0 / detp;

    p.v[0] = m->v[5] * invdet;
    p.v[1] = -m->v[1] * invdet;
    p.v[2] = -m->v[4] * invdet;
    p.v[3] = m->v[0] * invdet;

    rp.v[0] = (p.v[0] * m->v[2] + p.v[1] * m->v[6]);
    rp.v[1] = (p.v[0] * m->v[3] + p.v[1] * m->v[7]);
    rp.v[2] = (p.v[2] * m->v[2] + p.v[3] * m->v[6]);
    rp.v[3] = (p.v[2] * m->v[3] + p.v[3] * m->v[7]);

    pq.v[0] = (p.v[0] * m->v[8] + p.v[2] * m->v[9]);
    pq.v[1] = (p.v[1] * m->v[8] + p.v[3] * m->v[9]);
    pq.v[2] = (p.v[0] * m->v[12] + p.v[2] * m->v[13]);
    pq.v[3] = (p.v[1] * m->v[12] + p.v[3] * m->v[13]);

    rpq.v[0] = rp.v[0] * m->v[8] + rp.v[2] * m->v[9];
    rpq.v[1] = rp.v[1] * m->v[8] + rp.v[3] * m->v[9];
    rpq.v[2] = rp.v[0] * m->v[12] + rp.v[2] * m->v[13];
    rpq.v[3] = rp.v[1] * m->v[12] + rp.v[3] * m->v[13];

    s.v[0] = m->v[10] - rpq.v[0];
    s.v[1] = m->v[11] - rpq.v[1];
    s.v[2] = m->v[14] - rpq.v[2];
    s.v[3] = m->v[15] - rpq.v[3];

    invdet = 1.0 / (s.v[0] * s.v[3] - s.v[1] * s.v[2]);

    inv->v[10] = s.v[3] * invdet;
    inv->v[11] = - s.v[1] * invdet;
    inv->v[14] = - s.v[2] * invdet;
    inv->v[15] = s.v[0] * invdet;

    inv->v[2] = -inv->v[10] * rp.v[0] - m->v[14] * rp.v[1];
    inv->v[3] = -inv->v[11] * rp.v[0] - m->v[15] * rp.v[1];
    inv->v[6] = -inv->v[10] * rp.v[2] - m->v[14] * rp.v[3];
    inv->v[7] = -inv->v[11] * rp.v[2] - m->v[15] * rp.v[3];

    inv->v[8] = -pq.v[0] * inv->v[10] - pq.v[2] * inv->v[11];
    inv->v[9] = -pq.v[1] * inv->v[10] - pq.v[3] * inv->v[11];
    inv->v[12] = -pq.v[0] * inv->v[14] - pq.v[2] * inv->v[15];
    inv->v[13] = -pq.v[1] * inv->v[14] - pq.v[3] * inv->v[15];

    inv->v[0] = p.v[0] - (pq.v[0] * inv->v[2] + pq.v[2] * inv->v[3]);
    inv->v[1] = p.v[1] - (pq.v[1] * inv->v[2] + pq.v[3] * inv->v[3]);
    inv->v[4] = p.v[2] - (pq.v[0] * inv->v[6] + pq.v[2] * inv->v[7]);
    inv->v[5] = p.v[3] - (pq.v[1] * inv->v[6] + pq.v[3] * inv->v[7]);
}

void
mat2_product (mat2 * p, mat2 * a, mat2 * b)
{
    float av0 = a->v[0];
    float av1 = a->v[1];
    float bv0 = b->v[0];
    float bv2 = b->v[2];
    p->v[0] = a->v[0] * b->v[0] + a->v[2] * b->v[1];
    p->v[1] = a->v[1] * bv0 + a->v[3] * b->v[1];
    p->v[2] = av0 * b->v[2] + a->v[2] * b->v[3];
    p->v[3] = av1 * bv2 + a->v[3] * b->v[3];
}

void
mat3_product (mat3 * p, mat3 * a, mat3 * b)
{
    float bv0 = b->v[0];
    float bv1 = b->v[1];
    float bv3 = b->v[3];
    float bv4 = b->v[4];
    float bv6 = b->v[6];
    float bv7 = b->v[7];
    float av0 = a->v[0];
    float av1 = a->v[1];
    float av2 = a->v[2];
    float av3 = a->v[3];
    float av4 = a->v[4];
    float av5 = a->v[5];

    p->v[0] = a->v[0] * b->v[0] + a->v[3] * b->v[1] + a->v[6] * b->v[2];
    p->v[1] = a->v[1] * bv0 + a->v[4] * b->v[1] + a->v[7] * b->v[2];
    p->v[2] = a->v[2] * bv0 + a->v[5] * bv1 + a->v[8] * b->v[2];

    p->v[3] = av0 * b->v[3] + a->v[3] * b->v[4] + a->v[6] * b->v[5];
    p->v[4] = av1 * bv3 + a->v[4] * b->v[4] + a->v[7] * b->v[5];
    p->v[5] = av2 * bv3 + a->v[5] * bv4 + a->v[8] * b->v[5];

    p->v[6] = av0 * b->v[6] + av3 * b->v[7] + a->v[6] * b->v[8];
    p->v[7] = av1 * bv6 + av4 * b->v[7] + a->v[7] * b->v[8];
    p->v[8] = av2 * bv6 + av5 * bv7 + a->v[8] * b->v[8];
}

void
mat4_product (mat4 * p, mat4 * a, mat4 * b)
{
    float bv0 = b->v[0];
    float bv1 = b->v[1];
    float bv2 = b->v[2];
    float bv4 = b->v[4];
    float bv5 = b->v[5];
    float bv6 = b->v[6];
    float bv8 = b->v[8];
    float bv9 = b->v[9];
    float bv10 = b->v[10];
    float bv12 = b->v[12];
    float bv13 = b->v[13];
    float bv14 = b->v[14];
    float av0 = a->v[0];
    float av1 = a->v[1];
    float av2 = a->v[2];
    float av3 = a->v[3];
    float av4 = a->v[4];
    float av5 = a->v[5];
    float av6 = a->v[6];
    float av7 = a->v[7];
    float av8 = a->v[8];
    float av9 = a->v[9];
    float av10 = a->v[10];
    float av11 = a->v[11];

    p->v[0] = a->v[0] * b->v[0] + a->v[4] * b->v[1] + a->v[8] * b->v[2] + a->v[12] * b->v[3];
    p->v[1] = a->v[1] * bv0 + a->v[5] * b->v[1] + a->v[9] * b->v[2] + a->v[13] * b->v[3];
    p->v[2] = a->v[2] * bv0 + a->v[6] * bv1 + a->v[10] * b->v[2] + a->v[14] * b->v[3];
    p->v[3] = a->v[3] * bv0 + a->v[7] * bv1 + a->v[11] * bv2 + a->v[15] * b->v[3];

    p->v[4] = av0 * b->v[4] + a->v[4] * b->v[5] + a->v[8] * b->v[6] + a->v[12] * b->v[7];
    p->v[5] = av1 * b->v[4] + a->v[5] * b->v[5] + a->v[9] * b->v[6] + a->v[13] * b->v[7];
    p->v[6] = av2 * bv4 + a->v[6] * bv5 + a->v[10] * b->v[6] + a->v[14] * b->v[7];
    p->v[7] = av3 * bv4 + a->v[7] * bv5 + a->v[11] * bv6 + a->v[15] * b->v[7];

    p->v[8] = av0 * b->v[8] + av4 * b->v[9] + a->v[8] * b->v[10] + a->v[12] * b->v[11];
    p->v[9] = av1 * bv8 + av5 * b->v[9] + a->v[9] * b->v[10] + a->v[13] * b->v[11];
    p->v[10] = av2 * bv8 + av6 * bv9 + a->v[10] * b->v[10] + a->v[14] * b->v[11];
    p->v[11] = av3 * bv8 + av7 * bv9 + a->v[11] * bv10 + a->v[15] * b->v[11];

    p->v[12] = av0 * b->v[12] + av4 * b->v[13] + av8 * b->v[14] + a->v[12] * b->v[15];
    p->v[13] = av1 * bv12 + av5 * b->v[13] + av9 * b->v[14] + a->v[13] * b->v[15];
    p->v[14] = av2 * bv12 + av6 * bv13 + av10 * b->v[14] + a->v[14] * b->v[15];
    p->v[15] = av3 * bv12 + av7 * bv13 + av11 * bv14 + a->v[15] * b->v[15];
}

void
mat3_add_diag (mat3 * out, mat3 * in, float x, float y, float z)
{
    out->v[0] = in->v[0] + x;
    out->v[1] = in->v[1];
    out->v[2] = in->v[2];
    out->v[3] = in->v[3];
    out->v[4] = in->v[4] + y;
    out->v[5] = in->v[5];
    out->v[6] = in->v[6];
    out->v[6] = in->v[7];
    out->v[8] = in->v[8] + z;
}

void
mat3_substract_diag (mat3 * out, mat3 * in, float x, float y, float z)
{
    out->v[0] = in->v[0] - x;
    out->v[1] = in->v[1];
    out->v[2] = in->v[2];
    out->v[3] = in->v[3];
    out->v[4] = in->v[4] - y;
    out->v[5] = in->v[5];
    out->v[6] = in->v[6];
    out->v[6] = in->v[7];
    out->v[8] = in->v[8] - z;
}

void
mat3_add (mat3 * out, mat3 * m1, mat3 * m2)
{
    out->v[0] = m1->v[0] + m2->v[0];
    out->v[1] = m1->v[1] + m2->v[1];
    out->v[2] = m1->v[2] + m2->v[2];
    out->v[3] = m1->v[3] + m2->v[3];
    out->v[4] = m1->v[4] + m2->v[4];
    out->v[5] = m1->v[5] + m2->v[5];
    out->v[6] = m1->v[6] + m2->v[6];
    out->v[7] = m1->v[7] + m2->v[7];
    out->v[8] = m1->v[8] + m2->v[8];
}

void
mat3_substract (mat3 * out, mat3 * m1, mat3 * m2)
{
    out->v[0] = m1->v[0] - m2->v[0];
    out->v[1] = m1->v[1] - m2->v[1];
    out->v[2] = m1->v[2] - m2->v[2];
    out->v[3] = m1->v[3] - m2->v[3];
    out->v[4] = m1->v[4] - m2->v[4];
    out->v[5] = m1->v[5] - m2->v[5];
    out->v[6] = m1->v[6] - m2->v[6];
    out->v[7] = m1->v[7] - m2->v[7];
    out->v[8] = m1->v[8] - m2->v[8];
}

vec2
mat2_column (mat2 * m, int idx)
{
    vec2 v;
    idx = idx * 2;
    v.x = m->v[idx];
    v.y = m->v[idx+1];

    return v;
}

vec3
mat3_column (mat3 * m, int idx)
{
    vec3 v;
    idx = idx * 3;
    v.x = m->v[idx];
    v.y = m->v[idx+1];
    v.z = m->v[idx+2];

    return v;
}

vec4
mat4_column (mat4 * m, int idx)
{
    vec4 v;
    idx = idx * 4;
    v.x = m->v[idx];
    v.y = m->v[idx+1];
    v.z = m->v[idx+2];
    v.w = m->v[idx+3];

    return v;
}

void mat2_insert_col_1v (mat2 * m, int col, vec2 v)
{
    m->v[col * 2] = v.x;
    m->v[col * 2 + 1] = v.y;
}

void mat2_insert_col_2f (mat2 * m, int col, float x, float y)
{
    m->v[col * 2] = x;
    m->v[col * 2 + 1] = y;
}

void mat3_insert_col_1v (mat3 * m, int col, vec3 v)
{
    m->v[col*3] = v.x;
    m->v[col*3 + 1] = v.y;
    m->v[col*3 + 2] = v.z;
}

void mat3_insert_col_1v1f (mat3 * m, int col, vec2 v, float z)
{
    m->v[col*3] = v.x;
    m->v[col*3 + 1] = v.y;
    m->v[col*3 + 2] = z;
}

void mat3_insert_col_3f (mat3 * m, int col, float x, float y, float z)
{
    m->v[col*3] = x;
    m->v[col*3 + 1] = y;
    m->v[col*3 + 2] = z;
}

void mat4_insert_col_1v (mat4 * m, int col, vec4 v)
{
    m->v[col * 4] = v.x;
    m->v[col * 4 + 1] = v.y;
    m->v[col * 4 + 2] = v.z;
    m->v[col * 4 + 3] = v.w;
}

void mat4_insert_col_1v1f (mat4 * m, int col, vec3 v, float w)
{
    m->v[col * 4] = v.x;
    m->v[col * 4 + 1] = v.y;
    m->v[col * 4 + 2] = v.z;
    m->v[col * 4 + 3] = w;
}

void mat4_insert_col_1v2f (mat4 * m, int col, vec2 v, float z, float w)
{
    m->v[col * 4] = v.x;
    m->v[col * 4 + 1] = v.y;
    m->v[col * 4 + 2] = z;
    m->v[col * 4 + 3] = w;
}

void mat4_insert_col_4f (mat4 * m, int col, float x, float y, float z, float w)
{
    m->v[col * 4] = x;
    m->v[col * 4 + 1] = y;
    m->v[col * 4 + 2] = z;
    m->v[col * 4 + 3] = w;
}


vec2
mat2_mult (mat2 * mat, vec2 vec)
{
    vec2 result;

    result.x = mat->v[0] * vec.x + mat->v[2] * vec.y;
    result.y = mat->v[1] * vec.x + mat->v[3] * vec.y;

    return result;
}

vec3
mat3_mult (mat3 * mat, vec3 vec)
{
    vec3 r;

    r.x = (mat->v[0] * vec.x) + (mat->v[3] * vec.y) + (mat->v[6] * vec.z);
    r.y = (mat->v[1] * vec.x) + (mat->v[4] * vec.y) + (mat->v[7] * vec.z);
    r.z = (mat->v[2] * vec.x) + (mat->v[5] * vec.y) + (mat->v[8] * vec.z);

    return r;
}

vec4
mat4_mult (mat4 * mat, vec4 vec)
{
    vec4 result;

    result.x = (mat->v[0] * vec.x) + (mat->v[4] * vec.y) + (mat->v[8] * vec.z) + (mat->v[12] * vec.w);
    result.y = (mat->v[1] * vec.x) + (mat->v[5] * vec.y) + (mat->v[9] * vec.z) + (mat->v[13] * vec.w);
    result.z = (mat->v[2] * vec.x) + (mat->v[6] * vec.y) + (mat->v[10] * vec.z) + (mat->v[14] * vec.w);
    result.w = (mat->v[3] * vec.x) + (mat->v[7] * vec.y) + (mat->v[11] * vec.z) + (mat->v[15] * vec.w);

    return result;
}

int
mat3_eigen_values (float * h1, float * h2, float * h3, mat3 * m)
{
    float a;
    float b;
    float c;
    int n;
    vec3 v = vec3_zero ();

    a = m->v[0] + m->v[4] + m->v[8];

    b = m->v[1] * m->v[3] + m->v[2] * m->v[6] + m->v[7] * m->v[5] -
        m->v[0] * m->v[4] - m->v[0] * m->v[8] - m->v[4] * m->v[8];

    c = m->v[0] * (m->v[4] * m->v[8] - m->v[5] * m->v[7]) -
        m->v[1] * (m->v[3] * m->v[8] - m->v[5] * m->v[6]) +
        m->v[2] * (m->v[3] * m->v[7] - m->v[4] * m->v[6]);

    n = cubic_polynomial (&v, -1, a, b, c);
    if (v.y > v.x)
    {
        float t = v.x; v.x = v.y; v.y = t;
    }
    if (v.z > v.y)
    {
        float t = v.y; v.y = v.z; v.z = t;
    } 
    if (v.y > v.x)
    {
        float t = v.x; v.x = v.y; v.y = t;
    }

    *h1 = v.x;
    *h2 = v.y;
    *h3 = v.z;
    return n;
}

/**
|a b c x|
|d e f y|
|g h i z|
*/

/**/
static int find_pivot (float * max, mat3 * a, int k)
{
    int i_max;

    if (k == 0)
    {
        *max = fabs (a->v[k]); i_max = 0;
        if (fabs (a->v[k+1]) > *max) { *max = fabs (a->v[k+1]); i_max = 1;}
        if (fabs (a->v[k+2]) > *max) { *max = fabs (a->v[k+2]); i_max = 2;}
    }
    else if (k == 1)
    {
        k = 4;
        *max = fabs (a->v[k]); i_max = 1;
        if (fabs (a->v[k+1]) > *max) { *max = fabs (a->v[k+1]); i_max = 2;}
    }
    else
    {
        k = 8;
        *max = fabs (a->v[k]);
        i_max = 2;
    }
    return i_max;
}

static void swap_rows (mat3 * a, float * s, int l1, int l2)
{
    float tmp;
    if (l1 == l2) { return; }

    /* swap left part */
    tmp = s[l1];
    s[l1] = s[l2];
    s[l2] = tmp;

    /* swap first column */
    tmp = a->v[l1];
    a->v[l1] = a->v[l2];
    a->v[l2] = tmp;

    /* swap second column */
    l1 += 3;
    l2 += 3;
    tmp = a->v[l1];
    a->v[l1] = a->v[l2];
    a->v[l2] = tmp;

    /* swap third column */
    l1 += 3;
    l2 += 3;
    tmp = a->v[l1];
    a->v[l1] = a->v[l2];
    a->v[l2] = tmp;
}

static void normalize_row (mat3 * a, float * s, int l, int i)
{
    float d = a->v[l+i*3];
    a->v[l+i*3] = 1.0;

    s[l] = s[l] / d;
    if (i <= 1) { a->v[l+6] = a->v[l+6] / d; }
    if (i == 0) { a->v[l+3] = a->v[l+3] / d; }
}

static void substract_row (mat3 * a, float * s, int l1, int l2, int i)
{
    float r;
    if (i == 0)
    {
        r = - a->v[l2];
        a->v[l2] = 0;
        a->v[l2+3] = a->v[l2+3] + a->v[l1+3] * r;
        a->v[l2+6] = a->v[l2+6] + a->v[l1+6] * r;
    }
    else if (i == 1)
    {
        r = - a->v[l2+3];
        a->v[l2+3] = 0;
        a->v[l2+6] = a->v[l2+6] + a->v[l1+6] * r;
    }
    else if (i == 2)
    {
        r = - a->v[l2+6];
        a->v[l2+6] = 0;
    }

    s[l2] = s[l2] + s[l1] * r;

}

int
mat3_gaussian_elimination (mat3 * a, vec3 * v)
{
    float s[3];
    float max;
    int i_max;

    s[0] = v->x;
    s[1] = v->y;
    s[2] = v->z;

    i_max = find_pivot (&max, a, 0);
    /* If matrix is singular return error */
    if (float_approx_zero (max, 1e-05f)) { return -1; }
    swap_rows (a, s, 0, i_max);
    normalize_row (a, s, 0, 0);
    substract_row (a, s, 0, 1, 0);
    substract_row (a, s, 0, 2, 0);

    i_max = find_pivot (&max, a, 1);
    /* If matrix is singular return error */
    if (float_approx_zero (max, 1e-05f)) { return -1; }
    swap_rows (a, s, 1, i_max);
    normalize_row (a, s, 1, 1);
    substract_row (a, s, 1, 2, 1);

    /* If matrix is singular return error */
    if (float_approx_zero (a->v[8], 1e-05f)) { return -1; }
    normalize_row (a, s, 2, 2);

    v->x = s[0];
    v->y = s[1];
    v->z = s[2];
    return 0;
}

int
mat3_solve (mat3 * m, vec3 * v)
{
    mat3 a = *m;
    if (mat3_gaussian_elimination (&a, v) < 0) { return -1; }

    v->y = v->y - v->z * a.v[7];
    v->x = v->x - v->z * a.v[6] - v->y * a.v[3];
    return 0;
}

static void build_Q0 (mat3 * q, mat3 * m)
{
    float a = sqrt (m->v[0] * m->v[0] + m->v[1] * m->v[1] + m->v[2] * m->v[2]);
    if (m->v[0] > 0) { a = - a; }

    vec3 v = vec3_new (m->v[0] - a, m->v[1], m->v[2]);
    float norm = vec3_norm (v);
    if (norm > 0)
        v = vec3_scale (v, 1.0 / norm);

    q->v[0] = 1 - 2 * v.x * v.x;
    q->v[1] =   - 2 * v.y * v.x;
    q->v[2] =   - 2 * v.z * v.x;

    q->v[3] =   - 2 * v.x * v.y;
    q->v[4] = 1 - 2 * v.y * v.y;
    q->v[5] =   - 2 * v.z * v.y;

    q->v[6] =   - 2 * v.x * v.z;
    q->v[7] =   - 2 * v.y * v.z;
    q->v[8] = 1 - 2 * v.z * v.z;
}

static void build_Q1 (mat3 * q, mat3 * m)
{ 
    float a = sqrt (m->v[4] * m->v[4] + m->v[5] * m->v[5]);
    if (m->v[4] > 0) { a = -a; }

    vec2 v = vec2_new (m->v[4] - a, m->v[5]);
    float norm = vec2_norm (v);
    if (norm > 0)
        v = vec2_scale (v, 1.0 / norm);

    q->v[0] = 1;
    q->v[1] = 0;
    q->v[2] = 0;
    q->v[3] = 0;
    q->v[4] = 1 - 2 * v.x * v.x;
    q->v[5] =   - 2 * v.y * v.x;
    q->v[6] = 0;
    q->v[7] =   - 2 * v.x * v.y;
    q->v[8] = 1 - 2 * v.y * v.y;
}

void mat3_QR (mat3 * Q, mat3 * R, mat3 * m)
{
    mat3 tmp;
    mat3 Q0;
    mat3 Q1;

    build_Q0 (&Q0, m);
    mat3_product (&tmp, &Q0, m);
    build_Q1 (&Q1, &tmp);
    mat3_product (Q, &Q0, &Q1);

    mat3_transpose (&tmp, Q);
    mat3_product (R, &tmp, m);
}

void mat3_hess (mat3 * H, mat3 * Q, mat3 * m)
{
    mat3 tmp;

    float a = sqrt (m->v[1] * m->v[1] + m->v[2] * m->v[2]);
    if (m->v[1] > 0)
        a = - a;

    vec2 v = vec2_new (m->v[1] - a, m->v[2]);
    float norm = vec2_norm (v);
    if (norm > 0)
        v = vec2_scale (v, 1.0/norm);

    Q->v[0] = 1;
    Q->v[1] = 0;
    Q->v[2] = 0;
    Q->v[3] = 0;
    Q->v[4] = 1 - 2 * v.x * v.x;
    Q->v[5] =   - 2 * v.x * v.y;
    Q->v[6] = 0;
    Q->v[7] =   - 2 * v.x * v.y;
    Q->v[8] = 1 - 2 * v.y * v.y;

    mat3_product (&tmp, Q, m);
    mat3_product (H, &tmp, Q);
}

static void shift (mat3 * m, float s)
{
    m->v[0] = m->v[0] + s;
    m->v[4] = m->v[4] + s;
    m->v[8] = m->v[8] + s;
}


static void sort3 (float * val, vec3 * vec)
{
    if (val[1] > val[0])
    {
        float t = val[0];
        vec3 v = vec[0];
        val[0] = val[1];
        val[1] = t;
        vec[0] = vec[1];
        vec[1] = v;
    }
    if (val[2] > val[1])
    {
        float t = val[1];
        vec3 v = vec[1];
        val[1] = val[2];
        val[2] = t;
        vec[1] = vec[2];
        vec[2] = v;    
    }
    if (val[1] > val[0])
    { 
        float t = val[0];
        vec3 v = vec[0];
        val[0] = val[1];
        val[1] = t;
        vec[0] = vec[1];
        vec[1] = v;
    }
}

void mat3_eigen_vectors (vec3 * eigen_vectors, float * eigen_values, mat3 * A, int it)
{
    int i;
    mat3 H, S, Q, W, R;
    mat3_hess (&H, &W, A);

    float s = H.v[8];
    shift (&H, -H.v[8]);

    mat3_QR (&Q, &R, &H);
    mat3_product (&H, &R, &Q);
    S = Q;
    for (i = 0; i < it; i++)
    {
        mat3_QR (&Q, &R, &H);
        mat3_product (&H, &R, &Q);
        mat3_product (&S, &S, &Q);
    }

    shift (&H, s);
    mat3_product (&S, &W, &S);

    eigen_values[0] = H.v[0];
    eigen_values[1] = H.v[4];
    eigen_values[2] = H.v[8];
    eigen_vectors[0] = vec3_new (S.v[0], S.v[1], S.v[2]);
    eigen_vectors[1] = vec3_new (S.v[3], S.v[4], S.v[5]);
    eigen_vectors[2] = vec3_new (S.v[6], S.v[7], S.v[8]);
    sort3 (eigen_values, eigen_vectors);
}