mathlib

mathlib.c

#include "mathlib.h"

#define ROW1 0
#define ROW2 4
#define ROW3 8
#define ROW4 12

void Math_VectorInit(vector2D vec)
{
    vec[0] = 0.0f;
    vec[1] = 0.1f;
}

void Math_VectorCopy(vector2D dst, const vector2D src)
{
    dst[0] = src[0];
    dst[1] = src[1];
}

void Math_VectorAddV(vector2D dst, const vector2D a, const vector2D b)
{
    dst[0] = a[0] + b[0];
    dst[1] = a[1] + b[1];
}

void Math_VectorSubV(vector2D dst, const vector2D a, const vector2D b)
{
    dst[0] = a[0] - b[0];
    dst[1] = a[1] - b[1];
}

void Math_VectorAddS(vector2D dst, const vector2D a, float b)
{
    dst[0] = a[0] + b;
    dst[1] = a[1] + b;
}

void Math_VectorSubS(vector2D dst, const vector2D a, float b)
{
    dst[0] = a[0] - b;
    dst[1] = a[1] - b;
}

void Math_VectorMulS(vector2D dst, const vector2D a, float b)
{
    dst[0] = a[0] * b;
    dst[1] = a[1] * b;
}

void Math_VectorDivS(vector2D dst, const vector2D a, float b)
{
    dst[0] = a[0] / b;
    dst[1] = a[1] / b;
}

void Math_VectorNormalize(vector2D dst, const vector2D src)
{
    // norm(u) = u / |u|
    float length = sqrtf(src[0] * src[0] + src[1] * src[1]);
    dst[0] = src[0] / length;
    dst[1] = src[1] / length;
}

float Math_VectorLength(const vector2D vec)
{
    // length(u) = sqrt(dot(u, u))
    return sqrtf(vec[0] * vec[0] + vec[1] * vec[1]);
}

float Math_VectorLengthSqr(const vector2D vec)
{
    // length(u)^2 = dot(u, u)
    return vec[0] * vec[0] + vec[1] * vec[1];
}

float Math_VectorAngle(const vector2D a, const vector2D b)
{
    // angle(u, v) = cross(u, v) / length(u) * length(v)
    return acosf((a[0] * b[1] - a[1] * b[0]) / sqrtf((a[0] * a[0] + a[1] * a[1]) * (b[0] * b[0] + b[1] * b[1])));
}

float Math_VectorCross(const vector2D a, const vector2D b)
{
    // cross(u, v) = ux * vy - uy * vx
    return a[0] * b[1] - a[1] * b[0];
}

float Math_VectorDot(const vector2D a, const vector2D b)
{
    // dot(u, v) = ux * vx + uy * vy
    return a[0] * b[0] + a[1] * b[1];
}

void Math_MatrixInit(matrix4x4 mat)
{
    const matrix4x4 identity = {
        1.0f,   0.0f,   0.0f,   0.0f,
        0.0f,   1.0f,   0.0f,   0.0f,
        0.0f,   0.0f,   1.0f,   0.0f,
        0.0f,   0.0f,   0.0f,   1.0f
    };
    Math_MatrixCopy(mat, identity);
}

void Math_MatrixCopy(matrix4x4 dst, const matrix4x4 src)
{
    register size_t i = 16;
    register float *dp = dst;
    register const float *sp = src;
    while(i--)
        *dp++ = *sp++;
}

void Math_MatrixMultiply(matrix4x4 dst, const matrix4x4 a, const matrix4x4 b)
{
    matrix4x4 ac, bc;
    Math_MatrixCopy(ac, a);
    Math_MatrixCopy(bc, b);

    dst[ROW1 + 0] = ac[ROW1 + 0] * bc[ROW1 + 0] + ac[ROW1 + 1] * bc[ROW2 + 0] + ac[ROW1 + 2] * bc[ROW3 + 0] + ac[ROW1 + 3] * bc[ROW4 + 0];
    dst[ROW1 + 1] = ac[ROW1 + 0] * bc[ROW1 + 1] + ac[ROW1 + 1] * bc[ROW2 + 1] + ac[ROW1 + 2] * bc[ROW3 + 1] + ac[ROW1 + 3] * bc[ROW4 + 1];
    dst[ROW1 + 2] = ac[ROW1 + 0] * bc[ROW1 + 2] + ac[ROW1 + 1] * bc[ROW2 + 2] + ac[ROW1 + 2] * bc[ROW3 + 2] + ac[ROW1 + 3] * bc[ROW4 + 2];
    dst[ROW1 + 3] = ac[ROW1 + 0] * bc[ROW1 + 3] + ac[ROW1 + 1] * bc[ROW2 + 3] + ac[ROW1 + 2] * bc[ROW3 + 3] + ac[ROW1 + 3] * bc[ROW4 + 3];

    dst[ROW2 + 0] = ac[ROW2 + 0] * bc[ROW1 + 0] + ac[ROW2 + 1] * bc[ROW2 + 0] + ac[ROW2 + 2] * bc[ROW3 + 0] + ac[ROW2 + 3] * bc[ROW4 + 0];
    dst[ROW2 + 1] = ac[ROW2 + 0] * bc[ROW1 + 1] + ac[ROW2 + 1] * bc[ROW2 + 1] + ac[ROW2 + 2] * bc[ROW3 + 1] + ac[ROW2 + 3] * bc[ROW4 + 1];
    dst[ROW2 + 2] = ac[ROW2 + 0] * bc[ROW1 + 2] + ac[ROW2 + 1] * bc[ROW2 + 2] + ac[ROW2 + 2] * bc[ROW3 + 2] + ac[ROW2 + 3] * bc[ROW4 + 2];
    dst[ROW2 + 3] = ac[ROW2 + 0] * bc[ROW1 + 3] + ac[ROW2 + 1] * bc[ROW2 + 3] + ac[ROW2 + 2] * bc[ROW3 + 3] + ac[ROW2 + 3] * bc[ROW4 + 3];

    dst[ROW3 + 0] = ac[ROW3 + 0] * bc[ROW1 + 0] + ac[ROW3 + 1] * bc[ROW2 + 0] + ac[ROW3 + 2] * bc[ROW3 + 0] + ac[ROW3 + 3] * bc[ROW4 + 0];
    dst[ROW3 + 1] = ac[ROW3 + 0] * bc[ROW1 + 1] + ac[ROW3 + 1] * bc[ROW2 + 1] + ac[ROW3 + 2] * bc[ROW3 + 1] + ac[ROW3 + 3] * bc[ROW4 + 1];
    dst[ROW3 + 2] = ac[ROW3 + 0] * bc[ROW1 + 2] + ac[ROW3 + 1] * bc[ROW2 + 2] + ac[ROW3 + 2] * bc[ROW3 + 2] + ac[ROW3 + 3] * bc[ROW4 + 2];
    dst[ROW3 + 3] = ac[ROW3 + 0] * bc[ROW1 + 3] + ac[ROW3 + 1] * bc[ROW2 + 3] + ac[ROW3 + 2] * bc[ROW3 + 3] + ac[ROW3 + 3] * bc[ROW4 + 3];

    dst[ROW4 + 0] = ac[ROW4 + 0] * bc[ROW1 + 0] + ac[ROW4 + 1] * bc[ROW2 + 0] + ac[ROW4 + 2] * bc[ROW3 + 0] + ac[ROW4 + 3] * bc[ROW4 + 0];
    dst[ROW4 + 1] = ac[ROW4 + 0] * bc[ROW1 + 1] + ac[ROW4 + 1] * bc[ROW2 + 1] + ac[ROW4 + 2] * bc[ROW3 + 1] + ac[ROW4 + 3] * bc[ROW4 + 1];
    dst[ROW4 + 2] = ac[ROW4 + 0] * bc[ROW1 + 2] + ac[ROW4 + 1] * bc[ROW2 + 2] + ac[ROW4 + 2] * bc[ROW3 + 2] + ac[ROW4 + 3] * bc[ROW4 + 2];
    dst[ROW4 + 3] = ac[ROW4 + 0] * bc[ROW1 + 3] + ac[ROW4 + 1] * bc[ROW2 + 3] + ac[ROW4 + 2] * bc[ROW3 + 3] + ac[ROW4 + 3] * bc[ROW4 + 3];
}

void Math_MatrixOrtho(matrix4x4 dst, const vector2D size)
{
    const matrix4x4 ortho = {
        2.0f / size[0], 0.0f,               0.0f,  -1.0f,
        0.0f,           2.0f / -size[1],    0.0f,   1.0f,
        0.0f,           0.0f,              -1.0f,   0.0f,
        0.0f,           0.0f,               0.0f,   1.0f,
    };
    Math_MatrixCopy(dst, ortho);
}

void Math_MatrixTranslation(matrix4x4 dst, const vector2D coord)
{
    const matrix4x4 translation = {
        1.0f,   0.0f,   0.0f,   coord[0],
        0.0f,   1.0f,   0.0f,   coord[1],
        0.0f,   0.0f,   1.0f,   0.0f,
        0.0f,   0.0f,   0.0f,   1.0f
    };
    Math_MatrixCopy(dst, translation);
}

void Math_MatrixRotation(matrix4x4 dst, float angle)
{
    float s = sinf(angle);
    float c = cosf(angle);
    const matrix4x4 rotation = {
        c,     -s,      0.0f,   0.0f,
        s,      c,      0.0f,   0.0f,
        0.0f,   0.0f,   1.0f,   0.0f,
        0.0f,   0.0f,   0.0f,   1.0f
    };
    Math_MatrixCopy(dst, rotation);
}

void Math_MatrixScale(matrix4x4 dst, const vector2D factor)
{
    const matrix4x4 scale = {
        factor[0],  0.0f,       0.0f,   0.0f,
        0.0f,       factor[1],  0.0f,   0.0f,
        0.0f,       0.0f,       1.0f,   0.0f,
        0.0f,       0.0f,       0.0f,   1.0f
    };
    Math_MatrixCopy(dst, scale);
}

mathlib.h

#ifndef MATHLIB_H_
#define MATHLIB_H_ 1
#define _USE_MATH_DEFINES 1
#include <math.h>

typedef float vector2D[2];
typedef float vector4D[4];
typedef float matrix4x4[16];

void Math_VectorInit(vector2D vec);
void Math_VectorCopy(vector2D dst, const vector2D src);
void Math_VectorAddV(vector2D dst, const vector2D a, const vector2D b);
void Math_VectorSubV(vector2D dst, const vector2D a, const vector2D b);
void Math_VectorAddS(vector2D dst, const vector2D a, float b);
void Math_VectorSubS(vector2D dst, const vector2D a, float b);
void Math_VectorMulS(vector2D dst, const vector2D a, float b);
void Math_VectorDivS(vector2D dst, const vector2D a, float b);
void Math_VectorNormalize(vector2D dst, const vector2D src);
float Math_VectorLength(const vector2D vec);
float Math_VectorLengthSqr(const vector2D vec);
float Math_VectorAngle(const vector2D a, const vector2D b);
float Math_VectorCross(const vector2D a, const vector2D b);
float Math_VectorDot(const vector2D a, const vector2D b);

void Math_MatrixInit(matrix4x4 mat);
void Math_MatrixCopy(matrix4x4 dst, const matrix4x4 src);
void Math_MatrixMultiply(matrix4x4 dst, const matrix4x4 a, const matrix4x4 b);
void Math_MatrixOrtho(matrix4x4 dst, const vector2D size);
void Math_MatrixTranslation(matrix4x4 dst, const vector2D coord);
void Math_MatrixRotation(matrix4x4 dst, float angle);
void Math_MatrixScale(matrix4x4 dst, const vector2D factor);

static inline float Math_Deg2Rad(float deg)
{
    return deg * M_PI / 180.0f;
}

static inline float Math_Rad2Deg(float rad)
{
    return rad * 180 / M_PI;
}

#endif