AlgebraHelpers.hpp

Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file AlgebraHelpers.hpp

// This file is part of the Acts project.
//
// Copyright (C) 2016-2023 CERN for the benefit of the Acts project
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#pragma once

#include "Acts/Definitions/Algebra.hpp"

#include <bitset>
#include <optional>

#include "Eigen/Dense"

namespace Acts {

template <typename MatrixType>
MatrixType bitsetToMatrix(const std::bitset<MatrixType::RowsAtCompileTime *
                                            MatrixType::ColsAtCompileTime>
                              bs) {
  constexpr int rows = MatrixType::RowsAtCompileTime;
  constexpr int cols = MatrixType::ColsAtCompileTime;

  static_assert(rows != -1 && cols != -1,
                "bitsetToMatrix does not support dynamic matrices");

  MatrixType m;
  auto* p = m.data();
  for (size_t i = 0; i < rows * cols; i++) {
    p[i] = bs[rows * cols - 1 - i];
  }
  return m;
}

template <typename Derived>
auto matrixToBitset(const Eigen::PlainObjectBase<Derived>& m) {
  using MatrixType = Eigen::PlainObjectBase<Derived>;
  constexpr size_t rows = MatrixType::RowsAtCompileTime;
  constexpr size_t cols = MatrixType::ColsAtCompileTime;

  std::bitset<rows * cols> res;

  auto* p = m.data();
  for (size_t i = 0; i < rows * cols; i++) {
    res[rows * cols - 1 - i] = static_cast<bool>(p[i]);
  }

  return res;
}

template <typename A, typename B>
inline ActsMatrix<A::RowsAtCompileTime, B::ColsAtCompileTime> blockedMult(
    const A& a, const B& b) {
  // Extract the sizes of the matrix types that we receive as template
  // parameters.
  constexpr int M = A::RowsAtCompileTime;
  constexpr int N = A::ColsAtCompileTime;
  constexpr int P = B::ColsAtCompileTime;

  // Ensure that the second dimension of our first matrix equals the first
  // dimension of the second matrix, otherwise we cannot multiply.
  static_assert(N == B::RowsAtCompileTime);

  if constexpr (M <= 4 && N <= 4 && P <= 4) {
    // In cases where the matrices being multiplied are small, we can rely on
    // Eigen do to a good job, and we don't really need to do any blocking.
    return a * b;
  } else {
    // Here, we want to calculate the expression: C = AB, Eigen, natively,
    // doesn't do a great job at this if the matrices A and B are large
    // (roughly M >= 8, N >= 8, or P >= 8), and applies a slow GEMM operation.
    // We apply a blocked matrix multiplication operation to decompose the
    // multiplication into smaller operations, relying on the fact that:
    //
    // ┌         ┐   ┌         ┐ ┌         ┐
    // │ C₁₁ C₁₂ │ = │ A₁₁ A₁₂ │ │ B₁₁ B₁₂ │
    // │ C₂₁ C₂₂ │ = │ A₂₁ A₂₂ │ │ B₂₁ B₂₂ │
    // └         ┘   └         ┘ └         ┘
    //
    // where:
    //
    // C₁₁ = A₁₁ * B₁₁ + A₁₂ * B₂₁
    // C₁₂ = A₁₁ * B₁₂ + A₁₂ * B₂₂
    // C₂₁ = A₂₁ * B₁₁ + A₂₂ * B₂₁
    // C₂₂ = A₂₁ * B₁₂ + A₂₂ * B₂₂
    //
    // The sizes of these submatrices are roughly half (in each dimension) that
    // of the parent matrix. If the size of the parent matrix is even, we can
    // divide it exactly, If the size of the parent matrix is odd, then some
    // of the submatrices will be one larger than the others. In general, for
    // any matrix Q, the sizes of the submatrices are (where / denotes integer
    // division):
    //
    // Q₁₁ : M / 2 × P / 2
    // Q₁₂ : M / 2 × (P + 1) / 2
    // Q₂₁ : (M + 1) / 2 × P / 2
    // Q₂₂ : (M + 1) / 2 × (P + 1) / 2
    //
    // See https://csapp.cs.cmu.edu/public/waside/waside-blocking.pdf for a
    // more in-depth explanation of blocked matrix multiplication.
    constexpr int M1 = M / 2;
    constexpr int M2 = (M + 1) / 2;
    constexpr int N1 = N / 2;
    constexpr int N2 = (N + 1) / 2;
    constexpr int P1 = P / 2;
    constexpr int P2 = (P + 1) / 2;

    // Construct the end result in this matrix, which destroys a few of Eigen's
    // built-in optimization techniques, but sadly this is necessary.
    ActsMatrix<M, P> r;

    // C₁₁ = A₁₁ * B₁₁ + A₁₂ * B₂₁
    r.template topLeftCorner<M1, P1>().noalias() =
        a.template topLeftCorner<M1, N1>() *
            b.template topLeftCorner<N1, P1>() +
        a.template topRightCorner<M1, N2>() *
            b.template bottomLeftCorner<N2, P1>();

    // C₁₂ = A₁₁ * B₁₂ + A₁₂ * B₂₂
    r.template topRightCorner<M1, P2>().noalias() =
        a.template topLeftCorner<M1, N1>() *
            b.template topRightCorner<N1, P2>() +
        a.template topRightCorner<M1, N2>() *
            b.template bottomRightCorner<N2, P2>();

    // C₂₁ = A₂₁ * B₁₁ + A₂₂ * B₂₁
    r.template bottomLeftCorner<M2, P1>().noalias() =
        a.template bottomLeftCorner<M2, N1>() *
            b.template topLeftCorner<N1, P1>() +
        a.template bottomRightCorner<M2, N2>() *
            b.template bottomLeftCorner<N2, P1>();

    // C₂₂ = A₂₁ * B₁₂ + A₂₂ * B₂₂
    r.template bottomRightCorner<M2, P2>().noalias() =
        a.template bottomLeftCorner<M2, N1>() *
            b.template topRightCorner<N1, P2>() +
        a.template bottomRightCorner<M2, N2>() *
            b.template bottomRightCorner<N2, P2>();

    return r;
  }
}


template <typename MatrixType, typename ResultType = MatrixType>
std::optional<ResultType> safeInverse(const MatrixType& m) noexcept {
  ResultType result;
  bool invertible = false;

  m.computeInverseWithCheck(result, invertible);

  if (invertible) {
    return result;
  }

  return std::nullopt;
}

template <typename T>
struct ExpSafeLimit {};
template <>
struct ExpSafeLimit<double> {
  constexpr static double value = 500.0;
};
template <>
struct ExpSafeLimit<float> {
  constexpr static float value = 50.0;
};

template <typename T>
constexpr T safeExp(T val) noexcept {
  constexpr T maxExponent = ExpSafeLimit<T>::value;
  constexpr T minExponent = -maxExponent;
  if (val < minExponent) {
    return 0.0;
  }

  if (val > maxExponent) {
    return std::numeric_limits<T>::infinity();
  }

  return std::exp(val);
}

}  // namespace Acts