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