\m/

Matrix4x4.h

#ifndef MATRIX4X4_H
#define MATRIX4X4_H

#include <iostream>
#include <iomanip>
#include <cmath>
#include <array>

class Matrix4x4 {
private:
    std::array<std::array<float, 4>, 4> data;

public:
    // Constructors
    Matrix4x4();  // Default constructor (identity matrix)
    Matrix4x4(float value);  // Creates matrix with all elements set to value
    Matrix4x4(const std::array<std::array<float, 4>, 4>& other);
    
    // Access operators
    std::array<float, 4>& operator[](size_t row);
    const std::array<float, 4>& operator[](size_t row) const;
    
    // Matrix operations
    Matrix4x4 operator+(const Matrix4x4& other) const;
    Matrix4x4 operator-(const Matrix4x4& other) const;
    Matrix4x4 operator*(const Matrix4x4& other) const;
    Matrix4x4 operator*(float scalar) const;
    
    // Assignment operators
    Matrix4x4& operator+=(const Matrix4x4& other);
    Matrix4x4& operator-=(const Matrix4x4& other);
    Matrix4x4& operator*=(const Matrix4x4& other);
    Matrix4x4& operator*=(float scalar);
    
    // Comparison operators
    bool operator==(const Matrix4x4& other) const;
    bool operator!=(const Matrix4x4& other) const;
    
    // Utility functions
    void setIdentity();
    void setZero();
    Matrix4x4 transpose() const;
    float determinant() const;
    Matrix4x4 inverse() const;
    bool isIdentity() const;
    bool isInvertible() const;
    
    // Static factory methods
    static Matrix4x4 identity();
    static Matrix4x4 zero();
    static Matrix4x4 rotationX(float angle);  // Angle in radians
    static Matrix4x4 rotationY(float angle);
    static Matrix4x4 rotationZ(float angle);
    static Matrix4x4 rotation(float angle, float x, float y, float z);
    static Matrix4x4 translation(float x, float y, float z);
    static Matrix4x4 scale(float x, float y, float z);
    static Matrix4x4 perspective(float fov, float aspect, float near, float far);
    static Matrix4x4 lookAt(const std::array<float, 3>& eye,
                           const std::array<float, 3>& target,
                           const std::array<float, 3>& up);
    
    // Print function
    void print() const;
    friend std::ostream& operator<<(std::ostream& os, const Matrix4x4& matrix);
};

// Implementation
inline Matrix4x4::Matrix4x4() {
    setIdentity();
}

inline Matrix4x4::Matrix4x4(float value) {
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            data[i][j] = value;
        }
    }
}

inline Matrix4x4::Matrix4x4(const std::array<std::array<float, 4>, 4>& other) {
    data = other;
}

inline std::array<float, 4>& Matrix4x4::operator[](size_t row) {
    return data[row];
}

inline const std::array<float, 4>& Matrix4x4::operator[](size_t row) const {
    return data[row];
}

inline Matrix4x4 Matrix4x4::operator+(const Matrix4x4& other) const {
    Matrix4x4 result;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            result[i][j] = data[i][j] + other[i][j];
        }
    }
    return result;
}

inline Matrix4x4 Matrix4x4::operator-(const Matrix4x4& other) const {
    Matrix4x4 result;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            result[i][j] = data[i][j] - other[i][j];
        }
    }
    return result;
}

inline Matrix4x4 Matrix4x4::operator*(const Matrix4x4& other) const {
    Matrix4x4 result;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            result[i][j] = 0.0f;
            for (int k = 0; k < 4; ++k) {
                result[i][j] += data[i][k] * other[k][j];
            }
        }
    }
    return result;
}

inline Matrix4x4 Matrix4x4::operator*(float scalar) const {
    Matrix4x4 result;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            result[i][j] = data[i][j] * scalar;
        }
    }
    return result;
}

inline Matrix4x4& Matrix4x4::operator+=(const Matrix4x4& other) {
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            data[i][j] += other[i][j];
        }
    }
    return *this;
}

inline Matrix4x4& Matrix4x4::operator-=(const Matrix4x4& other) {
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            data[i][j] -= other[i][j];
        }
    }
    return *this;
}

inline Matrix4x4& Matrix4x4::operator*=(const Matrix4x4& other) {
    *this = *this * other;
    return *this;
}

inline Matrix4x4& Matrix4x4::operator*=(float scalar) {
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            data[i][j] *= scalar;
        }
    }
    return *this;
}

inline bool Matrix4x4::operator==(const Matrix4x4& other) const {
    const float epsilon = 1e-6f;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            if (std::abs(data[i][j] - other[i][j]) > epsilon) {
                return false;
            }
        }
    }
    return true;
}

inline bool Matrix4x4::operator!=(const Matrix4x4& other) const {
    return !(*this == other);
}

inline void Matrix4x4::setIdentity() {
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            data[i][j] = (i == j) ? 1.0f : 0.0f;
        }
    }
}

inline void Matrix4x4::setZero() {
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            data[i][j] = 0.0f;
        }
    }
}

inline Matrix4x4 Matrix4x4::transpose() const {
    Matrix4x4 result;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            result[i][j] = data[j][i];
        }
    }
    return result;
}

inline float Matrix4x4::determinant() const {
    // Using cofactor expansion along first row
    float det = 0.0f;
    
    for (int c = 0; c < 4; ++c) {
        // Create 3x3 submatrix by removing row 0 and column c
        std::array<std::array<float, 3>, 3> submatrix;
        int si = 0;
        for (int i = 1; i < 4; ++i) {
            int sj = 0;
            for (int j = 0; j < 4; ++j) {
                if (j != c) {
                    submatrix[si][sj] = data[i][j];
                    sj++;
                }
            }
            si++;
        }
        
        // Calculate 3x3 determinant
        float subdet = 
            submatrix[0][0] * (submatrix[1][1] * submatrix[2][2] - submatrix[1][2] * submatrix[2][1]) -
            submatrix[0][1] * (submatrix[1][0] * submatrix[2][2] - submatrix[1][2] * submatrix[2][0]) +
            submatrix[0][2] * (submatrix[1][0] * submatrix[2][1] - submatrix[1][1] * submatrix[2][0]);
        
        det += data[0][c] * subdet * ((c % 2) ? -1.0f : 1.0f);
    }
    
    return det;
}

inline Matrix4x4 Matrix4x4::inverse() const {
    if (!isInvertible()) {
        throw std::runtime_error("Matrix is not invertible");
    }
    
    // Calculate adjugate matrix
    Matrix4x4 adjoint;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            // Create 3x3 submatrix by removing row i and column j
            std::array<std::array<float, 3>, 3> submatrix;
            int si = 0;
            for (int ki = 0; ki < 4; ++ki) {
                if (ki != i) {
                    int sj = 0;
                    for (int kj = 0; kj < 4; ++kj) {
                        if (kj != j) {
                            submatrix[si][sj] = data[ki][kj];
                            sj++;
                        }
                    }
                    si++;
                }
            }
            
            // Calculate 3x3 determinant
            float det = 
                submatrix[0][0] * (submatrix[1][1] * submatrix[2][2] - submatrix[1][2] * submatrix[2][1]) -
                submatrix[0][1] * (submatrix[1][0] * submatrix[2][2] - submatrix[1][2] * submatrix[2][0]) +
                submatrix[0][2] * (submatrix[1][0] * submatrix[2][1] - submatrix[1][1] * submatrix[2][0]);
            
            // Apply cofactor sign
            adjoint[j][i] = det * ((i + j) % 2 ? -1.0f : 1.0f);
        }
    }
    
    return adjoint * (1.0f / determinant());
}

inline bool Matrix4x4::isIdentity() const {
    const float epsilon = 1e-6f;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            if (i == j) {
                if (std::abs(data[i][j] - 1.0f) > epsilon) return false;
            } else {
                if (std::abs(data[i][j]) > epsilon) return false;
            }
        }
    }
    return true;
}

inline bool Matrix4x4::isInvertible() const {
    const float epsilon = 1e-6f;
    return std::abs(determinant()) > epsilon;
}

inline Matrix4x4 Matrix4x4::identity() {
    Matrix4x4 result;
    result.setIdentity();
    return result;
}

inline Matrix4x4 Matrix4x4::zero() {
    Matrix4x4 result;
    result.setZero();
    return result;
}

inline Matrix4x4 Matrix4x4::rotationX(float angle) {
    float cos_a = std::cos(angle);
    float sin_a = std::sin(angle);
    
    Matrix4x4 result;
    result[0][0] = 1.0f; result[0][1] = 0.0f; result[0][2] = 0.0f; result[0][3] = 0.0f;
    result[1][0] = 0.0f; result[1][1] = cos_a; result[1][2] = sin_a; result[1][3] = 0.0f;
    result[2][0] = 0.0f; result[2][1] = -sin_a; result[2][2] = cos_a; result[2][3] = 0.0f;
    result[3][0] = 0.0f; result[3][1] = 0.0f; result[3][2] = 0.0f; result[3][3] = 1.0f;
    
    return result;
}

inline Matrix4x4 Matrix4x4::rotationY(float angle) {
    float cos_a = std::cos(angle);
    float sin_a = std::sin(angle);
    
    Matrix4x4 result;
    result[0][0] = cos_a; result[0][1] = 0.0f; result[0][2] = -sin_a; result[0][3] = 0.0f;
    result[1][0] = 0.0f; result[1][1] = 1.0f; result[1][2] = 0.0f; result[1][3] = 0.0f;
    result[2][0] = sin_a; result[2][1] = 0.0f; result[2][2] = cos_a; result[2][3] = 0.0f;
    result[3][0] = 0.0f; result[3][1] = 0.0f; result[3][2] = 0.0f; result[3][3] = 1.0f;
    
    return result;
}

inline Matrix4x4 Matrix4x4::rotationZ(float angle) {
    float cos_a = std::cos(angle);
    float sin_a = std::sin(angle);
    
    Matrix4x4 result;
    result[0][0] = cos_a; result[0][1] = sin_a; result[0][2] = 0.0f; result[0][3] = 0.0f;
    result[1][0] = -sin_a; result[1][1] = cos_a; result[1][2] = 0.0f; result[1][3] = 0.0f;
    result[2][0] = 0.0f; result[2][1] = 0.0f; result[2][2] = 1.0f; result[2][3] = 0.0f;
    result[3][0] = 0.0f; result[3][1] = 0.0f; result[3][2] = 0.0f; result[3][3] = 1.0f;
    
    return result;
}

inline Matrix4x4 Matrix4x4::rotation(float angle, float x, float y, float z) {
    // Normalize axis vector
    float length = std::sqrt(x*x + y*y + z*z);
    if (length < 1e-6f) {
        return identity();
    }
    
    x /= length;
    y /= length;
    z /= length;
    
    float cos_a = std::cos(angle);
    float sin_a = std::sin(angle);
    float one_minus_cos = 1.0f - cos_a;
    
    Matrix4x4 result;
    result[0][0] = x*x*one_minus_cos + cos_a;
    result[0][1] = x*y*one_minus_cos + z*sin_a;
    result[0][2] = x*z*one_minus_cos - y*sin_a;
    result[0][3] = 0.0f;
    
    result[1][0] = x*y*one_minus_cos - z*sin_a;
    result[1][1] = y*y*one_minus_cos + cos_a;
    result[1][2] = y*z*one_minus_cos + x*sin_a;
    result[1][3] = 0.0f;
    
    result[2][0] = x*z*one_minus_cos + y*sin_a;
    result[2][1] = y*z*one_minus_cos - x*sin_a;
    result[2][2] = z*z*one_minus_cos + cos_a;
    result[2][3] = 0.0f;
    
    result[3][0] = 0.0f;
    result[3][1] = 0.0f;
    result[3][2] = 0.0f;
    result[3][3] = 1.0f;
    
    return result;
}

inline Matrix4x4 Matrix4x4::translation(float x, float y, float z) {
    Matrix4x4 result;
    result.setIdentity();
    result[0][3] = x;
    result[1][3] = y;
    result[2][3] = z;
    return result;
}

inline Matrix4x4 Matrix4x4::scale(float x, float y, float z) {
    Matrix4x4 result;
    result.setIdentity();
    result[0][0] = x;
    result[1][1] = y;
    result[2][2] = z;
    return result;
}

inline Matrix4x4 Matrix4x4::perspective(float fov, float aspect_ratio, float near_plane, float far_plane) {
    float f = 1.0f / std::tan(fov / 2.0f);
    
    Matrix4x4 result;
    result[0][0] = f / aspect_ratio;
    result[0][1] = 0.0f;
    result[0][2] = 0.0f;
    result[0][3] = 0.0f;
    
    result[1][0] = 0.0f;
    result[1][1] = f;
    result[1][2] = 0.0f;
    result[1][3] = 0.0f;
    
    result[2][0] = 0.0f;
    result[2][1] = 0.0f;
    result[2][2] = (far_plane + near_plane) / (near_plane - far_plane);
    result[2][3] = (2.0f * far_plane * near_plane) / (near_plane - far_plane);
    
    result[3][0] = 0.0f;
    result[3][1] = 0.0f;
    result[3][2] = -1.0f;
    result[3][3] = 0.0f;
    
    return result;
}

inline void Matrix4x4::print() const {
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            std::cout << data[i][j] << " ";
        }
        std::cout << std::endl;
    }
}

// Overloaded operators
Matrix4x4 operator*(const Matrix4x4& a, const Matrix4x4& b) {
    Matrix4x4 result;
    
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            result[i][j] = 0.0f;
            for (int k = 0; k < 4; ++k) {
                result[i][j] += a[i][k] * b[k][j];
            }
        }
    }
    
    return result;
}

Matrix4x4 operator*(const Matrix4x4& m, float scalar) {
    Matrix4x4 result;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            result[i][j] = m[i][j] * scalar;
        }
    }
    return result;
}

Matrix4x4 operator*(float scalar, const Matrix4x4& m) {
    return m * scalar;
}

std::ostream& operator<<(std::ostream& os, const Matrix4x4& m) {
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            os << m[i][j] << " ";
        }
        os << std::endl;
    }
    return os;
}

#endif // MATRIX_4X4_H