utils.hpp

Go to the documentation of this file.

//
// Copyright (c) 2022-2024 INRIA
//
#ifndef PROXSUITE_PROXQP_SPARSE_UTILS_HPP
#define PROXSUITE_PROXQP_SPARSE_UTILS_HPP
 
#include <iostream>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>
 
#include "proxsuite/helpers/common.hpp"
#include <proxsuite/linalg/dense/core.hpp>
#include <proxsuite/linalg/sparse/core.hpp>
#include "proxsuite/proxqp/sparse/workspace.hpp"
#include <proxsuite/linalg/sparse/factorize.hpp>
#include <proxsuite/linalg/sparse/update.hpp>
#include <proxsuite/linalg/sparse/rowmod.hpp>
#include <proxsuite/proxqp/dense/views.hpp>
#include <proxsuite/proxqp/settings.hpp>
#include <proxsuite/linalg/veg/vec.hpp>
#include "proxsuite/proxqp/results.hpp"
#include "proxsuite/proxqp/utils/prints.hpp"
#include "proxsuite/proxqp/sparse/views.hpp"
#include "proxsuite/proxqp/sparse/model.hpp"
#include "proxsuite/proxqp/sparse/preconditioner/ruiz.hpp"
#include "proxsuite/proxqp/sparse/preconditioner/identity.hpp"
 
namespace proxsuite {
namespace proxqp {
namespace sparse {
 
template<typename T, typename I>
void
print_setup_header(const Settings<T>& settings,
                   Results<T>& results,
                   const Model<T, I>& model)
{
 
  proxsuite::proxqp::print_preambule();
 
  // Print variables and constraints
  std::cout << "problem:  " << std::noshowpos << std::endl;
  std::cout << "          variables n = " << model.dim
            << ", equality constraints n_eq = " << model.n_eq << ",\n"
            << "          inequality constraints n_in = " << model.n_in
            << ", nnz = " << model.H_nnz + model.A_nnz + model.C_nnz << ",\n"
            << std::endl;
 
  // Print Settings
  std::cout << "settings: " << std::endl;
  std::cout << "          backend = sparse," << std::endl;
  std::cout << "          sparse_backend = " << settings.sparse_backend;
  if (settings.sparse_backend == SparseBackend::Automatic) {
    std::cout << " -> " << results.info.sparse_backend;
  }
  std::cout << "," << std::endl;
  std::cout << "          eps_abs = " << settings.eps_abs
            << ", eps_rel = " << settings.eps_rel << std::endl;
  std::cout << "          eps_prim_inf = " << settings.eps_primal_inf
            << ", eps_dual_inf = " << settings.eps_dual_inf << "," << std::endl;
 
  std::cout << "          rho = " << results.info.rho
            << ", mu_eq = " << results.info.mu_eq
            << ", mu_in = " << results.info.mu_in << "," << std::endl;
  std::cout << "          max_iter = " << settings.max_iter
            << ", max_iter_in = " << settings.max_iter_in << "," << std::endl;
 
  if (settings.compute_preconditioner) {
    std::cout << "          scaling: on, " << std::endl;
  } else {
    std::cout << "          scaling: off, " << std::endl;
  }
  if (settings.compute_timings) {
    std::cout << "          timings: on, " << std::endl;
  } else {
    std::cout << "          timings: off, " << std::endl;
  }
  switch (settings.initial_guess) {
    case InitialGuessStatus::WARM_START:
      std::cout << "          initial guess: warm start. \n" << std::endl;
      break;
    case InitialGuessStatus::NO_INITIAL_GUESS:
      std::cout << "          initial guess: initial guess. \n" << std::endl;
      break;
    case InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT:
      std::cout
        << "          initial guess: warm start with previous result. \n"
        << std::endl;
      break;
    case InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT:
      std::cout
        << "          initial guess: cold start with previous result. \n"
        << std::endl;
      break;
    case InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS:
      std::cout
        << "          initial guess: equality constrained initial guess. \n"
        << std::endl;
  }
}
 
namespace detail {
 
template<typename T, typename I>
VEG_NO_INLINE void
noalias_gevmmv_add_impl( //
  VectorViewMut<T> out_l,
  VectorViewMut<T> out_r,
  proxsuite::linalg::sparse::MatRef<T, I> a,
  VectorView<T> in_l,
  VectorView<T> in_r)
{
  VEG_ASSERT_ALL_OF /* NOLINT */ (a.nrows() == out_r.dim,
                                  a.ncols() == in_r.dim,
                                  a.ncols() == out_l.dim,
                                  a.nrows() == in_l.dim);
  // equivalent to
  // out_r.to_eigen().noalias() += a.to_eigen() * in_r.to_eigen();
  // out_l.to_eigen().noalias() += a.to_eigen().transpose() * in_l.to_eigen();
 
  auto* ai = a.row_indices();
  auto* ax = a.values();
  auto n = a.ncols();
 
  for (usize j = 0; j < usize(n); ++j) {
    usize col_start = a.col_start(j);
    usize col_end = a.col_end(j);
 
    T acc0 = 0;
    T acc1 = 0;
    T acc2 = 0;
    T acc3 = 0;
 
    T in_rj = in_r(isize(j));
 
    usize pcount = col_end - col_start;
 
    usize p = col_start;
 
    auto zx = proxsuite::linalg::sparse::util::zero_extend;
 
    for (; p < col_start + pcount / 4 * 4; p += 4) {
      auto i0 = isize(zx(ai[p + 0]));
      auto i1 = isize(zx(ai[p + 1]));
      auto i2 = isize(zx(ai[p + 2]));
      auto i3 = isize(zx(ai[p + 3]));
 
      T ai0j = ax[p + 0];
      T ai1j = ax[p + 1];
      T ai2j = ax[p + 2];
      T ai3j = ax[p + 3];
 
      out_r(i0) += ai0j * in_rj;
      out_r(i1) += ai1j * in_rj;
      out_r(i2) += ai2j * in_rj;
      out_r(i3) += ai3j * in_rj;
 
      acc0 += ai0j * in_l(i0);
      acc1 += ai1j * in_l(i1);
      acc2 += ai2j * in_l(i2);
      acc3 += ai3j * in_l(i3);
    }
 
    for (; p < col_end; ++p) {
      auto i = isize(zx(ai[p]));
 
      T aij = ax[p];
      out_r(i) += aij * in_rj;
      acc0 += aij * in_l(i);
    }
 
    acc0 = ((acc0 + acc1) + (acc2 + acc3));
    out_l(isize(j)) += acc0;
  }
}
 
template<typename T, typename I>
VEG_NO_INLINE void
noalias_symhiv_add_impl( //
  VectorViewMut<T> out,
  proxsuite::linalg::sparse::MatRef<T, I> a,
  VectorView<T> in)
{
  VEG_ASSERT_ALL_OF /* NOLINT */ ( //
    a.nrows() == a.ncols(),
    a.nrows() == out.dim,
    a.ncols() == in.dim);
  // equivalent to
  // out.to_eigen().noalias() +=
  //        a.to_eigen().template selfadjointView<Eigen::Upper>() *
  // in.to_eigen();
 
  auto* ai = a.row_indices();
  auto* ax = a.values();
  auto n = a.ncols();
 
  for (usize j = 0; j < usize(n); ++j) {
    usize col_start = a.col_start(j);
    usize col_end = a.col_end(j);
 
    if (col_start == col_end) {
      continue;
    }
 
    T acc0 = 0;
    T acc1 = 0;
    T acc2 = 0;
    T acc3 = 0;
 
    T in_j = in(isize(j));
 
    usize pcount = col_end - col_start;
 
    auto zx = proxsuite::linalg::sparse::util::zero_extend;
 
    if (zx(ai[col_end - 1]) == j) {
      T ajj = ax[col_end - 1];
      out(isize(j)) += ajj * in_j;
      pcount -= 1;
    }
 
    usize p = col_start;
 
    for (; p < col_start + pcount / 4 * 4; p += 4) {
      auto i0 = isize(zx(ai[p + 0]));
      auto i1 = isize(zx(ai[p + 1]));
      auto i2 = isize(zx(ai[p + 2]));
      auto i3 = isize(zx(ai[p + 3]));
 
      T ai0j = ax[p + 0];
      T ai1j = ax[p + 1];
      T ai2j = ax[p + 2];
      T ai3j = ax[p + 3];
 
      out(i0) += ai0j * in_j;
      out(i1) += ai1j * in_j;
      out(i2) += ai2j * in_j;
      out(i3) += ai3j * in_j;
 
      acc0 += ai0j * in(i0);
      acc1 += ai1j * in(i1);
      acc2 += ai2j * in(i2);
      acc3 += ai3j * in(i3);
    }
    for (; p < col_start + pcount; ++p) {
      auto i = isize(zx(ai[p]));
 
      T aij = ax[p];
      out(i) += aij * in_j;
      acc0 += aij * in(i);
    }
    acc0 = ((acc0 + acc1) + (acc2 + acc3));
    out(isize(j)) += acc0;
  }
}
 
template<typename OutL, typename OutR, typename A, typename InL, typename InR>
void
noalias_gevmmv_add(OutL&& out_l,
                   OutR&& out_r,
                   A const& a,
                   InL const& in_l,
                   InR const& in_r)
{
  // noalias general vector matrix matrix vector add
  noalias_gevmmv_add_impl<typename A::Scalar, typename A::StorageIndex>(
    { proxqp::from_eigen, out_l },
    { proxqp::from_eigen, out_r },
    { proxsuite::linalg::sparse::from_eigen, a },
    { proxqp::from_eigen, in_l },
    { proxqp::from_eigen, in_r });
}
 
template<typename Out, typename A, typename In>
void
noalias_symhiv_add(Out&& out, A const& a, In const& in)
{
  // noalias symmetric (hi) matrix vector add
  noalias_symhiv_add_impl<typename A::Scalar, typename A::StorageIndex>(
    { proxqp::from_eigen, out },
    { proxsuite::linalg::sparse::from_eigen, a },
    { proxqp::from_eigen, in });
}
 
template<typename T, typename I>
struct AugmentedKkt : Eigen::EigenBase<AugmentedKkt<T, I>>
{
  struct Raw /* NOLINT */
  {
    proxsuite::linalg::sparse::MatRef<T, I> kkt_active;
    proxsuite::linalg::veg::Slice<bool> active_constraints;
    isize n;
    isize n_eq;
    isize n_in;
    T rho;
    T mu_eq;
    T mu_in;
  } _;
 
  AugmentedKkt /* NOLINT */ (Raw raw) noexcept
    : _{ raw }
  {
  }
 
  using Scalar = T;
  using RealScalar = T;
  using StorageIndex = I;
  enum
  {
    ColsAtCompileTime = Eigen::Dynamic,
    MaxColsAtCompileTime = Eigen::Dynamic,
    IsRowMajor = false,
  };
 
  auto rows() const noexcept -> isize { return _.n + _.n_eq + _.n_in; }
  auto cols() const noexcept -> isize { return rows(); }
  template<typename Rhs>
  auto operator*(Eigen::MatrixBase<Rhs> const& x) const
    -> Eigen::Product<AugmentedKkt, Rhs, Eigen::AliasFreeProduct>
  {
    return Eigen::Product< //
      AugmentedKkt,
      Rhs,
      Eigen::AliasFreeProduct>{
      *this,
      x.derived(),
    };
  }
};
 
template<typename T>
using VecMapMut = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>,
                             Eigen::Unaligned,
                             Eigen::Stride<Eigen::Dynamic, 1>>;
template<typename T>
using VecMap = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1> const,
                          Eigen::Unaligned,
                          Eigen::Stride<Eigen::Dynamic, 1>>;
 
template<typename V>
auto
vec(V const& v) -> VecMap<typename V::Scalar>
{
  static_assert(V::InnerStrideAtCompileTime == 1, ".");
  return {
    v.data(),
    v.rows(),
    v.cols(),
    Eigen::Stride<Eigen::Dynamic, 1>{
      v.outerStride(),
      v.innerStride(),
    },
  };
}
 
template<typename V>
auto
vec_mut(V&& v)
  -> VecMapMut<typename proxsuite::linalg::veg::uncvref_t<V>::Scalar>
{
  static_assert(
    proxsuite::linalg::veg::uncvref_t<V>::InnerStrideAtCompileTime == 1, ".");
  return {
    v.data(),
    v.rows(),
    v.cols(),
    Eigen::Stride<Eigen::Dynamic, 1>{
      v.outerStride(),
      v.innerStride(),
    },
  };
}
 
template<typename T, typename I>
auto
middle_cols(proxsuite::linalg::sparse::MatRef<T, I> mat,
            isize start,
            isize ncols,
            isize nnz) -> proxsuite::linalg::sparse::MatRef<T, I>
{
  VEG_ASSERT(start <= mat.ncols());
  VEG_ASSERT(ncols <= mat.ncols() - start);
 
  return {
    proxsuite::linalg::sparse::from_raw_parts,
    mat.nrows(),
    ncols,
    nnz,
    mat.col_ptrs() + start,
    mat.is_compressed() ? nullptr : (mat.nnz_per_col() + start),
    mat.row_indices(),
    mat.values(),
  };
}
 
template<typename T, typename I>
auto
middle_cols_mut(proxsuite::linalg::sparse::MatMut<T, I> mat,
                isize start,
                isize ncols,
                isize nnz) -> proxsuite::linalg::sparse::MatMut<T, I>
{
  VEG_ASSERT(start <= mat.ncols());
  VEG_ASSERT(ncols <= mat.ncols() - start);
  return {
    proxsuite::linalg::sparse::from_raw_parts,
    mat.nrows(),
    ncols,
    nnz,
    mat.col_ptrs_mut() + start,
    mat.is_compressed() ? nullptr : (mat.nnz_per_col_mut() + start),
    mat.row_indices_mut(),
    mat.values_mut(),
  };
}
 
template<typename T, typename I>
auto
top_rows_unchecked(proxsuite::linalg::veg::Unsafe /*unsafe*/,
                   proxsuite::linalg::sparse::MatRef<T, I> mat,
                   isize nrows) -> proxsuite::linalg::sparse::MatRef<T, I>
{
  VEG_ASSERT(nrows <= mat.nrows());
  return {
    proxsuite::linalg::sparse::from_raw_parts,
    nrows,
    mat.ncols(),
    mat.nnz(),
    mat.col_ptrs(),
    mat.nnz_per_col(),
    mat.row_indices(),
    mat.values(),
  };
}
 
template<typename T, typename I>
auto
top_rows_mut_unchecked(proxsuite::linalg::veg::Unsafe /*unsafe*/,
                       proxsuite::linalg::sparse::MatMut<T, I> mat,
                       isize nrows) -> proxsuite::linalg::sparse::MatMut<T, I>
{
  VEG_ASSERT(nrows <= mat.nrows());
  return {
    proxsuite::linalg::sparse::from_raw_parts,
    nrows,
    mat.ncols(),
    mat.nnz(),
    mat.col_ptrs_mut(),
    mat.nnz_per_col_mut(),
    mat.row_indices_mut(),
    mat.values_mut(),
  };
}
template<typename T, typename I, typename P>
bool
global_primal_residual_infeasibility(VectorViewMut<T> ATdy,
                                     VectorViewMut<T> CTdz,
                                     VectorViewMut<T> dy,
                                     VectorViewMut<T> dz,
                                     const QpView<T, I> qp_scaled,
                                     const Settings<T>& qpsettings,
                                     const P& ruiz)
{
 
  // The problem is primal infeasible if the following four conditions hold:
  //
  // ||unscaled(A^Tdy)|| <= eps_p_inf ||unscaled(dy)||
  // b^T dy <= -eps_p_inf ||unscaled(dy)||
  // ||unscaled(C^Tdz)|| <= eps_p_inf ||unscaled(dz)||
  // u^T [dz]_+ - l^T[-dz]_+ <= -eps_p_inf ||unscaled(dz)||
  //
  // the variables in entry are changed in place
 
  bool res = infty_norm(dy.to_eigen()) != 0 || infty_norm(dz.to_eigen()) != 0;
  if (!res) {
    return res;
  }
  ruiz.unscale_dual_residual_in_place(ATdy);
  ruiz.unscale_dual_residual_in_place(CTdz);
  T lower_bound_1 =
    dy.to_eigen().dot(qp_scaled.b.to_eigen()) +
    helpers::positive_part(dz.to_eigen()).dot(qp_scaled.u.to_eigen()) -
    helpers::negative_part(dz.to_eigen()).dot(qp_scaled.l.to_eigen());
  ruiz.unscale_dual_in_place_eq(dy);
  ruiz.unscale_dual_in_place_in(dz);
  T lower_bound_2 = infty_norm(ATdy.to_eigen() + CTdz.to_eigen());
  T upper_bound =
    qpsettings.eps_primal_inf *
    std::max(infty_norm(dy.to_eigen()), infty_norm(dz.to_eigen()));
  res = lower_bound_2 <= upper_bound && lower_bound_1 <= -upper_bound;
  return res;
}
template<typename T, typename I, typename P>
bool
global_dual_residual_infeasibility(VectorViewMut<T> Adx,
                                   VectorViewMut<T> Cdx,
                                   VectorViewMut<T> Hdx,
                                   VectorViewMut<T> dx,
                                   const QpView<T, I> qp_scaled,
                                   const Settings<T>& qpsettings,
                                   const Model<T, I>& qpmodel,
                                   const P& ruiz)
{
 
  // The problem is dual infeasible if one of the conditions hold:
  //
  // FIRST
  // ||unscaled(Adx)|| <= eps_d_inf ||unscaled(dx)||
  // unscaled(Cdx)_i \in [-eps_d_inf,eps_d_inf] ||unscaled(dx)|| if u_i and l_i
  // are finite                     or >=
  // -eps_d_inf||unscaled(dx)|| if u_i = +inf or <= eps_d_inf||unscaled(dx)|| if
  // l_i = -inf
  //
  // SECOND
  //
  // ||unscaled(Hdx)|| <= c eps_d_inf * ||unscaled(dx)||  and  q^Tdx <= -c
  // eps_d_inf  ||unscaled(dx)||
  // the variables in entry are changed in place
  ruiz.unscale_dual_residual_in_place(Hdx);
  ruiz.unscale_primal_residual_in_place_eq(Adx);
  ruiz.unscale_primal_residual_in_place_in(Cdx);
  T gdx = (dx.to_eigen()).dot(qp_scaled.g.to_eigen());
  ruiz.unscale_primal_in_place(dx);
 
  T bound = infty_norm(dx.to_eigen()) * qpsettings.eps_dual_inf;
  T bound_neg = -bound;
 
  bool first_cond = infty_norm(Adx.to_eigen()) <= bound;
 
  for (i64 iter = 0; iter < qpmodel.n_in; ++iter) {
    T Cdx_i = Cdx.to_eigen()[iter];
    if (qp_scaled.u.to_eigen()[iter] <= T(1.E20) &&
        qp_scaled.l.to_eigen()[iter] >= T(-1.E20)) {
      first_cond = first_cond && Cdx_i <= bound && Cdx_i >= bound_neg;
    } else if (qp_scaled.u.to_eigen()[iter] > T(1.E20)) {
      first_cond = first_cond && Cdx_i >= bound_neg;
    } else if (qp_scaled.l.to_eigen()[iter] < T(-1.E20)) {
      first_cond = first_cond && Cdx_i <= bound;
    }
  }
 
  bound *= ruiz.c;
  bound_neg *= ruiz.c;
  bool second_cond_alt1 =
    infty_norm(Hdx.to_eigen()) <= bound && gdx <= bound_neg;
  bound_neg *= qpsettings.eps_dual_inf;
 
  bool res = first_cond && second_cond_alt1 && infty_norm(dx.to_eigen()) != 0;
  return res;
}
 
template<typename T, typename I, typename P>
auto
unscaled_primal_dual_residual(Workspace<T, I>& work,
                              Results<T>& results,
                              const Settings<T>& settings,
                              VecMapMut<T> primal_residual_eq_scaled,
                              VecMapMut<T> primal_residual_in_scaled_lo,
                              VecMapMut<T> primal_residual_in_scaled_up,
                              VecMapMut<T> dual_residual_scaled,
                              T& primal_feasibility_eq_rhs_0,
                              T& primal_feasibility_in_rhs_0,
                              T& dual_feasibility_rhs_0,
                              T& dual_feasibility_rhs_1,
                              T& dual_feasibility_rhs_3,
                              T& rhs_duality_gap,
                              const P& precond,
                              Model<T, I> const& data,
                              const QpView<T, I> qp_scaled,
                              VecMapMut<T> x_e,
                              VecMapMut<T> y_e,
                              VecMapMut<T> z_e,
                              proxsuite::linalg::veg::dynstack::DynStackMut
                                stack) -> proxsuite::linalg::veg::Tuple<T, T>
{
  isize n = x_e.rows();
 
  LDLT_TEMP_VEC_UNINIT(T, tmp, n, stack);
  dual_residual_scaled = qp_scaled.g.to_eigen();
  {
    tmp.setZero();
    noalias_symhiv_add(tmp, qp_scaled.H.to_eigen(), x_e);
    dual_residual_scaled += tmp;
 
    precond.unscale_dual_residual_in_place(
      { proxqp::from_eigen, tmp }); // contains unscaled Hx
    dual_feasibility_rhs_0 = infty_norm(tmp);
    precond.unscale_primal_in_place({ proxqp::from_eigen, x_e });
    results.info.duality_gap = x_e.dot(data.g); // contains gTx
    rhs_duality_gap = std::fabs(results.info.duality_gap);
 
    const T xHx = (tmp).dot(x_e);
    results.info.duality_gap += xHx;
    rhs_duality_gap = std::max(rhs_duality_gap, std::abs(xHx));
    tmp += data.g; // contains now Hx+g
    precond.scale_primal_in_place({ proxqp::from_eigen, x_e });
 
    precond.unscale_dual_in_place_eq({ proxsuite::proxqp::from_eigen, y_e });
    const T by = (data.b).dot(y_e);
    results.info.duality_gap += by;
    rhs_duality_gap = std::max(rhs_duality_gap, std::abs(by));
    precond.scale_dual_in_place_eq({ proxsuite::proxqp::from_eigen, y_e });
 
    precond.unscale_dual_in_place_in({ proxsuite::proxqp::from_eigen, z_e });
 
    const T zl =
      helpers::select(work.active_set_low, results.z, 0)
        .dot(helpers::at_least(data.l, -helpers::infinite_bound<T>::value()));
    results.info.duality_gap += zl;
    rhs_duality_gap = std::max(rhs_duality_gap, std::abs(zl));
 
    const T zu =
      helpers::select(work.active_set_up, results.z, 0)
        .dot(helpers::at_most(data.u, helpers::infinite_bound<T>::value()));
    results.info.duality_gap += zu;
    rhs_duality_gap = std::max(rhs_duality_gap, std::abs(zu));
 
    precond.scale_dual_in_place_in({ proxsuite::proxqp::from_eigen, z_e });
  }
 
  {
    auto ATy = tmp;
    ATy.setZero();
    primal_residual_eq_scaled.setZero();
 
    detail::noalias_gevmmv_add(
      primal_residual_eq_scaled, ATy, qp_scaled.AT.to_eigen(), x_e, y_e);
 
    dual_residual_scaled += ATy;
 
    precond.unscale_dual_residual_in_place({ proxqp::from_eigen, ATy });
    dual_feasibility_rhs_1 = infty_norm(ATy);
  }
 
  {
    auto CTz = tmp;
    CTz.setZero();
    primal_residual_in_scaled_up.setZero();
 
    detail::noalias_gevmmv_add(
      primal_residual_in_scaled_up, CTz, qp_scaled.CT.to_eigen(), x_e, z_e);
 
    dual_residual_scaled += CTz;
 
    precond.unscale_dual_residual_in_place({ proxqp::from_eigen, CTz });
    dual_feasibility_rhs_3 = infty_norm(CTz);
  }
  precond.unscale_primal_residual_in_place_eq(
    { proxqp::from_eigen, primal_residual_eq_scaled });
  primal_feasibility_eq_rhs_0 = infty_norm(primal_residual_eq_scaled);
 
  precond.unscale_primal_residual_in_place_in(
    { proxqp::from_eigen, primal_residual_in_scaled_up });
  primal_feasibility_in_rhs_0 = infty_norm(primal_residual_in_scaled_up);
 
  auto b = data.b;
  auto l = data.l;
  auto u = data.u;
  primal_residual_in_scaled_lo =
    helpers::positive_part(primal_residual_in_scaled_up - u) +
    helpers::negative_part(primal_residual_in_scaled_up - l);
 
  primal_residual_eq_scaled -= b;
  T primal_feasibility_eq_lhs = infty_norm(primal_residual_eq_scaled);
  T primal_feasibility_in_lhs = infty_norm(primal_residual_in_scaled_lo);
  T primal_feasibility_lhs =
    std::max(primal_feasibility_eq_lhs, primal_feasibility_in_lhs);
 
  if ((settings.primal_infeasibility_solving &&
       results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE)) {
    tmp.setZero();
    {
      results.se = primal_residual_eq_scaled;
      results.si = primal_residual_in_scaled_lo;
      precond.unscale_primal_residual_in_place_eq(
        { proxqp::from_eigen,
          primal_residual_eq_scaled }); // E^{-1}(unscaled Ax-b)
      tmp.noalias() = qp_scaled.AT.to_eigen() * primal_residual_eq_scaled;
    }
 
    {
      precond.unscale_primal_residual_in_place_in(
        { proxqp::from_eigen,
          primal_residual_in_scaled_lo }); // E^{-1}(unscaled Ax-b)
      tmp.noalias() += qp_scaled.CT.to_eigen() * primal_residual_in_scaled_lo;
    }
    precond.unscale_dual_residual_in_place({ proxqp::from_eigen, tmp });
 
    primal_feasibility_lhs = infty_norm(tmp);
    precond.scale_primal_residual_in_place_eq(
      { proxqp::from_eigen, primal_residual_eq_scaled });
  }
 
  // scaled Ax - b
  precond.scale_primal_residual_in_place_eq(
    { proxqp::from_eigen, primal_residual_eq_scaled });
  // scaled Cx
  precond.scale_primal_residual_in_place_in(
    { proxqp::from_eigen, primal_residual_in_scaled_up });
 
  precond.unscale_dual_residual_in_place(
    { proxqp::from_eigen, dual_residual_scaled });
  T dual_feasibility_lhs = infty_norm(dual_residual_scaled);
  precond.scale_dual_residual_in_place(
    { proxqp::from_eigen, dual_residual_scaled });
 
  return proxsuite::linalg::veg::tuplify(primal_feasibility_lhs,
                                         dual_feasibility_lhs);
}
 
} // namespace detail
} // namespace sparse
} // namespace proxqp
} // namespace proxsuite
 
namespace Eigen {
namespace internal {
template<typename T, typename I>
struct traits<proxsuite::proxqp::sparse::detail::AugmentedKkt<T, I>>
  : Eigen::internal::traits<Eigen::SparseMatrix<T, Eigen::ColMajor, I>>
{};
 
template<typename Rhs, typename T, typename I>
struct generic_product_impl<
  proxsuite::proxqp::sparse::detail::AugmentedKkt<T, I>,
  Rhs,
  SparseShape,
  DenseShape,
  GemvProduct>
  : generic_product_impl_base<
      proxsuite::proxqp::sparse::detail::AugmentedKkt<T, I>,
      Rhs,
      generic_product_impl<
        proxsuite::proxqp::sparse::detail::AugmentedKkt<T, I>,
        Rhs>>
{
  using Mat_ = proxsuite::proxqp::sparse::detail::AugmentedKkt<T, I>;
 
  using Scalar = typename Product<Mat_, Rhs>::Scalar;
 
  template<typename Dst>
  static void scaleAndAddTo(Dst& dst,
                            Mat_ const& lhs,
                            Rhs const& rhs,
                            PROXSUITE_MAYBE_UNUSED Scalar const& alpha)
  {
    using proxsuite::linalg::veg::isize;
 
    VEG_ASSERT(alpha == Scalar(1));
    proxsuite::proxqp::sparse::detail::noalias_symhiv_add(
      dst, lhs._.kkt_active.to_eigen(), rhs);
 
    {
      isize n = lhs._.n;
      isize n_eq = lhs._.n_eq;
      isize n_in = lhs._.n_in;
 
      auto dst_x = dst.head(n);
      auto dst_y = dst.segment(n, n_eq);
      auto dst_z = dst.tail(n_in);
 
      auto rhs_x = rhs.head(n);
      auto rhs_y = rhs.segment(n, n_eq);
      auto rhs_z = rhs.tail(n_in);
 
      dst_x += lhs._.rho * rhs_x;
      dst_y += (-1 / lhs._.mu_eq) * rhs_y;
      for (isize i = 0; i < n_in; ++i) {
        dst_z[i] +=
          (lhs._.active_constraints[i] ? -1 / lhs._.mu_in : T(1)) * rhs_z[i];
      }
    }
  }
};
} // namespace internal
} // namespace Eigen
 
#endif /* end of include guard PROXSUITE_PROXQP_SPARSE_UTILS_HPP */