libigl_Martin/min_quad_dense.cpp

#include "min_quad_dense.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include "EPS.h"

template <typename T>
IGL_INLINE void igl::min_quad_dense_precompute(
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& Aeq,    
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& S)
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Mat;
        // This threshold seems to matter a lot but I'm not sure how to
        // set it
  const T treshold = igl::FLOAT_EPS;
  //const T treshold = igl::DOUBLE_EPS;

  const int n = A.rows();
  assert(A.cols() == n);
  const int m = Aeq.rows();
  assert(Aeq.cols() == n);

  // Lagrange multipliers method:
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> LM(n + m, n + m);
  LM.block(0, 0, n, n) = A;
  LM.block(0, n, n, m) = Aeq.transpose();
  LM.block(n, 0, m, n) = Aeq;
  LM.block(n, n, m, m).setZero();

//#define USE_LU_DECOMPOSITION
#ifdef USE_LU_DECOMPOSITION    
  // if LM is close to singular, use at your own risk :)
  Mat LMpinv = LM.inverse();
#else // use SVD
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec; 
  Vec singValues;
  Eigen::JacobiSVD<Mat> svd;
  svd.compute(LM, Eigen::ComputeFullU | Eigen::ComputeFullV );
  const Mat& u = svd.matrixU();
  const Mat& v = svd.matrixV();
  const Vec& singVals = svd.singularValues();

  Vec pi_singVals(n + m);
  int zeroed = 0;
  for (int i=0; i<n + m; i++)
  {
    T sv = singVals(i, 0);
    assert(sv >= 0);      
               // printf("sv: %lg ? %lg\n",(double) sv,(double)treshold);
    if (sv > treshold) pi_singVals(i, 0) = T(1) / sv;
    else 
    {
      pi_singVals(i, 0) = T(0);
      zeroed++;
    }
  }

  printf("min_quad_dense_precompute: %i singular values zeroed (threshold = %e)\n", zeroed, treshold);
  Eigen::DiagonalMatrix<T, Eigen::Dynamic> pi_diag(pi_singVals);

  Mat LMpinv = v * pi_diag * u.transpose();
#endif
  S = LMpinv.block(0, 0, n, n + m);

  //// debug:
  //mlinit(&g_pEngine);
  //
  //mlsetmatrix(&g_pEngine, "A", A);
  //mlsetmatrix(&g_pEngine, "Aeq", Aeq);
  //mlsetmatrix(&g_pEngine, "LM", LM);
  //mlsetmatrix(&g_pEngine, "u", u);
  //mlsetmatrix(&g_pEngine, "v", v);
  //MatrixXd svMat = singVals;
  //mlsetmatrix(&g_pEngine, "singVals", svMat);
  //mlsetmatrix(&g_pEngine, "LMpinv", LMpinv);
  //mlsetmatrix(&g_pEngine, "S", S);

  //int hu = 1;
}

#ifndef IGL_HEADER_ONLY
// Explicit template specialization
#endif