xref: /aosp_15_r20/external/angle/src/common/matrix_utils.h (revision 8975f5c5ed3d1c378011245431ada316dfb6f244)

//
// Copyright 2015 The ANGLE Project Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
//
// Matrix:
//   Utility class implementing various matrix operations.
//   Supports matrices with minimum 2 and maximum 4 number of rows/columns.
//
// TODO: Check if we can merge Matrix.h in sample_util with this and replace it with this
// implementation.
// TODO: Rename this file to Matrix.h once we remove Matrix.h in sample_util.

#ifndef COMMON_MATRIX_UTILS_H_
#define COMMON_MATRIX_UTILS_H_

#include <array>
#include <vector>

#include "common/debug.h"
#include "common/mathutil.h"
#include "common/vector_utils.h"

namespace
{
template <typename T4x4>
void CofactorTransposed(const T4x4 &mat, T4x4 &coft)
{
    coft(0, 0) = mat(1, 1) * mat(2, 2) * mat(3, 3) + mat(2, 1) * mat(3, 2) * mat(1, 3) +
                 mat(3, 1) * mat(1, 2) * mat(2, 3) - mat(1, 1) * mat(3, 2) * mat(2, 3) -
                 mat(2, 1) * mat(1, 2) * mat(3, 3) - mat(3, 1) * mat(2, 2) * mat(1, 3);
    coft(1, 0) = -(mat(1, 0) * mat(2, 2) * mat(3, 3) + mat(2, 0) * mat(3, 2) * mat(1, 3) +
                   mat(3, 0) * mat(1, 2) * mat(2, 3) - mat(1, 0) * mat(3, 2) * mat(2, 3) -
                   mat(2, 0) * mat(1, 2) * mat(3, 3) - mat(3, 0) * mat(2, 2) * mat(1, 3));
    coft(2, 0) = mat(1, 0) * mat(2, 1) * mat(3, 3) + mat(2, 0) * mat(3, 1) * mat(1, 3) +
                 mat(3, 0) * mat(1, 1) * mat(2, 3) - mat(1, 0) * mat(3, 1) * mat(2, 3) -
                 mat(2, 0) * mat(1, 1) * mat(3, 3) - mat(3, 0) * mat(2, 1) * mat(1, 3);
    coft(3, 0) = -(mat(1, 0) * mat(2, 1) * mat(3, 2) + mat(2, 0) * mat(3, 1) * mat(1, 2) +
                   mat(3, 0) * mat(1, 1) * mat(2, 2) - mat(1, 0) * mat(3, 1) * mat(2, 2) -
                   mat(2, 0) * mat(1, 1) * mat(3, 2) - mat(3, 0) * mat(2, 1) * mat(1, 2));
    coft(0, 1) = -(mat(0, 1) * mat(2, 2) * mat(3, 3) + mat(2, 1) * mat(3, 2) * mat(0, 3) +
                   mat(3, 1) * mat(0, 2) * mat(2, 3) - mat(0, 1) * mat(3, 2) * mat(2, 3) -
                   mat(2, 1) * mat(0, 2) * mat(3, 3) - mat(3, 1) * mat(2, 2) * mat(0, 3));
    coft(1, 1) = mat(0, 0) * mat(2, 2) * mat(3, 3) + mat(2, 0) * mat(3, 2) * mat(0, 3) +
                 mat(3, 0) * mat(0, 2) * mat(2, 3) - mat(0, 0) * mat(3, 2) * mat(2, 3) -
                 mat(2, 0) * mat(0, 2) * mat(3, 3) - mat(3, 0) * mat(2, 2) * mat(0, 3);
    coft(2, 1) = -(mat(0, 0) * mat(2, 1) * mat(3, 3) + mat(2, 0) * mat(3, 1) * mat(0, 3) +
                   mat(3, 0) * mat(0, 1) * mat(2, 3) - mat(0, 0) * mat(3, 1) * mat(2, 3) -
                   mat(2, 0) * mat(0, 1) * mat(3, 3) - mat(3, 0) * mat(2, 1) * mat(0, 3));
    coft(3, 1) = mat(0, 0) * mat(2, 1) * mat(3, 2) + mat(2, 0) * mat(3, 1) * mat(0, 2) +
                 mat(3, 0) * mat(0, 1) * mat(2, 2) - mat(0, 0) * mat(3, 1) * mat(2, 2) -
                 mat(2, 0) * mat(0, 1) * mat(3, 2) - mat(3, 0) * mat(2, 1) * mat(0, 2);
    coft(0, 2) = mat(0, 1) * mat(1, 2) * mat(3, 3) + mat(1, 1) * mat(3, 2) * mat(0, 3) +
                 mat(3, 1) * mat(0, 2) * mat(1, 3) - mat(0, 1) * mat(3, 2) * mat(1, 3) -
                 mat(1, 1) * mat(0, 2) * mat(3, 3) - mat(3, 1) * mat(1, 2) * mat(0, 3);
    coft(1, 2) = -(mat(0, 0) * mat(1, 2) * mat(3, 3) + mat(1, 0) * mat(3, 2) * mat(0, 3) +
                   mat(3, 0) * mat(0, 2) * mat(1, 3) - mat(0, 0) * mat(3, 2) * mat(1, 3) -
                   mat(1, 0) * mat(0, 2) * mat(3, 3) - mat(3, 0) * mat(1, 2) * mat(0, 3));
    coft(2, 2) = mat(0, 0) * mat(1, 1) * mat(3, 3) + mat(1, 0) * mat(3, 1) * mat(0, 3) +
                 mat(3, 0) * mat(0, 1) * mat(1, 3) - mat(0, 0) * mat(3, 1) * mat(1, 3) -
                 mat(1, 0) * mat(0, 1) * mat(3, 3) - mat(3, 0) * mat(1, 1) * mat(0, 3);
    coft(3, 2) = -(mat(0, 0) * mat(1, 1) * mat(3, 2) + mat(1, 0) * mat(3, 1) * mat(0, 2) +
                   mat(3, 0) * mat(0, 1) * mat(1, 2) - mat(0, 0) * mat(3, 1) * mat(1, 2) -
                   mat(1, 0) * mat(0, 1) * mat(3, 2) - mat(3, 0) * mat(1, 1) * mat(0, 2));
    coft(0, 3) = -(mat(0, 1) * mat(1, 2) * mat(2, 3) + mat(1, 1) * mat(2, 2) * mat(0, 3) +
                   mat(2, 1) * mat(0, 2) * mat(1, 3) - mat(0, 1) * mat(2, 2) * mat(1, 3) -
                   mat(1, 1) * mat(0, 2) * mat(2, 3) - mat(2, 1) * mat(1, 2) * mat(0, 3));
    coft(1, 3) = mat(0, 0) * mat(1, 2) * mat(2, 3) + mat(1, 0) * mat(2, 2) * mat(0, 3) +
                 mat(2, 0) * mat(0, 2) * mat(1, 3) - mat(0, 0) * mat(2, 2) * mat(1, 3) -
                 mat(1, 0) * mat(0, 2) * mat(2, 3) - mat(2, 0) * mat(1, 2) * mat(0, 3);
    coft(2, 3) = -(mat(0, 0) * mat(1, 1) * mat(2, 3) + mat(1, 0) * mat(2, 1) * mat(0, 3) +
                   mat(2, 0) * mat(0, 1) * mat(1, 3) - mat(0, 0) * mat(2, 1) * mat(1, 3) -
                   mat(1, 0) * mat(0, 1) * mat(2, 3) - mat(2, 0) * mat(1, 1) * mat(0, 3));
    coft(3, 3) = mat(0, 0) * mat(1, 1) * mat(2, 2) + mat(1, 0) * mat(2, 1) * mat(0, 2) +
                 mat(2, 0) * mat(0, 1) * mat(1, 2) - mat(0, 0) * mat(2, 1) * mat(1, 2) -
                 mat(1, 0) * mat(0, 1) * mat(2, 2) - mat(2, 0) * mat(1, 1) * mat(0, 2);
}
}  // namespace

namespace angle
{

template <typename T>
class Matrix
{
  public:
    Matrix(const std::vector<T> &elements, const unsigned int numRows, const unsigned int numCols)
        : mElements(elements), mRows(numRows), mCols(numCols)
    {
        ASSERT(rows() >= 1 && rows() <= 4);
        ASSERT(columns() >= 1 && columns() <= 4);
    }

    Matrix(const std::vector<T> &elements, const unsigned int size)
        : mElements(elements), mRows(size), mCols(size)
    {
        ASSERT(rows() >= 1 && rows() <= 4);
        ASSERT(columns() >= 1 && columns() <= 4);
    }

    Matrix(const T *elements, const unsigned int size) : mRows(size), mCols(size)
    {
        ASSERT(rows() >= 1 && rows() <= 4);
        ASSERT(columns() >= 1 && columns() <= 4);
        for (size_t i = 0; i < size * size; i++)
            mElements.push_back(elements[i]);
    }

    const T &operator()(const unsigned int rowIndex, const unsigned int columnIndex) const
    {
        ASSERT(rowIndex < mRows);
        ASSERT(columnIndex < mCols);
        return mElements[rowIndex * columns() + columnIndex];
    }

    T &operator()(const unsigned int rowIndex, const unsigned int columnIndex)
    {
        ASSERT(rowIndex < mRows);
        ASSERT(columnIndex < mCols);
        return mElements[rowIndex * columns() + columnIndex];
    }

    const T &at(const unsigned int rowIndex, const unsigned int columnIndex) const
    {
        ASSERT(rowIndex < mRows);
        ASSERT(columnIndex < mCols);
        return operator()(rowIndex, columnIndex);
    }

    Matrix<T> operator*(const Matrix<T> &m)
    {
        ASSERT(columns() == m.rows());

        unsigned int resultRows = rows();
        unsigned int resultCols = m.columns();
        Matrix<T> result(std::vector<T>(resultRows * resultCols), resultRows, resultCols);
        for (unsigned int i = 0; i < resultRows; i++)
        {
            for (unsigned int j = 0; j < resultCols; j++)
            {
                T tmp = 0.0f;
                for (unsigned int k = 0; k < columns(); k++)
                    tmp += at(i, k) * m(k, j);
                result(i, j) = tmp;
            }
        }

        return result;
    }

    void operator*=(const Matrix<T> &m)
    {
        ASSERT(columns() == m.rows());
        Matrix<T> res  = (*this) * m;
        size_t numElts = res.elements().size();
        mElements.resize(numElts);
        memcpy(mElements.data(), res.data(), numElts * sizeof(float));
    }

    bool operator==(const Matrix<T> &m) const
    {
        ASSERT(columns() == m.columns());
        ASSERT(rows() == m.rows());
        return mElements == m.elements();
    }

    bool operator!=(const Matrix<T> &m) const { return !(mElements == m.elements()); }

    bool nearlyEqual(T epsilon, const Matrix<T> &m) const
    {
        ASSERT(columns() == m.columns());
        ASSERT(rows() == m.rows());
        const auto &otherElts = m.elements();
        for (size_t i = 0; i < otherElts.size(); i++)
        {
            if ((mElements[i] - otherElts[i] > epsilon) && (otherElts[i] - mElements[i] > epsilon))
                return false;
        }
        return true;
    }

    unsigned int size() const
    {
        ASSERT(rows() == columns());
        return rows();
    }

    unsigned int rows() const { return mRows; }

    unsigned int columns() const { return mCols; }

    std::vector<T> elements() const { return mElements; }
    T *data() { return mElements.data(); }
    const T *constData() const { return mElements.data(); }

    Matrix<T> compMult(const Matrix<T> &mat1) const
    {
        Matrix result(std::vector<T>(mElements.size()), rows(), columns());
        for (unsigned int i = 0; i < rows(); i++)
        {
            for (unsigned int j = 0; j < columns(); j++)
            {
                T lhs        = at(i, j);
                T rhs        = mat1(i, j);
                result(i, j) = rhs * lhs;
            }
        }

        return result;
    }

    Matrix<T> outerProduct(const Matrix<T> &mat1) const
    {
        unsigned int cols = mat1.columns();
        Matrix result(std::vector<T>(rows() * cols), rows(), cols);
        for (unsigned int i = 0; i < rows(); i++)
            for (unsigned int j = 0; j < cols; j++)
                result(i, j) = at(i, 0) * mat1(0, j);

        return result;
    }

    Matrix<T> transpose() const
    {
        Matrix result(std::vector<T>(mElements.size()), columns(), rows());
        for (unsigned int i = 0; i < columns(); i++)
            for (unsigned int j = 0; j < rows(); j++)
                result(i, j) = at(j, i);

        return result;
    }

    T determinant() const
    {
        ASSERT(rows() == columns());

        switch (size())
        {
            case 2:
                return at(0, 0) * at(1, 1) - at(0, 1) * at(1, 0);

            case 3:
                return at(0, 0) * at(1, 1) * at(2, 2) + at(0, 1) * at(1, 2) * at(2, 0) +
                       at(0, 2) * at(1, 0) * at(2, 1) - at(0, 2) * at(1, 1) * at(2, 0) -
                       at(0, 1) * at(1, 0) * at(2, 2) - at(0, 0) * at(1, 2) * at(2, 1);

            case 4:
            {
                const float minorMatrices[4][3 * 3] = {{
                                                           at(1, 1),
                                                           at(2, 1),
                                                           at(3, 1),
                                                           at(1, 2),
                                                           at(2, 2),
                                                           at(3, 2),
                                                           at(1, 3),
                                                           at(2, 3),
                                                           at(3, 3),
                                                       },
                                                       {
                                                           at(1, 0),
                                                           at(2, 0),
                                                           at(3, 0),
                                                           at(1, 2),
                                                           at(2, 2),
                                                           at(3, 2),
                                                           at(1, 3),
                                                           at(2, 3),
                                                           at(3, 3),
                                                       },
                                                       {
                                                           at(1, 0),
                                                           at(2, 0),
                                                           at(3, 0),
                                                           at(1, 1),
                                                           at(2, 1),
                                                           at(3, 1),
                                                           at(1, 3),
                                                           at(2, 3),
                                                           at(3, 3),
                                                       },
                                                       {
                                                           at(1, 0),
                                                           at(2, 0),
                                                           at(3, 0),
                                                           at(1, 1),
                                                           at(2, 1),
                                                           at(3, 1),
                                                           at(1, 2),
                                                           at(2, 2),
                                                           at(3, 2),
                                                       }};
                return at(0, 0) * Matrix<T>(minorMatrices[0], 3).determinant() -
                       at(0, 1) * Matrix<T>(minorMatrices[1], 3).determinant() +
                       at(0, 2) * Matrix<T>(minorMatrices[2], 3).determinant() -
                       at(0, 3) * Matrix<T>(minorMatrices[3], 3).determinant();
            }

            default:
                UNREACHABLE();
                break;
        }

        return T();
    }

    Matrix<T> inverse() const
    {
        ASSERT(rows() == columns());

        Matrix<T> coft(std::vector<T>(mElements.size()), rows(), columns());
        switch (size())
        {
            case 2:
                coft(0, 0) = at(1, 1);
                coft(1, 0) = -at(1, 0);
                coft(0, 1) = -at(0, 1);
                coft(1, 1) = at(0, 0);
                break;

            case 3:
                coft(0, 0) = at(1, 1) * at(2, 2) - at(2, 1) * at(1, 2);
                coft(1, 0) = -(at(1, 0) * at(2, 2) - at(2, 0) * at(1, 2));
                coft(2, 0) = at(1, 0) * at(2, 1) - at(2, 0) * at(1, 1);
                coft(0, 1) = -(at(0, 1) * at(2, 2) - at(2, 1) * at(0, 2));
                coft(1, 1) = at(0, 0) * at(2, 2) - at(2, 0) * at(0, 2);
                coft(2, 1) = -(at(0, 0) * at(2, 1) - at(2, 0) * at(0, 1));
                coft(0, 2) = at(0, 1) * at(1, 2) - at(1, 1) * at(0, 2);
                coft(1, 2) = -(at(0, 0) * at(1, 2) - at(1, 0) * at(0, 2));
                coft(2, 2) = at(0, 0) * at(1, 1) - at(1, 0) * at(0, 1);
                break;

            case 4:
                CofactorTransposed(*this, coft);
                break;

            default:
                UNREACHABLE();
                break;
        }

        // The inverse of A is the transpose of the cofactor matrix times the reciprocal of the
        // determinant of A.
        T det = determinant();
        Matrix<T> result(std::vector<T>(mElements.size()), rows(), columns());
        for (unsigned int i = 0; i < rows(); i++)
            for (unsigned int j = 0; j < columns(); j++)
                result(i, j) = (det != static_cast<T>(0)) ? coft(i, j) / det : T();

        return result;
    }

    void setToIdentity()
    {
        ASSERT(rows() == columns());

        const auto one  = T(1);
        const auto zero = T(0);

        for (auto &e : mElements)
            e = zero;

        for (unsigned int i = 0; i < rows(); ++i)
        {
            const auto pos = i * columns() + (i % columns());
            mElements[pos] = one;
        }
    }

    template <unsigned int Size>
    static void setToIdentity(T (&matrix)[Size])
    {
        static_assert(gl::iSquareRoot<Size>() != 0, "Matrix is not square.");

        const auto cols = gl::iSquareRoot<Size>();
        const auto one  = T(1);
        const auto zero = T(0);

        for (auto &e : matrix)
            e = zero;

        for (unsigned int i = 0; i < cols; ++i)
        {
            const auto pos = i * cols + (i % cols);
            matrix[pos]    = one;
        }
    }

  protected:
    std::vector<T> mElements;
    unsigned int mRows;
    unsigned int mCols;
};

// Not derived from Matrix<float>: fixed-size std::array instead, to avoid malloc
class Mat4
{
  public:
    Mat4();
    Mat4(const Matrix<float> generalMatrix);
    Mat4(const std::vector<float> &elements);
    Mat4(const float *elements);
    Mat4(float m00,
         float m01,
         float m02,
         float m03,
         float m10,
         float m11,
         float m12,
         float m13,
         float m20,
         float m21,
         float m22,
         float m23,
         float m30,
         float m31,
         float m32,
         float m33);

    static Mat4 Rotate(float angle, const Vector3 &axis);
    static Mat4 Translate(const Vector3 &t);
    static Mat4 Scale(const Vector3 &s);
    static Mat4 Frustum(float l, float r, float b, float t, float n, float f);
    static Mat4 Perspective(float fov, float aspectRatio, float n, float f);
    static Mat4 Ortho(float l, float r, float b, float t, float n, float f);

    Mat4 product(const Mat4 &m);
    Vector4 product(const Vector4 &b);
    void dump();

    float *data() { return mElements.data(); }
    const float *constData() const { return mElements.data(); }

    float operator()(const unsigned int rowIndex, const unsigned int columnIndex) const
    {
        ASSERT(rowIndex < 4);
        ASSERT(columnIndex < 4);
        return mElements[rowIndex * 4 + columnIndex];
    }

    float &operator()(const unsigned int rowIndex, const unsigned int columnIndex)
    {
        ASSERT(rowIndex < 4);
        ASSERT(columnIndex < 4);
        return mElements[rowIndex * 4 + columnIndex];
    }

    float at(const unsigned int rowIndex, const unsigned int columnIndex) const
    {
        ASSERT(rowIndex < 4);
        ASSERT(columnIndex < 4);
        return operator()(rowIndex, columnIndex);
    }

    bool operator==(const Mat4 &m) const { return mElements == m.elements(); }

    bool nearlyEqual(float epsilon, const Mat4 &m) const
    {
        const auto &otherElts = m.elements();
        for (size_t i = 0; i < otherElts.size(); i++)
        {
            if ((mElements[i] - otherElts[i] > epsilon) && (otherElts[i] - mElements[i] > epsilon))
                return false;
        }
        return true;
    }

    const std::array<float, 4 * 4> &elements() const { return mElements; }

    Mat4 transpose() const
    {
        Mat4 result;
        for (unsigned int i = 0; i < 4; i++)
            for (unsigned int j = 0; j < 4; j++)
                result(i, j) = at(j, i);

        return result;
    }

    Mat4 inverse() const
    {
        Mat4 coft;
        CofactorTransposed(*this, coft);

        float det = at(0, 0) * coft(0, 0) + at(0, 1) * coft(1, 0) + at(0, 2) * coft(2, 0) +
                    at(0, 3) * coft(3, 0);

        Mat4 result = coft;
        for (int i = 0; i < 16; i++)
        {
            result.data()[i] /= det;
        }

        return result;
    }

  private:
    std::array<float, 4 * 4> mElements;
};

}  // namespace angle

#endif  // COMMON_MATRIX_UTILS_H_