// This file is part of libigl, a simple c++ geometry processing library. // // Copyright (C) 2016 Alec Jacobson // // This Source Code Form is subject to the terms of the Mozilla Public License // v. 2.0. If a copy of the MPL was not distributed with this file, You can // obtain one at http://mozilla.org/MPL/2.0/. #include "min_quad_with_fixed.h" #include "slice.h" #include "is_symmetric.h" #include "find.h" #include "sparse.h" #include "repmat.h" #include "EPS.h" #include "cat.h" //#include // Bug in unsupported/Eigen/SparseExtra needs iostream first #include #include #include #include #include template IGL_INLINE bool igl::min_quad_with_fixed_precompute( const Eigen::SparseMatrix& A2, const Eigen::MatrixBase & known, const Eigen::SparseMatrix& Aeq, const bool pd, min_quad_with_fixed_data & data ) { //#define MIN_QUAD_WITH_FIXED_CPP_DEBUG using namespace Eigen; using namespace std; const Eigen::SparseMatrix A = 0.5*A2; #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" pre"<= 0)&& "known indices should be in [0,n)"); assert((kr == 0 || known.maxCoeff() < n) && "known indices should be in [0,n)"); assert(neq <= n && "Number of equality constraints should be less than DOFs"); // cache known data.known = known; // get list of unknown indices data.unknown.resize(n-kr); std::vector unknown_mask; unknown_mask.resize(n,true); for(int i = 0;i 0) { data.unknown_lagrange.head(data.unknown.size()) = data.unknown; } if(data.lagrange.size() > 0) { data.unknown_lagrange.tail(data.lagrange.size()) = data.lagrange; } SparseMatrix Auu; slice(A,data.unknown,data.unknown,Auu); assert(Auu.size() != 0 && Auu.rows() > 0 && "There should be at least one unknown."); // Positive definiteness is *not* determined, rather it is given as a // parameter data.Auu_pd = pd; if(data.Auu_pd) { // PD implies symmetric data.Auu_sym = true; // This is an annoying assertion unless EPS can be chosen in a nicer way. //assert(is_symmetric(Auu,EPS())); assert(is_symmetric(Auu,1.0) && "Auu should be symmetric if positive definite"); }else { // determine if A(unknown,unknown) is symmetric and/or positive definite VectorXi AuuI,AuuJ; MatrixXd AuuV; find(Auu,AuuI,AuuJ,AuuV); data.Auu_sym = is_symmetric(Auu,EPS()*AuuV.maxCoeff()); } // Determine number of linearly independent constraints int nc = 0; if(neq>0) { #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" qr"<(data.Aequ.transpose().eval()),"AeqT")< new_A; SparseMatrix AeqT = Aeq.transpose(); SparseMatrix Z(neq,neq); // This is a bit slower. But why isn't cat fast? new_A = cat(1, cat(2, A, AeqT ), cat(2, Aeq, Z )); // precompute RHS builders if(kr > 0) { SparseMatrix Aulk,Akul; // Slow slice(new_A,data.unknown_lagrange,data.known,Aulk); //// This doesn't work!!! //data.preY = Aulk + Akul.transpose(); // Slow if(data.Auu_sym) { data.preY = Aulk*2; }else { slice(new_A,data.known,data.unknown_lagrange,Akul); SparseMatrix AkulT = Akul.transpose(); data.preY = Aulk + AkulT; } }else { data.preY.resize(data.unknown_lagrange.size(),0); } // Positive definite and no equality constraints (Positive definiteness // implies symmetric) #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" factorize"<::LLT; }else { #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" ldlt"< NA; slice(new_A,data.unknown_lagrange,data.unknown_lagrange,NA); data.NA = NA; // Ideally we'd use LDLT but Eigen doesn't support positive semi-definite // matrices: // http://forum.kde.org/viewtopic.php?f=74&t=106962&p=291990#p291990 if(data.Auu_sym && false) { data.ldlt.compute(NA); switch(data.ldlt.info()) { case Eigen::Success: break; case Eigen::NumericalIssue: cerr<<"Error: Numerical issue."<::LDLT; }else { #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" lu"<1/2 data.lu.compute(NA); //std::cout<<"NA=["<::LU; } } }else { #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" Aeq_li=false"< AeqTR,AeqTQ; AeqTR = data.AeqTQR.matrixR(); // This shouldn't be necessary AeqTR.prune(static_cast(0.0)); #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" matrixQ"<(0.0)); //cout<<"AeqTQ: "< I(neq,neq); I.setIdentity(); data.AeqTE = data.AeqTQR.colsPermutation() * I; data.AeqTET = data.AeqTQR.colsPermutation().transpose() * I; assert(AeqTR.rows() == nu && "#rows in AeqTR should match #unknowns"); assert(AeqTR.cols() == neq && "#cols in AeqTR should match #constraints"); assert(AeqTQ.rows() == nu && "#rows in AeqTQ should match #unknowns"); assert(AeqTQ.cols() == nu && "#cols in AeqTQ should match #unknowns"); //cout<<" slice"< QRAuu = data.AeqTQ2T * Auu * data.AeqTQ2; { #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" factorize"<::QR_LLT; } #ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG cout<<" smash"< Auk; slice(A,data.unknown,data.known,Auk); SparseMatrix Aku; slice(A,data.known,data.unknown,Aku); SparseMatrix AkuT = Aku.transpose(); data.preY = Auk + AkuT; // Needed during solve data.Auu = Auu; slice(Aeq,data.known,2,data.Aeqk); assert(data.Aeqk.rows() == neq); assert(data.Aeqk.cols() == data.known.size()); } return true; } template < typename T, typename DerivedB, typename DerivedY, typename DerivedBeq, typename DerivedZ, typename Derivedsol> IGL_INLINE bool igl::min_quad_with_fixed_solve( const min_quad_with_fixed_data & data, const Eigen::MatrixBase & B, const Eigen::MatrixBase & Y, const Eigen::MatrixBase & Beq, Eigen::PlainObjectBase & Z, Eigen::PlainObjectBase & sol) { using namespace std; using namespace Eigen; typedef Matrix VectorXT; typedef Matrix MatrixXT; // number of known rows int kr = data.known.size(); if(kr!=0) { assert(kr == Y.rows()); } // number of columns to solve int cols = Y.cols(); assert(B.cols() == 1 || B.cols() == cols); assert(Beq.size() == 0 || Beq.cols() == 1 || Beq.cols() == cols); // resize output Z.resize(data.n,cols); // Set known values for(int i = 0;i < kr;i++) { for(int j = 0;j < cols;j++) { Z(data.known(i),j) = Y(i,j); } } if(data.Aeq_li) { // number of lagrange multipliers aka linear equality constraints int neq = data.lagrange.size(); // append lagrange multiplier rhs's MatrixXT BBeq(B.rows() + Beq.rows(),cols); if(B.size() > 0) { BBeq.topLeftCorner(B.rows(),cols) = B.replicate(1,B.cols()==cols?1:cols); } if(Beq.size() > 0) { BBeq.bottomLeftCorner(Beq.rows(),cols) = -2.0*Beq.replicate(1,Beq.cols()==cols?1:cols); } // Build right hand side MatrixXT BBequlcols; igl::slice(BBeq,data.unknown_lagrange,1,BBequlcols); MatrixXT NB; if(kr == 0) { NB = BBequlcols; }else { NB = data.preY * Y + BBequlcols; } //std::cout<<"NB=["<::LLT: sol = data.llt.solve(NB); break; case igl::min_quad_with_fixed_data::LDLT: sol = data.ldlt.solve(NB); break; case igl::min_quad_with_fixed_data::LU: // Not a bottleneck sol = data.lu.solve(NB); break; default: cerr<<"Error: invalid solver type"<::QR_LLT); MatrixXT eff_Beq; // Adjust Aeq rhs to include known parts eff_Beq = //data.AeqTQR.colsPermutation().transpose() * (-data.Aeqk * Y + Beq); data.AeqTET * (-data.Aeqk * Y + Beq.replicate(1,Beq.cols()==cols?1:cols)); // Where did this -0.5 come from? Probably the same place as above. MatrixXT Bu; slice(B,data.unknown,1,Bu); MatrixXT NB; NB = -0.5*(Bu.replicate(1,B.cols()==cols?1:cols) + data.preY * Y); // Trim eff_Beq const int nc = data.AeqTQR.rank(); const int neq = Beq.rows(); eff_Beq = eff_Beq.topLeftCorner(nc,cols).eval(); data.AeqTR1T.template triangularView().solveInPlace(eff_Beq); // Now eff_Beq = (data.AeqTR1T \ (data.AeqTET * (-data.Aeqk * Y + Beq))) MatrixXT lambda_0; lambda_0 = data.AeqTQ1 * eff_Beq; //cout<().solveInPlace(temp1); //cout< IGL_INLINE bool igl::min_quad_with_fixed_solve( const min_quad_with_fixed_data & data, const Eigen::MatrixBase & B, const Eigen::MatrixBase & Y, const Eigen::MatrixBase & Beq, Eigen::PlainObjectBase & Z) { Eigen::Matrix sol; return min_quad_with_fixed_solve(data,B,Y,Beq,Z,sol); } template < typename T, typename Derivedknown, typename DerivedB, typename DerivedY, typename DerivedBeq, typename DerivedZ> IGL_INLINE bool igl::min_quad_with_fixed( const Eigen::SparseMatrix& A, const Eigen::MatrixBase & B, const Eigen::MatrixBase & known, const Eigen::MatrixBase & Y, const Eigen::SparseMatrix& Aeq, const Eigen::MatrixBase & Beq, const bool pd, Eigen::PlainObjectBase & Z) { min_quad_with_fixed_data data; if(!min_quad_with_fixed_precompute(A,known,Aeq,pd,data)) { return false; } return min_quad_with_fixed_solve(data,B,Y,Beq,Z); } #ifdef IGL_STATIC_LIBRARY // Explicit template instantiation #if EIGEN_VERSION_AT_LEAST(3,3,0) #else template bool igl::min_quad_with_fixed_solve, Eigen::Matrix const>, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase, Eigen::Matrix const> > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed_solve, Eigen::CwiseNullaryOp, Eigen::Matrix >, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase > const&, Eigen::MatrixBase, Eigen::Matrix > > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&); #endif template bool igl::min_quad_with_fixed, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(Eigen::SparseMatrix const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::SparseMatrix const&, Eigen::MatrixBase > const&, bool, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed_solve, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed_precompute >(Eigen::SparseMatrix const&, Eigen::MatrixBase > const&, Eigen::SparseMatrix const&, bool, igl::min_quad_with_fixed_data&); template bool igl::min_quad_with_fixed_solve, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed_solve, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed_precompute >(Eigen::SparseMatrix const&, Eigen::MatrixBase > const&, Eigen::SparseMatrix const&, bool, igl::min_quad_with_fixed_data&); template bool igl::min_quad_with_fixed_solve, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed_solve, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed_solve, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed_solve, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(igl::min_quad_with_fixed_data const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::PlainObjectBase >&); template bool igl::min_quad_with_fixed, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix, Eigen::Matrix >(Eigen::SparseMatrix const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::MatrixBase > const&, Eigen::SparseMatrix const&, Eigen::MatrixBase > const&, bool, Eigen::PlainObjectBase >&); #endif