You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
125 lines
8.5 KiB
125 lines
8.5 KiB
#include "quadprog.h"
|
|
#include "min_quad_with_fixed.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)
|
|
{
|
|
// Alec 16/2/2021:
|
|
// igl::quadprog implements a very simple primal active set method. The new
|
|
// igl::min_quad_with_fixed is very fast for small dense problems so the
|
|
// iterations of igl::quadprog become very fast. Even if it ends up doing many
|
|
// more iterations than igl::copyleft::quadprog it would be much faster (in
|
|
// reality it doesn't do that many more iterations). It's a healthy 10-100x
|
|
// faster than igl::copyleft::quadprog for specific cases of QPs.
|
|
//
|
|
// Unfortunately, that set is limited. igl::quadprog is really good at tiny
|
|
// box-constrained QPs with a positive definite objective (like the kind that show
|
|
// up in dual contouring). igl::copyleft::quadprog handles more general problems
|
|
// (and also starts to beat igl::quadprog when the number of variables gets over
|
|
// ~20). I tried extending igl::quadprog so that we could use it for
|
|
// igl::copyleft::progressive_hulls and drop igl::copyleft::quadprog but it was
|
|
// trickier than I thought. Something like qpmad or the non GPL version of
|
|
// quadrog++ would be good future PR.
|
|
//
|
|
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.
|
|
// Windows needs template arguments spelled out
|
|
x = min_quad_with_fixed<Scalar,n,m>(H,f,k,bc,A,b);
|
|
// 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));
|
|
if(lambda_i > worst_lambda)
|
|
{
|
|
best_remove = i;
|
|
worst_lambda = lambda_i;
|
|
}
|
|
}
|
|
}
|
|
// bias toward adding constraints
|
|
if(best_add >= 0)
|
|
{
|
|
const auto i = best_add;
|
|
assert(!k(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));
|
|
k(i) = false;
|
|
}else /*if(best_add < 0 && best_remove < 0)*/
|
|
{
|
|
return x;
|
|
}
|
|
}
|
|
// Should never happen.
|
|
assert(false && "quadprog failed after too many iterations");
|
|
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;
|
|
// Windows needs template parameters spelled out
|
|
return quadprog<Scalar,n,m>(
|
|
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&);
|
|
template Eigen::Matrix<float, 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<float, 2>(Eigen::Matrix<float, 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<float, 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<float, 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<float, 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<float, 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<float, 3>(Eigen::Matrix<float, 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<float, 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<float, 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<float, 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
|
|
|