Householder.h

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 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_HOUSEHOLDER_H
#define EIGEN_HOUSEHOLDER_H
 
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
 
namespace Eigen {
 
namespace internal {
template <int N>
struct decrement_size : std::integral_constant<int, N - 1> {};
template <>
struct decrement_size<0> : std::integral_constant<int, 0> {};
template <>
struct decrement_size<Dynamic> : std::integral_constant<int, Dynamic> {};
 
template <typename RealScalar>
struct householder_norm_accumulator : stable_norm_accumulator<RealScalar> {};
 
template <>
struct householder_norm_accumulator<float> {
  // Stable norm reductions can avoid underflow by scaling in float. Householder construction must also preserve each
  // subnormal component while forming scale-free tau and essential-vector ratios, so widen only these intermediates.
  using type = double;
};
 
template <typename Scalar, typename Accumulator, bool IsComplex = NumTraits<Scalar>::IsComplex>
struct householder_rescale;
 
template <typename Scalar, typename Accumulator>
struct householder_rescale<Scalar, Accumulator, false> {
  EIGEN_DEVICE_FUNC static Scalar run(const Scalar& value, const Accumulator& scale) {
    return Scalar(Accumulator(value) / scale);
  }
 
  // This overload only keeps the shared C++14 call site well-formed; real inputs return before reaching it.
  EIGEN_DEVICE_FUNC static Scalar zero_tail_tau(const Accumulator& scaledReal, const Accumulator&,
                                                const Scalar& scaledBeta) {
    return Scalar(1) - Scalar(scaledReal) / scaledBeta;
  }
 
  template <typename EssentialPart, typename TailView>
  EIGEN_DEVICE_FUNC static void run(EssentialPart& essential, const TailView& tail, const Accumulator& scale,
                                    const Scalar& denominator) {
    essential = ((tail.template cast<Accumulator>().array() / scale) / Accumulator(denominator))
                    .matrix()
                    .template cast<Scalar>();
  }
};
 
template <typename Scalar, typename Accumulator>
struct householder_rescale<Scalar, Accumulator, true> {
  using RealScalar = typename NumTraits<Scalar>::Real;
 
  EIGEN_DEVICE_FUNC static Scalar run(const Scalar& value, const Accumulator& scale) {
    return Scalar(RealScalar(Accumulator(numext::real(value)) / scale),
                  RealScalar(Accumulator(numext::imag(value)) / scale));
  }
 
  EIGEN_DEVICE_FUNC static Scalar zero_tail_tau(const Accumulator& scaledReal, const Accumulator& scaledImag,
                                                const RealScalar& scaledBeta) {
    return Scalar(RealScalar(1) - RealScalar(scaledReal) / scaledBeta, RealScalar(scaledImag) / scaledBeta);
  }
 
  template <typename EssentialPart, typename TailView>
  EIGEN_DEVICE_FUNC static void run(EssentialPart& essential, const TailView& tail, const Accumulator& scale,
                                    const Scalar& denominator) {
    run(essential, tail, scale, denominator, complex_array_access<Scalar>());
  }
 
 private:
  template <typename EssentialPart, typename TailView>
  EIGEN_DEVICE_FUNC static void run(EssentialPart& essential, const TailView& tail, const Accumulator& scale,
                                    const Scalar& denominator, std::true_type) {
    essential.realView().array() =
        (tail.realView().array().template cast<Accumulator>() / scale).template cast<RealScalar>();
    essential.array() /= denominator;
  }
 
  template <typename EssentialPart, typename TailView>
  EIGEN_DEVICE_FUNC static void run(EssentialPart& essential, const TailView& tail, const Accumulator& scale,
                                    const Scalar& denominator, std::false_type) {
    for (Index i = 0; i < tail.size(); ++i) essential.coeffRef(i) = run(tail.coeff(i), scale) / denominator;
  }
};
}  // namespace internal

template <typename Derived>
EIGEN_DEVICE_FUNC void MatrixBase<Derived>::makeHouseholderInPlace(Scalar& tau, RealScalar& beta) {
  VectorBlock<Derived, internal::decrement_size<Base::SizeAtCompileTime>::value> essentialPart(derived(), 1,
                                                                                               size() - 1);
  makeHouseholder(essentialPart, tau, beta);
}

template <typename Derived>
template <typename EssentialPart>
EIGEN_DEVICE_FUNC void MatrixBase<Derived>::makeHouseholder(EssentialPart& essential, Scalar& tau,
                                                            RealScalar& beta) const {
  using numext::conj;
 
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(EssentialPart)
  const VectorBlock<const Derived, EssentialPart::SizeAtCompileTime> tail(derived(), 1, size() - 1);
 
  const RealScalar tailSqNorm = size() == 1 ? RealScalar(0) : tail.unwind().squaredNorm();
  Scalar c0 = coeff(0);
  const RealScalar tol = (std::numeric_limits<RealScalar>::min)();
  RealScalar unscaledNormThreshold = tol;
  // Whether the direct construction's abs2(c0) + tailSqNorm would exceed the range. Integer scalars keep the direct
  // path they have always taken; the scaled path divides by a component maximum, which does not apply to them.
  bool unscaledSqNormOverflows = false;
  EIGEN_IF_CONSTEXPR (!NumTraits<RealScalar>::IsInteger) {
    const RealScalar precision = RealScalar(NumTraits<RealScalar>::epsilon());
    // With flush-to-zero arithmetic, every tail component square below tol can be lost. Account for every component
    // so the discarded contribution is at most epsilon relative to a squared norm above this threshold. The narrow
    // normal range of half makes this scaled path common for moderately small inputs; preserving the bound there is
    // intentional.
    const RealScalar componentCount = RealScalar(size() - 1) * RealScalar(NumTraits<Scalar>::IsComplex ? 2 : 1);
    unscaledNormThreshold = (tol / precision) * componentCount;
 
    // Both terms overflow well before the reflector stops being representable, so classify the input before the
    // squares are formed: abs2(c0) is at most twice the square of the larger component of c0, and the tail's own
    // reduction has already overflowed if tailSqNorm exceeds the bound. Testing the sum with isinf() instead would
    // not survive -ffinite-math-only, which folds that test away, whereas a comparison against a finite bound is
    // still evaluated.
    const RealScalar sqNormBound = NumTraits<RealScalar>::highest() / RealScalar(2);
    const RealScalar componentBound = numext::sqrt<RealScalar>(sqNormBound / RealScalar(2));
    const RealScalar c0Max = numext::maxi(numext::abs(numext::real(c0)), numext::abs(numext::imag(c0)));
    unscaledSqNormOverflows = !(c0Max <= componentBound) || !(tailSqNorm <= sqNormBound);
  }
 
  // The scaled path forms the reflector from ratios of the largest component and never squares an unscaled
  // coefficient, so it is also the path for inputs the direct construction cannot square.
  if ((tailSqNorm <= unscaledNormThreshold || unscaledSqNormOverflows) && !(numext::isnan)(c0)) {
    using Accumulator = typename internal::householder_norm_accumulator<RealScalar>::type;
    const auto tailView = tail.unwind();
    const auto tailComponents = tailView.realView();
    // Component maxima cannot underflow when a representable tail is nonzero.
    const Accumulator tailMax =
        tailView.size() == 0 ? Accumulator(0) : Accumulator(tailComponents.cwiseAbs().maxCoeff());
    if (numext::is_exactly_zero(tailMax) && numext::is_exactly_zero(numext::imag(c0))) {
      tau = RealScalar(0);
      beta = numext::real(c0);
      essential.setZero();
      return;
    }
    const Accumulator c0RealAbs = numext::abs(Accumulator(numext::real(c0)));
    const Accumulator c0ImagAbs = numext::abs(Accumulator(numext::imag(c0)));
    const Accumulator c0Max = numext::maxi(c0RealAbs, c0ImagAbs);
    const Accumulator scale = numext::maxi(c0Max, tailMax);
    const RealScalar realScale = RealScalar(scale);
    // A target that flushes this scale cannot form meaningful ratios from the entirely subnormal vector.
    if (scale < Accumulator(tol) && numext::is_exactly_zero(realScale + realScale)) {
      tau = RealScalar(0);
      beta = numext::real(c0);
      essential.setZero();
      return;
    }
    if (numext::is_exactly_zero(tailMax)) {
      const Accumulator scaledReal = Accumulator(numext::real(c0)) / scale;
      const Accumulator scaledImag = Accumulator(numext::imag(c0)) / scale;
      RealScalar scaledBeta = RealScalar(numext::hypot(scaledReal, scaledImag));
      if (numext::real(c0) >= RealScalar(0)) scaledBeta = -scaledBeta;
      beta = RealScalar(scale * Accumulator(scaledBeta));
      essential.setZero();
      tau = internal::householder_rescale<Scalar, Accumulator>::zero_tail_tau(scaledReal, scaledImag, scaledBeta);
      return;
    }
    // Form the reflector from scale-free ratios to preserve subnormal inputs and avoid overflowing c0 - beta.
    Accumulator scaledTailSqNorm;
    EIGEN_IF_CONSTEXPR (std::is_same<RealScalar, float>::value) {
      // Double has enough exponent range to square every finite float scale without underflow or overflow.
      scaledTailSqNorm = tailComponents.template cast<Accumulator>().squaredNorm() / (scale * scale);
    } else {
      scaledTailSqNorm = (tailComponents.template cast<Accumulator>().array() / scale).matrix().squaredNorm();
    }
    if (numext::is_exactly_zero(RealScalar(tailMax / scale)) && numext::is_exactly_zero(numext::imag(c0))) {
      tau = RealScalar(0);
      beta = numext::real(c0);
      essential.setZero();
      return;
    }
    const Scalar scaledC0 = internal::householder_rescale<Scalar, Accumulator>::run(c0, scale);
    RealScalar scaledBeta =
        RealScalar(numext::hypot(Accumulator(numext::abs(scaledC0)), numext::sqrt(scaledTailSqNorm)));
    if (numext::real(c0) >= RealScalar(0)) scaledBeta = -scaledBeta;
    beta = RealScalar(scale * Accumulator(scaledBeta));
    internal::householder_rescale<Scalar, Accumulator>::run(essential, tailView, scale, scaledC0 - scaledBeta);
    tau = conj(Scalar(RealScalar(1)) - scaledC0 / scaledBeta);
    return;
  }
  beta = numext::sqrt<RealScalar>(numext::abs2(c0) + tailSqNorm);
  if (numext::real(c0) >= RealScalar(0)) beta = -beta;
  essential = tail.unwind() / (c0 - beta);
  tau = conj((beta - c0) / beta);
}
 
namespace internal {
 
template <typename Derived, typename EssentialPart,
          bool Fused = !Derived::IsRowMajor && EssentialPart::ColsAtCompileTime == 1 &&
                       (EssentialPart::RowsAtCompileTime == 1 || EssentialPart::RowsAtCompileTime == 2)>
struct householder_apply_left_impl {
  using Scalar = typename Derived::Scalar;
  static EIGEN_DEVICE_FUNC void run(MatrixBase<Derived>& mat, const EssentialPart& essential, const Scalar& tau,
                                    Scalar* workspace) {
    Map<typename plain_row_type<typename Derived::PlainObject>::type> tmp(workspace, mat.cols());
    Block<Derived, EssentialPart::SizeAtCompileTime, Derived::ColsAtCompileTime> bottom(mat.derived(), 1, 0,
                                                                                        mat.rows() - 1, mat.cols());
    tmp.noalias() = essential.adjoint() * bottom.unwind();
    tmp = tau * (tmp + mat.row(0));
    mat.row(0) -= tmp;
    bottom.unwind().noalias() -= essential * tmp;
  }
};
 
// Two- and three-element reflectors on column-major storage: finishing each column before advancing replaces
// four strided row passes, which thrash the cache at large outer strides (issue #3160).
template <typename Derived, typename EssentialPart>
struct householder_apply_left_impl<Derived, EssentialPart, true> {
  using Scalar = typename Derived::Scalar;
  static EIGEN_DEVICE_FUNC void run(MatrixBase<Derived>& mat, const EssentialPart& essential, const Scalar& tau,
                                    Scalar*) {
    // Evaluated once so that the column loop reads plain coefficients; tau may reference a coefficient of mat.
    const Matrix<Scalar, EssentialPart::RowsAtCompileTime, 1> v = essential;
    const Scalar tauValue = tau;
    Block<Derived, EssentialPart::RowsAtCompileTime, Derived::ColsAtCompileTime> bottom(mat.derived(), 1, 0,
                                                                                        mat.rows() - 1, mat.cols());
    for (Index j = 0; j < mat.cols(); ++j) {
      const Scalar tmp = tauValue * (v.dot(bottom.col(j)) + mat.coeff(0, j));
      mat.coeffRef(0, j) -= tmp;
      bottom.col(j) -= v * tmp;
    }
  }
};
 
template <typename Derived, typename EssentialPart,
          bool Fused = !Derived::IsRowMajor && bool(traits<Derived>::Flags & DirectAccessBit) &&
                       inner_stride_at_compile_time<Derived>::value == 1 &&
                       packet_traits<typename Derived::Scalar>::Vectorizable && EssentialPart::ColsAtCompileTime == 1 &&
                       (EssentialPart::RowsAtCompileTime == 1 || EssentialPart::RowsAtCompileTime == 2)>
struct householder_apply_right_impl {
  using Scalar = typename Derived::Scalar;
  static EIGEN_DEVICE_FUNC void run(MatrixBase<Derived>& mat, const EssentialPart& essential, const Scalar& tau,
                                    Scalar* workspace) {
    Map<typename plain_col_type<typename Derived::PlainObject>::type> tmp(workspace, mat.rows());
    Block<Derived, Derived::RowsAtCompileTime, EssentialPart::SizeAtCompileTime> right(mat.derived(), 0, 1, mat.rows(),
                                                                                       mat.cols() - 1);
    tmp.noalias() = right.unwind() * essential;
    // M H = M - (M v) tau v^*: tau multiplies from the right, as in the one-column branch.
    tmp = (tmp + mat.col(0)) * tau;
    mat.col(0) = mat.col(0) - tmp;
    right.unwind().noalias() -= tmp * essential.adjoint();
  }
};
 
// Two- and three-element reflectors on contiguous columns: one pass down the rows updates all the columns, where the
// general path makes four passes through a temporary. These are the O(n^3) updates of the Francis QR step.
template <typename Derived, typename EssentialPart>
struct householder_apply_right_impl<Derived, EssentialPart, true> {
  using Scalar = typename Derived::Scalar;
  using Packet = typename packet_traits<Scalar>::type;
  static constexpr int K = EssentialPart::RowsAtCompileTime;
 
  static EIGEN_DEVICE_FUNC void run(MatrixBase<Derived>& mat, const EssentialPart& essential, const Scalar& tau,
                                    Scalar*) {
    constexpr Index PacketSize = unpacket_traits<Packet>::size;
    eigen_assert(mat.cols() == K + 1);
    // Copied first: tau and the essential part may alias coefficients of mat.
    Scalar v[K], vc[K];
    for (int j = 0; j < K; ++j) {
      v[j] = essential.coeff(j);
      vc[j] = numext::conj(v[j]);
    }
    const Scalar tauValue = tau;
    const Index rows = mat.rows();
    Scalar* col[K + 1];
    for (int j = 0; j <= K; ++j) col[j] = &mat.derived().coeffRef(0, j);
 
    const Packet ptau = pset1<Packet>(tauValue);
    Packet pv[K], pvc[K];
    for (int j = 0; j < K; ++j) {
      pv[j] = pset1<Packet>(v[j]);
      pvc[j] = pset1<Packet>(vc[j]);
    }
    const Index vectorEnd = numext::round_down(rows, PacketSize);
    for (Index i = 0; i < vectorEnd; i += PacketSize) {
      Packet x[K + 1];
      for (int j = 0; j <= K; ++j) x[j] = ploadu<Packet>(col[j] + i);
      Packet t = x[0];
      for (int j = 0; j < K; ++j) t = pmadd(x[j + 1], pv[j], t);
      t = pmul(t, ptau);
      pstoreu(col[0] + i, psub(x[0], t));
      for (int j = 0; j < K; ++j) pstoreu(col[j + 1] + i, pnmadd(t, pvc[j], x[j + 1]));
    }
    for (Index i = vectorEnd; i < rows; ++i) {
      Scalar t = col[0][i];
      for (int j = 0; j < K; ++j) t += col[j + 1][i] * v[j];
      t *= tauValue;
      col[0][i] -= t;
      for (int j = 0; j < K; ++j) col[j + 1][i] -= t * vc[j];
    }
  }
};
 
}  // namespace internal

template <typename Derived>
template <typename EssentialPart>
EIGEN_DEVICE_FUNC void MatrixBase<Derived>::applyHouseholderOnTheLeft(const EssentialPart& essential, const Scalar& tau,
                                                                      Scalar* workspace) {
  if (rows() == 1) {
    // H = 1 - tau multiplies from the left, which a noncommutative scalar distinguishes.
    *this = (Scalar(1) - tau) * *this;
  } else if (!numext::is_exactly_zero(tau)) {
    internal::householder_apply_left_impl<Derived, EssentialPart>::run(*this, essential, tau, workspace);
  }
}

template <typename Derived>
template <typename EssentialPart>
EIGEN_DEVICE_FUNC void MatrixBase<Derived>::applyHouseholderOnTheRight(const EssentialPart& essential,
                                                                       const Scalar& tau, Scalar* workspace) {
  if (cols() == 1) {
    *this *= Scalar(1) - tau;
  } else if (!numext::is_exactly_zero(tau)) {
    internal::householder_apply_right_impl<Derived, EssentialPart>::run(*this, essential, tau, workspace);
  }
}
 
}  // end namespace Eigen
 
#endif  // EIGEN_HOUSEHOLDER_H