|
@@ -0,0 +1,290 @@
|
|
|
+#ifndef IGL_MIN_QUAD_WITH_FIXED_H
|
|
|
+#define IGL_MIN_QUAD_WITH_FIXED_H
|
|
|
+
|
|
|
+#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
|
|
|
+#include <Eigen/Core>
|
|
|
+#include <Eigen/Dense>
|
|
|
+#include <Eigen/Sparse>
|
|
|
+
|
|
|
+namespace igl
|
|
|
+{
|
|
|
+ template <typename T>
|
|
|
+ struct min_quad_with_fixed_data;
|
|
|
+ // MIN_QUAD_WITH_FIXED Minimize quadratic energy Z'*A*Z + Z'*B + C with
|
|
|
+ // constraints that Z(known) = Y, optionally also subject to the constraints
|
|
|
+ // Aeq*Z = Beq
|
|
|
+ //
|
|
|
+ // Templates:
|
|
|
+ // T should be a eigen matrix primitive type like int or double
|
|
|
+ // Inputs:
|
|
|
+ // A n by n matrix of quadratic coefficients
|
|
|
+ // B n by 1 column of linear coefficients
|
|
|
+ // known list of indices to known rows in Z
|
|
|
+ // Y list of fixed values corresponding to known rows in Z
|
|
|
+ // Optional:
|
|
|
+ // Aeq m by n list of linear equality constraint coefficients
|
|
|
+ // Beq m by 1 list of linear equality constraint constant values
|
|
|
+ // pd flag specifying whether A(unknown,unknown) is positive definite
|
|
|
+ // Outputs:
|
|
|
+ // data factorization struct with all necessary information to solve
|
|
|
+ // using min_quad_with_fixed_solve
|
|
|
+ // Returns true on success, false on error
|
|
|
+ template <typename T>
|
|
|
+ bool min_quad_with_fixed_precompute(
|
|
|
+ const Eigen::SparseMatrix<T>& A,
|
|
|
+ const Eigen::MatrixXi & known,
|
|
|
+ const Eigen::SparseMatrix<T>& Aeq,
|
|
|
+ const bool pd,
|
|
|
+ min_quad_with_fixed_data<T> & data
|
|
|
+ );
|
|
|
+
|
|
|
+ // Solves a system previously factored using min_quad_with_fixed_precompute
|
|
|
+ // Inputs:
|
|
|
+ // data factorization struct with all necessary precomputation to solve
|
|
|
+ // Outputs:
|
|
|
+ // Z n by cols solution
|
|
|
+ // Returns true on success, false on error
|
|
|
+ template <typename T>
|
|
|
+ bool min_quad_with_fixed_solve(
|
|
|
+ const min_quad_with_fixed_data<T> & data,
|
|
|
+ const Eigen::Matrix<T,Eigen::Dynamic,1> & B,
|
|
|
+ const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & Y,
|
|
|
+ const Eigen::Matrix<T,Eigen::Dynamic,1> & Beq,
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & Z);
|
|
|
+}
|
|
|
+
|
|
|
+// Implementation
|
|
|
+#include <Eigen/SparseExtra>
|
|
|
+#include <cassert>
|
|
|
+#include <cstdio>
|
|
|
+#include "slice.h"
|
|
|
+#include "is_symmetric.h"
|
|
|
+
|
|
|
+#include "find.h"
|
|
|
+#include "sparse.h"
|
|
|
+#include "lu_lagrange.h"
|
|
|
+
|
|
|
+template <typename T>
|
|
|
+struct igl::min_quad_with_fixed_data
|
|
|
+{
|
|
|
+ int n;
|
|
|
+ bool Auu_pd;
|
|
|
+ bool Auu_sym;
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> known;
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> unknown;
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> lagrange;
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> unknown_lagrange;
|
|
|
+ SparseMatrix<T> preY;
|
|
|
+ SparseMatrix<T> L;
|
|
|
+ SparseMatrix<T> U;
|
|
|
+};
|
|
|
+
|
|
|
+template <typename T>
|
|
|
+bool igl::min_quad_with_fixed_precompute(
|
|
|
+ const Eigen::SparseMatrix<T>& A,
|
|
|
+ const Eigen::Matrix<int,Eigen::Dynamic,1> & known,
|
|
|
+ const Eigen::SparseMatrix<T>& Aeq,
|
|
|
+ const bool pd,
|
|
|
+ igl::min_quad_with_fixed_data<T> & data
|
|
|
+ )
|
|
|
+{
|
|
|
+ // number of rows
|
|
|
+ int n = A.rows();
|
|
|
+ // cache problem size
|
|
|
+ data.n = n;
|
|
|
+
|
|
|
+ int neq = Aeq.rows();
|
|
|
+ // defulat is to have 0 linear equality constraints
|
|
|
+ if(Aeq.size() != 0)
|
|
|
+ {
|
|
|
+ //Aeq = Eigen::SparseMatrix<T>(0,n);
|
|
|
+ assert(n == Aeq.cols());
|
|
|
+ }
|
|
|
+
|
|
|
+ assert(A.rows() == n);
|
|
|
+ assert(A.cols() == n);
|
|
|
+
|
|
|
+ // number of known rows
|
|
|
+ int kr = known.size();
|
|
|
+
|
|
|
+ assert(kr == 0 || known.minCoeff() >= 0);
|
|
|
+ assert(kr == 0 || known.maxCoeff() < n);
|
|
|
+ assert(neq <= n);
|
|
|
+
|
|
|
+ // cache known
|
|
|
+ data.known = known;
|
|
|
+ // get list of unknown indices
|
|
|
+ data.unknown.resize(n-kr);
|
|
|
+ std::vector<bool> unknown_mask;
|
|
|
+ unknown_mask.resize(n,true);
|
|
|
+ for(int i = 0;i<kr;i++)
|
|
|
+ {
|
|
|
+ unknown_mask[known(i)] = false;
|
|
|
+ }
|
|
|
+ int u = 0;
|
|
|
+ for(int i = 0;i<n;i++)
|
|
|
+ {
|
|
|
+ if(unknown_mask[i])
|
|
|
+ {
|
|
|
+ data.unknown(u) = i;
|
|
|
+ u++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // get list of lagrange multiplier indices
|
|
|
+ data.lagrange.resize(neq);
|
|
|
+ for(int i = 0;i<neq;i++)
|
|
|
+ {
|
|
|
+ data.lagrange(i) = n + i;
|
|
|
+ }
|
|
|
+ // cache unknown followed by lagrange indices
|
|
|
+ data.unknown_lagrange.resize(data.unknown.size()+data.lagrange.size());
|
|
|
+ data.unknown_lagrange << data.unknown, data.lagrange;
|
|
|
+
|
|
|
+ Eigen::SparseMatrix<T> Auu;
|
|
|
+ igl::slice(A,data.unknown,data.unknown,Auu);
|
|
|
+
|
|
|
+ // determine if A(unknown,unknown) is symmetric and/or positive definite
|
|
|
+ data.Auu_sym = igl::is_symmetric(Auu);
|
|
|
+ // Positive definiteness is *not* determined, rather it is given as a
|
|
|
+ // parameter
|
|
|
+ data.Auu_pd = pd;
|
|
|
+
|
|
|
+ // Append lagrange multiplier quadratic terms
|
|
|
+ SparseMatrix<T> new_A;
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> AI;
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> AJ;
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,1> AV;
|
|
|
+ igl::find(A,AI,AJ,AV);
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> AeqI(0,1);
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> AeqJ(0,1);
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,1> AeqV(0,1);
|
|
|
+ if(neq > 0)
|
|
|
+ {
|
|
|
+ igl::find(Aeq,AeqI,AeqJ,AeqV);
|
|
|
+ }
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> new_AI(AV.size()+AeqV.size()*2);
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> new_AJ(AV.size()+AeqV.size()*2);
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,1> new_AV(AV.size()+AeqV.size()*2);
|
|
|
+ new_AI << AI, (AeqI.array()+n).matrix(), AeqJ;
|
|
|
+ new_AJ << AJ, AeqJ, (AeqI.array()+n).matrix();
|
|
|
+ new_AV << AV, AeqV, AeqV;
|
|
|
+ //new_AI.block(0,0,n,1) = AI;
|
|
|
+ //new_AJ.block(0,0,n,1) = AJ;
|
|
|
+ //new_AV.block(0,0,n,1) = AV;
|
|
|
+ //new_AI.block(n,0,neq,1) = AeqI+n;
|
|
|
+ //new_AJ.block(n,0,neq,1) = AeqJ;
|
|
|
+ //new_AV.block(n,0,neq,1) = AeqV;
|
|
|
+ //new_AI.block(n+neq,0,neq,1) = AeqJ;
|
|
|
+ //new_AJ.block(n+neq,0,neq,1) = AeqI+n;
|
|
|
+ //new_AV.block(n+neq,0,neq,1) = AeqV;
|
|
|
+ igl::sparse(new_AI,new_AJ,new_AV,n+neq,n+neq,new_A);
|
|
|
+
|
|
|
+ // precompute RHS builders
|
|
|
+ Eigen::SparseMatrix<T> Aulk;
|
|
|
+ igl::slice(new_A,data.unknown_lagrange,data.known,Aulk);
|
|
|
+ Eigen::SparseMatrix<T> Akul;
|
|
|
+ igl::slice(new_A,data.known,data.unknown_lagrange,Akul);
|
|
|
+
|
|
|
+ //// This doesn't work!!!
|
|
|
+ //data.preY = Aulk + Akul.transpose();
|
|
|
+ Eigen::SparseMatrix<T> AkulT = Akul.transpose();
|
|
|
+ //// Resize preY before assigning
|
|
|
+ //data.preY.resize(data.unknown_lagrange.size(),data.known.size());
|
|
|
+ data.preY = Aulk + AkulT;
|
|
|
+
|
|
|
+ // Create factorization
|
|
|
+ if(data.Auu_sym && data.Auu_pd)
|
|
|
+ {
|
|
|
+ Eigen::SparseMatrix<T> Aequ(0,0);
|
|
|
+ if(neq>0)
|
|
|
+ {
|
|
|
+ Eigen::Matrix<int,Eigen::Dynamic,1> Aeq_rows;
|
|
|
+ Aeq_rows.resize(neq);
|
|
|
+ for(int i = 0;i<neq;i++)
|
|
|
+ {
|
|
|
+ Aeq_rows(i) = i;
|
|
|
+ }
|
|
|
+ igl::slice(Aeq,Aeq_rows,data.unknown,Aequ);
|
|
|
+ }
|
|
|
+ // Get transpose of Aequ
|
|
|
+ Eigen::SparseMatrix<T> AequT = Aequ.transpose();
|
|
|
+ // Compute LU decomposition
|
|
|
+ bool lu_success = igl::lu_lagrange(Auu,AequT,data.L,data.U);
|
|
|
+ if(!lu_success)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }else
|
|
|
+ {
|
|
|
+ Eigen::SparseMatrix<T> NA;
|
|
|
+ igl::slice(new_A,data.unknown_lagrange,data.unknown_lagrange,NA);
|
|
|
+ assert(false);
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+template <typename T>
|
|
|
+bool igl::min_quad_with_fixed_solve(
|
|
|
+ const igl::min_quad_with_fixed_data<T> & data,
|
|
|
+ const Eigen::Matrix<T,Eigen::Dynamic,1> & B,
|
|
|
+ const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & Y,
|
|
|
+ const Eigen::Matrix<T,Eigen::Dynamic,1> & Beq,
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & Z)
|
|
|
+{
|
|
|
+ // 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();
|
|
|
+
|
|
|
+ // number of lagrange multipliers aka linear equality constraints
|
|
|
+ int neq = data.lagrange.size();
|
|
|
+
|
|
|
+ if(neq != 0)
|
|
|
+ {
|
|
|
+ }
|
|
|
+
|
|
|
+ // append lagrange multiplier rhs's
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,1> BBeq(B.size() + Beq.size());
|
|
|
+ BBeq << B, (Beq*-2.0);
|
|
|
+
|
|
|
+ // Build right hand side
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,1> BBequl;
|
|
|
+ igl::slice(BBeq,data.unknown_lagrange,BBequl);
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> BBequlcols;
|
|
|
+ igl::repmat(BBequl,1,cols,BBequlcols);
|
|
|
+ Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> NB;
|
|
|
+ NB = data.preY * Y + BBequlcols;
|
|
|
+
|
|
|
+ // 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);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ data.L.template triangularView<Lower>().solveInPlace(NB);
|
|
|
+ data.U.template triangularView<Upper>().solveInPlace(NB);
|
|
|
+ // Now NB contains sol/-0.5
|
|
|
+ NB *= -0.5;
|
|
|
+ // Now NB contains solution
|
|
|
+ // Place solution in Z
|
|
|
+ for(int i = 0;i<(NB.rows()-neq);i++)
|
|
|
+ {
|
|
|
+ for(int j = 0;j<NB.cols();j++)
|
|
|
+ {
|
|
|
+ Z(data.unknown_lagrange(i),j) = NB(i,j);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+#endif
|