mat4x4.h

#pragma once
#include <stdio.h>

typedef float Mat4x4[4][4];
typedef float Vec4x1[4];

extern void Mat4x4Add(Mat4x4 des, Mat4x4 src1, Mat4x4 src2);
extern void Mat4x4Copy(Mat4x4 des, Mat4x4 src);
extern void Mat4x4Mul(Mat4x4 des, Mat4x4 src1, Mat4x4 src2);
extern void Mat4x4MulScalar(Mat4x4 des, Mat4x4 src, float s);
extern void Mat4x4MulVec(Vec4x1 des, Mat4x4 src1, Vec4x1 src2);
extern void Mat4x4Printf(Mat4x4 m, const char* s);
extern void Mat4x4SetI(Mat4x4 m);
extern float Mat4x4Trace(Mat4x4 m);
extern void Mat4x4Transpose(Mat4x4 des, Mat4x4 src);
extern void Vec4x1Printf(Vec4x1 v, const char* s);

inline void Mat4x4SetRow(Mat4x4 m, int row, float a0, float a1, float a2, float a3)
{
    m[row][0] = a0;
    m[row][1] = a1;
    m[row][2] = a2;
    m[row][3] = a3;
}

inline void Vec4x1Set(Vec4x1 v, float a0, float a1, float a2, float a3)
{
    v[0] = a0;
    v[1] = a1;
    v[2] = a2;
    v[3] = a3;
}

mat4x4.cpp

#include "mat4x4.h"

void Mat4x4Add(Mat4x4 des, Mat4x4 src1, Mat4x4 src2)
{
    des[0][0] = src1[0][0] + src2[0][0];
    des[0][1] = src1[0][1] + src2[0][1];
    des[0][2] = src1[0][2] + src2[0][2];
    des[0][3] = src1[0][3] + src2[0][3];

    des[1][0] = src1[1][0] + src2[1][0];
    des[1][1] = src1[1][1] + src2[1][1];
    des[1][2] = src1[1][2] + src2[1][2];
    des[1][3] = src1[1][3] + src2[1][3];

    des[2][0] = src1[2][0] + src2[2][0];
    des[2][1] = src1[2][1] + src2[2][1];
    des[2][2] = src1[2][2] + src2[2][2];
    des[2][3] = src1[2][3] + src2[2][3];

    des[3][0] = src1[3][0] + src2[3][0];
    des[3][1] = src1[3][1] + src2[3][1];
    des[3][2] = src1[3][2] + src2[3][2];
    des[3][3] = src1[3][3] + src2[3][3];
}

void Mat4x4Copy(Mat4x4 des, Mat4x4 src)
{
    des[0][0] = src[0][0];
    des[0][1] = src[0][1];
    des[0][2] = src[0][2];
    des[0][3] = src[0][3];

    des[1][0] = src[1][0];
    des[1][1] = src[1][1];
    des[1][2] = src[1][2];
    des[1][3] = src[1][3];

    des[2][0] = src[2][0];
    des[2][1] = src[2][1];
    des[2][2] = src[2][2];
    des[2][3] = src[2][3];

    des[3][0] = src[3][0];
    des[3][1] = src[3][1];
    des[3][2] = src[3][2];
    des[3][3] = src[3][3];
}

void Mat4x4Mul(Mat4x4 des, Mat4x4 src1, Mat4x4 src2)
{
	__attribute__ ((aligned(32))) Mat4x4 src1_temp, src2_temp;

    if (des == src1)
    {
        Mat4x4Copy(src1_temp, src1);
        src1 = src1_temp;
    }

    if (des == src2)
    {
        Mat4x4Copy(src2_temp, src2);
        src2 = src2_temp;
    }

    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            float sum = 0.0f;

            sum += src1[i][0] * src2[0][j];
            sum += src1[i][1] * src2[1][j];
            sum += src1[i][2] * src2[2][j];
            sum += src1[i][3] * src2[3][j];
            des[i][j] = sum;
        }
    }
}

void Mat4x4MulScalar(Mat4x4 des, Mat4x4 src, float s)
{
    des[0][0] = src[0][0] * s;
    des[0][1] = src[0][1] * s;
    des[0][2] = src[0][2] * s;
    des[0][3] = src[0][3] * s;

    des[1][0] = src[1][0] * s;
    des[1][1] = src[1][1] * s;
    des[1][2] = src[1][2] * s;
    des[1][3] = src[1][3] * s;

    des[2][0] = src[2][0] * s;
    des[2][1] = src[2][1] * s;
    des[2][2] = src[2][2] * s;
    des[2][3] = src[2][3] * s;

    des[3][0] = src[3][0] * s;
    des[3][1] = src[3][1] * s;
    des[3][2] = src[3][2] * s;
    des[3][3] = src[3][3] * s;
}

void Mat4x4MulVec(Vec4x1 des, Mat4x4 src1, Vec4x1 src2)
{
        Vec4x1 des_temp;

        for (int i = 0; i < 4; i++)
        {
            float sum = 0.0f;
            sum += src1[i][0] * src2[0];
            sum += src1[i][1] * src2[1];
            sum += src1[i][2] * src2[2];
            sum += src1[i][3] * src2[3];
            des_temp[i] = sum;
        }

        des[0] = des_temp[0];
        des[1] = des_temp[1];
        des[2] = des_temp[2];
        des[3] = des_temp[3];
}

void Mat4x4Printf(Mat4x4 m, const char* s)
{
    const char* f = "%14.6f %14.6f %14.6f %14.6f\n";
 
    printf("%s", s);
    printf(f, m[0][0], m[0][1], m[0][2], m[0][3]);
    printf(f, m[1][0], m[1][1], m[1][2], m[1][3]);
    printf(f, m[2][0], m[2][1], m[2][2], m[2][3]);
    printf(f, m[3][0], m[3][1], m[3][2], m[3][3]);
}

void Mat4x4SetI(Mat4x4 m)
{
    m[0][0] = 1;
    m[0][1] = 0;
    m[0][2] = 0;
    m[0][3] = 0;

    m[1][0] = 0;
    m[1][1] = 1;
    m[1][2] = 0;
    m[1][3] = 0;

    m[2][0] = 0;
    m[2][1] = 0;
    m[2][2] = 1;
    m[2][3] = 0;

    m[3][0] = 0;
    m[3][1] = 0;
    m[3][2] = 0;
    m[3][3] = 1;
}

float Mat4x4Trace(Mat4x4 x)
{
    return x[0][0] + x[1][1] + x[2][2] + x[3][3];
}

void Mat4x4Transpose(Mat4x4 des, Mat4x4 src)
{
    des[0][0] = src[0][0];
    des[0][1] = src[1][0];
    des[0][2] = src[2][0];
    des[0][3] = src[3][0];

    des[1][0] = src[0][1];
    des[1][1] = src[1][1];
    des[1][2] = src[2][1];
    des[1][3] = src[3][1];

    des[2][0] = src[0][2];
    des[2][1] = src[1][2];
    des[2][2] = src[2][2];
    des[2][3] = src[3][2];

    des[3][0] = src[0][3];
    des[3][1] = src[1][3];
    des[3][2] = src[2][3];
    des[3][3] = src[3][3];
}

void Vec4x1Printf(Vec4x1 v, const char* s)
{
    const char* f = "%14.6f %14.6f %14.6f %14.6f\n";

    printf("%s", s);
    printf(f, v[0], v[1], v[2], v[3]);
}