bool
solveSparseLinearSystemLQ (Eigen::SparseMatrix<double>* A, Eigen::MatrixXd* b, Eigen::MatrixXd* x)
{
Eigen::SparseMatrix<double> At(A->cols(), A->rows());
Eigen::MatrixXd Atb(A->cols(), b->cols());
At = A->transpose();
Eigen::SparseMatrix<double> AtA = At * (*A);
Atb = At * (*b);
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
solver.compute(AtA);
if(solver.info()!=Eigen::Success) {
// decomposition failed
std::cout << "decomposition failed\n";
}
(*x) = solver.solve(Atb);
if(solver.info()!=Eigen::Success) {
std::cout << "solver failed: " << solver.info() << "\n";
return false;
} else {
return true;
}
}
bool StiffnessSolver::SolveSystem(SpMat &K, VX &D, VX &F, int verbose, int &info)
{
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
solver.compute(K);
info = 0;
if (solver.info() != Eigen::Success)
{
fprintf(stderr, "SolverSystem(Ver.Eigen): Error in Decomposition!\n");
assert(0);
return false;
}
VX Diag = solver.vectorD();
if (verbose)
{
fprintf(stdout, "---LDLt Decomposition Diagnoal vector D---\n");
}
for (int i = 0; i < Diag.size(); i++)
{
if (Diag[i] < EPS)
{
fprintf(stderr, " SolveSystem(Ver.Eigen): negative number found on diagonal ...\n");
fprintf(stderr, " d[%d] = %11.4e\n", i, Diag[i]);
info--;
assert(0);
return false;
}
if (verbose)
{
fprintf(stdout, "d[%d] = %.5f\n", i, Diag[i]);
}
}
if (info < 0)
{
fprintf(stderr, "Stiffness Matrix is not positive definite: %d negative elements\n", info);
fprintf(stderr, "found on decomp diagonal of K.\n");
fprintf(stderr, "The stucture may have mechanism and thus not stable in general\n");
fprintf(stderr, "Please Make sure that all six\n");
fprintf(stderr, "rigid body translations are restrained!\n");
assert(0);
return false;
}
D = solver.solve(F);
if (solver.info() != Eigen::Success)
{
fprintf(stderr, "SolverSystem(Ver.Eigen): Error in Solving!\n");
assert(0);
return false;
}
return true;
}
Mat solveQurdOpt(SpMat L, SpMat C, VectorXd alpha_star){
//
omp_set_num_threads(1);
Eigen::setNbThreads(1);
cout << "solving quadratic optimization proble .............." << endl;
double lambda = 0.000001;
SpMat D(L.rows(), L.cols());
D.setIdentity();
SpMat b = alpha_star.sparseView();
SpMat A, alpha;
A = L + C + lambda * D;
b = C * b;
Eigen::SimplicialLDLT<SpMat> solver;
cout << "begin to factorize A" << endl;
solver.compute(A);
if(solver.info() != Eigen::Success)
cout << "decomposition failed" << endl;
else
cout << "decomposition success" << endl;
cout << "begin to solve !" << endl;
alpha = solver.solve(b);
cout << "solve success" << endl;
Mat alpha_mat;
eigen2cv(MatrixXd(alpha), alpha_mat);
alpha_mat = alpha_mat.t();
alpha_mat = alpha_mat.reshape(0, imgSize.width).t();
alpha_mat = alpha_mat * 0.5 + 0.5;
alpha_mat = max(min(alpha_mat, 1), 0);
//showSavePicLBDM("alpha", alpha_mat, showImgLBDM, saveImgLBDM);
return alpha_mat;
}
bool IMEXIntegrator::setRotations() const {
const real newtonThreshold = 1.0e-5; //should be able to get this almost exact
std::vector<Triplet> triplets;
Eigen::SparseMatrix<real> hess(r.numEdges()-2, r.numEdges()-2);
VecXe rot = r.next().rot;
VecXe grad = VecXe::Zero(r.numEdges()-2); // Assumes edges are clamped
bool newtonConverge = false;
std::vector<Vec3e> curveBinorm;
for (int i=1; i<r.numCPs()-1; i++) {
Vec3e tPrev = r.next().edge(i-1).normalized();
Vec3e tNext = r.next().edge(i).normalized();
real chi = 1.0 + (tPrev.dot(tNext));
curveBinorm.push_back(2.0*tPrev.cross(tNext)/chi);
}
int newtonIterations = 0;
do {
triplets.clear();
calcRotEqs(r, rot, curveBinorm, grad, triplets);
real resid = grad.norm();
if (resid < newtonThreshold || newtonIterations > 4) {
if (resid > 1.0e-5 * r.numEdges()) { return false; }
newtonConverge = true;
break;
}
newtonIterations++;
hess.setFromTriplets(triplets.begin(), triplets.end());
hess += hessBase;
Eigen::SimplicialLDLT<Eigen::SparseMatrix<real>> sLDLT;
sLDLT.compute(hess);
VecXe sol = sLDLT.solve(grad);
assert(sLDLT.info() == Eigen::Success);
rot.block(1, 0, r.numEdges()-2, 1) += sol;
} while (!newtonConverge);
if (newtonConverge) {
r.next().rot = rot;
}
return newtonConverge;
}
// The Real Core Function doing the actual mesh processing.
bool FilterHarmonicPlugin::applyFilter(QAction * action, MeshDocument & md, RichParameterSet & par, vcg::CallBackPos * cb)
{
switch(ID(action))
{
case FP_SCALAR_HARMONIC_FIELD :
{
typedef vcg::GridStaticPtr<CMeshO::VertexType, CMeshO::ScalarType> VertexGrid;
typedef double CoeffScalar; // TODO, when moving the code to a class make it a template (CoeffScalar = double)
typedef CMeshO::ScalarType ScalarType;
typedef CMeshO::CoordType CoordType;
typedef CMeshO::VertexType VertexType;
typedef CMeshO::FaceType FaceType;
typedef Eigen::Triplet<CoeffScalar> T;
typedef Eigen::SparseMatrix<CoeffScalar> SpMat; //sparse matrix type of double
CMeshO & m = md.mm()->cm;
vcg::tri::Allocator<CMeshO>::CompactFaceVector(m);
vcg::tri::Allocator<CMeshO>::CompactVertexVector(m);
md.mm()->updateDataMask(MeshModel::MM_FACEFACETOPO | MeshModel::MM_VERTMARK);
vcg::tri::UpdateBounding<CMeshO>::Box(m);
vcg::tri::UpdateTopology<CMeshO>::FaceFace(m);
int n = m.VN();
int fn = m.FN();
std::vector<T> coeffs; // coefficients of the system
std::map<size_t,CoeffScalar> sums; // row sum of the coefficient
SpMat laplaceMat; // the system to be solved
laplaceMat.resize(n, n);
Log("Generating coefficients.`");
cb(0, "Generating coefficients...");
vcg::tri::UpdateFlags<CMeshO>::FaceClearV(m);
// Iterate over the faces
for (size_t i = 0; i < m.face.size(); ++i)
{
CMeshO::FaceType & f = m.face[i];
if (f.IsD())
{
assert(int(i) == fn);
break; // TODO FIX the indexing of vertices
}
assert(!f.IsV());
f.SetV();
// Generate coefficients for each edge
for (int idx = 0; idx < 3; ++idx)
{
CoeffScalar weight;
WeightInfo res = ComputeWeight<FaceType, CoeffScalar>(f, idx, weight);
switch (res)
{
case EdgeAlreadyVisited : continue;
case Success : break;
case BorderEdge :
this->errorMessage = "Mesh not closed, cannot compute harmonic field on mesh containing holes or borders";
return false;
default: assert(0);
}
// if (weight < 0) weight = 0; // TODO check if negative weight may be an issue
// Add the weight to the coefficients vector for both the vertices of the considered edge
size_t v0_idx = vcg::tri::Index(m, f.V0(idx));
size_t v1_idx = vcg::tri::Index(m, f.V1(idx));
coeffs.push_back(T(v0_idx, v1_idx, -weight));
coeffs.push_back(T(v1_idx, v0_idx, -weight));
// Add the weight to the row sum
sums[v0_idx] += weight;
sums[v1_idx] += weight;
}
f.SetV();
}
// Fill the system matrix
Log("Fill the system matrix");
cb(10, "Filling the system matrix...");
laplaceMat.reserve(coeffs.size());
for (std::map<size_t,CoeffScalar>::const_iterator it = sums.begin(); it != sums.end(); ++it)
{
coeffs.push_back(T(it->first, it->first, it->second));
}
laplaceMat.setFromTriplets(coeffs.begin(), coeffs.end());
// Get the two vertices with value set
VertexGrid vg;
vg.Set(m.vert.begin(), m.vert.end());
vcg::vertex::PointDistanceFunctor<ScalarType> pd;
vcg::tri::Tmark<CMeshO, VertexType> mv;
mv.SetMesh(&m);
mv.UnMarkAll();
CoordType closestP;
ScalarType minDist = 0;
VertexType * vp0 = vcg::GridClosest(vg, pd, mv, par.getPoint3f("point1"), m.bbox.Diag(), minDist, closestP);
VertexType * vp1 = vcg::GridClosest(vg, pd, mv, par.getPoint3f("point2"), m.bbox.Diag(), minDist, closestP);
if (vp0 == NULL || vp1 == NULL || vp0 == vp1)
{
this->errorMessage = "Error occurred for selected points.";
return false;
}
size_t v0_idx = vcg::tri::Index(m, vp0);
size_t v1_idx = vcg::tri::Index(m, vp1);
// Add penalty factor alpha
Log("Setting up the system matrix");
const CoeffScalar alpha = pow(10, 8);
Eigen::Matrix<CoeffScalar, Eigen::Dynamic, 1> b, x; // Unknown and known terms vectors
b.setZero(n);
b(v0_idx) = alpha * par.getFloat("value1");
b(v1_idx) = alpha * par.getFloat("value2");
laplaceMat.coeffRef(v0_idx, v0_idx) += alpha;
laplaceMat.coeffRef(v1_idx, v1_idx) += alpha;
// Solve system laplacianMat x = b
Log("System matrix decomposition...");
cb(20, "System matrix decomposition...");
Eigen::SimplicialLDLT<Eigen::SparseMatrix<CoeffScalar> > solver; // TODO eventually use another solver
solver.compute(laplaceMat);
if(solver.info() != Eigen::Success)
{
// decomposition failed
this->errorMessage = "System matrix decomposition failed: ";
if (solver.info() == Eigen::NumericalIssue)
this->errorMessage += "numerical issue.";
else if (solver.info() == Eigen::NoConvergence)
this->errorMessage += "no convergence.";
else if (solver.info() == Eigen::InvalidInput)
this->errorMessage += "invalid input.";
return false;
}
Log("Solving the system...");
cb(85, "Solving the system...");
x = solver.solve(b);
if(solver.info() != Eigen::Success)
{
// solving failed
this->errorMessage = "System solving failed.";
return false;
}
// Colorize bands for the 0-1 interval
if (par.getBool("colorize"))
{
float steps = 20.0f;
for (size_t i = 0; int(i) < n; ++i)
{
bool on = (int)(x[i]*steps)%2 == 1;
if (on)
{
m.vert[i].C() = vcg::Color4b::ColorRamp(0,2,x[i]);
}
else
{
m.vert[i].C() = vcg::Color4b::White;
}
}
}
// Set field value into vertex quality attribute
for (size_t i = 0; int(i) < n; ++i)
{
m.vert[i].Q() = x[i];
}
cb(100, "Done.");
return true;
}
default : assert(0);
}
return false;
}
LaplacianOperator::LaplacianOperator( ObjEntity* mesh,MeshDrawerImpl* drawer ):mState(STATE_CHOOSING_STATIC),OperatorImpl(mesh,drawer)
{
mVerticesController = new VerticesController(mesh,drawer);
assert(mesh != nullptr);
clock_t clock_start;
clock_start = clock();
mMeshVertexCount = mesh->getVertexCount();
initAdjacentMatrix();
// printMatrix(adjacentMatrix,"adjacentMatrix");
// printMatrix(degreeMatrix,"degreeMatrix");
Eigen::SparseMatrix<double> laplacianOperator;
computeLaplacianOperator(laplacianOperator);
#ifdef MY_DEBUG
ofstream out("debug.txt",ios::app);
out << "laplacianOperator:";
for (Eigen::SparseMatrix<double>::InnerIterator it(laplacianOperator,283); it; it++)
{
out << it.row() << ' ';
}
out << endl;
#endif // MY_DEBUG
//printMatrix(laplacianOperator,"laplacianOperator");
Eigen::Matrix<double,Eigen::Dynamic,3> verInput(mMeshVertexCount,3);
for (int i = 0; i < mMeshVertexCount; i++)
{
Vertex* v = mesh->m_vertexList->at(i);
verInput(i,0) = v->x; verInput(i,1) = v->y; verInput(i,2) = v->z;
}
Eigen::Matrix<double,Eigen::Dynamic,3> deltaInput = laplacianOperator * verInput;
#ifdef MY_DEBUG
out << "verInput 283:" << verInput(283,0) << ' ' << verInput(283,1) << ' ' << verInput(283,2) << endl;
out << "deltaInput 283:" << deltaInput(283,0) << ' ' << deltaInput(283,1) << ' ' << deltaInput(283,2) << endl;
#endif // MY_DEBUG
//printMatrix(deltaInput,"deltaInput");
Eigen::SparseMatrix<double> laplacianOperator3D(6*mMeshVertexCount,3*mMeshVertexCount);
laplacianOperator3D.reserve(Eigen::VectorXi::Constant(3*mMeshVertexCount,40));
//Eigen::MatrixXd laplacianOperator(laplacianOperator);
vector<Vertex*>* meshVertex = mesh->m_vertexList;
/* operate laplacianOperator3D */
for (int i = 0; i < mMeshVertexCount; i++)
{
//printf("%d ",i);
/* Generate Matrix A*/
Vertex* v = meshVertex->at(i);
vector<Vertex*> vertexInA;
vertexInA.push_back(v);
vector<int> adjIndex;
getAdjVertex(i,adjIndex);
for (int j = 0; j < adjIndex.size(); j++)
{
vertexInA.push_back(meshVertex->at(adjIndex.at(j)));
}
Eigen::Matrix<double,Eigen::Dynamic,7> matrixA;
matrixA.resize(vertexInA.size()*3,7);
for (int j = 0; j < vertexInA.size(); j++)
{
Eigen::Matrix<double,3,7> m;
constructMatrix(vertexInA.at(j),m);
int r = matrixA.rows();
matrixA.middleRows<3>(j*3) = m;
// printMatrix(m,"m");
}
Eigen::Matrix<double,7,Eigen::Dynamic> pinvA;
/* 当该顶点是孤立的的时候，transposeA*matrixA可能是奇异的，因此pinv就会出现-1.#IND*/
if(adjIndex.size() == 0)
{
pinvA.resize(7,3);
pinvA.setZero();
}
else
{
Eigen::Matrix<double,7,Eigen::Dynamic> transposeA = matrixA.transpose();
pinvA = (transposeA * matrixA).inverse()*transposeA;
}
#ifdef MY_DEBUG
cout << "MatrixA" << endl << matrixA << endl << endl << "PinvA:" << endl;
cout << pinvA << endl;
ofstream sqare("transposeAmatrixA.txt");
sqare << (transposeA * matrixA) << endl << endl;
sqare << (transposeA * matrixA).inverse() << endl;
sqare.close();
#endif // MY_DEBUG
Eigen::Matrix<double,1,Eigen::Dynamic> s = pinvA.row(0);
Eigen::Matrix<double,1,Eigen::Dynamic> h1 = pinvA.row(1);
Eigen::Matrix<double,1,Eigen::Dynamic> h2 = pinvA.row(2);
Eigen::Matrix<double,1,Eigen::Dynamic> h3 = pinvA.row(3);
Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaX = s*deltaInput(i,0)-h3*deltaInput(i,1)+h2*deltaInput(i,2);
Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaY = h3*deltaInput(i,0)+s*deltaInput(i,1)-h1*deltaInput(i,2);
Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaZ = -h2*deltaInput(i,0)-h1*deltaInput(i,1)+s*deltaInput(i,2);
Eigen::Matrix<double,3,Eigen::Dynamic> TDelta;
TDelta.resize(3,TDeltaX.cols());
TDelta.row(0) = TDeltaX; TDelta.row(1) = TDeltaY; TDelta.row(2) = TDeltaZ;
for (int j = 0; j < 3; j++)
{
int opRow = j*mMeshVertexCount+i;
laplacianOperator3D.insert(opRow,i) = j == 0 ? -TDelta(j,0) + laplacianOperator.coeff(i,i) : -TDelta(j,0);
laplacianOperator3D.insert(opRow,i+mMeshVertexCount) = j == 1 ? -TDelta(j,1) + laplacianOperator.coeff(i,i) : -TDelta(j,1);
laplacianOperator3D.insert(opRow,i+2*mMeshVertexCount) = j == 2 ? -TDelta(j,2) + laplacianOperator.coeff(i,i) : -TDelta(j,2);
int curTDeltaCol = 3;
for (int a = 0; a < adjIndex.size(); a++)
{
int adj = adjIndex.at(a);
laplacianOperator3D.insert(opRow,adj) = j == 0 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) : -TDelta(j,curTDeltaCol++);
laplacianOperator3D.insert(opRow,adj+mMeshVertexCount) = j == 1 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) : -TDelta(j,curTDeltaCol++);
laplacianOperator3D.insert(opRow,adj+2*mMeshVertexCount) = j == 2 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) : -TDelta(j,curTDeltaCol++);
}
}
}
#ifdef MY_DEBUG
ofstream outL3d("laplacianOperator3D.txt");
outL3d << laplacianOperator3D << endl;
outL3d.close();
#endif
int mMeshVertexCount_X3 = 3*mMeshVertexCount;
Eigen::SparseMatrix<double>& A_prime = laplacianOperator3D;
// printMatrix(laplacianOperator3D,"laplacianOperator3D");
// printMatrix(A_prime,"A_prime");
int offset = 0;
for(int j = 0; j < mMeshVertexCount_X3; j+=3)
{
A_prime.insert(mMeshVertexCount_X3+j,offset) = 1;
A_prime.insert(mMeshVertexCount_X3+j+1,offset+mMeshVertexCount) = 1;
A_prime.insert(mMeshVertexCount_X3+j+2,offset+2*mMeshVertexCount) = 1;
offset++;
}
Eigen::VectorXd b(mMeshVertexCount_X3*2);
b.setZero();
int ret;
defoMesh = ObjUtility::createObjEntity("defo.obj",ret);
vector<Vertex*>* defoVertex = defoMesh->m_vertexList;
int defoSize = defoVertex->size();
int nodefoSize = mMeshVertexCount;
for (int j = 0; j < mMeshVertexCount; j++)
{
b(mMeshVertexCount_X3+3*j) = defoVertex->at(j)->x;
b(mMeshVertexCount_X3+3*j+1) = defoVertex->at(j)->y;
b(mMeshVertexCount_X3+3*j+2) = defoVertex->at(j)->z;
}
A_prime.makeCompressed();
Eigen::SparseMatrix<double> A_prime_T = Eigen::SparseMatrix<double>(A_prime.transpose());
Eigen::VectorXd A_prime_T_b = A_prime_T*b;
Eigen::SparseMatrix<double> sym = A_prime_T*A_prime;
long t = (clock()-clock_start);
printf("constructTime:%d\n",t);
#ifdef MY_DEBUG
ofstream outSym("sym.txt");
outSym << sym << endl;
outSym.close();
ofstream outAprimeT("AprimeT.txt");
outAprimeT << A_prime_T << endl;
outAprimeT.close();
ofstream outAprime("A_prime.txt");
outAprime << A_prime << endl;
outAprime.close();
#endif
#ifdef MY_DEBUG
Eigen::EigenSolver<Eigen::MatrixXd > es(sym);
ofstream outEigen("eigenvalues.txt",ios::app);
outEigen << es.eigenvalues() << endl;
outEigen.close();
#endif // MY_DEBUG
clock_start = clock();
//Eigen::SimplicialLLT<Eigen::SparseMatrix<double> > solver;
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
solver.compute(sym);
t = (clock()-clock_start);
printf("factorizationTime:%d\n",t);
clock_start = clock();
// ofstream outLU("LU.txt",ios::app);
// outLU << "L:" << endl;
// outLU << solver.matrixL() << endl;
// outLU << "------------------------------------------------------------------" << endl;
// outLU << "U:" << endl;
// outLU << solver.matrixU() << endl;
// outLU.close();
if(solver.info()!= Eigen::Success) {
// decomposition failed
return;
}
Eigen::VectorXd v_p = solver.solve(A_prime_T_b);
t = (clock()-clock_start);
printf("SolveTime:%d\n",t);
//printMatrix(v_p,"v_p");
writeToDisk(v_p);
if(solver.info()!= Eigen::Success) {
// solving failed
return;
}
// Eigen::VectorXd v_p = A_prime.householderQr().solve(b);
// //Eigen::VectorXd v_p = A_prime.jacobiSvd().solve(b);
//
//
// printMatrix(laplacianOperator3D,"laplacianOperator3D");
// printMatrix(A_prime,"A_prime");
// printMatrix(adjacentMatrix,"adjacentMatrix");
// printMatrix(b,"b");
// printMatrix(v_p,"v_p");
//
//cout << laplacianOperator;
}