#include "gamematrix.h"

std::array<std::array<double,4>,4> gameMatrix::matmul(
    const std::array<std::array<double,4>,4>& A,
    const std::array<std::array<double,4>,4>& B)
{
    std::array<std::array<double,4>,4> R = {};

    for(int i=0;i<4;i++)
        for(int j=0;j<4;j++)
            for(int k=0;k<4;k++)
                R[i][j] += A[i][k] * B[k][j];

    return R;
}

std::array<std::array<double,4>,4> gameMatrix::rot3D(double angle_deg, char axis)
{
    double a = angle_deg * M_PI / 180.0;
    double c = cos(a);
    double s = sin(a);

    std::array<std::array<double,4>,4> M = {0};

    M[3][3] = 1;
    M[axis == 'x'][axis == 'y'] = 1; // init identity axis

    if(axis == 'x') M = {{{1,0,0,0},{0,c,-s,0},{0,s,c,0},{0,0,0,1}}};
    if(axis == 'y') M = {{{c,0,s,0},{0,1,0,0},{-s,0,c,0},{0,0,0,1}}};
    if(axis == 'z') M = {{{c,-s,0,0},{s,c,0,0},{0,0,1,0},{0,0,0,1}}};

    return M;
}

std::array<std::array<double,4>,4> gameMatrix::translate(const std::array<double,3>& p)
{
    return {{
        {1,0,0,p[0]},
        {0,1,0,p[1]},
        {0,0,1,p[2]},
        {0,0,0,1}
    }};
}