PartialPivLU.h

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// 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/.
// SPDX-License-Identifier: MPL-2.0
 
#ifndef EIGEN_PARTIALLU_H
#define EIGEN_PARTIALLU_H
 
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
 
namespace Eigen {
 
namespace internal {
template <typename MatrixType_, typename PermutationIndex_>
struct traits<PartialPivLU<MatrixType_, PermutationIndex_> > : traits<MatrixType_> {
  using XprKind = MatrixXpr;
  using StorageKind = SolverStorage;
  using StorageIndex = PermutationIndex_;
  using BaseTraits = traits<MatrixType_>;
  enum { Flags = BaseTraits::Flags & RowMajorBit, CoeffReadCost = Dynamic };
};
 
}  // end namespace internal

template <typename MatrixType_, typename PermutationIndex_>
class PartialPivLU : public SolverBase<PartialPivLU<MatrixType_, PermutationIndex_> > {
 public:
  using MatrixType = MatrixType_;
  using Base = SolverBase<PartialPivLU>;
  friend class SolverBase<PartialPivLU>;
 
  EIGEN_GENERIC_PUBLIC_INTERFACE(PartialPivLU)
  enum {
    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
  };
  using PermutationIndex = PermutationIndex_;
  using PermutationType = PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime, PermutationIndex>;
  using TranspositionType = Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime, PermutationIndex>;
  using PlainObject = typename MatrixType::PlainObject;

  ComputationInfo info() const {
    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
    return Success;
  }

  PartialPivLU();

  explicit PartialPivLU(Index size);

  template <typename InputType>
  explicit PartialPivLU(const EigenBase<InputType>& matrix);

  template <typename InputType>
  explicit PartialPivLU(EigenBase<InputType>& matrix);
 
  template <typename InputType>
  PartialPivLU& compute(const EigenBase<InputType>& matrix) {
    m_lu = matrix.derived();
    compute();
    return *this;
  }

  inline const MatrixType& matrixLU() const {
    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
    return m_lu;
  }

  inline const PermutationType& permutationP() const {
    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
    return m_p;
  }
 
#ifdef EIGEN_PARSED_BY_DOXYGEN
  template <typename Rhs>
  inline Solve<PartialPivLU, Rhs> solve(const MatrixBase<Rhs>& b) const;
#endif

  inline RealScalar rcond() const {
    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
    return internal::rcond_estimate_helper(m_l1_norm, *this);
  }

  inline Inverse<PartialPivLU> inverse() const {
    eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
    return Inverse<PartialPivLU>(*this);
  }

  Scalar determinant() const;

  RealScalar absDeterminant() const;

  RealScalar logAbsDeterminant() const;

  Scalar signDeterminant() const;
 
  MatrixType reconstructedMatrix() const;
 
  constexpr Index rows() const noexcept { return m_lu.rows(); }
  constexpr Index cols() const noexcept { return m_lu.cols(); }
 
#ifndef EIGEN_PARSED_BY_DOXYGEN
  template <typename RhsType, typename DstType>
  EIGEN_DEVICE_FUNC void _solve_impl(const RhsType& rhs, DstType& dst) const {
    /* The decomposition PA = LU can be rewritten as A = P^{-1} L U.
     * So we proceed as follows:
     * Step 1: compute c = Pb.
     * Step 2: replace c by the solution x to Lx = c.
     * Step 3: replace c by the solution x to Ux = c.
     */
 
    // Step 1
    dst = permutationP() * rhs;
 
    // Step 2
    m_lu.template triangularView<UnitLower>().solveInPlace(dst);
 
    // Step 3
    m_lu.template triangularView<Upper>().solveInPlace(dst);
  }
 
  template <bool Conjugate, typename RhsType, typename DstType>
  EIGEN_DEVICE_FUNC void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const {
    /* The decomposition PA = LU can be rewritten as A^T = U^T L^T P.
     * So we proceed as follows:
     * Step 1: compute c as the solution to L^T c = b
     * Step 2: replace c by the solution x to U^T x = c.
     * Step 3: update  c = P^-1 c.
     */
 
    eigen_assert(rhs.rows() == m_lu.cols());
 
    // Step 1
    dst = m_lu.template triangularView<Upper>().transpose().template conjugateIf<Conjugate>().solve(rhs);
    // Step 2
    m_lu.template triangularView<UnitLower>().transpose().template conjugateIf<Conjugate>().solveInPlace(dst);
    // Step 3
    dst = permutationP().transpose() * dst;
  }
#endif
 
 protected:
  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
  void compute();
 
  MatrixType m_lu;
  PermutationType m_p;
  TranspositionType m_rowsTranspositions;
  RealScalar m_l1_norm;
  signed char m_det_p;
  bool m_isInitialized;
};
 
template <typename MatrixType, typename PermutationIndex>
PartialPivLU<MatrixType, PermutationIndex>::PartialPivLU()
    : m_lu(), m_p(), m_rowsTranspositions(), m_l1_norm(0), m_det_p(0), m_isInitialized(false) {}
 
template <typename MatrixType, typename PermutationIndex>
PartialPivLU<MatrixType, PermutationIndex>::PartialPivLU(Index size)
    : m_lu(size, size), m_p(size), m_rowsTranspositions(size), m_l1_norm(0), m_det_p(0), m_isInitialized(false) {}
 
template <typename MatrixType, typename PermutationIndex>
template <typename InputType>
PartialPivLU<MatrixType, PermutationIndex>::PartialPivLU(const EigenBase<InputType>& matrix)
    : m_lu(matrix.rows(), matrix.cols()),
      m_p(matrix.rows()),
      m_rowsTranspositions(matrix.rows()),
      m_l1_norm(0),
      m_det_p(0),
      m_isInitialized(false) {
  compute(matrix.derived());
}
 
template <typename MatrixType, typename PermutationIndex>
template <typename InputType>
PartialPivLU<MatrixType, PermutationIndex>::PartialPivLU(EigenBase<InputType>& matrix)
    : m_lu(matrix.derived()),
      m_p(matrix.rows()),
      m_rowsTranspositions(matrix.rows()),
      m_l1_norm(0),
      m_det_p(0),
      m_isInitialized(false) {
  compute();
}
 
namespace internal {

template <int K, int Size, bool Last = (K == Size - 1)>
struct unrolled_partial_lu_step {
  template <typename MatrixTypeRef, typename PivIndex>
  static EIGEN_STRONG_INLINE void run(MatrixTypeRef& lu, PivIndex* row_transpositions, PivIndex& nb_transpositions,
                                      Index& first_zero_pivot) {
    using Scalar = typename MatrixTypeRef::Scalar;
    using Scoring = scalar_score_coeff_op<Scalar>;
    using Score = typename Scoring::result_type;
    constexpr int Remaining = Size - K - 1;
 
    Index row_of_biggest_in_col;
    const Score biggest_in_corner =
        lu.col(K).template tail<Size - K>().unaryExpr(Scoring()).maxCoeff(&row_of_biggest_in_col);
    row_of_biggest_in_col += K;
    row_transpositions[K] = PivIndex(row_of_biggest_in_col);
    if (!numext::is_exactly_zero(biggest_in_corner)) {
      if (K != row_of_biggest_in_col) {
        lu.row(K).swap(lu.row(row_of_biggest_in_col));
        ++nb_transpositions;
      }
      lu.col(K).template tail<Remaining>() /= lu.coeff(K, K);
    } else if (first_zero_pivot == -1) {
      first_zero_pivot = K;
    }
    lu.template bottomRightCorner<Remaining, Remaining>().noalias() -=
        lu.col(K).template tail<Remaining>() * lu.row(K).template tail<Remaining>();
    unrolled_partial_lu_step<K + 1, Size>::run(lu, row_transpositions, nb_transpositions, first_zero_pivot);
  }
};
 
template <int K, int Size>
struct unrolled_partial_lu_step<K, Size, /*Last=*/true> {
  template <typename MatrixTypeRef, typename PivIndex>
  static EIGEN_STRONG_INLINE void run(MatrixTypeRef& lu, PivIndex* row_transpositions, PivIndex&,
                                      Index& first_zero_pivot) {
    using Scoring = scalar_score_coeff_op<typename MatrixTypeRef::Scalar>;
    row_transpositions[K] = PivIndex(K);
    if (numext::is_exactly_zero(Scoring()(lu.coeff(K, K))) && first_zero_pivot == -1) first_zero_pivot = K;
  }
};

template <bool Unroll, int Size>
struct unrolled_partial_lu {
  template <typename MatrixTypeRef, typename PivIndex>
  static EIGEN_STRONG_INLINE bool run(MatrixTypeRef&, PivIndex*, PivIndex&, Index&) {
    return false;
  }
};
 
template <int Size>
struct unrolled_partial_lu<true, Size> {
  template <typename MatrixTypeRef, typename PivIndex>
  static EIGEN_STRONG_INLINE bool run(MatrixTypeRef& lu, PivIndex* row_transpositions, PivIndex& nb_transpositions,
                                      Index& first_zero_pivot) {
    nb_transpositions = 0;
    first_zero_pivot = -1;
    unrolled_partial_lu_step<0, Size>::run(lu, row_transpositions, nb_transpositions, first_zero_pivot);
    return true;
  }
};

template <typename Scalar, int StorageOrder, typename PivIndex, int SizeAtCompileTime = Dynamic>
struct generic_partial_lu_impl {
  static constexpr int UnBlockedBound = 16;
  static constexpr bool UnBlockedAtCompileTime = SizeAtCompileTime != Dynamic && SizeAtCompileTime <= UnBlockedBound;
  static constexpr int ActualSizeAtCompileTime = UnBlockedAtCompileTime ? SizeAtCompileTime : Dynamic;
  // Unrolling gives the pivot search, scaling and update of every step compile-time extents; runtime-sized blocks'
  // vectorized loops cost more than their arithmetic (1.5-4.6x for 3x3 to 6x6 double, gcc and clang, AVX2). At 16,
  // gcc's unrolled code is slower than the loop.
  static constexpr int UnrolledBound = 12;
  static constexpr bool UnrolledAtCompileTime = UnBlockedAtCompileTime && SizeAtCompileTime <= UnrolledBound;
  // Remaining rows and columns at compile-time:
  static constexpr int RRows = SizeAtCompileTime == 2 ? 1 : Dynamic;
  static constexpr int RCols = SizeAtCompileTime == 2 ? 1 : Dynamic;
  using MatrixType = Matrix<Scalar, ActualSizeAtCompileTime, ActualSizeAtCompileTime, StorageOrder>;
  using MatrixTypeRef = Ref<MatrixType>;
  using BlockType = Ref<Matrix<Scalar, Dynamic, Dynamic, StorageOrder>>;
  using RealScalar = typename MatrixType::RealScalar;
 
  static void apply_row_transpositions(BlockType& matrix, Index first, Index count, const PivIndex* transpositions) {
    EIGEN_IF_CONSTEXPR (StorageOrder == ColMajor) {
      // Keep the pivot rows of one column in cache, even when the outer stride maps every column to the same set.
      for (Index j = 0; j < matrix.cols(); ++j)
        for (Index i = first; i < first + count; ++i)
          numext::swap(matrix.coeffRef(i, j), matrix.coeffRef(transpositions[i], j));
    } else {
      for (Index i = first; i < first + count; ++i) matrix.row(i).swap(matrix.row(transpositions[i]));
    }
  }

  static Index unblocked_lu(MatrixTypeRef& lu, PivIndex* row_transpositions, PivIndex& nb_transpositions) {
    using Scoring = scalar_score_coeff_op<Scalar>;
    using Score = typename Scoring::result_type;
    {
      Index first_zero_pivot;
      if (unrolled_partial_lu<UnrolledAtCompileTime, SizeAtCompileTime>::run(lu, row_transpositions, nb_transpositions,
                                                                             first_zero_pivot))
        return first_zero_pivot;
    }
    const Index rows = lu.rows();
    const Index cols = lu.cols();
    const Index size = (std::min)(rows, cols);
    // For small compile-time matrices and square runtime matrices it is worth processing the last row separately:
    //  speedup: +100% for 2x2, +10% for others.
    const bool process_last_row_separately = UnBlockedAtCompileTime || rows == cols;
    const Index endk = process_last_row_separately ? size - 1 : size;
    nb_transpositions = 0;
    Index first_zero_pivot = -1;
    for (Index k = 0; k < endk; ++k) {
      int rrows = internal::convert_index<int>(rows - k - 1);
      int rcols = internal::convert_index<int>(cols - k - 1);
 
      Index row_of_biggest_in_col;
      Score biggest_in_corner = lu.col(k).tail(rows - k).unaryExpr(Scoring()).maxCoeff(&row_of_biggest_in_col);
      row_of_biggest_in_col += k;
 
      row_transpositions[k] = PivIndex(row_of_biggest_in_col);
 
      if (!numext::is_exactly_zero(biggest_in_corner)) {
        if (k != row_of_biggest_in_col) {
          lu.row(k).swap(lu.row(row_of_biggest_in_col));
          ++nb_transpositions;
        }
 
        lu.col(k).tail(fix<RRows>(rrows)) /= lu.coeff(k, k);
      } else if (first_zero_pivot == -1) {
        // the pivot is exactly zero, we record the index of the first pivot which is exactly 0,
        // and continue the factorization such we still have P A = L U
        first_zero_pivot = k;
      }
 
      // Skip the trailing update for rectangular panels with no remaining columns.
      if (rrows > 0 && rcols > 0)
        lu.bottomRightCorner(fix<RRows>(rrows), fix<RCols>(rcols)).noalias() -=
            lu.col(k).tail(fix<RRows>(rrows)) * lu.row(k).tail(fix<RCols>(rcols));
    }
 
    // special handling of the last entry
    if (process_last_row_separately) {
      Index k = endk;
      row_transpositions[k] = PivIndex(k);
      if (numext::is_exactly_zero(Scoring()(lu(k, k))) && first_zero_pivot == -1) first_zero_pivot = k;
    }
 
    return first_zero_pivot;
  }

  static Index blocked_lu(Index rows, Index cols, Scalar* lu_data, Index luStride, PivIndex* row_transpositions,
                          PivIndex& nb_transpositions, Index maxBlockSize = 256) {
    MatrixTypeRef lu = MatrixType::Map(lu_data, rows, cols, OuterStride<>(luStride));
 
    const Index size = (std::min)(rows, cols);
 
    // if the matrix is too small, no blocking:
    EIGEN_IF_CONSTEXPR (UnBlockedAtCompileTime) {
      return unblocked_lu(lu, row_transpositions, nb_transpositions);
    } else if (size <= UnBlockedBound) {
      return unblocked_lu(lu, row_transpositions, nb_transpositions);
    }
 
    // automatically adjust the number of subdivisions to the size
    // of the matrix so that there is enough sub blocks:
    Index blockSize;
    {
      blockSize = size / 8;
      blockSize = (blockSize / 16) * 16;
      blockSize = (std::min)((std::max)(blockSize, Index(8)), maxBlockSize);
    }
 
    nb_transpositions = 0;
    Index first_zero_pivot = -1;
    for (Index k = 0; k < size; k += blockSize) {
      Index bs = (std::min)(size - k, blockSize);  // actual size of the block
      Index trows = rows - k - bs;                 // trailing rows
      Index tsize = cols - k - bs;                 // trailing columns
 
      // partition the matrix:
      //                          A00 | A01 | A02
      // lu  = A_0 | A_1 | A_2 =  A10 | A11 | A12
      //                          A20 | A21 | A22
      BlockType A_2 = lu.block(0, k + bs, rows, tsize);
      BlockType A11 = lu.block(k, k, bs, bs);
      BlockType A12 = lu.block(k, k + bs, bs, tsize);
      BlockType A21 = lu.block(k + bs, k, trows, bs);
      BlockType A22 = lu.block(k + bs, k + bs, trows, tsize);
 
      PivIndex nb_transpositions_in_panel;
      // recursively call the blocked LU algorithm on [A11^T A21^T]^T
      // with a very small blocking size:
      Index ret = blocked_lu(trows + bs, bs, &lu.coeffRef(k, k), luStride, row_transpositions + k,
                             nb_transpositions_in_panel, 16);
      if (ret >= 0 && first_zero_pivot == -1) first_zero_pivot = k + ret;
 
      nb_transpositions += nb_transpositions_in_panel;
      // update permutations and apply them to A_0
      if (k > 0) {
        BlockType A_0 = lu.block(0, 0, rows, k);
        for (Index i = k; i < k + bs; ++i) row_transpositions[i] += internal::convert_index<PivIndex>(k);
        apply_row_transpositions(A_0, k, bs, row_transpositions);
      }
 
      if (tsize) {
        // apply permutations to A_2
        apply_row_transpositions(A_2, k, bs, row_transpositions);
 
        // A12 = A11^-1 A12
        A11.template triangularView<UnitLower>().solveInPlace(A12);
 
        A22.noalias() -= A21 * A12;
      }
    }
    return first_zero_pivot;
  }
};
 
template <typename Scalar, int StorageOrder, typename PivIndex, int SizeAtCompileTime = Dynamic>
struct partial_lu_impl : generic_partial_lu_impl<Scalar, StorageOrder, PivIndex, SizeAtCompileTime> {};

template <typename MatrixType, typename TranspositionType>
void partial_lu_inplace(MatrixType& lu, TranspositionType& row_transpositions,
                        typename TranspositionType::StorageIndex& nb_transpositions) {
  // Special-case of zero matrix.
  if (lu.rows() == 0 || lu.cols() == 0) {
    nb_transpositions = 0;
    return;
  }
  eigen_assert(lu.cols() == row_transpositions.size());
  eigen_assert(row_transpositions.size() < 2 ||
               (&row_transpositions.coeffRef(1) - &row_transpositions.coeffRef(0)) == 1);
 
  partial_lu_impl<typename MatrixType::Scalar, MatrixType::Flags & RowMajorBit ? RowMajor : ColMajor,
                  typename TranspositionType::StorageIndex,
                  internal::min_size_prefer_fixed(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime)>::
      blocked_lu(lu.rows(), lu.cols(), &lu.coeffRef(0, 0), lu.outerStride(), &row_transpositions.coeffRef(0),
                 nb_transpositions);
}

template <typename Derived>
typename traits<Derived>::Scalar partial_lu_determinant(const Derived& m) {
  using Scalar = typename traits<Derived>::Scalar;
  if (m.rows() == 0) return Scalar(1);
  EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
 
  using PlainObject = typename plain_matrix_type<Derived>::type;
  using TranspositionType =
      Transpositions<PlainObject::RowsAtCompileTime, PlainObject::MaxRowsAtCompileTime, DefaultPermutationIndex>;
 
  eigen_assert(m.rows() < NumTraits<DefaultPermutationIndex>::highest());
  PlainObject lu(m);
 
  TranspositionType row_transpositions(lu.rows());
  typename TranspositionType::StorageIndex nb_transpositions;
  partial_lu_inplace(lu, row_transpositions, nb_transpositions);
 
  return Scalar((nb_transpositions % 2) ? -1 : 1) * lu.diagonal().prod();
}
 
}  // end namespace internal
 
template <typename MatrixType, typename PermutationIndex>
void PartialPivLU<MatrixType, PermutationIndex>::compute() {
  eigen_assert(m_lu.rows() < NumTraits<PermutationIndex>::highest());
 
  if (m_lu.cols() > 0)
    m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff();
  else
    m_l1_norm = RealScalar(0);
 
  eigen_assert(m_lu.rows() == m_lu.cols() && "PartialPivLU is only for square (and moreover invertible) matrices");
  const Index size = m_lu.rows();
 
  m_rowsTranspositions.resize(size);
 
  typename TranspositionType::StorageIndex nb_transpositions;
  internal::partial_lu_inplace(m_lu, m_rowsTranspositions, nb_transpositions);
  m_det_p = (nb_transpositions % 2) ? -1 : 1;
 
  m_p = m_rowsTranspositions;
 
  m_isInitialized = true;
}
 
template <typename MatrixType, typename PermutationIndex>
typename PartialPivLU<MatrixType, PermutationIndex>::Scalar PartialPivLU<MatrixType, PermutationIndex>::determinant()
    const {
  eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
  return Scalar(m_det_p) * m_lu.diagonal().prod();
}
 
template <typename MatrixType, typename PermutationIndex>
typename PartialPivLU<MatrixType, PermutationIndex>::RealScalar
PartialPivLU<MatrixType, PermutationIndex>::absDeterminant() const {
  eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
  return numext::abs(m_lu.diagonal().prod());
}
 
template <typename MatrixType, typename PermutationIndex>
typename PartialPivLU<MatrixType, PermutationIndex>::RealScalar
PartialPivLU<MatrixType, PermutationIndex>::logAbsDeterminant() const {
  eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
  return m_lu.diagonal().cwiseAbs().array().log().sum();
}
 
template <typename MatrixType, typename PermutationIndex>
typename PartialPivLU<MatrixType, PermutationIndex>::Scalar
PartialPivLU<MatrixType, PermutationIndex>::signDeterminant() const {
  eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
  return Scalar(m_det_p) * m_lu.diagonal().array().sign().prod();
}

template <typename MatrixType, typename PermutationIndex>
MatrixType PartialPivLU<MatrixType, PermutationIndex>::reconstructedMatrix() const {
  eigen_assert(m_isInitialized && "LU is not initialized.");
  // LU
  MatrixType res = m_lu.template triangularView<UnitLower>().toDenseMatrix() * m_lu.template triangularView<Upper>();
 
  // P^{-1}(LU)
  res = m_p.inverse() * res;
 
  return res;
}
 
/***** Implementation details *****************************************************/
 
namespace internal {
 
/***** Implementation of inverse() *****************************************************/
template <typename DstXprType, typename MatrixType, typename PermutationIndex>
struct Assignment<
    DstXprType, Inverse<PartialPivLU<MatrixType, PermutationIndex> >,
    internal::assign_op<typename DstXprType::Scalar, typename PartialPivLU<MatrixType, PermutationIndex>::Scalar>,
    Dense2Dense> {
  using LuType = PartialPivLU<MatrixType, PermutationIndex>;
  using SrcXprType = Inverse<LuType>;
  static void run(DstXprType& dst, const SrcXprType& src,
                  const internal::assign_op<typename DstXprType::Scalar, typename LuType::Scalar>&) {
    dst = src.nestedExpression().solve(MatrixType::Identity(src.rows(), src.cols()));
  }
};
}  // end namespace internal
 
/******** MatrixBase methods *******/

template <typename Derived>
template <typename PermutationIndex>
inline PartialPivLU<typename MatrixBase<Derived>::PlainObject, PermutationIndex> MatrixBase<Derived>::partialPivLu()
    const {
  return PartialPivLU<PlainObject, PermutationIndex>(eval());
}

template <typename Derived>
template <typename PermutationIndex>
inline PartialPivLU<typename MatrixBase<Derived>::PlainObject, PermutationIndex> MatrixBase<Derived>::lu() const {
  return PartialPivLU<PlainObject, PermutationIndex>(eval());
}
 
}  // end namespace Eigen
 
#endif  // EIGEN_PARTIALLU_H