int
main() {
vector<Triplet<double>> trips(4);
trips.push_back(Triplet<double>(0, 0, .435));
trips.push_back(Triplet<double>(1, 1, .435));
trips.push_back(Triplet<double>(2, 2, .435));
trips.push_back(Triplet<double>(3, 3, .435));
SparseMatrix<double> A;
A.resize(4, 4);
A.setFromTriplets(trips.begin(), trips.end());
SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> solverA;
solverA.compute(A);
VectorXd B;
B.resize(4);
B(0) = .435;
B(1) = .435;
B(2) = .435;
B(3) = .435;
VectorXd X = solverA.solve(B);
for (int i = 0; i < 4; i++) cout << X(i) << endl;
}
template<typename Scalar> void test_sparseqr_scalar()
{
typedef SparseMatrix<Scalar,ColMajor> MatrixType;
typedef Matrix<Scalar,Dynamic,Dynamic> DenseMat;
typedef Matrix<Scalar,Dynamic,1> DenseVector;
MatrixType A;
DenseMat dA;
DenseVector refX,x,b;
SparseQR<MatrixType, COLAMDOrdering<int> > solver;
generate_sparse_rectangular_problem(A,dA);
b = dA * DenseVector::Random(A.cols());
solver.compute(A);
if(internal::random<float>(0,1)>0.5f)
solver.factorize(A); // this checks that calling analyzePattern is not needed if the pattern do not change.
if (solver.info() != Success)
{
std::cerr << "sparse QR factorization failed\n";
exit(0);
return;
}
x = solver.solve(b);
if (solver.info() != Success)
{
std::cerr << "sparse QR factorization failed\n";
exit(0);
return;
}
VERIFY_IS_APPROX(A * x, b);
//Compare with a dense QR solver
ColPivHouseholderQR<DenseMat> dqr(dA);
refX = dqr.solve(b);
VERIFY_IS_EQUAL(dqr.rank(), solver.rank());
if(solver.rank()==A.cols()) // full rank
VERIFY_IS_APPROX(x, refX);
// else
// VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );
// Compute explicitly the matrix Q
MatrixType Q, QtQ, idM;
Q = solver.matrixQ();
//Check ||Q' * Q - I ||
QtQ = Q * Q.adjoint();
idM.resize(Q.rows(), Q.rows()); idM.setIdentity();
VERIFY(idM.isApprox(QtQ));
// Q to dense
DenseMat dQ;
dQ = solver.matrixQ();
VERIFY_IS_APPROX(Q, dQ);
}
bool solve(const int mode)
{
bool batch = (mode !=2 || first_ordered_node_ == 0);
bool order = (mode !=0 && n_nodes_ > 1);
// BATCH
if (batch)
{
// REORDER
if (order)
ordering(0);
//print_problem();
// SOLVE
t_solving_ = clock();
A_.makeCompressed();
solver_.compute(A_);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_incr_ = solver_.solve(b_);
R_ = solver_.matrixR();
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
}
// INCREMENTAL
else
{
// REORDER SUBPROBLEM
ordering(first_ordered_node_);
//print_problem();
// SOLVE ORDERED SUBPROBLEM
t_solving_= clock();
A_nodes_.makeCompressed();
A_.makeCompressed();
// finding measurements block
SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
// std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
int unordered_variables = nodes_.at(first_ordered_node_).location;
SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
solver_.compute(A_partial);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
//std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
// store new part of R
eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
R_.makeCompressed();
// solving not ordered subproblem
if (unordered_variables > 0)
{
//std::cout << "--------------------- solving unordered part" << std::endl;
SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
//std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
//std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
solver_.compute(R1);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
}
}
// UNDO ORDERING FOR RESULT
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
x_incr_ = acc_permutation.inverse() * x_incr_;
time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
return 1;
}
void ImplicitNewmark::renderNewtonsMethod(){
//Implicit Code
v_k.setZero();
x_k.setZero();
x_k = x_old;
v_k = v_old;
VectorXd f_old = f;
forceGradient.setZero();
bool Nan=false;
int NEWTON_MAX = 100, i =0;
double gamma = 0.5;
double beta =0.25;
// 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();
NewmarkXtoTV(x_k, TVk);//TVk value changed in function
NewmarkCalculateElasticForceGradient(TVk, forceGradient);
NewmarkCalculateForces(TVk, forceGradient, x_k, f);
VectorXd g = x_k - x_old - h*v_old - (h*h/2)*(1-2*beta)*InvMass*f_old - (h*h*beta)*InvMass*f;
grad_g = Ident - h*h*beta*InvMass*(forceGradient+(rayleighCoeff/h)*forceGradient);
// VectorXd g = RegMass*x_k - RegMass*x_old - RegMass*h*v_old - (h*h/2)*(1-2*beta)*f_old - (h*h*beta)*f;
// grad_g = RegMass - h*h*beta*(forceGradient+(rayleighCoeff/h)*forceGradient);
// cout<<"G"<<t<<endl;
// cout<<g<<endl<<endl;
// cout<<"G Gradient"<<t<<endl;
// cout<<grad_g<<endl;
//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);
// VectorXd deltaX = -1* llt.solve(g);
//Sparse QR
SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> sqr;
sqr.compute(grad_g);
VectorXd deltaX = -1*sqr.solve(g);
// CholmodSimplicialLLT<SparseMatrix<double>> cholmodllt;
// cholmodllt.compute(grad_g);
// VectorXd deltaX = -cholmodllt.solve(g);
x_k+=deltaX;
if(x_k != x_k){
Nan = true;
break;
}
if(g.squaredNorm()<.00000001){
break;
}
}
if(Nan){
cout<<"ERROR NEWMARK: Newton's method doesn't converge"<<endl;
cout<<i<<endl;
exit(0);
}
if(i== NEWTON_MAX){
cout<<"ERROR NEWMARK: Newton max reached"<<endl;
cout<<i<<endl;
exit(0);
}
v_old = v_old + h*(1-gamma)*InvMass*f_old + h*gamma*InvMass*f;
x_old = x_k;
}
