lu_lagrange.cpp 4.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159
  1. // This file is part of libigl, a simple c++ geometry processing library.
  2. //
  3. // Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
  4. //
  5. // This Source Code Form is subject to the terms of the Mozilla Public License
  6. // v. 2.0. If a copy of the MPL was not distributed with this file, You can
  7. // obtain one at http://mozilla.org/MPL/2.0/.
  8. #include "lu_lagrange.h"
  9. // Cholesky LLT decomposition for symmetric positive definite
  10. //#include <Eigen/SparseExtra>
  11. // Bug in unsupported/Eigen/SparseExtra needs iostream first
  12. #include <iostream>
  13. #include <unsupported/Eigen/SparseExtra>
  14. #include <cassert>
  15. #include <cstdio>
  16. #include "find.h"
  17. #include "sparse.h"
  18. template <typename T>
  19. IGL_INLINE bool igl::lu_lagrange(
  20. const Eigen::SparseMatrix<T> & ATA,
  21. const Eigen::SparseMatrix<T> & C,
  22. Eigen::SparseMatrix<T> & L,
  23. Eigen::SparseMatrix<T> & U)
  24. {
  25. #if EIGEN_VERSION_AT_LEAST(3,0,92)
  26. #if defined(_WIN32)
  27. #pragma message("lu_lagrange has not yet been implemented for your Eigen Version")
  28. #else
  29. #warning lu_lagrange has not yet been implemented for your Eigen Version
  30. #endif
  31. return false;
  32. #else
  33. // number of unknowns
  34. int n = ATA.rows();
  35. // number of lagrange multipliers
  36. int m = C.cols();
  37. assert(ATA.cols() == n);
  38. if(m != 0)
  39. {
  40. assert(C.rows() == n);
  41. if(C.nonZeros() == 0)
  42. {
  43. // See note above about empty columns in C
  44. fprintf(stderr,"Error: lu_lagrange() C has columns but no entries\n");
  45. return false;
  46. }
  47. }
  48. // Check that each column of C has at least one entry
  49. std::vector<bool> has_entry; has_entry.resize(C.cols(),false);
  50. // Iterate over outside
  51. for(int k=0; k<C.outerSize(); ++k)
  52. {
  53. // Iterate over inside
  54. for(typename Eigen::SparseMatrix<T>::InnerIterator it (C,k); it; ++it)
  55. {
  56. has_entry[it.col()] = true;
  57. }
  58. }
  59. for(int i=0;i<(int)has_entry.size();i++)
  60. {
  61. if(!has_entry[i])
  62. {
  63. // See note above about empty columns in C
  64. fprintf(stderr,"Error: lu_lagrange() C(:,%d) has no entries\n",i);
  65. return false;
  66. }
  67. }
  68. // Cholesky factorization of ATA
  69. //// Eigen fails if you give a full view of the matrix like this:
  70. //Eigen::SparseLLT<SparseMatrix<T> > ATA_LLT(ATA);
  71. Eigen::SparseMatrix<T> ATA_LT = ATA.template triangularView<Eigen::Lower>();
  72. Eigen::SparseLLT<Eigen::SparseMatrix<T> > ATA_LLT(ATA_LT);
  73. Eigen::SparseMatrix<T> J = ATA_LLT.matrixL();
  74. //if(!ATA_LLT.succeeded())
  75. if(!((J*0).eval().nonZeros() == 0))
  76. {
  77. fprintf(stderr,"Error: lu_lagrange() failed to factor ATA\n");
  78. return false;
  79. }
  80. if(m == 0)
  81. {
  82. // If there are no constraints (C is empty) then LU decomposition is just L
  83. // and L' from cholesky decomposition
  84. L = J;
  85. U = J.transpose();
  86. }else
  87. {
  88. // Construct helper matrix M
  89. Eigen::SparseMatrix<T> M = C;
  90. J.template triangularView<Eigen::Lower>().solveInPlace(M);
  91. // Compute cholesky factorizaiton of M'*M
  92. Eigen::SparseMatrix<T> MTM = M.transpose() * M;
  93. Eigen::SparseLLT<Eigen::SparseMatrix<T> > MTM_LLT(MTM.template triangularView<Eigen::Lower>());
  94. Eigen::SparseMatrix<T> K = MTM_LLT.matrixL();
  95. //if(!MTM_LLT.succeeded())
  96. if(!((K*0).eval().nonZeros() == 0))
  97. {
  98. fprintf(stderr,"Error: lu_lagrange() failed to factor MTM\n");
  99. return false;
  100. }
  101. // assemble LU decomposition of Q
  102. Eigen::Matrix<int,Eigen::Dynamic,1> MI;
  103. Eigen::Matrix<int,Eigen::Dynamic,1> MJ;
  104. Eigen::Matrix<T,Eigen::Dynamic,1> MV;
  105. igl::find(M,MI,MJ,MV);
  106. Eigen::Matrix<int,Eigen::Dynamic,1> KI;
  107. Eigen::Matrix<int,Eigen::Dynamic,1> KJ;
  108. Eigen::Matrix<T,Eigen::Dynamic,1> KV;
  109. igl::find(K,KI,KJ,KV);
  110. Eigen::Matrix<int,Eigen::Dynamic,1> JI;
  111. Eigen::Matrix<int,Eigen::Dynamic,1> JJ;
  112. Eigen::Matrix<T,Eigen::Dynamic,1> JV;
  113. igl::find(J,JI,JJ,JV);
  114. int nnz = JV.size() + MV.size() + KV.size();
  115. Eigen::Matrix<int,Eigen::Dynamic,1> UI(nnz);
  116. Eigen::Matrix<int,Eigen::Dynamic,1> UJ(nnz);
  117. Eigen::Matrix<T,Eigen::Dynamic,1> UV(nnz);
  118. UI << JJ, MI, (KJ.array() + n).matrix();
  119. UJ << JI, (MJ.array() + n).matrix(), (KI.array() + n).matrix();
  120. UV << JV, MV, KV*-1;
  121. igl::sparse(UI,UJ,UV,U);
  122. Eigen::Matrix<int,Eigen::Dynamic,1> LI(nnz);
  123. Eigen::Matrix<int,Eigen::Dynamic,1> LJ(nnz);
  124. Eigen::Matrix<T,Eigen::Dynamic,1> LV(nnz);
  125. LI << JI, (MJ.array() + n).matrix(), (KI.array() + n).matrix();
  126. LJ << JJ, MI, (KJ.array() + n).matrix();
  127. LV << JV, MV, KV;
  128. igl::sparse(LI,LJ,LV,L);
  129. }
  130. return true;
  131. #endif
  132. }
  133. #ifdef IGL_STATIC_LIBRARY
  134. // Explicit template specialization
  135. template bool igl::lu_lagrange<double>(Eigen::SparseMatrix<double, 0, int> const&, Eigen::SparseMatrix<double, 0, int> const&, Eigen::SparseMatrix<double, 0, int>&, Eigen::SparseMatrix<double, 0, int>&);
  136. #endif