// [[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; }
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); }
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; }
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; }
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; }
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(); }
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; }