is_locally_infeasible.hpp

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/SparseCore>
 
// See docs/algorithms.md#Works_cited for citation definitions
 
namespace slp {
 
template <typename Scalar>
bool is_equality_locally_infeasible(
    const Eigen::SparseMatrix<Scalar>& A_e,
    const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e) {
  // The equality constraints are locally infeasible if
  //
  //   Aₑᵀcₑ → 0
  //   ‖cₑ‖ > ε
  //
  // See "Infeasibility detection" in section 6 of [3].
  return A_e.rows() > 0 && (A_e.transpose() * c_e).norm() < Scalar(1e-6) &&
         c_e.norm() > Scalar(1e-2);
}
 
template <typename Scalar>
bool is_inequality_locally_infeasible(
    const Eigen::SparseMatrix<Scalar>& A_i,
    const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i) {
  // The inequality constraints are locally infeasible if
  //
  //   Aᵢᵀcᵢ⁺ → 0
  //   ‖cᵢ⁺‖ > ε
  //
  // where cᵢ⁺ = min(cᵢ, 0).
  //
  // See "Infeasibility detection" in section 6 of [3].
  //
  // cᵢ⁺ is used instead of cᵢ⁻ from the paper to follow the convention that
  // feasible inequality constraints are ≥ 0.
  if (A_i.rows() > 0) {
    Eigen::Vector<Scalar, Eigen::Dynamic> c_i_plus = c_i.cwiseMin(Scalar(0));
    if ((A_i.transpose() * c_i_plus).norm() < Scalar(1e-6) &&
        c_i_plus.norm() > Scalar(1e-6)) {
      return true;
    }
  }
 
  return false;
}
 
}  // namespace slp