Ejemplo n.º 1
0
// [[Rcpp::depends(RcppEigen)]]
// [[Rcpp::export]]
Eigen::MatrixXd cgm_c(SEXP As, SEXP bs) {
  const MSpMat A = as<MSpMat>(As);
  //const Map<MatrixXd> A(as<Map<MatrixXd> > (As));
  const Map<MatrixXd> b(as<Map<MatrixXd> > (bs));
  ConjugateGradient<SparseMatrix<double> > cg;
  cg.setTolerance(1e-06);
  //ConjugateGradient<MatrixXd> cg;
  //cg.compute(A);
  MatrixXd x=cg.compute(A).solve(b);
  return x;
}
Ejemplo n.º 2
0
void LeastSquareFitClass::solveLeastSquareFit(MatrixXd A,
                                              VectorXd dataPoints,
                                              VectorXd *bestFit){

   VectorXd b = A.adjoint() * dataPoints;
   MatrixXd M = A.adjoint() * A;

   // Solving M x = b 
   ConjugateGradient<MatrixXd> cg;
   cg.compute(M);
   (*bestFit) = cg.solve(b);

}
Ejemplo n.º 3
0
int sipg_sem_1d<FLOAT_TYPE>::solve()
{

  stiffness_matrix_generation.start(); // start timer  
  compute_stiffness_matrix();
  stiffness_matrix_generation.stop();  // stop timer


  compute_rhs_vector();



#ifdef USE_LAPACK

  std::vector<int> ipiv(_vec_size,0); // pivot
  _u = _rhs;

  linear_system_solution.start(); // start timer

  int info = lapacke_gesv( LAPACK_ROW_MAJOR,
                           _vec_size,
                           1,
                            _A.data(),
                           _vec_size,
                           ipiv.data(),
                           _u.data(),
                           1                );

#else

  ConjugateGradient<Eigen::SparseMatrix<FLOAT_TYPE> > cg;
  cg.compute(_A);
  _u = cg.solve(_rhs);
 
#endif


  linear_system_solution.stop(); // stop timer 

  compute_err_norms();

  return info;

}
Ejemplo n.º 4
0
void LeastSquareFitClass::nothing(MatrixXd A, VectorXd dataPoints){

   VectorXd b = A.adjoint() * dataPoints;
   MatrixXd M = A.adjoint() * A;

   //cout << "The matrix A is of size "
   //          << A.rows() << "x" << A.cols() << std::endl;
   //cout << "Here is the matrix b \n" << b << endl;
   //cout << "Here is the matrix M \n" << M << endl;

   // Solving M x = b 
   int n = b.size();
   VectorXd x(n);
   ConjugateGradient<MatrixXd> cg;
   cg.compute(M);
   x = cg.solve(b);
 
   //cout << "#iterations:     " << cg.iterations() << std::endl;
   //cout << "estimated error: " << cg.error()      << std::endl;
   //cout << "Final Result x = \n" << x << std::endl;

}
Ejemplo n.º 5
0
void Simulation::staticSolveNewtonsForces(MatrixXd& TV, MatrixXi& TT, MatrixXd& B, VectorXd& fixed_forces, int ignorePastIndex){
	cout<<"----------------Static Solve Newtons, Fix Forces-------------"<<endl;
	//Newtons method static solve for minimum Strain E
	SparseMatrix<double> forceGradient;
	forceGradient.resize(3*TV.rows(), 3*TV.rows());
	SparseMatrix<double> forceGradientStaticBlock;
	forceGradientStaticBlock.resize(3*ignorePastIndex, 3*ignorePastIndex);
	VectorXd f, x;
	f.resize(3*TV.rows());
	f.setZero();
	x.resize(3*TV.rows());
	x.setZero();
	setTVtoX(x, TV);
	cout<<x<<endl;
	int NEWTON_MAX = 100, k=0;
	for(k=0; k<NEWTON_MAX; k++){
		xToTV(x, TV);

		calculateForceGradient(TV, forceGradient);
		calculateElasticForces(f, TV);
		for(unsigned int i=0; i<fixed_forces.rows(); i++){
			if(abs(fixed_forces(i))>0.000001){
				if(i>3*ignorePastIndex){
					cout<<"Problem Check simulation.cpp file"<<endl;
					cout<<ignorePastIndex<<endl;
					cout<<i<<" - "<<fixed_forces(i)<<endl;
					exit(0);
				}
				f(i) = fixed_forces(i);
			}
		}
		
		//Block forceGrad and f to exclude the fixed verts
		forceGradientStaticBlock = forceGradient.block(0,0, 3*(ignorePastIndex), 3*ignorePastIndex);
		VectorXd fblock = f.head(ignorePastIndex*3);

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

		// Conj Grad
		ConjugateGradient<SparseMatrix<double>> cg;
		cg.compute(forceGradientStaticBlock);
		if(cg.info() == Eigen::NumericalIssue){
			cout<<"ConjugateGradient numerical issue"<<endl;
			exit(0);
		}
		VectorXd deltaX = -1*cg.solve(fblock);

		// // Sparse Cholesky LL^T
		// SimplicialLLT<SparseMatrix<double>> llt;
		// llt.compute(forceGradientStaticBlock);
		// if(llt.info() == Eigen::NumericalIssue){
		// 	cout<<"Possibly using a non- pos def matrix in the LLT method"<<endl;
		// 	exit(0);
		// }
		// VectorXd deltaX = -1*llt.solve(fblock);

		x.segment(0,3*(ignorePastIndex))+=deltaX;
		cout<<"		Newton Iter "<<k<<endl;

		if(x != x){
			cout<<"NAN"<<endl;
			exit(0);
		}
		cout<<"fblock square norm"<<endl;
		cout<<fblock.squaredNorm()/fblock.size()<<endl;
		double strainE = 0;
		for(int i=0; i< M.tets.size(); i++){
			strainE += M.tets[i].undeformedVol*M.tets[i].energyDensity;
		}
		cout<<strainE<<endl;
		if (fblock.squaredNorm()/fblock.size() < 0.00001){
			break;
		}

	}
	if(k== NEWTON_MAX){
		cout<<"ERROR Static Solve: Newton max reached"<<endl;
		cout<<k<<endl;
		exit(0);
	}
	double strainE = 0;
	for(int i=0; i< M.tets.size(); i++){
		strainE += M.tets[i].undeformedVol*M.tets[i].energyDensity;
	}
	cout<<"strain E"<<strainE<<endl;
	cout<<"x[0] "<<x(0)<<endl;
	cout<<"x[1] "<<x(1)<<endl;
	exit(0);				

	cout<<"-------------------"<<endl;
}
Ejemplo n.º 6
0
void Gelatin::step(double h, const Vector3d &grav, const vector< shared_ptr<FreakFace> > faces)
{
	aTrips.clear();
	//v.setZero();
	//f.setZero();
//
//	vTrips.clear();
//    fTrips.clear();

    VectorXd b(n);
    b.setZero();

	for (int i = 0; i < particles.size(); i++) {
	    if (!particles[i]->fixed) {
	        int index = particles[i]->i;
	        double m = particles[i]->m;

	        double val = m + h * damping(0) * m;
            aTrips.push_back(Trip(index, index, val));
            aTrips.push_back(Trip(index+1, index+1, val));
            aTrips.push_back(Trip(index+2, index+2, val));

            Vector3d v = particles[i]->v;
            Vector3d fg = h * m * grav;
            b(index) = m * v(0) + fg(0);
            b(index+1) = m * v(1) + fg(1);
            b(index+2) = m * v(2) + fg(2);
        }
	}

	for (int i = 0; i < springs.size(); i++) {
	    double E = springs[i]->E;
	    double L = springs[i]->L;
	    auto p0 = springs[i]->p0;
	    auto p1 = springs[i]->p1;

	    //MatrixXd delta(3, 1);
        Vector3d delta = p1->x - p0->x;
	    double l = delta.norm();

	    Vector3d fs = E * (l - L) * (delta / l);

	    if (!springs[i]->p0->fixed) {
	        b(p0->i) += h * fs(0);
	        b(p0->i+1) += h * fs(1);
	        b(p0->i+2) += h * fs(2);
	    }
	    if (!springs[i]->p1->fixed) {
	        b(p1->i) -= h * fs(0);
            b(p1->i+1) -= h * fs(1);
            b(p1->i+2) -= h * fs(2);
	    }

        if (!springs[i]->p0->fixed && !springs[i]->p1->fixed) {
            Matrix3d ks =  (E / (l * l)) * ((1.0 - (l - L)/l) * (delta * delta.transpose())
                + ((l - L)/l) * double(delta.transpose() * delta) * Matrix3d::Identity());
            int i0 = p0->i;
            int i1 = p1->i;

            addKs(ks, i0, i1, h);
        }
	}

    Eigen::SparseMatrix<double> sparseA(n,n);
    sparseA.setFromTriplets(aTrips.begin(), aTrips.end());

    ConjugateGradient< SparseMatrix<double> > cg;
    cg.setMaxIterations(25);
    cg.setTolerance(1e-3);
    cg.compute(sparseA);

    //v = cg.solve(b);
    v = cg.solveWithGuess(b, v);

	//v = A.ldlt().solve(b);

	for (int i = 0; i < particles.size(); i++) {
	    if (!particles[i]->fixed) {
            int index = particles[i]->i;
            particles[i]->v = v.block<3,1>(index, 0);
            particles[i]->x += h * particles[i]->v;
        }
	}

	collide(faces);

	// Update position and normal buffers
	updatePosNor();
}
Ejemplo n.º 7
0
void FluidSolver::pressureSolve(float timeStepSec)
{
  // NOTE: assumptions are made here that the only "SOLID" cells in the sim
  // are the walls of the simulation, and are therefore not handled in this
  // method.  Instead, they are handled in the boundaryCollide method where
  // velocities entering or exiting a boundary are simply set to 0.0f.

  // The pressureSolve routine does the following:
  // * Calculate the negative divergence b with moditications at solid walls.
  // * Set the entries of A (stored in Adiag, etc.)
  // * Solve the Ap = b using the conjugate gradient algorithm.
  // * Compute the new velocities according to the updated pressure.

  // Calculate the dimensionality of our vectors/matrix.
  const unsigned width  = _grid.getColCount() - 1;
  const unsigned height = _grid.getRowCount() - 1;
  int dim = width * height;

  // Calculate the negative divergence throughout the simulation.
  VectorXd b(dim);
  for (unsigned y = 0; y < height; ++y)
    for (unsigned x = 0; x < width; ++x) {
      unsigned index = y * width + x;
      b(index) = -_grid.getVelocityDivergence(x, y);
    }

  // Update the negative divergence to account for solid boundaries.
  // TODO assumes that the only solids are at the boundaries, which have 0 vel.
  // Bottom row.
  for (unsigned x = 0; x < width; ++x) {
    unsigned y = 0;
    unsigned index = y * width + x;
    b(index) -= _grid(x,y).vel[Cell::Y];
  }
  // Top row.
  for (unsigned x = 0; x < width; ++x) {
    unsigned y = height - 1;
    unsigned index = y * width + x;
    b(index) += _grid(x,y+1).vel[Cell::Y];
  }
  // Left column.
  for (unsigned y = 0; y < height; ++y) {
    unsigned x = 0;
    unsigned index = y * width + x;
    b(index) -= _grid(x,y).vel[Cell::X];
  }
  // Right column.
  for (unsigned y = 0; y < height; ++y) {
    unsigned x = width - 1;
    unsigned index = y * width + x;
    b(index) += _grid(x+1,y).vel[Cell::X];
  }

  // Enforce the compatibility condition.
  // TODO don't need this if we have a free surface.
  /*
  double mean = 0.0;
  for (unsigned y = 0; y < height; ++y)
    for (unsigned x = 0; x < width; ++x) {
      unsigned index = y * width + x;
      mean += b(index);
    }
  //  std::cout.precision(15);
  //  std::cout << "Overall Divergence: " << mean << std::endl;
  mean /= dim;
  //  std::cout << "PerEntry Divergence: " << mean << std::endl;
  //  std::cout << "Pre-compat Divergence: " << std::endl << b << std::endl;
  for (unsigned y = 0; y < height; ++y)
    for (unsigned x = 0; x < width; ++x) {
      unsigned index = y * width + x;
      b(index) -= mean;
    }
  //  std::cout << "Post-compat Divergence: " << std::endl << b << std::endl;
  */

  // Set the entries of A. 
  std::vector< Tripletd > vals;
  SparseMatrix<double,RowMajor> A(dim, dim);
  for (unsigned y = 0; y < height; ++y)
    for (unsigned x = 0; x < width; ++x) {
      Cell &cell = _grid(x,y);
      unsigned i = y * width + x;  // this cell's col/row in A.
      unsigned j;                  // neighbor cell's col/row in A

      switch (cell.cellType) {
      case (Cell::SOLID):
	// If this cell is a SOLID, leave the entire row as 0's.
	break;

      case (Cell::AIR):
	// If this cell is AIR, increment neighboring fluid diagonals' coeff.
	if (cell.neighbors[Cell::POS_X]->cellType == Cell::FLUID) {
	  j = y * width + x + 1;                        // rt neighbor's idx
	  vals.push_back( Tripletd(j,j,timeStepSec) );  // rt neighbor's diag
	}
	if (cell.neighbors[Cell::POS_Y]->cellType == Cell::FLUID) {
	  j = (y + 1) * width + x;                      // up neighbor's idx
	  vals.push_back( Tripletd(j,j,timeStepSec) );  // up neighbor's diag
	}
	break;

      case (Cell::FLUID):
	// Cell is fluid. Determine coefficients of self and neighbors.
	if (cell.neighbors[Cell::POS_X]->cellType == Cell::FLUID) {
	  j = y * width + x + 1;                        // rt neighbor's idx
	  vals.push_back( Tripletd(i,i,timeStepSec) );  // my diagonal coeff
	  vals.push_back( Tripletd(i,j,-timeStepSec) ); // rt neighbor's coeff
	  vals.push_back( Tripletd(j,j,timeStepSec) );  // rt neighbor's diag
	}
	else if (cell.neighbors[Cell::POS_X]->cellType == Cell::AIR) {
	  vals.push_back( Tripletd(i,i,timeStepSec) );
	}
	if (cell.neighbors[Cell::POS_Y]->cellType == Cell::FLUID) {
	  j = (y + 1) * width + x;                      // up neighbor's idx
	  vals.push_back( Tripletd(i,i,timeStepSec) );  // my diagonal coeff
	  vals.push_back( Tripletd(i,j,-timeStepSec) ); // up neighbor's coeff
	  vals.push_back( Tripletd(j,j,timeStepSec) );  // up neighbor's diag
	}
	else if (cell.neighbors[Cell::POS_Y]->cellType == Cell::AIR) {
	  vals.push_back( Tripletd(i,i,timeStepSec) );
	}
	break;
	
      default:
	break;
      }
    }
  A.setFromTriplets(vals.begin(), vals.end());

  // Solve for the new pressure values, p.
  VectorXd p(dim);
  ConjugateGradient< SparseMatrix<double,RowMajor> > cg;
  cg.compute(A);
  p = cg.solve(b);
  if (cg.info() == Success)
    std::cout << "SUCCESS: Convergence!" << std::endl;
  else 
    std::cout << "FAILED: No Convergence..." << std::endl;
  //  std::cout << "#iterations:     " << cg.iterations() << std::endl;
  //  std::cout << "estimated error: " << cg.error()      << std::endl;  
  //  std::cout << "A: " << std::endl << A << std::endl;
  //  std::cout << "Pressure: " << std::endl << p << std::endl;
  //  std::cout << "Divergence: " << std::endl << b << std::endl;
  //  std::cout << "Ap: " << std::endl << A*p << std::endl;

  // Set new pressure values.
  for (unsigned y = 0; y < height; ++y)
    for (unsigned x = 0; x < width; ++x) {
      unsigned index = y * width + x;
      _grid(x, y).pressure = p(index);
    }

  // Modify velocity field based on updated pressure scalar field.
  for (unsigned y = 0; y < height; ++y)
    for (unsigned x = 0; x < width; ++x) {
      Cell &cell = _grid(x,y);
      float pressureVel = timeStepSec * cell.pressure;
      if (cell.cellType == Cell::FLUID) {
	// Update all neighboring velocities touched by this pressure.
	cell.vel[Cell::X] -= pressureVel;
	cell.vel[Cell::Y] -= pressureVel;
	if (cell.neighbors[Cell::POS_X])
	  cell.neighbors[Cell::POS_X]->vel[Cell::X] += pressureVel;
	if (cell.neighbors[Cell::POS_Y])
	  cell.neighbors[Cell::POS_Y]->vel[Cell::Y] += pressureVel;
      }
    }

  // Calculate the negative divergence throughout the simulation.
  for (unsigned y = 0; y < height; ++y)
    for (unsigned x = 0; x < width; ++x) {
      unsigned index = y * width + x;
      b(index) = -_grid.getVelocityDivergence(x, y);
    }
  std::cout << "New Divergence: " << std::endl << b << std::endl;
}
void DiffusionSolverFE_CPU_Implicit::Implicit(ConcentrationField_t const &concentrationField, DiffusionData const &diffData, 
	EigenRealVector const &b, EigenRealVector &x){
	size_t totalSize=(fieldDim.x+2)*(fieldDim.y+2)*(fieldDim.z+2);
	size_t totalExtSize=h_celltype_field->getArraySize();

	cerr<<"Field size: "<<totalSize<<"; total size: "<<totalExtSize<<endl;

	
	//SparseMatrix<float, RowMajor> eigenM(totalSize,totalSize);
	//float s=1;

	bool const is2D=(fieldDim.z==1);

	int neighbours=is2D?4:6;
	 
	//LARGE_INTEGER tb, te, fq;
	//QueryPerformanceFrequency(&fq);

	EigenSparseMatrix eigenM_new(totalSize,totalSize);
	//QueryPerformanceCounter(&tb);
	eigenM_new.reserve(totalSize*is2D?5:7);
	cout<<"Assembling matrix... ";
	int prevZ=-1;

	//TODO: unify this with OpenCL matrix assembling...
	for(size_t i=0; i<totalSize; ++i)//TODO: 2D simultaions can be arranged more effective, I guess...
	{
		
		int z=i/((fieldDim.x+2)*(fieldDim.y+2));
		int remz=i%((fieldDim.x+2)*(fieldDim.y+2));

		eigenM_new.startVec(i);

		int y=remz/(fieldDim.x+2);
		int x=remz%(fieldDim.x+2);

		//unsigned char currentCellType=h_celltype_field->getDirect(x+1,y+1,z+1);
		unsigned char currentCellType=h_celltype_field->getByIndex(i);
		float currentDiffCoef=diffData.diffCoef[currentCellType];//*(diffData.deltaT)/(deltaX*deltaX);
	
		int lastJ=-1;
		if(!is2D&&z>0){
			int j=flatExtInd(x,y,z-1, fieldDim);
			lastJ=j;
			//ASSERT_OR_THROW("wrong order", j<(int)i);
			//ASSERT_OR_THROW("wrong index", j<(int)totalSize);
			eigenM_new.insertBack(i,j)=-currentDiffCoef;
		}
		if(y>0){
			int j=flatExtInd(x,y-1,z, fieldDim);
		//	ASSERT_OR_THROW("wrong order 1", j>lastJ);
		//	ASSERT_OR_THROW("wrong index", j<(int)totalSize);
			lastJ=j;
			eigenM_new.insertBack(i,j)=-currentDiffCoef;
		}

		if(x>0){
			int j=flatExtInd(x-1,y,z, fieldDim);
		//	ASSERT_OR_THROW("wrong order 2", j>lastJ);
		//	ASSERT_OR_THROW("wrong index", j<(int)totalSize);
			lastJ=j;
			eigenM_new.insertBack(i,j)=-currentDiffCoef;
		}

		//ASSERT_OR_THROW("wrong order 3", (int)i>lastJ);
		lastJ=i;
		eigenM_new.insertBack(i,i)=1+neighbours*currentDiffCoef;

		if(x<fieldDim.x-1){
			int j=flatExtInd(x+1,y,z, fieldDim);
		//	ASSERT_OR_THROW("wrong order 4", j>lastJ);
		//	ASSERT_OR_THROW("wrong index", j<(int)totalSize);
			lastJ=j;
			eigenM_new.insertBack(i,j)=-currentDiffCoef;
		}

		if(y<fieldDim.y-1){
			int j=flatExtInd(x,y+1,z, fieldDim);
		//	ASSERT_OR_THROW("wrong order 5", j>lastJ);
		//	ASSERT_OR_THROW("wrong index", j<(int)totalSize);
			lastJ=j;
			eigenM_new.insertBack(i,j)=-currentDiffCoef;
		}

		if(!is2D&&z<fieldDim.z-1){
			int j=flatExtInd(x,y,z+1, fieldDim);
			//ASSERT_OR_THROW("wrong order 6", j>lastJ);
			//ASSERT_OR_THROW("wrong index", j<(int)totalSize);
			lastJ=j;
			eigenM_new.insertBack(i,j)=-currentDiffCoef;
		}

	}

	cout<<"done"<<endl;
	eigenM_new.finalize();
	eigenM_new.makeCompressed();  
//	QueryPerformanceCounter(&te);
	//cout<<"completelly done"<<endl;

//	double time=(double)(te.QuadPart-tb.QuadPart)/(double)fq.QuadPart;
//	std::cerr<<"It took "<<time<<" s to assemble a matrix with new algorithm\n";

//	CompareMatrices(eigenM, eigenM_new);

//	SparseMatrix<float,RowMajor> eigenM(eigenDynM);

	
	ConjugateGradient<EigenSparseMatrix> cg;

	cout<<"Preparing system... ";
//	QueryPerformanceCounter(&tb);
	cg.compute(eigenM_new);
//	QueryPerformanceCounter(&te);
	cout<<"done"<<endl;

//	double timePrep=(double)(te.QuadPart-tb.QuadPart)/(double)fq.QuadPart;

	cout<<"Solving system... ";
//	QueryPerformanceCounter(&tb);
	x = cg.solve(b);
	//QueryPerformanceCounter(&te);
	cout<<"done"<<endl;
	
	//time=(double)(te.QuadPart-tb.QuadPart)/(double)fq.QuadPart;
	//std::cerr<<"It took "<<time<<" s to find the solution with explicit solver and "<<timePrep<<" for preparing the system\n";

	std::cout << "#iterations:     " << cg.iterations() << std::endl;
	std::cout << "estimated error: " << cg.error()      << std::endl; 
}