FastMatrixMathHelper.h

Go to the documentation of this file.

#ifndef FASTMATRIXMATHHELPER_H
#define FASTMATRIXMATHHELPER_H 1

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


#include "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