Matrix3x3.hpp

Go to the documentation of this file.

#ifndef PHYTH_MATRIX3X3_HPP
#define PHYTH_MATRIX3X3_HPP
 
#include "Phyth/Core/Quantity.hpp"
#include "Matrix.hpp"
#include "Vector3.hpp"
#include <ostream>
#include <stdexcept>
 
namespace Phyth {
    template <typename T>
    using Matrix3x3 = Matrix<3, 3, T>;
 
    template<typename T>
    struct Matrix<3, 3, T> {
        static_assert(is_quantity_v<T>, "Matrix3x3 only supports Quantity types");
 
        Vector3<T> x_basis;
        Vector3<T> y_basis;
        Vector3<T> z_basis;
 
        constexpr Matrix() : x_basis(0, 0, 0), y_basis(0, 0, 0), z_basis(0, 0, 0) {
        }
 
        constexpr Matrix(const Vector3<T> &col0, const Vector3<T> &col1, const Vector3<T> &col2)
            : x_basis(col0), y_basis(col1), z_basis(col2) {
        }
 
        constexpr Matrix(
            T m00, T m01, T m02,
            T m10, T m11, T m12,
            T m20, T m21, T m22
        ) : x_basis(m00, m10, m20),
            y_basis(m01, m11, m21),
            z_basis(m02, m12, m22) {
        }
 
        static constexpr Matrix Identity() {
            return Matrix(
                T(1), T(0), T(0),
                T(0), T(1), T(0),
                T(0), T(0), T(1)
            );
        }
 
        [[nodiscard]] constexpr Matrix Transpose() const {
            return Matrix(
                x_basis.x, y_basis.x, z_basis.x,
                x_basis.y, y_basis.y, z_basis.y,
                x_basis.z, y_basis.z, z_basis.z
            );
        }
 
        [[nodiscard]] constexpr T Trace() const {
            return x_basis.x + y_basis.y + z_basis.z;
        }
 
        [[nodiscard]] constexpr auto Symmetric() const {
            return (*this + this->Transpose()) * T(0.5);
        }
 
        [[nodiscard]] constexpr auto Antisymmetric() const {
            return (*this - this->Transpose()) * T(0.5);
        }
 
        [[nodiscard]] const T& At(const int i, const int j) const {
            if (i < 0 || i > 2 || j < 0 || j > 2) {
                throw std::out_of_range("Matrix3x3::at: index out of range");
            }
            switch (j) {
                case 0: return x_basis[i];
                case 1: return y_basis[i];
                case 2: return z_basis[i];
                default: throw std::out_of_range("Matrix3x3::at: invalid column");
            }
        }
 
        T& At(const int i, const int j) {
            if (i < 0 || i > 2 || j < 0 || j > 2) {
                throw std::out_of_range("Matrix3x3::at: index out of range");
            }
            switch (j) {
                case 0: return x_basis[i];
                case 1: return y_basis[i];
                case 2: return z_basis[i];
                default: throw std::out_of_range("Matrix3x3::at: invalid column");
            }
        }
 
        [[nodiscard]] constexpr const T& operator()(const int i, const int j) const {
            if (j == 0) return x_basis[i];
            if (j == 1) return y_basis[i];
            return z_basis[i];
        }
 
        constexpr T& operator()(const int i, const int j) {
            if (j == 0) return x_basis[i];
            if (j == 1) return y_basis[i];
            return z_basis[i];
        }
 
        template<typename U>
        constexpr auto operator+(const Matrix3x3<U> &other) const {
            return Matrix3x3<decltype(x_basis + other.x_basis)>(
                x_basis + other.x_basis,
                y_basis + other.y_basis,
                z_basis + other.z_basis
            );
        }
 
        template<typename U>
        constexpr auto operator-(const Matrix3x3<U> &other) const {
            return Matrix3x3<decltype(x_basis - other.x_basis)>(
                x_basis - other.x_basis,
                y_basis - other.y_basis,
                z_basis - other.z_basis
            );
        }
 
        constexpr auto operator-() const {
            return Matrix(-x_basis, -y_basis, -z_basis);
        }
 
        template<typename U>
        constexpr auto operator*(U scalar) const {
            return Matrix3x3<decltype(x_basis * scalar)>(
                x_basis * scalar,
                y_basis * scalar,
                z_basis * scalar
            );
        }
 
        template<typename U>
        constexpr auto operator*(const Vector3<U> &v) const {
            // M * v = x_basis * v.x + y_basis * v.y + z_basis * v.z
            return x_basis * v.x + y_basis * v.y + z_basis * v.z;
        }
 
        template<typename U>
        constexpr auto operator*(const Matrix3x3<U> &other) const {
            // A * B: column j of result = A * (column j of B)
            return Matrix3x3<decltype(*this * other.x_basis)>(
                *this * other.x_basis,
                *this * other.y_basis,
                *this * other.z_basis
            );
        }
 
        template<typename U>
        constexpr Matrix &operator+=(const Matrix3x3<U> &other) {
            x_basis += other.x_basis;
            y_basis += other.y_basis;
            z_basis += other.z_basis;
            return *this;
        }
 
        template<typename U>
        constexpr Matrix &operator-=(const Matrix3x3<U> &other) {
            x_basis -= other.x_basis;
            y_basis -= other.y_basis;
            z_basis -= other.z_basis;
            return *this;
        }
 
        template<typename U>
        constexpr Matrix &operator*=(U scalar) {
            x_basis *= scalar;
            y_basis *= scalar;
            z_basis *= scalar;
            return *this;
        }
 
        template<typename U>
        constexpr bool operator==(const Matrix3x3<U> &other) const {
            return x_basis == other.x_basis && y_basis == other.y_basis && z_basis == other.z_basis;
        }
 
        template<typename U>
        constexpr bool operator!=(const Matrix3x3<U> &other) const {
            return !(*this == other);
        }
 
        template<typename UnitT>
        [[nodiscard]] Matrix3x3<Quantity<UnitT> > As() const {
            return Matrix3x3<Quantity<UnitT> >(
                x_basis.template As<UnitT>(),
                y_basis.template As<UnitT>(),
                z_basis.template As<UnitT>()
            );
        }
 
        friend std::ostream& operator<<(std::ostream& os, const Matrix& m) {
            os << "[" << m.x_basis.x << " " << m.y_basis.x << " " << m.z_basis.x << "\n"
               << " " << m.x_basis.y << " " << m.y_basis.y << " " << m.z_basis.y << "\n"
               << " " << m.x_basis.z << " " << m.y_basis.z << " " << m.z_basis.z << "]";
            return os;
        }
    };
 
    template<typename T, typename U>
    constexpr auto operator*(T scalar, const Matrix3x3<U> &m) {
        return m * scalar;
    }
}
 
#endif // PHYTH_MATRIX3X3_HPP