void crankNicolson (Ref<VectorXd> u,
const double delta_t,
const double h,
const double kappa) {
const int N = sqrt(u.rows());
static SparseMatrix<double> C;
static SimplicialLLT<SparseMatrix<double>> Bdecomp;
static double mu;
static int oldN;
if (oldN != N) {
mu = delta_t * kappa / (h * h);
vector<Triplet<double>> tripletList;
tripletList.reserve (5 * N * N);
tripletList.push_back (Triplet<double> (0, 0, -4 * mu));
for (int i = 1; i < N * N; ++i) {
tripletList.push_back (Triplet<double> (i, i, -4 * mu));
tripletList.push_back (Triplet<double> (i, i - 1, mu));
tripletList.push_back (Triplet<double> (i - 1, i, mu));
}
for (int i = 0; i < N * N - N; ++i) {
tripletList.push_back (Triplet<double> (i, i + N, mu));
tripletList.push_back (Triplet<double> (i + N, i, mu));
}
SparseMatrix<double> A (N * N, N * N);
A.setFromTriplets (tripletList.begin (), tripletList.end ());
SparseMatrix<double> B (N * N, N * N);
B.setIdentity();
B -= 0.5 * A;
Bdecomp.compute (B);
C.resize (N * N, N * N);
C.setIdentity();
C += 0.5 * A;
oldN = N;
}
VectorXd rhs = C * u;
u = Bdecomp.solve (rhs);
}
void ImplicitEuler::renderNewtonsMethod(VectorXd& ext_force){
//Implicit Code
v_k.setZero();
x_k.setZero();
x_k = x_old;
v_k = v_old;
int ignorePastIndex = TV.rows() - fixedVerts.size();
SparseMatrix<double> forceGradientStaticBlock;
forceGradientStaticBlock.resize(3*ignorePastIndex, 3*ignorePastIndex);
SparseMatrix<double> RegMassBlock;
RegMassBlock.resize(3*ignorePastIndex, 3*ignorePastIndex);
RegMassBlock = RegMass.block(0, 0, 3*ignorePastIndex, 3*ignorePastIndex);
forceGradient.setZero();
bool Nan=false;
int NEWTON_MAX = 10, i =0;
// 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();
ImplicitXtoTV(x_k, TVk);//TVk value changed in function
ImplicitCalculateElasticForceGradient(TVk, forceGradient);
ImplicitCalculateForces(TVk, forceGradient, x_k, f);
f = f+ext_force;
// VectorXd g_block = x_k - x_old -h*v_old -h*h*InvMass*f;
// grad_g = Ident - h*h*InvMass*forceGradient - h*rayleighCoeff*InvMass*forceGradient;
//Block forceGrad and f to exclude the fixed verts
forceGradientStaticBlock = forceGradient.block(0,0, 3*(ignorePastIndex), 3*ignorePastIndex);
VectorXd g = RegMass*x_k - RegMass*x_old - h*RegMass*v_old - h*h*f;
VectorXd g_block = g.head(ignorePastIndex*3);
grad_g = RegMassBlock - h*h*forceGradientStaticBlock - h*rayleighCoeff*forceGradientStaticBlock;
//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);
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(g_block);
x_k.segment(0, 3*(ignorePastIndex)) += deltaX;
//Sparse QR
// SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> sqr;
// sqr.compute(grad_g);
// VectorXd deltaX = -1*sqr.solve(g_block);
// x_k.segment(0, 3*(ignorePastIndex)) += deltaX;
// CholmodSimplicialLLT<SparseMatrix<double>> cholmodllt;
// cholmodllt.compute(grad_g);
// VectorXd deltaX = -cholmodllt.solve(g_block);
if(x_k != x_k){
Nan = true;
break;
}
if(g_block.squaredNorm()<.00000001){
break;
}
}
if(Nan){
cout<<"ERROR: Newton's method doesn't converge"<<endl;
cout<<i<<endl;
exit(0);
}
if(i== NEWTON_MAX){
cout<<"ERROR: Newton max reached"<<endl;
cout<<i<<endl;
exit(0);
}
v_old.setZero();
v_old = (x_k - x_old)/h;
x_old = x_k;
}
