|
@@ -13,93 +13,100 @@
|
|
|
|
|
|
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>
|
|
|
- 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)
|
|
|
- {
|
|
|
- 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;
|
|
|
+ // 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);
|
|
|
+ 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();
|
|
|
+ // 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();
|
|
|
+#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();
|
|
|
+ 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++;
|
|
|
- }
|
|
|
- }
|
|
|
+ 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);
|
|
|
+ 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();
|
|
|
+ Mat LMpinv = v * pi_diag * u.transpose();
|
|
|
#endif
|
|
|
- S = LMpinv.block(0, 0, n, n + m);
|
|
|
+ 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);
|
|
|
+ //// 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;
|
|
|
- }
|
|
|
+ //int hu = 1;
|
|
|
}
|
|
|
|
|
|
#endif
|