예제 #1
0
int
main() {
   vector<Triplet<double>> trips(4);
   trips.push_back(Triplet<double>(0, 0, .435));
   trips.push_back(Triplet<double>(1, 1, .435));
   trips.push_back(Triplet<double>(2, 2, .435));
   trips.push_back(Triplet<double>(3, 3, .435));

   SparseMatrix<double> A;
   A.resize(4, 4);
   A.setFromTriplets(trips.begin(), trips.end());
   SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> solverA;
   solverA.compute(A);

   VectorXd B;
   B.resize(4);
   B(0) = .435;
   B(1) = .435;
   B(2) = .435;
   B(3) = .435;

   VectorXd X = solverA.solve(B);

   for (int i = 0; i < 4; i++) cout << X(i) << endl;
}
template<typename Scalar> void test_sparseqr_scalar()
{
  typedef SparseMatrix<Scalar,ColMajor> MatrixType; 
  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMat;
  typedef Matrix<Scalar,Dynamic,1> DenseVector;
  MatrixType A;
  DenseMat dA;
  DenseVector refX,x,b; 
  SparseQR<MatrixType, COLAMDOrdering<int> > solver; 
  generate_sparse_rectangular_problem(A,dA);
  
  b = dA * DenseVector::Random(A.cols());
  solver.compute(A);
  if(internal::random<float>(0,1)>0.5f)
    solver.factorize(A);  // this checks that calling analyzePattern is not needed if the pattern do not change.
  if (solver.info() != Success)
  {
    std::cerr << "sparse QR factorization failed\n";
    exit(0);
    return;
  }
  x = solver.solve(b);
  if (solver.info() != Success)
  {
    std::cerr << "sparse QR factorization failed\n";
    exit(0);
    return;
  }
  
  VERIFY_IS_APPROX(A * x, b);
  
  //Compare with a dense QR solver
  ColPivHouseholderQR<DenseMat> dqr(dA);
  refX = dqr.solve(b);
  
  VERIFY_IS_EQUAL(dqr.rank(), solver.rank());
  if(solver.rank()==A.cols()) // full rank
    VERIFY_IS_APPROX(x, refX);
//   else
//     VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );

  // Compute explicitly the matrix Q
  MatrixType Q, QtQ, idM;
  Q = solver.matrixQ();
  //Check  ||Q' * Q - I ||
  QtQ = Q * Q.adjoint();
  idM.resize(Q.rows(), Q.rows()); idM.setIdentity();
  VERIFY(idM.isApprox(QtQ));
  
  // Q to dense
  DenseMat dQ;
  dQ = solver.matrixQ();
  VERIFY_IS_APPROX(Q, dQ);
}
예제 #3
0
        bool solve(const int mode)
        {
            bool batch = (mode !=2 || first_ordered_node_ == 0);
            bool order = (mode !=0 && n_nodes_ > 1);

            // BATCH
            if (batch)
            {
                // REORDER
                if (order)
                    ordering(0);

                //print_problem();

                // SOLVE
                t_solving_ = clock();
                A_.makeCompressed();
                solver_.compute(A_);
                if (solver_.info() != Success)
                {
                    std::cout << "decomposition failed" << std::endl;
                    return 0;
                }
                x_incr_ = solver_.solve(b_);
                R_ = solver_.matrixR();
                //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
                time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
            }
            // INCREMENTAL
            else
            {
                // REORDER SUBPROBLEM
                ordering(first_ordered_node_);
                //print_problem();

                // SOLVE ORDERED SUBPROBLEM
                t_solving_= clock();
                A_nodes_.makeCompressed();
                A_.makeCompressed();

                // finding measurements block
                SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
        //        std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
        //        std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
                int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
                int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
                int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
                int unordered_variables = nodes_.at(first_ordered_node_).location;

                SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
                solver_.compute(A_partial);
                if (solver_.info() != Success)
                {
                    std::cout << "decomposition failed" << std::endl;
                    return 0;
                }
                //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
                x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));

                // store new part of R
                eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
                //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
                addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
                //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
                R_.makeCompressed();

                // solving not ordered subproblem
                if (unordered_variables > 0)
                {
                    //std::cout << "--------------------- solving unordered part" << std::endl;
                    SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
                    //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
                    SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
                    //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
                    solver_.compute(R1);
                    if (solver_.info() != Success)
                    {
                        std::cout << "decomposition failed" << std::endl;
                        return 0;
                    }
                    x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
                }

            }
            // UNDO ORDERING FOR RESULT
            PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
            nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
            x_incr_ = acc_permutation.inverse() * x_incr_;

            time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;

            return 1;
        }
예제 #4
0
void ImplicitNewmark::renderNewtonsMethod(){
	//Implicit Code
	v_k.setZero();
	x_k.setZero();
	x_k = x_old;
	v_k = v_old;
	VectorXd f_old = f;

	forceGradient.setZero();
	bool Nan=false;
	int NEWTON_MAX = 100, i =0;
	double gamma = 0.5;
	double beta =0.25;

	// cout<<"--------"<<simTime<<"-------"<<endl;
	// cout<<"x_k"<<endl;
	// cout<<x_k<<endl<<endl;
	// cout<<"v_k"<<endl;
	// cout<<v_k<<endl<<endl;
	// cout<<"--------------------"<<endl;
	for( i=0; i<NEWTON_MAX; i++){
		grad_g.setZero();
	
		NewmarkXtoTV(x_k, TVk);//TVk value changed in function
		NewmarkCalculateElasticForceGradient(TVk, forceGradient); 
		NewmarkCalculateForces(TVk, forceGradient, x_k, f);

		VectorXd g = x_k - x_old - h*v_old - (h*h/2)*(1-2*beta)*InvMass*f_old - (h*h*beta)*InvMass*f;
		grad_g = Ident - h*h*beta*InvMass*(forceGradient+(rayleighCoeff/h)*forceGradient);

		// VectorXd g = RegMass*x_k - RegMass*x_old - RegMass*h*v_old - (h*h/2)*(1-2*beta)*f_old - (h*h*beta)*f;
		// grad_g = RegMass - h*h*beta*(forceGradient+(rayleighCoeff/h)*forceGradient);
	
		// cout<<"G"<<t<<endl;
		// cout<<g<<endl<<endl;
		// cout<<"G Gradient"<<t<<endl;
		// cout<<grad_g<<endl;

		//solve for delta x
		// Conj Grad
		// ConjugateGradient<SparseMatrix<double>> cg;
		// cg.compute(grad_g);
		// VectorXd deltaX = -1*cg.solve(g);

		// Sparse Cholesky LL^T
		// SimplicialLLT<SparseMatrix<double>> llt;
		// llt.compute(grad_g);
		// VectorXd deltaX = -1* llt.solve(g);

		//Sparse QR 
		SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> sqr;
		sqr.compute(grad_g);
		VectorXd deltaX = -1*sqr.solve(g);

		// CholmodSimplicialLLT<SparseMatrix<double>> cholmodllt;
		// cholmodllt.compute(grad_g);
		// VectorXd deltaX = -cholmodllt.solve(g);
		

		x_k+=deltaX;
		if(x_k != x_k){
			Nan = true;
			break;
		}
		if(g.squaredNorm()<.00000001){
			break;
		}
	}
	if(Nan){
		cout<<"ERROR NEWMARK: Newton's method doesn't converge"<<endl;
		cout<<i<<endl;
		exit(0);
	}
	if(i== NEWTON_MAX){
		cout<<"ERROR NEWMARK: Newton max reached"<<endl;
		cout<<i<<endl;
		exit(0);
	}
	v_old = v_old + h*(1-gamma)*InvMass*f_old + h*gamma*InvMass*f;
	x_old = x_k;
}