#include "gamematrix.h"

namespace gameMatrix {

    Mat4 matmul(const Mat4& A, const Mat4& B)
    {
        Mat4 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;
    }

    Mat4 rot3D(double angle_deg, char axis)
    {
        double a = angle_deg * M_PI / 180.0;
        double c = cos(a);
        double s = sin(a);

        Mat4 M = {0};
        for (int i=0;i<4;i++) M[i][i] = 1;

        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;
    }

    Mat4 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}
        }};
    }

}