active set works but is a little slow, full() is obsolete, explicits

Former-commit-id: b597019a0a802381d7ece50455fb37de230142ce

include/igl/active_set.cpp

@@ -1,6 +1,7 @@
 #include "active_set.h"
 #include "min_quad_with_fixed.h"
 #include "slice.h"
+#include "matlab_format.h"
 
 #include <iostream>
 #include <limits>
@@ -79,42 +80,70 @@ IGL_INLINE igl::SolverStatus igl::active_set(
     assert(Z.rows() == n);
     assert(Z.cols() == 1);
   }
+  assert(known.cols() == 1);
+  // Number of knowns
+  const int nk = known.size();
 
   // Initialize active sets
-  Matrix<bool,Dynamic,1> as_lx(n,1);
-  Matrix<bool,Dynamic,1> as_ux(n,1);
-  Matrix<bool,Dynamic,1> as_ieq(Aieq.rows(),1);
+  typedef bool BOOL;
+#define TRUE true
+#define FALSE false
+  Matrix<BOOL,Dynamic,1> as_lx = Matrix<BOOL,Dynamic,1>::Constant(n,1,FALSE);
+  Matrix<BOOL,Dynamic,1> as_ux = Matrix<BOOL,Dynamic,1>::Constant(n,1,FALSE);
+  Matrix<BOOL,Dynamic,1> as_ieq(Aieq.rows(),1);
+
+  // Keep track of previous Z for comparison
+  Eigen::PlainObjectBase<DerivedZ> old_Z;
+  old_Z = PlainObjectBase<DerivedZ>::Constant(
+      n,1,numeric_limits<typename DerivedZ::Scalar>::max());
 
   int iter = 0;
   while(true)
   {
     // FIND BREACHES OF CONSTRAINTS
+    int new_as_lx = 0;
+    int new_as_ux = 0;
     if(Z.size() > 0)
     {
       for(int z = 0;z < n;z++)
       {
         if(Z(z) < lx(z))
         {
-          as_lx(z) = true;
+          new_as_lx += (as_lx(z)?0:1);
+          //new_as_lx++;
+          as_lx(z) = TRUE;
         }
         if(Z(z) > ux(z))
         {
-          as_ux(z) = true;
+          new_as_ux += (as_ux(z)?0:1);
+          //new_as_ux++;
+          as_ux(z) = TRUE;
         }
       }
+      //cout<<"new_as_lx: "<<new_as_lx<<endl;
+      //cout<<"new_as_ux: "<<new_as_ux<<endl;
+      const double diff = (Z-old_Z).squaredNorm();
+      //cout<<"diff: "<<diff<<endl;
+      if(diff < params.solution_diff_threshold)
+      {
+        ret = SOLVER_STATUS_CONVERGED;
+        break;
+      }
+      old_Z = Z;
     }
 
-    const int as_lx_count = count(as_lx.data(),as_lx.data()+n,true);
-    const int as_ux_count = count(as_ux.data(),as_ux.data()+n,true);
+    const int as_lx_count = count(as_lx.data(),as_lx.data()+n,TRUE);
+    const int as_ux_count = count(as_ux.data(),as_ux.data()+n,TRUE);
+
     // PREPARE FIXED VALUES
     Eigen::PlainObjectBase<Derivedknown> known_i;
-    known_i.resize((int)known.size() + as_lx_count + as_ux_count,1);
+    known_i.resize(nk + as_lx_count + as_ux_count,1);
     Eigen::PlainObjectBase<DerivedY> Y_i;
-    Y_i.resize((int)known.size() + as_lx_count + as_ux_count,1);
+    Y_i.resize(nk + as_lx_count + as_ux_count,1);
     {
       known_i.block(0,0,known.rows(),known.cols()) = known;
       Y_i.block(0,0,Y.rows(),Y.cols()) = Y;
-      int k = known.size();
+      int k = nk;
       // Then all lx
       for(int z = 0;z < n;z++)
       {
@@ -138,6 +167,7 @@ IGL_INLINE igl::SolverStatus igl::active_set(
       assert(k==Y_i.size());
       assert(k==known_i.size());
     }
+    //cout<<matlab_format((known_i.array()+1).eval(),"known_i")<<endl;
 
     min_quad_with_fixed_data<AT> data;
     if(!min_quad_with_fixed_precompute(A,known_i,Aeq,params.Auu_pd,data))
@@ -163,35 +193,36 @@ IGL_INLINE igl::SolverStatus igl::active_set(
     slice(B,known_i,Bk);
     MatrixXd Lambda_known_i = -(Ak*Z + 0.5*Bk);
     // reverse the lambda values for lx
-    Lambda_known_i.block(known.size(),0,as_lx_count,1) = 
-      (-1*Lambda_known_i.block(known.size(),0,as_lx_count,1)).eval();
+    Lambda_known_i.block(nk,0,as_lx_count,1) = 
+      (-1*Lambda_known_i.block(nk,0,as_lx_count,1)).eval();
     
     // Remove from active set
-    for(int z = 0;z<as_lx_count;z++)
+    for(int l = 0;l<as_lx_count;l++)
     {
-      if(Lambda_known_i(known.size() + z) < params.inactive_threshold)
+      if(Lambda_known_i(nk + l) < params.inactive_threshold)
       {
-        as_lx(known_i(z)) = false;
+        as_lx(known_i(nk + l)) = FALSE;
       }
     }
-    for(int z = 0;z<as_ux_count;z++)
+    for(int u = 0;u<as_ux_count;u++)
     {
-      if(Lambda_known_i(known.size() + as_lx_count + z) < 
+      if(Lambda_known_i(nk + as_lx_count + u) < 
         params.inactive_threshold)
       {
-        as_ux(known_i(z)) = false;
+        as_ux(known_i(nk + as_lx_count + u)) = FALSE;
       }
     }
 
     iter++;
+    //cout<<iter<<endl;
     if(params.max_iter>0 && iter>=params.max_iter)
     {
       ret = SOLVER_STATUS_MAX_ITER;
       break;
     }
+
   }
 
-finish:
   return ret;
 }
 

include/igl/active_set.h

@@ -77,17 +77,25 @@ namespace igl
 struct igl::active_set_params
 {
   // Input parameters for active_set:
-  //   Auu_pd  whethter Auu is positive definite {false}
+  //   Auu_pd  whether Auu is positive definite {false}
   //   max_iter  Maximum number of iterations ({0} = Infinity)
   //   inactive_threshold  Threshold on Lagrange multiplier values to determine
   //     whether to keep constraints active {EPS}
+  //   constraint_threshold  Threshold on whether constraints are violated (0
+  //     is perfect) {EPS}
+  //   solution_diff_threshold  Threshold on the squared norm of the difference
+  //     between two consecutive solutions {EPS}
   bool Auu_pd;
   int max_iter;
   double inactive_threshold;
+  double constraint_threshold;
+  double solution_diff_threshold;
   active_set_params():
     Auu_pd(false),
     max_iter(-1),
-    inactive_threshold(igl::DOUBLE_EPS)
+    inactive_threshold(igl::DOUBLE_EPS),
+    constraint_threshold(igl::DOUBLE_EPS),
+    solution_diff_threshold(igl::DOUBLE_EPS)
     {};
 };
 

include/igl/full.cpp

@@ -5,6 +5,8 @@ IGL_INLINE void igl::full(
   const Eigen::SparseMatrix<T> & A,
   Eigen::PlainObjectBase<DerivedB>& B)
 {
+#warning "Obsolete. Just call B = Matrix(A)"
+  assert(false);
   using namespace Eigen;
   B = PlainObjectBase<DerivedB >::Zero(A.rows(),A.cols());
   // Iterate over outside
@@ -23,6 +25,8 @@ IGL_INLINE void igl::full(
   const Eigen::PlainObjectBase<DerivedA>& A,
   Eigen::PlainObjectBase<DerivedB>& B)
 {
+#warning "Obsolete. Just call B = Matrix(A)"
+  assert(false);
   B = A;
 }
 

include/igl/full.h

@@ -6,7 +6,10 @@
 #include <Eigen/Sparse>
 namespace igl
 {
+  // This is totally unnecessary. You can just call MatrixXd B = MatrixXd(A);
+  //
   // Convert a sparsematrix into a full one
+  //
   // Templates:
   //   T  should be a eigen sparse matrix primitive type like int or double
   // Input:

include/igl/matlab_format.cpp

@@ -94,4 +94,5 @@ template Eigen::WithFormat<Eigen::Array<int, -1, -1, 0, -1, -1> > const igl::mat
 template Eigen::WithFormat<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const igl::matlab_format<Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, std::basic_string<char, std::char_traits<char>, std::allocator<char> >);
 template Eigen::WithFormat<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const igl::matlab_format<Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, std::basic_string<char, std::char_traits<char>, std::allocator<char> >);
 template Eigen::WithFormat<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const igl::matlab_format<Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, std::basic_string<char, std::char_traits<char>, std::allocator<char> >);
+template Eigen::WithFormat<Eigen::Array<int, -1, 1, 0, -1, 1> > const igl::matlab_format<Eigen::Array<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Array<int, -1, 1, 0, -1, 1> > const&, std::basic_string<char, std::char_traits<char>, std::allocator<char> >);
 #endif

include/igl/min_quad_with_fixed.cpp

@@ -202,8 +202,6 @@ IGL_INLINE bool igl::min_quad_with_fixed_precompute(
       // Resort to LU
       // Bottleneck >1/2
       data.lu.compute(NA); 
-      MatrixXd NAf;
-      full(NA,NAf);
       //std::cout<<"NA=["<<std::endl<<NA<<std::endl<<"];"<<std::endl;
       switch(data.lu.info())
       {