#include "Matrix.h"

Matrix Matrix::identity()
{
    Matrix res;
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            res[i][j] = i == j ? 1.0 : 0.0;
        }
    }
    return res;
}

double Matrix::determinant()
{
    return m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][3]
         + m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][1]
         + m_matrix[0][0]*m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][2]
         + m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][3]*m_matrix[3][2]

         + m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][3]
         + m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][0]
         + m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][1]*m_matrix[3][3]
         + m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][3]*m_matrix[3][0]

         + m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][1]
         + m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][2]*m_matrix[3][1]
         + m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][2]
         + m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][0]

         - m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][3]*m_matrix[3][2]
         - m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][3]
         - m_matrix[0][0]*m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][1]
         - m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][2]*m_matrix[3][3]

         - m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][0]
         - m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][2]
         - m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][3]*m_matrix[3][1]
         - m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][3]

         - m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][0]
         - m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][1]*m_matrix[3][2]
         - m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][0]
         - m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][1];
}

Matrix operator*(const Matrix & m1, const Matrix & m2)
{
    Matrix res;
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            res[i][j] = m1[i][0] * m2[0][j]
                      + m1[i][1] * m2[1][j]
                      + m1[i][2] * m2[2][j]
                      + m1[i][3] * m2[3][j];
        }
    }
    return res;
}

Vector operator*(const Matrix & m, const Vector & v)
{
    Vector res;
    for (int i = 0; i < 4; i++)
    {
        res[i] = m[i][0] * v[0]
               + m[i][1] * v[1]
               + m[i][2] * v[2]
               + m[i][3]; /* v[3] is implicitly 1.0 */
    }
    return res;
}