void ConjugateGradientTest::test_to_XML(void)
{
message += "test_to_XML\n";
ConjugateGradient cg;
tinyxml2::XMLDocument* cgd = cg.to_XML();
assert_true(cgd != NULL, LOG);
}
// [[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 ConjugateGradientTest::test_set_training_direction_method(void)
{
message += "test_set_training_direction_method\n";
ConjugateGradient cg;
cg.set_training_direction_method(ConjugateGradient::FR);
assert_true(cg.get_training_direction_method() == ConjugateGradient::FR, LOG);
cg.set_training_direction_method(ConjugateGradient::PR);
assert_true(cg.get_training_direction_method() == ConjugateGradient::PR, LOG);
}
void ConjugateGradientTest::test_get_training_direction_method(void)
{
message += "test_get_training_direction_method\n";
ConjugateGradient cg;
cg.set_training_direction_method(ConjugateGradient::PR);
ConjugateGradient::TrainingDirectionMethod training_direction_method = cg.get_training_direction_method();
assert_true(training_direction_method == ConjugateGradient::PR, LOG);
}
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 ConjugateGradientTest::test_set_reserve_all_training_history(void)
{
message += "test_set_reserve_all_training_history\n";
ConjugateGradient cg;
cg.set_reserve_all_training_history(true);
assert_true(cg.get_reserve_parameters_history() == true, LOG);
assert_true(cg.get_reserve_parameters_norm_history() == true, LOG);
assert_true(cg.get_reserve_performance_history() == true, LOG);
assert_true(cg.get_reserve_gradient_history() == true, LOG);
assert_true(cg.get_reserve_gradient_norm_history() == true, LOG);
assert_true(cg.get_reserve_training_direction_history() == true, LOG);
assert_true(cg.get_reserve_training_rate_history() == true, LOG);
assert_true(cg.get_reserve_elapsed_time_history() == true, LOG);
assert_true(cg.get_reserve_generalization_performance_history() == true, LOG);
}
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;
}
bool Neighbourhood::computeQuadric()
{
//invalidate previous quadric (if any)
m_structuresValidity &= (~FLAG_QUADRIC);
assert(m_associatedCloud);
if (!m_associatedCloud)
return false;
unsigned count = m_associatedCloud->size();
assert(CC_LOCAL_MODEL_MIN_SIZE[QUADRIC] >= 5);
if (count < CC_LOCAL_MODEL_MIN_SIZE[QUADRIC])
return false;
const PointCoordinateType* lsPlane = getLSPlane();
if (!lsPlane)
return false;
//we get centroid (should already be up-to-date - see computeCovarianceMatrix)
const CCVector3* G = getGravityCenter();
assert(G);
//get the best projection axis
Tuple3ub idx(0/*x*/,1/*y*/,2/*z*/); //default configuration: z is the "normal" direction, we use (x,y) as the base plane
PointCoordinateType nxx = lsPlane[0]*lsPlane[0];
PointCoordinateType nyy = lsPlane[1]*lsPlane[1];
PointCoordinateType nzz = lsPlane[2]*lsPlane[2];
if (nxx > nyy)
{
if (nxx > nzz)
{
//as x is the "normal" direction, we use (y,z) as the base plane
idx.x = 1/*y*/; idx.y = 2/*z*/; idx.z = 0/*x*/;
}
}
else
{
if (nyy > nzz)
{
//as y is the "normal" direction, we use (z,x) as the base plane
idx.x = 2/*z*/; idx.y = 0/*x*/; idx.z = 1/*y*/;
}
}
//compute the A matrix and b vector
std::vector<float> A;
std::vector<float> b;
try
{
A.resize(6*count);
b.resize(count);
}
catch (std::bad_alloc)
{
//not enough memory
return false;
}
float lmax2 = 0; //max (squared) dimension
//for all points
{
float* _A = &(A[0]);
float* _b = &(b[0]);
for (unsigned i=0; i<count; ++i)
{
CCVector3 P = *m_associatedCloud->getPoint(i) - *G;
float lX = static_cast<float>(P.u[idx.x]);
float lY = static_cast<float>(P.u[idx.y]);
float lZ = static_cast<float>(P.u[idx.z]);
*_A++ = 1.0f;
*_A++ = lX;
*_A++ = lY;
*_A = lX*lX;
//by the way, we track the max 'X' squared dimension
if (*_A > lmax2)
lmax2 = *_A;
++_A;
*_A++ = lX*lY;
*_A = lY*lY;
//by the way, we track the max 'Y' squared dimension
if (*_A > lmax2)
lmax2 = *_A;
++_A;
*_b++ = lZ;
lZ *= lZ;
//and don't forget to track the max 'Z' squared dimension as well
if (lZ > lmax2)
lmax2 = lZ;
}
}
//conjugate gradient initialization
//we solve tA.A.X=tA.b
ConjugateGradient<6,double> cg;
CCLib::SquareMatrixd& tAA = cg.A();
double* tAb = cg.b();
//compute tA.A and tA.b
{
for (unsigned i=0; i<6; ++i)
{
//tA.A part
for (unsigned j=i; j<6; ++j)
{
double tmp = 0;
float* _Ai = &(A[i]);
float* _Aj = &(A[j]);
for (unsigned k=0; k<count; ++k)
{
//tmp += A[(6*k)+i] * A[(6*k)+j];
tmp += static_cast<double>(*_Ai) * static_cast<double>(*_Aj);
_Ai += 6;
_Aj += 6;
}
tAA.m_values[j][i] = tAA.m_values[i][j] = tmp;
}
//tA.b part
{
double tmp = 0;
float* _Ai = &(A[i]);
float* _b = &(b[i]);
for (unsigned k=0; k<count; ++k)
{
//tmp += A[(6*k)+i]*b[k];
tmp += static_cast<double>(*_Ai) * static_cast<double>(*_b++);
_Ai += 6;
}
tAb[i] = tmp;
}
}
}
//first guess for X: plane equation (a0.x+a1.y+a2.z=a3 --> z = a3/a2 - a0/a2.x - a1/a2.y)
double X0[6] = {static_cast<double>(/*lsPlane[3]/lsPlane[idx.z]*/0), //DGM: warning, points have already been recentred around the gravity center! So forget about a3
static_cast<double>(-lsPlane[idx.x]/lsPlane[idx.z]),
static_cast<double>(-lsPlane[idx.y]/lsPlane[idx.z]),
0,
0,
0 };
//special case: a0 = a1 = a2 = 0! //happens for perfectly flat surfaces!
if (X0[1] == 0 && X0[2] == 0)
X0[0] = 1.0;
//init. conjugate gradient
cg.initConjugateGradient(X0);
//conjugate gradient iterations
{
double convergenceThreshold = lmax2 * 1.0e-8; //max. error for convergence = 1e-8 of largest cloud dimension (empirical!)
for (unsigned i=0; i<1500; ++i)
{
double lastError = cg.iterConjugateGradient(X0);
if (lastError < convergenceThreshold) //converged
break;
}
}
//fprintf(fp,"X%i=(%f,%f,%f,%f,%f,%f)\n",i,X0[0],X0[1],X0[2],X0[3],X0[4],X0[5]);
//fprintf(fp,"lastError=%E/%E\n",lastError,convergenceThreshold);
//fclose(fp);
//output
{
for (unsigned i=0; i<6; ++i)
{
m_quadricEquation[i] = static_cast<PointCoordinateType>(X0[i]);
}
m_quadricEquationDirections = idx;
m_structuresValidity |= FLAG_QUADRIC;
}
return true;
}
bool Neighbourhood::computeHeightFunction()
{
//invalidate previous quadric (if any)
structuresValidity &= (~HEIGHT_FUNCTION);
assert(m_associatedCloud);
if (!m_associatedCloud)
return false;
unsigned count = m_associatedCloud->size();
assert(CC_LOCAL_MODEL_MIN_SIZE[HF] >= 5);
if (count < CC_LOCAL_MODEL_MIN_SIZE[HF])
return false;
const PointCoordinateType* lsq = getLSQPlane();
if (!lsq)
return false;
//we get centroid (should already be up-to-date - see computeCovarianceMatrix)
const CCVector3* G = getGravityCenter();
assert(G);
//get the best projection axis
uchar idx_X=0/*x*/,idx_Y=1/*y*/,idx_Z=2/*z*/; //default configuration: z is the "normal" direction, we use (x,y) as the base plane
PointCoordinateType nxx = lsq[0]*lsq[0];
PointCoordinateType nyy = lsq[1]*lsq[1];
PointCoordinateType nzz = lsq[2]*lsq[2];
if (nxx > nyy)
{
if (nxx > nzz)
{
//as x is the "normal" direction, we use (y,z) as the base plane
idx_X=1/*y*/; idx_Y=2/*z*/; idx_Z=0/*x*/;
}
}
else
{
if (nyy > nzz)
{
//as y is the "normal" direction, we use (z,x) as the base plane
idx_X=2/*z*/; idx_Y=0/*x*/; idx_Z=1/*y*/;
}
}
//compute the A matrix and b vector
float *A = new float[6*count];
float *b = new float[count];
float lmax2=0; //max (squared) dimension
//for all points
{
float* _A=A;
float* _b=b;
for (unsigned i=0;i<count;++i)
{
CCVector3 P = *m_associatedCloud->getPoint(i) - *G;
float lX = (float)P.u[idx_X];
float lY = (float)P.u[idx_Y];
float lZ = (float)P.u[idx_Z];
*_A++ = 1.0;
*_A++ = lX;
*_A++ = lY;
*_A = lX*lX;
//by the way, we track the max 'X' squared dimension
if (*_A>lmax2)
lmax2=*_A;
++_A;
*_A++ = lX*lY;
*_A = lY*lY;
//by the way, we track the max 'Y' squared dimension
if (*_A>lmax2)
lmax2=*_A;
++_A;
*_b++ = lZ;
lZ *= lZ;
//and don't forget to track the max 'Z' squared dimension as well
if (lZ>lmax2)
lmax2=lZ;
}
}
//conjugate gradient initialization
//we solve tA.A.X=tA.b
ConjugateGradient<6,double> cg;
CCLib::SquareMatrixd& tAA = cg.A();
double* tAb = cg.b();
//compute tA.A and tA.b
{
for (unsigned i=0; i<6; ++i)
{
//tA.A part
for (unsigned j=i; j<6; ++j)
{
double tmp = 0;
float* _Ai = A+i;
float* _Aj = A+j;
for (unsigned k=0; k<count; ++k)
{
//tmp += A[(6*k)+i]*A[(6*k)+j];
tmp += (double)((*_Ai) * (*_Aj));
_Ai += 6;
_Aj += 6;
}
tAA.m_values[j][i] = tAA.m_values[i][j] = tmp;
}
//tA.b part
{
double tmp = 0;
float* _Ai = A+i;
float* _b = b;
for (unsigned k=0; k<count; ++k)
{
//tmp += A[(6*k)+i]*b[k];
tmp += (double)((*_Ai) * (*_b++));
_Ai += 6;
}
tAb[i] = tmp;
}
}
}
//first guess for X: plane equation (a0.x+a1.y+a2.z=a3 --> z = a3/a2 - a0/a2.x - a1/a2.y)
double X0[6];
X0[0] = (double)/*lsq[3]/lsq[idx_Z]*/0; //DGM: warning, points have already been recentred around the gravity center! So forget about a3
X0[1] = (double)(-lsq[idx_X]/lsq[idx_Z]);
X0[2] = (double)(-lsq[idx_Y]/lsq[idx_Z]);
X0[3] = 0;
X0[4] = 0;
X0[5] = 0;
//special case: a0 = a1 = a2 = 0! //happens for perfectly flat surfaces!
if (X0[1] == 0.0 && X0[2] == 0.0)
X0[0] = 1.0;
//init. conjugate gradient
cg.initConjugateGradient(X0);
//conjugate gradient iterations
{
double convergenceThreshold = (double)lmax2 * 1e-8; //max. error for convergence = 1e-8 of largest cloud dimension (empirical!)
for (unsigned i=0; i<1500; ++i)
{
double lastError = cg.iterConjugateGradient(X0);
if (lastError < convergenceThreshold) //converged
break;
}
}
//fprintf(fp,"X%i=(%f,%f,%f,%f,%f,%f)\n",i,X0[0],X0[1],X0[2],X0[3],X0[4],X0[5]);
//fprintf(fp,"lastError=%E/%E\n",lastError,convergenceThreshold);
//fclose(fp);
delete[] A;
A=0;
delete[] b;
b=0;
//output
{
for (unsigned i=0;i<6;++i)
theHeightFunction[i]=(PointCoordinateType)X0[i];
theHeightFunctionDirections[0]=idx_X;
theHeightFunctionDirections[1]=idx_Y;
theHeightFunctionDirections[2]=idx_Z;
structuresValidity |= HEIGHT_FUNCTION;
}
return true;
}
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;
}