glmath

#ifndef GLMATH
#define GLMATH

#include <cmath>

struct vec2 {
    float x, y;

    vec2() : x(0), y(0) {}
    vec2(float x, float y) : x(x), y(y) {}

    float magnitude() {
        return sqrtf(dot(*this));
    }

    vec2 normalize() {
        float l = magnitude();
        return l != 0 ? (*this / l) : vec2();
    }

    float dot(const vec2& a) {
        return x * a.x + y * a.y;
    }

    const float& operator[] (int index) const {
        return (&x)[index];
    }

    float& operator[] (int index) {
        return (&x)[index];
    }

    vec2 operator-() const {
        return vec2(-x, -y);
    }

    vec2 operator-(const vec2& a) const {
        return vec2(x - a.x, y - a.y);
    }

    vec2 operator+(const vec2& a) const {
        return vec2(x + a.x, y + a.y);
    }

    vec2 operator*(const float &a) const {
        return vec2(x * a, y * a);
    }

    friend vec2 operator*(const float &a, const vec2 &b) {
        return b * a;
    }

    vec2 operator/(const float &a) {
        return operator*(1.0 / a);
    }
};


struct vec3 {
    float x, y, z;

    vec3() : x(0), y(0), z(0) {}
    vec3(vec2 v, float z) : x(v.x), y(v.y), z(z) {}
    vec3(float x, float y, float z) : x(x), y(y), z(z) {}

    float magnitude() {
        return sqrtf(dot(*this));
    }

    vec3 normalize() {
        float l = magnitude();
        return l != 0 ? (*this / l) : vec3();
    }

    float dot(const vec3& a) {
        return x * a.x + y * a.y + z * a.z;
    }

    vec3 cross(const vec3& a) {
        return vec3(
            y * a.z - z * a.y,
            z * a.x - x * a.z,
            x * a.y - y * a.x
        );
    }

    const float& operator[] (int index) const {
        return (&x)[index];
    }

    float& operator[] (int index) {
        return (&x)[index];
    }

    vec3 operator-() const {
        return vec3(-x, -y, -z);
    }

    vec3 operator-(const vec3& a) const {
        return vec3(x - a.x, y - a.y, z - a.z);
    }

    vec3 operator+(const vec3& a) const {
        return vec3(x + a.x, y + a.y, z + a.z);
    }

    vec3 operator*(const float &a) const {
        return vec3(x * a, y * a, z * a);
    }

    friend vec3 operator*(const float &a, const vec3 &b) {
        return b * a;
    }

    vec3 operator/(const float &a) {
        return operator*(1.0 / a);
    }
};

struct vec4 {
    float x, y, z, w;

    vec4() : x(0), y(0), z(0), w(0) {}
    vec4(vec3 v, float w = 0) : x(v.x), y(v.y), z(v.z), w(w) {}
    vec4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}

    float magnitude() {
        return sqrtf(dot(*this));
    }

    vec4 normalize() {
        float l = magnitude();
        return l ? vec4(x / l, y / l, z / l, w / l) : vec4(0, 0, 0, 0);
    }

    float dot(const vec4& a) {
        return x * a.x + y * a.y + z * a.z + w * a.w;
    }

    const float& operator[] (int index) const {
        return (&x)[index];
    }

    float& operator[] (int index) {
        return (&x)[index];
    }

    vec4 operator-() const {
        return vec4(-x, -y, -z, -w);
    }

    vec4 operator-(const vec4& a) const {
        return vec4(x - a.x, y - a.y, z - a.z, w - a.w);
    }

    vec4 operator+(const vec4& a) const {
        return vec4(x + a.x, y + a.y, z + a.z, w + a.w);
    }

    vec4 operator*(const float &a) const {
        return vec4(x * a, y * a, z * a, w * a);
    }

    friend vec4 operator*(const float &a, const vec4 &b) {
        return b * a;
    }

    vec4 operator/(const float &a) const {
        return operator*(1.0 / a);
    }
};

struct mat3 {
    vec3 x, y, z;

    mat3() : x(1, 0, 0), y(0, 1, 0), z(0, 0, 1) {}
    mat3(vec3 x, vec3 y, vec3 z) : x(x), y(y), z(z) {}

    const vec3& operator[](int index) const {
        return (&x)[index];
    }

    vec3& operator[](int index) {
        return (&x)[index];
    }

    friend vec3 operator*(const vec3& v, const mat3& m) {
        return vec3(
            v[0] * m[0][0] + v[1] * m[0][1] + v[2] * m[0][2],
            v[0] * m[1][0] + v[1] * m[1][1] + v[2] * m[1][2],
            v[0] * m[2][0] + v[1] * m[2][1] + v[2] * m[2][2]
        );
    }

    friend vec3 operator*(const mat3& m, const vec3& v) {
        return vec3(
            m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2],
            m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2],
            m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2]
        );
    }
};

struct mat4 {
    vec4 x, y, z, w;

    mat4() : x(1, 0, 0, 0), y(0, 1, 0, 0), z(0, 0, 1, 0), w(0, 0, 0, 1) {}
    mat4(const mat3 &m, vec4 w = vec4(0, 0, 0, 1)) : x(m.x, 0), y(m.y, 0), z(m.z, 0), w(w) {}
    mat4(vec4 x, vec4 y, vec4 z, vec4 w) : x(x), y(y), z(z), w(w) {}

    const vec4& operator[](int index) const {
        return (&x)[index];
    }

    vec4& operator[](int index) {
        return (&x)[index];
    }

    static mat4 translate(float x, float y, float z) {
        return {
            {1, 0, 0, 0},
            {0, 1, 0, 0},
            {0, 0, 1, 0},
            {x, y, z, 1}
        };
    }

    static mat4 translate(const vec3 &position) {
        return {
            { 1, 0, 0, 0},
            { 0, 1, 0, 0},
            { 0, 0, 1, 0},
            {position, 1}
        };
    }

    static mat4 rotate(float y, float p, float r) {
        float cy = cos(y);
        float sy = sin(y);

        float cp = cos(p);
        float sp = sin(p);

        float cr = cos(r);
        float sr = sin(r);

        // return mat4 (
        //  {sr * sy * sp + cr * cp, cr * sy * sp - sr * cp, -cy * sp, 0},
        //  {               sr * cp,                cr * cp,       sy, 0},
        //  {sr * sy * cp - cr * sp, sy * cp * cr + sp * sr, -cy * cp, 0},
        //  {                     0,                      0,        0, 1}
        // );

        mat4 rotateX = {
            {1,  0,   0, 0},
            {0, cy, -sy, 0},
            {0, sy,  cy, 0},
            {0,  0,   0, 1}
        };

        mat4 rotateY = {
            { cp, 0, sp, 0},
            {  0, 1,  0, 0},
            {-sp, 0, cp, 0},
            {  0, 0,  0, 1}
        };

        mat4 rotateZ = {
            {cr, -sr, 0, 0},
            {sr,  cr, 0, 0},
            { 0,   0, 1, 0},
            { 0,   0, 0, 1}
        };

        return rotateZ * rotateY * rotateX;
    }

    static mat4 scale(float x, float y, float z) {
        return {
            {x, 0, 0, 0},
            {0, y, 0, 0},
            {0, 0, z, 0},
            {0, 0, 0, 1}
        };
    }

    static mat4 scale(const vec3 &scale) {
        return mat4::scale(scale.x, scale.y, scale.z);
    }

    friend vec4 operator*(const vec4& v, const mat4& m) {
        return vec4(
            v[0] * m[0][0] + v[1] * m[0][1] + v[2] * m[0][2] + v[3] * m[0][3],
            v[0] * m[1][0] + v[1] * m[1][1] + v[2] * m[1][2] + v[3] * m[1][3],
            v[0] * m[2][0] + v[1] * m[2][1] + v[2] * m[2][2] + v[3] * m[2][3],
            v[0] * m[3][0] + v[1] * m[3][1] + v[2] * m[3][2] + v[3] * m[3][3]
        );
    }

    friend vec4 operator*(const mat4& m, const vec4& v) {
        return vec4(
            m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2] + m[3][0] * v[3],
            m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2] + m[3][1] * v[3],
            m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2] + m[3][2] * v[3],
            m[0][3] * v[0] + m[1][3] * v[1] + m[2][3] * v[2] + m[3][3] * v[3]
        );
    }

    friend mat4 operator*(const mat4& m1, const mat4& m2) {
        vec4 X = m1 * m2[0];
        vec4 Y = m1 * m2[1];
        vec4 Z = m1 * m2[2];
        vec4 W = m1 * m2[3];
        return mat4(X, Y, Z, W);
    }

    static mat4 lookat(vec3 position, vec3 target) {
        vec3 d = (target - position).normalize();

        return rotate(atan2(d.x, d.y), asin(d.y), 0) * mat4(
            vec4(1, 0, 0, 0),
            vec4(0, 1, 0, 0),
            vec4(0, 0, 1, 0),
            vec4(-position, 1)
        );
    }

    static mat4 orthographic(float size, float aspect, float near, float far) {
        float f = 2.0f / size;
        float a = 2.0f * far * near / (near - far);
        float b = (near + far) / (near - far);

        return mat4(
            vec4(f/aspect, 0, 0, 0),
            vec4(       0, f, 0, 0),
            vec4(       0, 0, a, b),
            vec4(       0, 0, 0, 1)
        );
    }

    static mat4 perspective(float fov, float aspect, float near, float far) {
        float f = 1.0f / tanf(fov * M_PI / 360.0);

        float a = (near + far) / (near - far);
        float b = 2.0f * far / (near - far);

        return mat4(
            vec4(f/aspect, 0,  0, 0),
            vec4(       0, f,  0, 0),
            vec4(       0, 0,  a, b),
            vec4(       0, 0, -1, 0)
        );
    }
};

#endif