Matrix.h

Go to the documentation of this file.

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <Jolt/Math/Vector.h>
#include <Jolt/Math/GaussianElimination.h>
 
JPH_NAMESPACE_BEGIN
 
template <uint Rows, uint Cols>
class [[nodiscard]] Matrix
{
public:
    inline                                  Matrix() = default;
    inline                                  Matrix(const Matrix &inM2)                              { *this = inM2; }
 
    inline uint                             GetRows() const                                         { return Rows; }
    inline uint                             GetCols() const                                         { return Cols; }
 
    inline void                             SetZero()
    {
        for (uint c = 0; c < Cols; ++c)
            mCol[c].SetZero();
    }
 
    inline static Matrix                    sZero()                                                 { Matrix m; m.SetZero(); return m; }
 
    inline bool                             IsZero() const
    {
        for (uint c = 0; c < Cols; ++c)
            if (!mCol[c].IsZero())
                return false;
 
        return true;
    }
 
    inline void                             SetIdentity()
    {
        // Clear matrix
        SetZero();
 
        // Set diagonal to 1
        for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc)
            mCol[rc].mF32[rc] = 1.0f;
    }
 
    inline static Matrix                    sIdentity()                                             { Matrix m; m.SetIdentity(); return m; }
 
    bool                                    IsIdentity() const                                      { return *this == sIdentity(); }
 
    inline void                             SetDiagonal(const Vector<Rows < Cols? Rows : Cols> &inV)
    {
        // Clear matrix
        SetZero();
 
        // Set diagonal
        for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc)
            mCol[rc].mF32[rc] = inV[rc];
    }
 
    inline static Matrix                    sDiagonal(const Vector<Rows < Cols? Rows : Cols> &inV)
    {
        Matrix m;
        m.SetDiagonal(inV);
        return m;
    }
 
    template <class OtherMatrix>
        void                                CopyPart(const OtherMatrix &inM, uint inSourceRow, uint inSourceCol, uint inNumRows, uint inNumCols, uint inDestRow, uint inDestCol)
        {
            for (uint c = 0; c < inNumCols; ++c)
                for (uint r = 0; r < inNumRows; ++r)
                    mCol[inDestCol + c].mF32[inDestRow + r] = inM(inSourceRow + r, inSourceCol + c);
        }
 
    inline float                            operator () (uint inRow, uint inColumn) const
    {
        JPH_ASSERT(inRow < Rows);
        JPH_ASSERT(inColumn < Cols);
        return mCol[inColumn].mF32[inRow];
    }
 
    inline float &                          operator () (uint inRow, uint inColumn)
    {
        JPH_ASSERT(inRow < Rows);
        JPH_ASSERT(inColumn < Cols);
        return mCol[inColumn].mF32[inRow];
    }
 
    inline bool                             operator == (const Matrix &inM2) const
    {
        for (uint c = 0; c < Cols; ++c)
            if (mCol[c] != inM2.mCol[c])
                return false;
        return true;
    }
 
    inline bool                             operator != (const Matrix &inM2) const
    {
        for (uint c = 0; c < Cols; ++c)
            if (mCol[c] != inM2.mCol[c])
                return true;
        return false;
    }
 
    inline Matrix &                         operator = (const Matrix &inM2)
    {
        for (uint c = 0; c < Cols; ++c)
            mCol[c] = inM2.mCol[c];
        return *this;
    }
 
    template <uint OtherCols>
    inline Matrix<Rows, OtherCols>  operator * (const Matrix<Cols, OtherCols> &inM) const
    {
        Matrix<Rows, OtherCols> m;
        for (uint c = 0; c < OtherCols; ++c)
            for (uint r = 0; r < Rows; ++r)
            {
                float dot = 0.0f;
                for (uint i = 0; i < Cols; ++i)
                    dot += mCol[i].mF32[r] * inM.mCol[c].mF32[i];
                m.mCol[c].mF32[r] = dot;
            }
        return m;
    }
 
    inline Vector<Rows>                     operator * (const Vector<Cols> &inV) const
    {
        Vector<Rows> v;
        for (uint r = 0; r < Rows; ++r)
        {
            float dot = 0.0f;
            for (uint c = 0; c < Cols; ++c)
                dot += mCol[c].mF32[r] * inV.mF32[c];
            v.mF32[r] = dot;
        }
        return v;
    }
 
    inline Matrix                           operator * (float inV) const
    {
        Matrix m;
        for (uint c = 0; c < Cols; ++c)
            m.mCol[c] = mCol[c] * inV;
        return m;
    }
 
    inline friend Matrix                    operator * (float inV, const Matrix &inM)
    {
        return inM * inV;
    }
 
    inline Matrix                           operator + (const Matrix &inM) const
    {
        Matrix m;
        for (uint c = 0; c < Cols; ++c)
            m.mCol[c] = mCol[c] + inM.mCol[c];
        return m;
    }
 
    inline Matrix                           operator - (const Matrix &inM) const
    {
        Matrix m;
        for (uint c = 0; c < Cols; ++c)
            m.mCol[c] = mCol[c] - inM.mCol[c];
        return m;
    }
 
    inline Matrix<Cols, Rows>               Transposed() const
    {
        Matrix<Cols, Rows> m;
        for (uint r = 0; r < Rows; ++r)
            for (uint c = 0; c < Cols; ++c)
                m.mCol[r].mF32[c] = mCol[c].mF32[r];
        return m;
    }
 
    bool                                    SetInversed(const Matrix &inM)
    {
        if constexpr (Rows != Cols) JPH_ASSERT(false);
        Matrix copy(inM);
        SetIdentity();
        return GaussianElimination(copy, *this);
    }
 
    inline Matrix                           Inversed() const
    {
        Matrix m;
        m.SetInversed(*this);
        return m;
    }
 
    friend ostream &                        operator << (ostream &inStream, const Matrix &inM)
    {
        for (uint i = 0; i < Cols - 1; ++i)
            inStream << inM.mCol[i] << ", ";
        inStream << inM.mCol[Cols - 1];
        return inStream;
    }
 
    const Vector<Rows> &                    GetColumn(int inIdx) const                  { return mCol[inIdx]; }
    Vector<Rows> &                          GetColumn(int inIdx)                        { return mCol[inIdx]; }
 
    Vector<Rows>                            mCol[Cols];                                 
};
 
// The template specialization doesn't sit well with Doxygen
#ifndef JPH_PLATFORM_DOXYGEN
 
template <>
inline bool Matrix<2, 2>::SetInversed(const Matrix<2, 2> &inM)
{
    // Fetch elements
    float a = inM.mCol[0].mF32[0];
    float b = inM.mCol[1].mF32[0];
    float c = inM.mCol[0].mF32[1];
    float d = inM.mCol[1].mF32[1];
 
    // Calculate determinant
    float det = a * d - b * c;
    if (det == 0.0f)
        return false;
 
    // Construct inverse
    mCol[0].mF32[0] = d / det;
    mCol[1].mF32[0] = -b / det;
    mCol[0].mF32[1] = -c / det;
    mCol[1].mF32[1] = a / det;
    return true;
}
 
#endif // !JPH_PLATFORM_DOXYGEN
 
JPH_NAMESPACE_END