libigl/include/igl/quadprog.cpp

#include "quadprog.h"
#include "min_quad_with_fixed.h"
#include <igl/matlab_format.h>
#include <iostream>

template <typename Scalar, int n, int m>
IGL_INLINE Eigen::Matrix<Scalar,n,1> igl::quadprog(
  const Eigen::Matrix<Scalar,n,n> & H,
  const Eigen::Matrix<Scalar,n,1> & f,
  const Eigen::Matrix<Scalar,m,n> & A,
  const Eigen::Matrix<Scalar,m,1> & b,
  const Eigen::Matrix<Scalar,n,1> & lb,
  const Eigen::Matrix<Scalar,n,1> & ub)
{
  typedef Eigen::Matrix<Scalar,n,1> VectorSn;
  typedef Eigen::Array<bool,n,1>    Arraybn;
  assert( (lb.array() < ub.array() ).all() );
  const int dyn_n = n == Eigen::Dynamic ? H.rows() : n;
  VectorSn x(dyn_n);
  VectorSn bc = VectorSn::Constant(dyn_n,1,-1e26);
  Arraybn k = Arraybn::Constant(dyn_n,1,false);
  Eigen::Index iter;
  // n³ is probably way too conservative. 
  for(iter = 0;iter<dyn_n*dyn_n*dyn_n;iter++)
  {
    // For dual contouring 99% of the time the active set is empty.
    // Optimize for this common case.
    x = min_quad_with_fixed(H,f,k,bc,A,b);
    //std::cout<<igl::matlab_format(x,"x")<<std::endl;
    // constraint violations 
    VectorSn vl = lb-x;
    VectorSn vu = x-ub;

    // try to add/remove constraints
    Eigen::Index best_add = -1; Scalar worst_offense = 0;
    bool add_lower;
    Eigen::Index best_remove = -1; Scalar worst_lambda = 0;
    for(Eigen::Index i = 0;i<dyn_n;i++)
    {
      if(vl(i)>worst_offense)
      {
        best_add = i;
        add_lower = true;
        worst_offense = vl(i);
      }
      if(vu(i)>worst_offense)
      {
        best_add = i;
        add_lower = false;
        worst_offense = vu(i);
      }
      // bias toward adding constraints
      if(best_add<0 && k(i))
      {
        const Scalar sign = bc(i)==ub(i)?1:-1;
        const Scalar lambda_i = sign * (H.row(i)*x+f(i));
        //printf("  considering k(%d) (λ = %g)\n",i,lambda_i);
        if(lambda_i > worst_lambda)
        {
          //printf("  removing k(%d) (λ = %g)\n",i,lambda_i);
          best_remove = i;
          worst_lambda = lambda_i;
        }
      }
    }
    // bias toward adding constraints
    if(best_add >= 0)
    {
      const auto i = best_add;
      assert(!k(i));
      //add_lower ? printf("  adding lb(%d)\n",i) : printf("  adding lb(%d)\n",i);
      bc(i) = add_lower ? lb(i) : ub(i);
      k(i) = true;
    }else if(best_remove >= 0)
    {
      const auto i = best_remove;
      assert(k(i));
      //printf("  removing k(%d) (λ = %g)\n",i,worst_lambda);
      k(i) = false;
    }else /*if(best_add < 0 && best_remove < 0)*/
    {
      std::cout<<igl::matlab_format(x,"x")<<std::endl;
      return x;
    }
  }
  // Should never happen.
  assert(false && "quadprog failed after too many iterations");
  //std::cout<<igl::eigen_format(H,"H")<<std::endl;
  //std::cout<<igl::eigen_format(f,"f")<<std::endl;
  //std::cout<<igl::eigen_format(lb,"lb")<<std::endl;
  //std::cout<<igl::eigen_format(ub,"ub")<<std::endl;
  return VectorSn::Zero(dyn_n);
}

template <typename Scalar, int n>
IGL_INLINE Eigen::Matrix<Scalar,n,1> igl::quadprog(
  const Eigen::Matrix<Scalar,n,n> & H,
  const Eigen::Matrix<Scalar,n,1> & f,
  const Eigen::Matrix<Scalar,n,1> & lb,
  const Eigen::Matrix<Scalar,n,1> & ub)
{
  const int m = n == Eigen::Dynamic ? Eigen::Dynamic : 0;
  return quadprog(
    H,f,
    Eigen::Matrix<Scalar,m,n>(0,H.cols()),
    Eigen::Matrix<Scalar,m,1>(0,1),
    lb,ub);
}


#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template Eigen::Matrix<double, 2, 1, ((Eigen::StorageOptions)0) | ((((2) == (1)) && ((1) != (1))) ? ((Eigen::StorageOptions)1) : ((((1) == (1)) && ((2) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 2, 1> igl::quadprog<double, 2>(Eigen::Matrix<double, 2, 2, ((Eigen::StorageOptions)0) | ((((2) == (1)) && ((2) != (1))) ? ((Eigen::StorageOptions)1) : ((((2) == (1)) && ((2) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 2, 2> const&, Eigen::Matrix<double, 2, 1, ((Eigen::StorageOptions)0) | ((((2) == (1)) && ((1) != (1))) ? ((Eigen::StorageOptions)1) : ((((1) == (1)) && ((2) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 2, 1> const&, Eigen::Matrix<double, 2, 1, ((Eigen::StorageOptions)0) | ((((2) == (1)) && ((1) != (1))) ? ((Eigen::StorageOptions)1) : ((((1) == (1)) && ((2) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 2, 1> const&, Eigen::Matrix<double, 2, 1, ((Eigen::StorageOptions)0) | ((((2) == (1)) && ((1) != (1))) ? ((Eigen::StorageOptions)1) : ((((1) == (1)) && ((2) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 2, 1> const&);
template Eigen::Matrix<double, 3, 1, ((Eigen::StorageOptions)0) | ((((3) == (1)) && ((1) != (1))) ? ((Eigen::StorageOptions)1) : ((((1) == (1)) && ((3) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 1> igl::quadprog<double, 3>(Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0) | ((((3) == (1)) && ((3) != (1))) ? ((Eigen::StorageOptions)1) : ((((3) == (1)) && ((3) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, Eigen::Matrix<double, 3, 1, ((Eigen::StorageOptions)0) | ((((3) == (1)) && ((1) != (1))) ? ((Eigen::StorageOptions)1) : ((((1) == (1)) && ((3) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 1> const&, Eigen::Matrix<double, 3, 1, ((Eigen::StorageOptions)0) | ((((3) == (1)) && ((1) != (1))) ? ((Eigen::StorageOptions)1) : ((((1) == (1)) && ((3) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 1> const&, Eigen::Matrix<double, 3, 1, ((Eigen::StorageOptions)0) | ((((3) == (1)) && ((1) != (1))) ? ((Eigen::StorageOptions)1) : ((((1) == (1)) && ((3) != (1))) ? ((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 1> const&);
#endif