FastMatrixMathHelper.h

Go to the documentation of this file.

#ifndef FASTMATRIXMATHHELPER_H
#define FASTMATRIXMATHHELPER_H 1

// C/C++ standard libraries
#include <algorithm> // std::for_each()
#include <array>
#include <cmath>     // std::sqrt()
#include <iterator>  // std::begin(), std::end()
#include <stdexcept> // std::range_error
#include <tuple>

#include "lardataalg/Utilities/StatCollector.h" // lar::util::identity

namespace lar {
  namespace util {

    namespace details {

      template <typename T, unsigned int DIM>
      struct FastMatrixOperationsBase {
        using Data_t = T;

        static constexpr unsigned int Dim = DIM; 

        using Matrix_t = std::array<Data_t, Dim * Dim>;
        using Vector_t = std::array<Data_t, Dim>;

        static Vector_t MatrixVectorProduct(Matrix_t const& mat, Vector_t const& vec);

        static constexpr Data_t sqr(Data_t v) { return v * v; }
      }; // struct FastMatrixOperationsBase<>

      template <typename T, unsigned int DIM>
      struct FastMatrixOperations : public FastMatrixOperationsBase<T, DIM> {
        using Base_t = FastMatrixOperationsBase<T, DIM>;
        using Data_t = typename Base_t::Data_t;
        using Matrix_t = typename Base_t::Matrix_t;

        static Data_t Determinant(Matrix_t const& mat);

        static Matrix_t InvertMatrix(Matrix_t const& mat, Data_t det);

        static Matrix_t InvertSymmetricMatrix(Matrix_t const& mat, Data_t det);

        static Matrix_t InvertMatrix(Matrix_t const& mat)
        {
          return InvertMatrix(mat, Determinant(mat));
        }

        static Matrix_t InvertSymmetricMatrix(Matrix_t const& mat)
        {
          return InvertSymmetricMatrix(mat, Determinant(mat));
        }

      }; // struct FastMatrixOperations<>

    } // namespace details
  }   // namespace util
} // namespace lar

//******************************************************************************
//***  template implementation
//***
namespace lar {
  namespace util {

    namespace details {

      template <unsigned int NCols>
      constexpr size_t MatrixIndex(unsigned int row, unsigned int col)
      {
        return row * NCols + col;
      }

      template <unsigned int N>
      struct DeterminantHelperBase {
        static constexpr size_t index(unsigned int row, unsigned int col)
        {
          return MatrixIndex<N>(row, col);
        }
      }; // struct DeterminantHelperBase<>

      template <typename T,
                unsigned int N,
                unsigned int... RnC // all row indices, then all column indices
                >
      struct DeterminantHelper : public DeterminantHelperBase<N> {
        using DeterminantHelperBase<N>::index;

        static T compute(T const* data);
      }; // struct DeterminantHelper<>

      template <typename T, unsigned int N, unsigned int R, unsigned int C>
      struct DeterminantHelper<T, N, R, C> : public DeterminantHelperBase<N> {
        using DeterminantHelperBase<N>::index;
        static_assert(R < N, "invalid row index specified");
        static_assert(C < N, "invalid column index specified");

        static T compute(T const* data) { return data[index(R, C)]; }
      }; // struct DeterminantHelper<>

      template <typename T,
                unsigned int N,
                unsigned int R1,
                unsigned int R2,
                unsigned int C1,
                unsigned int C2>
      struct DeterminantHelper<T, N, R1, R2, C1, C2> : public DeterminantHelperBase<N> {
        using DeterminantHelperBase<N>::index;
        static_assert(R1 < N, "invalid row index specified");
        static_assert(R2 < N, "invalid row index specified");
        static_assert(C1 < N, "invalid column index specified");
        static_assert(C2 < N, "invalid column index specified");

        static constexpr T sqr(T v) { return v * v; }

        static T compute(T const* data)
        {
          return data[index(R1, C1)] * data[index(R2, C2)] -
                 data[index(R1, C2)] * data[index(R2, C1)];
          /* // also
                data[index(R1, C1)] * UpHelper<R2, C2>::compute(data)
              - data[index(R1, C2)] * UpHelper<R2, C1>::compute(data)
             */
        }
      }; // struct DeterminantHelper<>

      template <typename T,
                unsigned int N,
                unsigned int R1,
                unsigned int R2,
                unsigned int R3,
                unsigned int C1,
                unsigned int C2,
                unsigned int C3>
      struct DeterminantHelper<T, N, R1, R2, R3, C1, C2, C3> : public DeterminantHelperBase<N> {
        using DeterminantHelperBase<N>::index;
        static_assert(R1 < N, "invalid row index specified");
        static_assert(R2 < N, "invalid row index specified");
        static_assert(R3 < N, "invalid row index specified");
        static_assert(C1 < N, "invalid column index specified");
        static_assert(C2 < N, "invalid column index specified");
        static_assert(C3 < N, "invalid column index specified");
        template <unsigned int sR1, unsigned int sR2, unsigned int sC1, unsigned int sC2>
        using UpHelper = DeterminantHelper<T, N, sR1, sR2, sC1, sC2>;
        static T compute(T const* data)
        {
          return data[index(R1, C1)] * UpHelper<R2, R3, C2, C3>::compute(data) -
                 data[index(R1, C2)] * UpHelper<R2, R3, C1, C3>::compute(data) +
                 data[index(R1, C3)] * UpHelper<R2, R3, C1, C2>::compute(data);
        }
      }; // struct DeterminantHelper<>

      template <typename T,
                unsigned int N,
                unsigned int R1,
                unsigned int R2,
                unsigned int R3,
                unsigned int R4,
                unsigned int C1,
                unsigned int C2,
                unsigned int C3,
                unsigned int C4>
      struct DeterminantHelper<T, N, R1, R2, R3, R4, C1, C2, C3, C4>
        : public DeterminantHelperBase<N> {
        using DeterminantHelperBase<N>::index;
        static_assert(R1 < N, "invalid row index specified");
        static_assert(R2 < N, "invalid row index specified");
        static_assert(R3 < N, "invalid row index specified");
        static_assert(R4 < N, "invalid row index specified");
        static_assert(C1 < N, "invalid column index specified");
        static_assert(C2 < N, "invalid column index specified");
        static_assert(C3 < N, "invalid column index specified");
        static_assert(C4 < N, "invalid column index specified");
        template <unsigned int sR1,
                  unsigned int sR2,
                  unsigned int sR3,
                  unsigned int sC1,
                  unsigned int sC2,
                  unsigned int sC3>
        using UpHelper = DeterminantHelper<T, N, sR1, sR2, sR3, sC1, sC2, sC3>;
        static T compute(T const* data)
        {
          return data[index(R1, C1)] * UpHelper<R2, R3, R4, C2, C3, C4>::compute(data) -
                 data[index(R1, C2)] * UpHelper<R2, R3, R4, C1, C3, C4>::compute(data) +
                 data[index(R1, C3)] * UpHelper<R2, R3, R4, C1, C2, C4>::compute(data) -
                 data[index(R1, C4)] * UpHelper<R2, R3, R4, C1, C2, C3>::compute(data);
        }
      }; // struct DeterminantHelper<>

      template <typename T>
      struct FastMatrixOperations<T, 2> : public FastMatrixOperationsBase<T, 2> {
        using Base_t = FastMatrixOperationsBase<T, 2>;
        static constexpr unsigned int Dim = Base_t::Dim;
        using Data_t = typename Base_t::Data_t;
        using Matrix_t = typename Base_t::Matrix_t;

        static Data_t Determinant(Matrix_t const& mat)
        {
          return DeterminantHelper<T, Dim, 0, 1, 0, 1>::compute(mat.data());
        }

        static Matrix_t InvertMatrix(Matrix_t const& mat, Data_t det);

        static Matrix_t InvertSymmetricMatrix(Matrix_t const& mat, Data_t det);

        static Matrix_t InvertMatrix(Matrix_t const& mat)
        {
          return InvertMatrix(mat, Determinant(mat));
        }

        static Matrix_t InvertSymmetricMatrix(Matrix_t const& mat)
        {
          return InvertSymmetricMatrix(mat, Determinant(mat));
        }

      }; // struct FastMatrixOperations<T, 2>

      template <typename T>
      struct FastMatrixOperations<T, 3> : public FastMatrixOperationsBase<T, 3> {
        using Base_t = FastMatrixOperationsBase<T, 3>;
        static constexpr unsigned int Dim = Base_t::Dim;
        using Data_t = typename Base_t::Data_t;
        using Matrix_t = typename Base_t::Matrix_t;

        static Data_t Determinant(Matrix_t const& mat)
        {
          return DeterminantHelper<T, Dim, 0, 1, 2, 0, 1, 2>::compute(mat.data());
        }

        static Matrix_t InvertMatrix(Matrix_t const& mat, Data_t det);

        static Matrix_t InvertSymmetricMatrix(Matrix_t const& mat, Data_t det);

        static Matrix_t InvertMatrix(Matrix_t const& mat)
        {
          return InvertMatrix(mat, Determinant(mat));
        }

        static Matrix_t InvertSymmetricMatrix(Matrix_t const& mat)
        {
          return InvertSymmetricMatrix(mat, Determinant(mat));
        }

      }; // struct FastMatrixOperations<T, 3>

      template <typename T>
      struct FastMatrixOperations<T, 4> : public FastMatrixOperationsBase<T, 4> {
        using Base_t = FastMatrixOperationsBase<T, 4>;
        static constexpr unsigned int Dim = Base_t::Dim;
        using Data_t = typename Base_t::Data_t;
        using Matrix_t = typename Base_t::Matrix_t;

        static Data_t Determinant(Matrix_t const& mat)
        {
          return DeterminantHelper<T, Dim, 0, 1, 2, 3, 0, 1, 2, 3>::compute(mat.data());
        }

        static Matrix_t InvertMatrix(Matrix_t const& mat, Data_t det);

        static Matrix_t InvertSymmetricMatrix(Matrix_t const& mat, Data_t det);

        static Matrix_t InvertMatrix(Matrix_t const& mat)
        {
          return InvertMatrix(mat, Determinant(mat));
        }

        static Matrix_t InvertSymmetricMatrix(Matrix_t const& mat)
        {
          return InvertSymmetricMatrix(mat, Determinant(mat));
        }

      }; // struct FastMatrixOperations<T, 3>

    } // namespace details
  }   // namespace util
} // namespace lar

template <typename T, unsigned int DIM>
auto lar::util::details::FastMatrixOperationsBase<T, DIM>::MatrixVectorProduct(Matrix_t const& mat,
                                                                               Vector_t const& vec)
  -> Vector_t
{
  // not really fast, but there is probably not much to fasten...
  Vector_t res;
  Data_t const* mat_row = mat.data();
  for (size_t r = 0; r < Dim; ++r) {
    Data_t elem = Data_t(0);
    for (size_t c = 0; c < Dim; ++c)
      elem += *(mat_row++) * vec[c];
    res[r] = elem;
  } // for
  return res;
} // FastMatrixOperationsBase<>::MatrixVectorProduct()

template <typename T>
auto lar::util::details::FastMatrixOperations<T, 2>::InvertMatrix(Matrix_t const& mat, Data_t det)
  -> Matrix_t
{
  Matrix_t Inverse;
  Inverse[0 * Dim + 0] = mat[1 * Dim + 1] / det;
  Inverse[0 * Dim + 1] = -mat[0 * Dim + 1] / det;
  Inverse[1 * Dim + 0] = -mat[1 * Dim + 0] / det;
  Inverse[1 * Dim + 1] = mat[0 * Dim + 0] / det;
  return Inverse;
} // FastMatrixOperations<T, 2>::InvertMatrix()

template <typename T>
auto lar::util::details::FastMatrixOperations<T, 2>::InvertSymmetricMatrix(Matrix_t const& mat,
                                                                           Data_t det) -> Matrix_t
{
  Matrix_t Inverse;
  Inverse[0 * Dim + 0] = mat[1 * Dim + 1] / det;
  Inverse[0 * Dim + 1] = Inverse[1 * Dim + 0] = -mat[1 * Dim + 0] / det;
  Inverse[1 * Dim + 1] = mat[0 * Dim + 0] / det;
  return Inverse;
} // FastMatrixOperations<T, 2>::InvertMatrix()

template <typename T>
auto lar::util::details::FastMatrixOperations<T, 3>::InvertMatrix(Matrix_t const& mat, Data_t det)
  -> Matrix_t
{
  Data_t const* data = mat.data();
  Matrix_t Inverse;
  //
  // Basically using Cramer's rule;
  // each element [r,c] gets assigned the determinant of the submatrix
  //   after removing c from the rows and r from the columns
  //   (effectively assigning the transpose of the minor matrix)
  //   with the usual sign -1^(r+c)
  //
  //
  Inverse[0 * Dim + 0] = DeterminantHelper<T, 3, 1, 2, 1, 2>::compute(data) / det;
  Inverse[0 * Dim + 1] = -DeterminantHelper<T, 3, 0, 2, 1, 2>::compute(data) / det;
  Inverse[0 * Dim + 2] = DeterminantHelper<T, 3, 0, 1, 1, 2>::compute(data) / det;
  Inverse[1 * Dim + 0] = -DeterminantHelper<T, 3, 1, 2, 0, 2>::compute(data) / det;
  Inverse[1 * Dim + 1] = DeterminantHelper<T, 3, 0, 2, 0, 2>::compute(data) / det;
  Inverse[1 * Dim + 2] = -DeterminantHelper<T, 3, 0, 1, 0, 2>::compute(data) / det;
  Inverse[2 * Dim + 0] = DeterminantHelper<T, 3, 1, 2, 0, 1>::compute(data) / det;
  Inverse[2 * Dim + 1] = -DeterminantHelper<T, 3, 0, 2, 0, 1>::compute(data) / det;
  Inverse[2 * Dim + 2] = DeterminantHelper<T, 3, 0, 1, 0, 1>::compute(data) / det;
  return Inverse;
} // FastMatrixOperations<T, 3>::InvertMatrix()

template <typename T>
auto lar::util::details::FastMatrixOperations<T, 3>::InvertSymmetricMatrix(Matrix_t const& mat,
                                                                           Data_t det) -> Matrix_t
{
  //
  // Same algorithm as InvertMatrix(), but use the fact that the result is
  // also symmetric
  //
  Data_t const* data = mat.data();
  Matrix_t Inverse;
  Inverse[0 * Dim + 0] = DeterminantHelper<T, 3, 1, 2, 1, 2>::compute(data) / det;
  Inverse[0 * Dim + 1] = Inverse[1 * Dim + 0] =
    -DeterminantHelper<T, 3, 1, 2, 0, 2>::compute(data) / det;
  Inverse[0 * Dim + 2] = Inverse[2 * Dim + 0] =
    DeterminantHelper<T, 3, 1, 2, 0, 1>::compute(data) / det;
  Inverse[1 * Dim + 1] = DeterminantHelper<T, 3, 0, 2, 0, 2>::compute(data) / det;
  Inverse[1 * Dim + 2] = Inverse[2 * Dim + 1] =
    -DeterminantHelper<T, 3, 0, 2, 0, 1>::compute(data) / det;
  Inverse[2 * Dim + 2] = DeterminantHelper<T, 3, 0, 1, 0, 1>::compute(data) / det;
  return Inverse;
} // FastMatrixOperations<T, 3>::InvertSymmetricMatrix()

template <typename T>
auto lar::util::details::FastMatrixOperations<T, 4>::InvertMatrix(Matrix_t const& mat, Data_t det)
  -> Matrix_t
{
  //
  // Basically using Cramer's rule;
  // each element [r,c] gets assigned the determinant of the submatrix
  //   after removing c from the rows and r from the columns
  //   (effectively assigning the transpose of the minor matrix)
  //   with the usual sign -1^(r+c)
  //
  //
  Data_t const* data = mat.data();
  Matrix_t Inverse;
  Inverse[0 * Dim + 0] = DeterminantHelper<T, 4, 1, 2, 3, 1, 2, 3>::compute(data) / det;
  Inverse[0 * Dim + 1] = -DeterminantHelper<T, 4, 0, 2, 3, 1, 2, 3>::compute(data) / det;
  Inverse[0 * Dim + 2] = DeterminantHelper<T, 4, 0, 1, 3, 1, 2, 3>::compute(data) / det;
  Inverse[0 * Dim + 3] = -DeterminantHelper<T, 4, 0, 1, 2, 1, 2, 3>::compute(data) / det;
  Inverse[1 * Dim + 0] = -DeterminantHelper<T, 4, 1, 2, 3, 0, 2, 3>::compute(data) / det;
  Inverse[1 * Dim + 1] = DeterminantHelper<T, 4, 0, 2, 3, 0, 2, 3>::compute(data) / det;
  Inverse[1 * Dim + 2] = -DeterminantHelper<T, 4, 0, 1, 3, 0, 2, 3>::compute(data) / det;
  Inverse[1 * Dim + 3] = DeterminantHelper<T, 4, 0, 1, 2, 0, 2, 3>::compute(data) / det;
  Inverse[2 * Dim + 0] = DeterminantHelper<T, 4, 1, 2, 3, 0, 1, 3>::compute(data) / det;
  Inverse[2 * Dim + 1] = -DeterminantHelper<T, 4, 0, 2, 3, 0, 1, 3>::compute(data) / det;
  Inverse[2 * Dim + 2] = DeterminantHelper<T, 4, 0, 1, 3, 0, 1, 3>::compute(data) / det;
  Inverse[2 * Dim + 3] = -DeterminantHelper<T, 4, 0, 1, 2, 0, 1, 3>::compute(data) / det;
  Inverse[3 * Dim + 0] = -DeterminantHelper<T, 4, 1, 2, 3, 0, 1, 2>::compute(data) / det;
  Inverse[3 * Dim + 1] = DeterminantHelper<T, 4, 0, 2, 3, 0, 1, 2>::compute(data) / det;
  Inverse[3 * Dim + 2] = -DeterminantHelper<T, 4, 0, 1, 3, 0, 1, 2>::compute(data) / det;
  Inverse[3 * Dim + 3] = DeterminantHelper<T, 4, 0, 1, 2, 0, 1, 2>::compute(data) / det;
  return Inverse;
} // FastMatrixOperations<T, 4>::InvertMatrix()

template <typename T>
auto lar::util::details::FastMatrixOperations<T, 4>::InvertSymmetricMatrix(Matrix_t const& mat,
                                                                           Data_t det) -> Matrix_t
{
  //
  // Same algorithm as InvertMatrix(), but use the fact that the result is
  // also symmetric
  //
  Data_t const* data = mat.data();
  Matrix_t Inverse;
  Inverse[0 * Dim + 0] = DeterminantHelper<T, 4, 1, 2, 3, 1, 2, 3>::compute(data) / det;
  Inverse[0 * Dim + 1] = Inverse[1 * Dim + 0] =
    -DeterminantHelper<T, 4, 1, 2, 3, 0, 2, 3>::compute(data) / det;
  Inverse[0 * Dim + 2] = Inverse[2 * Dim + 0] =
    DeterminantHelper<T, 4, 1, 2, 3, 0, 1, 3>::compute(data) / det;
  Inverse[0 * Dim + 3] = Inverse[3 * Dim + 0] =
    -DeterminantHelper<T, 4, 1, 2, 3, 0, 1, 2>::compute(data) / det;
  Inverse[1 * Dim + 1] = DeterminantHelper<T, 4, 0, 2, 3, 0, 2, 3>::compute(data) / det;
  Inverse[1 * Dim + 2] = Inverse[2 * Dim + 1] =
    -DeterminantHelper<T, 4, 0, 2, 3, 3, 0, 1>::compute(data) / det;
  Inverse[1 * Dim + 3] = Inverse[3 * Dim + 1] =
    DeterminantHelper<T, 4, 0, 2, 3, 0, 1, 2>::compute(data) / det;
  Inverse[2 * Dim + 2] = DeterminantHelper<T, 4, 0, 1, 3, 0, 1, 3>::compute(data) / det;
  Inverse[2 * Dim + 3] = Inverse[3 * Dim + 2] =
    -DeterminantHelper<T, 4, 0, 1, 3, 0, 1, 2>::compute(data) / det;
  Inverse[3 * Dim + 3] = DeterminantHelper<T, 4, 0, 1, 2, 0, 1, 2>::compute(data) / det;
  return Inverse;
} // FastMatrixOperations<T, 4>::InvertSymmetricMatrix()

#endif // FASTMATRIXMATHHELPER_H