libigl_Martin/min_quad_dense.h

#ifndef IGL_MIN_QUAD_DENSE_H
#define IGL_MIN_QUAD_DENSE_H

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>

//// debug
//#include <matlabinterface.h>
//Engine *g_pEngine;

#include <EPS.h>

namespace igl
{
  // MIN_QUAD_WITH_FIXED Minimize quadratic energy Z'*A*Z + Z'*B + C
  // subject to linear constraints Aeq*Z = Beq
  //
  // Templates:
  //   T  should be a eigen matrix primitive type like float or double
  // Inputs:
  //   A  n by n matrix of quadratic coefficients
  //   B  n by 1 column of linear coefficients
  //   Aeq  m by n list of linear equality constraint coefficients
  //   Beq  m by 1 list of linear equality constraint constant values
  // Outputs:
  //   S  n by (n + m) "solve" matrix, such that S*[B', Beq'] is a solution
  // Returns true on success, false on error
  template <typename T>
  inline void 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);
}

// Implementation
template <typename T>
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;
}

#endif