surround_view/service-impl/Matrix4x4.h

/*
 * Copyright 2020 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_
#define SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_

#include <array>
#include <cassert>
#include <cmath>
#include <iosfwd>

template <class VType>
class Matrix4x4 {
private:
    VType m[4][4];

public:
    typedef Matrix4x4<VType> Self;
    typedef VType BaseType;
    typedef std::array<VType, 4> MVector;

    // Initialize the matrix to 0
    Matrix4x4() {
        m[0][3] = m[0][2] = m[0][1] = m[0][0] = VType();
        m[1][3] = m[1][2] = m[1][1] = m[1][0] = VType();
        m[2][3] = m[2][2] = m[2][1] = m[2][0] = VType();
        m[3][3] = m[3][2] = m[3][1] = m[3][0] = VType();
    }

    // Explicitly set every element on construction
    Matrix4x4(const VType& m00, const VType& m01, const VType& m02, const VType& m03,
              const VType& m10, const VType& m11, const VType& m12, const VType& m13,
              const VType& m20, const VType& m21, const VType& m22, const VType& m23,
              const VType& m30, const VType& m31, const VType& m32, const VType& m33) {
        m[0][0] = m00;
        m[0][1] = m01;
        m[0][2] = m02;
        m[0][3] = m03;

        m[1][0] = m10;
        m[1][1] = m11;
        m[1][2] = m12;
        m[1][3] = m13;

        m[2][0] = m20;
        m[2][1] = m21;
        m[2][2] = m22;
        m[2][3] = m23;

        m[3][0] = m30;
        m[3][1] = m31;
        m[3][2] = m32;
        m[3][3] = m33;
    }

    // Casting constructor
    template <class VType2>
    static Matrix4x4 cast(const Matrix4x4<VType2>& mb) {
        return Matrix4x4(static_cast<VType>(mb(0, 0)), static_cast<VType>(mb(0, 1)),
                         static_cast<VType>(mb(0, 2)), static_cast<VType>(mb(0, 3)),
                         static_cast<VType>(mb(1, 0)), static_cast<VType>(mb(1, 1)),
                         static_cast<VType>(mb(1, 2)), static_cast<VType>(mb(1, 3)),
                         static_cast<VType>(mb(2, 0)), static_cast<VType>(mb(2, 1)),
                         static_cast<VType>(mb(2, 2)), static_cast<VType>(mb(2, 3)),
                         static_cast<VType>(mb(3, 0)), static_cast<VType>(mb(3, 1)),
                         static_cast<VType>(mb(3, 2)), static_cast<VType>(mb(3, 3)));
    }

    // Change the value of all the coefficients of the matrix
    inline Matrix4x4& set(const VType& m00, const VType& m01, const VType& m02, const VType& m03,
                          const VType& m10, const VType& m11, const VType& m12, const VType& m13,
                          const VType& m20, const VType& m21, const VType& m22, const VType& m23,
                          const VType& m30, const VType& m31, const VType& m32, const VType& m33) {
        m[0][0] = m00;
        m[0][1] = m01;
        m[0][2] = m02;
        m[0][3] = m03;

        m[1][0] = m10;
        m[1][1] = m11;
        m[1][2] = m12;
        m[1][3] = m13;

        m[2][0] = m20;
        m[2][1] = m21;
        m[2][2] = m22;
        m[2][3] = m23;

        m[3][0] = m30;
        m[3][1] = m31;
        m[3][2] = m32;
        m[3][3] = m33;
        return (*this);
    }

    // Matrix addition
    inline Matrix4x4& operator+=(const Matrix4x4& addFrom) {
        m[0][0] += addFrom.m[0][0];
        m[0][1] += addFrom.m[0][1];
        m[0][2] += addFrom.m[0][2];
        m[0][3] += addFrom.m[0][3];

        m[1][0] += addFrom.m[1][0];
        m[1][1] += addFrom.m[1][1];
        m[1][2] += addFrom.m[1][2];
        m[1][3] += addFrom.m[1][3];

        m[2][0] += addFrom.m[2][0];
        m[2][1] += addFrom.m[2][1];
        m[2][2] += addFrom.m[2][2];
        m[2][3] += addFrom.m[2][3];

        m[3][0] += addFrom.m[3][0];
        m[3][1] += addFrom.m[3][1];
        m[3][2] += addFrom.m[3][2];
        m[3][3] += addFrom.m[3][3];
        return (*this);
    }

    // Matrix subtration
    inline Matrix4x4& operator-=(const Matrix4x4& subFrom) {
        m[0][0] -= subFrom.m[0][0];
        m[0][1] -= subFrom.m[0][1];
        m[0][2] -= subFrom.m[0][2];
        m[0][3] -= subFrom.m[0][3];

        m[1][0] -= subFrom.m[1][0];
        m[1][1] -= subFrom.m[1][1];
        m[1][2] -= subFrom.m[1][2];
        m[1][3] -= subFrom.m[1][3];

        m[2][0] -= subFrom.m[2][0];
        m[2][1] -= subFrom.m[2][1];
        m[2][2] -= subFrom.m[2][2];
        m[2][3] -= subFrom.m[2][3];

        m[3][0] -= subFrom.m[3][0];
        m[3][1] -= subFrom.m[3][1];
        m[3][2] -= subFrom.m[3][2];
        m[3][3] -= subFrom.m[3][3];
        return (*this);
    }

    // Matrix multiplication by a scalar
    inline Matrix4x4& operator*=(const VType& k) {
        m[0][0] *= k;
        m[0][1] *= k;
        m[0][2] *= k;
        m[0][3] *= k;

        m[1][0] *= k;
        m[1][1] *= k;
        m[1][2] *= k;
        m[1][3] *= k;

        m[2][0] *= k;
        m[2][1] *= k;
        m[2][2] *= k;
        m[2][3] *= k;

        m[3][0] *= k;
        m[3][1] *= k;
        m[3][2] *= k;
        m[3][3] *= k;
        return (*this);
    }

    // Matrix addition
    inline Matrix4x4 operator+(const Matrix4x4& mb) const { return Matrix4x4(*this) += mb; }

    // Matrix subtraction
    inline Matrix4x4 operator-(const Matrix4x4& mb) const { return Matrix4x4(*this) -= mb; }

    // Change the sign of all the coefficients in the matrix
    friend inline Matrix4x4 operator-(const Matrix4x4& vb) {
        return Matrix4x4(-vb.m[0][0], -vb.m[0][1], -vb.m[0][2], -vb.m[0][3], -vb.m[1][0],
                         -vb.m[1][1], -vb.m[1][2], -vb.m[1][3], -vb.m[2][0], -vb.m[2][1],
                         -vb.m[2][2], -vb.m[2][3], -vb.m[3][0], -vb.m[3][1], -vb.m[3][2],
                         -vb.m[3][3]);
    }

    // Matrix multiplication by a scalar
    inline Matrix4x4 operator*(const VType& k) const { return Matrix4x4(*this) *= k; }

    // Multiplication by a scaler
    friend inline Matrix4x4 operator*(const VType& k, const Matrix4x4& mb) {
        return Matrix4x4(mb) * k;
    }

    // Matrix multiplication
    friend Matrix4x4 operator*(const Matrix4x4& a, const Matrix4x4& b) {
        return Matrix4x4::fromCols(a * b.col(0), a * b.col(1), a * b.col(2), a * b.col(3));
    }

    // Multiplication of a matrix by a vector
    friend MVector operator*(const Matrix4x4& a, const MVector& b) {
        return MVector{dotProd(a.row(0), b), dotProd(a.row(1), b), dotProd(a.row(2), b),
                       dotProd(a.row(3), b)};
    }

    // Return the trace of the matrix
    inline VType trace() const { return m[0][0] + m[1][1] + m[2][2] + m[3][3]; }

    // Return a pointer to the data array for interface with other libraries
    // like opencv
    VType* data() { return reinterpret_cast<VType*>(m); }
    const VType* data() const { return reinterpret_cast<const VType*>(m); }

    // Return matrix element (i,j) with 0<=i<=3 0<=j<=3
    inline VType& operator()(const int i, const int j) {
        assert(i >= 0);
        assert(i < 4);
        assert(j >= 0);
        assert(j < 4);
        return m[i][j];
    }

    inline VType operator()(const int i, const int j) const {
        assert(i >= 0);
        assert(i < 4);
        assert(j >= 0);
        assert(j < 4);
        return m[i][j];
    }

    // Return matrix element (i/4,i%4) with 0<=i<=15 (access concatenated rows).
    inline VType& operator[](const int i) {
        assert(i >= 0);
        assert(i < 16);
        return reinterpret_cast<VType*>(m)[i];
    }
    inline VType operator[](const int i) const {
        assert(i >= 0);
        assert(i < 16);
        return reinterpret_cast<const VType*>(m)[i];
    }

    // Return the transposed matrix
    inline Matrix4x4 transpose() const {
        return Matrix4x4(m[0][0], m[1][0], m[2][0], m[3][0], m[0][1], m[1][1], m[2][1], m[3][1],
                         m[0][2], m[1][2], m[2][2], m[3][2], m[0][3], m[1][3], m[2][3], m[3][3]);
    }

    // Returns the transpose of the matrix of the cofactors.
    // (Useful for inversion for example.)
    inline Matrix4x4 comatrixTransposed() const {
        const auto cof = [this](unsigned int row, unsigned int col) {
            unsigned int r0 = (row + 1) % 4;
            unsigned int r1 = (row + 2) % 4;
            unsigned int r2 = (row + 3) % 4;
            unsigned int c0 = (col + 1) % 4;
            unsigned int c1 = (col + 2) % 4;
            unsigned int c2 = (col + 3) % 4;

            VType minor = m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) -
                    m[r1][c0] * (m[r0][c1] * m[r2][c2] - m[r2][c1] * m[r0][c2]) +
                    m[r2][c0] * (m[r0][c1] * m[r1][c2] - m[r1][c1] * m[r0][c2]);
            return (row + col) & 1 ? -minor : minor;
        };

        // Transpose
        return Matrix4x4(cof(0, 0), cof(1, 0), cof(2, 0), cof(3, 0), cof(0, 1), cof(1, 1),
                         cof(2, 1), cof(3, 1), cof(0, 2), cof(1, 2), cof(2, 2), cof(3, 2),
                         cof(0, 3), cof(1, 3), cof(2, 3), cof(3, 3));
    }

    // Return dot production of two the vectors
    static inline VType dotProd(const MVector& lhs, const MVector& rhs) {
        return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2] + lhs[3] * rhs[3];
    }

    // Return the 4D vector at row i
    inline MVector row(const int i) const {
        assert(i >= 0);
        assert(i < 4);
        return MVector{m[i][0], m[i][1], m[i][2], m[i][3]};
    }

    // Return the 4D vector at col i
    inline MVector col(const int i) const {
        assert(i >= 0);
        assert(i < 4);
        return MVector{m[0][i], m[1][i], m[2][i], m[3][i]};
    }

    // Create a matrix from 4 row vectors
    static inline Matrix4x4 fromRows(const MVector& v1, const MVector& v2, const MVector& v3,
                                     const MVector& v4) {
        return Matrix4x4(v1[0], v1[1], v1[2], v1[3], v2[0], v2[1], v2[2], v2[3], v3[0], v3[1],
                         v3[2], v3[3], v4[0], v4[1], v4[2], v4[3]);
    }

    // Create a matrix from 3 column vectors
    static inline Matrix4x4 fromCols(const MVector& v1, const MVector& v2, const MVector& v3,
                                     const MVector& v4) {
        return Matrix4x4(v1[0], v2[0], v3[0], v4[0], v1[1], v2[1], v3[1], v4[1], v1[2], v2[2],
                         v3[2], v4[2], v1[3], v2[3], v3[3], v4[3]);
    }

    // Set the vector in row i to be v1
    void setRow(int i, const MVector& v1) {
        assert(i >= 0);
        assert(i < 4);
        m[i][0] = v1[0];
        m[i][1] = v1[1];
        m[i][2] = v1[2];
        m[i][3] = v1[3];
    }

    // Set the vector in column i to be v1
    void setCol(int i, const MVector& v1) {
        assert(i >= 0);
        assert(i < 4);
        m[0][i] = v1[0];
        m[1][i] = v1[1];
        m[2][i] = v1[2];
        m[3][i] = v1[3];
    }

    // Return the identity matrix
    static inline Matrix4x4 identity() {
        return Matrix4x4(VType(1), VType(), VType(), VType(), VType(), VType(1), VType(), VType(),
                         VType(), VType(), VType(1), VType(), VType(), VType(), VType(), VType(1));
    }

    // Return a matrix full of zeros
    static inline Matrix4x4 zero() { return Matrix4x4(); }

    // Return a diagonal matrix with the coefficients in v
    static inline Matrix4x4 diagonal(const MVector& v) {
        return Matrix4x4(v[0], VType(), VType(), VType(),  //
                         VType(), v[1], VType(), VType(),  //
                         VType(), VType(), v[2], VType(),  //
                         VType(), VType(), VType(), v[3]);
    }

    // Return the matrix vvT
    static Matrix4x4 sym4(const MVector& v) {
        return Matrix4x4(v[0] * v[0], v[0] * v[1], v[0] * v[2], v[0] * v[3], v[1] * v[0],
                         v[1] * v[1], v[1] * v[2], v[1] * v[3], v[2] * v[0], v[2] * v[1],
                         v[2] * v[2], v[2] * v[3], v[3] * v[0], v[3] * v[1], v[3] * v[2],
                         v[3] * v[3]);
    }

    // Return the Frobenius norm of the matrix: sqrt(sum(aij^2))
    VType frobeniusNorm() const {
        VType sum = VType();
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                sum += m[i][j] * m[i][j];
            }
        }
        return std::sqrt(sum);
    }

    // Return true is one of the elements of the matrix is NaN
    bool isNaN() const {
        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                if (isnan(m[i][j])) {
                    return true;
                }
            }
        }
        return false;
    }

    friend bool operator==(const Matrix4x4& a, const Matrix4x4& b) {
        return a.m[0][0] == b.m[0][0] && a.m[0][1] == b.m[0][1] && a.m[0][2] == b.m[0][2] &&
                a.m[0][3] == b.m[0][3] && a.m[1][0] == b.m[1][0] && a.m[1][1] == b.m[1][1] &&
                a.m[1][2] == b.m[1][2] && a.m[1][3] == b.m[1][3] && a.m[2][0] == b.m[2][0] &&
                a.m[2][1] == b.m[2][1] && a.m[2][2] == b.m[2][2] && a.m[2][3] == b.m[2][3] &&
                a.m[3][0] == b.m[3][0] && a.m[3][1] == b.m[3][1] && a.m[3][2] == b.m[3][2] &&
                a.m[3][3] == b.m[3][3];
    }

    friend bool operator!=(const Matrix4x4& a, const Matrix4x4& b) { return !(a == b); }
};

typedef Matrix4x4<int> Matrix4x4I;
typedef Matrix4x4<float> Matrix4x4F;
typedef Matrix4x4<double> Matrix4x4D;

#endif  // #ifndef SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_