コード例 #1
0
ファイル: covariance.cpp プロジェクト: smwesten-usgs/pestpp
Mat Mat::inv()
{
	if (nrow() != ncol()) throw runtime_error("Mat::inv() error: only symmetric positive definite matrices can be inverted with Mat::inv()");
	if (mattype == MatType::DIAGONAL)
	{
		Eigen::VectorXd diag = matrix.diagonal();
		diag = 1.0 / diag.array();
		vector<Eigen::Triplet<double>> triplet_list;
		
		for (int i = 0; i != diag.size(); ++i)
		{
			triplet_list.push_back(Eigen::Triplet<double>(i, i, diag[i]));
		}
		Eigen::SparseMatrix<double> inv_mat(triplet_list.size(),triplet_list.size());
		inv_mat.setZero();
		inv_mat.setFromTriplets(triplet_list.begin(), triplet_list.end());
		return Mat(row_names, col_names, inv_mat);
	}
	
	//Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> solver;
	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
	solver.compute(matrix);
	Eigen::SparseMatrix<double> I(nrow(), nrow());
	I.setIdentity();
	Eigen::SparseMatrix<double> inv_mat = solver.solve(I);
	return Mat(row_names, col_names, inv_mat);
}
コード例 #2
0
ファイル: covariance.cpp プロジェクト: smwesten-usgs/pestpp
void Mat::inv_ip(Logger *log)
{
	if (nrow() != ncol()) throw runtime_error("Mat::inv() error: only symmetric positive definite matrices can be inverted with Mat::inv()");
	if (mattype == MatType::DIAGONAL)
	{
		log->log("inverting diagonal matrix in place");
		log->log("extracting diagonal");
		Eigen::VectorXd diag = matrix.diagonal();
		log->log("inverting diagonal");
		diag = 1.0 / diag.array();
		log->log("buidling triplets");
		vector<Eigen::Triplet<double>> triplet_list;
		for (int i = 0; i != diag.size(); ++i)
		{
			triplet_list.push_back(Eigen::Triplet<double>(i, i, diag[i]));
		}
		log->log("resizeing matrix to size " + triplet_list.size());
		matrix.resize(triplet_list.size(), triplet_list.size());
		matrix.setZero();
		log->log("setting matrix from triplets");
		matrix.setFromTriplets(triplet_list.begin(),triplet_list.end());
		//Eigen::SparseMatrix<double> inv_mat(triplet_list.size(), triplet_list.size());
		//inv_mat.setZero();
		//inv_mat.setFromTriplets(triplet_list.begin(), triplet_list.end());
		//matrix = inv_mat;
		//cout << "diagonal inv_ip()" << endl;
		/*matrix.resize(triplet_list.size(), triplet_list.size());
		matrix.setZero();
		matrix.setFromTriplets(triplet_list.begin(),triplet_list.end());*/
		return;
	}

	//Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> solver;
	log->log("inverting non-diagonal matrix in place");
	log->log("instantiate solver");
	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
	log->log("computing inverse");
	solver.compute(matrix);
	log->log("getting identity instance for solution");
	Eigen::SparseMatrix<double> I(nrow(), nrow());
	I.setIdentity();
	//Eigen::SparseMatrix<double> inv_mat = solver.solve(I);
	//matrix = inv_mat;
	log->log("solving");
	matrix = solver.solve(I);
	//cout << "full inv_ip()" << endl;
	/*matrix.setZero();
	matrix = solver.solve(I);*/
	
}
コード例 #3
0
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;
}
コード例 #4
0
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;
}
コード例 #5
0
ファイル: opennurbs_fit.cpp プロジェクト: cogitokat/brlcad
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;
      }
}
コード例 #6
0
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;
}
コード例 #7
0
ファイル: backendeigen.hpp プロジェクト: TrojanXu/feelpp
typename BackendEigen<T,_Options>::solve_return_type
BackendEigen<T,_Options>::solve( sparse_matrix_type const& _A,
                                 vector_type& _x,
                                 const vector_type& _b,
                                 mpl::bool_<false>)
{
    bool reusePC = ( this->precMatrixStructure() == SAME_PRECONDITIONER );

    eigen_sparse_matrix_type const& A( dynamic_cast<eigen_sparse_matrix_type const&>( _A ) );
    eigen_vector_type      & x( dynamic_cast<eigen_vector_type      &>( _x ) );
    eigen_vector_type const& b( dynamic_cast<eigen_vector_type const&>( _b ) );

    //x.vec()=A.mat().template fullPivLu().solve(b.vec());
    Eigen::SimplicialLDLT<typename eigen_sparse_matrix_type::matrix_type> solver;
    solver.compute(A.mat());
    x.vec() = solver.solve(b.vec());

    // if(solver.info()!=Eigen::Succeeded) {
    //     // solving failed
    //     return boost::make_tuple(false,1,1e-10);;
    // }
    return solve_return_type( boost::make_tuple(true,1,1e-10) );
} // BackendEigen::solve
IGL_INLINE double igl::polyvector_field_poisson_reconstruction(
  const Eigen::PlainObjectBase<DerivedV> &Vcut,
  const Eigen::PlainObjectBase<DerivedF> &Fcut,
  const Eigen::PlainObjectBase<DerivedS> &sol3D_combed,
  Eigen::PlainObjectBase<DerivedSF> &scalars,
  Eigen::PlainObjectBase<DerivedS> &sol3D_recon,
  Eigen::PlainObjectBase<DerivedE> &max_error )
  {
    Eigen::SparseMatrix<typename DerivedV::Scalar> gradMatrix;
    igl::grad(Vcut, Fcut, gradMatrix);

    Eigen::VectorXd FAreas;
    igl::doublearea(Vcut, Fcut, FAreas);
    FAreas = FAreas.array() * .5;

    int nf = FAreas.rows();
    Eigen::SparseMatrix<typename DerivedV::Scalar> M,M1;
    Eigen::VectorXi II = igl::colon<int>(0, nf-1);

    igl::sparse(II, II, FAreas, M1);
    igl::repdiag(M1, 3, M) ;

    int half_degree = sol3D_combed.cols()/3;

    sol3D_recon.setZero(sol3D_combed.rows(),sol3D_combed.cols());

    int numF = Fcut.rows();
    scalars.setZero(Vcut.rows(),half_degree);

    Eigen::SparseMatrix<typename DerivedV::Scalar> Q = gradMatrix.transpose()* M *gradMatrix;

    //fix one point at Ik=fix, value at fixed xk=0
    int fix = 0;
    Eigen::VectorXi Ik(1);Ik<<fix;
    Eigen::VectorXd xk(1);xk<<0;

    //unknown indices
    Eigen::VectorXi Iu(Vcut.rows()-1,1);
    Iu<<igl::colon<int>(0, fix-1),  igl::colon<int>(fix+1,Vcut.rows()-1);

    Eigen::SparseMatrix<typename DerivedV::Scalar> Quu, Quk;
    igl::slice(Q, Iu, Iu, Quu);
    igl::slice(Q, Iu, Ik, Quk);
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<typename DerivedV::Scalar> > solver;
    solver.compute(Quu);


    Eigen::VectorXd vec; vec.setZero(3*numF,1);
    for (int i =0; i<half_degree; ++i)
    {
      vec<<sol3D_combed.col(i*3+0),sol3D_combed.col(i*3+1),sol3D_combed.col(i*3+2);
      Eigen::VectorXd b = gradMatrix.transpose()* M * vec;
      Eigen::VectorXd bu = igl::slice(b, Iu);

      Eigen::VectorXd rhs = bu-Quk*xk;
      Eigen::MatrixXd yu = solver.solve(rhs);

      Eigen::VectorXi index = i*Eigen::VectorXi::Ones(Iu.rows(),1);
      igl::slice_into(yu, Iu, index, scalars);scalars(Ik[0],i)=xk[0];
    }

    //    evaluate gradient of found scalar function
    for (int i =0; i<half_degree; ++i)
    {
      Eigen::VectorXd vec_poisson = gradMatrix*scalars.col(i);
      sol3D_recon.col(i*3+0) = vec_poisson.segment(0*numF, numF);
      sol3D_recon.col(i*3+1) = vec_poisson.segment(1*numF, numF);
      sol3D_recon.col(i*3+2) = vec_poisson.segment(2*numF, numF);
    }

    max_error.setZero(numF,1);
    for (int i =0; i<half_degree; ++i)
    {
      Eigen::VectorXd diff = (sol3D_recon.block(0, i*3, numF, 3)-sol3D_combed.block(0, i*3, numF, 3)).rowwise().norm();
      diff = diff.array() / sol3D_combed.block(0, i*3, numF, 3).rowwise().norm().array();
      max_error = max_error.cwiseMax(diff.cast<typename DerivedE::Scalar>());
    }

    return max_error.mean();
  }
コード例 #9
0
// 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;
}
コード例 #10
0
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;
}