Homec4science
Diffusion SpecMiCP / ReactMiCP (bin/unsaturated) Browse

specmicp/src/reactmicp/micpsolvers/micpsolver.inl
9a4089e18a62bin/unsaturated

Last Change
Raw File

micpsolver.inl
View Options

#include "micpsolver.hpp" // for syntaxic coloration only
#include "micpsolver/ncp_function.hpp"
#include "utils/log.hpp"
#include <iostream>
#include <unsupported/Eigen/SparseExtra>
namespace specmicp {
namespace reactmicp {
namespace micpsolver {
// ============================= //
// //
// Main algorithm //
// -------------- //
// //
// ============================= //
template <class program_t>
MiCPParabolicSolverReturnCode MiCPParabolicSolver<program_t>::solve_one_timestep(double dt,
ParabolicVariables& x)
{
int cnt = 0;
m_residuals.resize(get_neq());
MiCPParabolicSolverReturnCode retcode = MiCPParabolicSolverReturnCode::NotConvergedYet;
Eigen::VectorXd update(get_neq());
Eigen::VectorXd predictor;
m_program->set_predictor(x, predictor, get_options().alpha, dt);
setup_residuals(x);
m_norm_0 = m_phi_residuals.lpNorm<Eigen::Infinity>();
while (retcode == MiCPParabolicSolverReturnCode::NotConvergedYet)
{
//std::cout << "iteration : " << cnt << std::endl;
DEBUG << "Iteration : " << cnt;
// (S.0) hook
const bool may_have_converged = get_program()->hook_start_iteration(x, m_phi_residuals.norm());
// (S.1) compute the residuals
setup_residuals(x);
//std::cout << m_phi_residuals << std::endl;
// (S.2) check the convergence
retcode = check_convergence(cnt, update, x, m_phi_residuals, may_have_converged);
get_performance().return_code = retcode;
//std::cout << "Retcode : " << (int) retcode << std::endl;
if (retcode != MiCPParabolicSolverReturnCode::NotConvergedYet) break;
++cnt;
// (S.3) compute the jacobian
compute_search_direction(update, x, dt);
// (S.4) linesearch and update
int retcode = linesearch(update, x, predictor, dt);
if (retcode == 1)
{
ERROR << "Linesearch has failed - minimum step allowed taken";
}
//m_program->projection(x);
const int ndf = m_program->get_ndf();
for (int i=0; i<ndf; ++i)
{
for (int node=0; node<5; ++node)
{
std::cout << x.displacement(i+ndf*node) << "\t";
}
std::cout << std::endl;
}
}
get_performance().nb_iterations = cnt;
std::cout << "Nb iterations : " << cnt << std::endl;
return retcode;
}
template <class program_t>
MiCPParabolicSolverReturnCode MiCPParabolicSolver<program_t>::check_convergence(
int nb_iterations,
const Eigen::VectorXd& update,
const ParabolicVariables& solution,
const Eigen::VectorXd& residuals,
bool may_have_converged)
{
MiCPParabolicSolverReturnCode termcode = MiCPParabolicSolverReturnCode::NotConvergedYet;
const double norm_residuals = residuals.lpNorm<Eigen::Infinity>();
WARNING << "Residuals : " << nb_iterations << " - " << norm_residuals/m_norm_0;
if (norm_residuals/m_norm_0 < get_options().residuals_tolerance)
{
if (may_have_converged == true)
termcode = MiCPParabolicSolverReturnCode::ResidualMinimized;
}
else if (nb_iterations/m_norm_0 > 0 and update.lpNorm<Eigen::Infinity>() < get_options().step_tolerance)
{
if (norm_residuals > get_options().threshold_stationary_point)
{
ERROR << "Stationary point detected !";
termcode = MiCPParabolicSolverReturnCode::StationaryPoint;
}
if (may_have_converged == true)
{
WARNING << "Error is minimized - may indicate a stationnary point";
termcode = MiCPParabolicSolverReturnCode::ErrorMinimized;
}
}
else if (nb_iterations > get_options().maximum_iterations)
{
ERROR << "Maximum number of iteration reached (" << get_options().maximum_iterations << ")";
termcode = MiCPParabolicSolverReturnCode::MaxIterations;
}
else if (get_performance().maximum_step_taken)
{
++get_performance().nb_consecutive_maximum_step_taken;
++get_performance().nb_maximum_step_taken;
if (get_performance().nb_consecutive_maximum_step_taken == get_options().maximum_iterations_at_maximum_step) {
ERROR << "Divergence detected - Maximum step length taken two many times";
termcode = MiCPParabolicSolverReturnCode::MaxStepTakenTooManyTimes;
}
}
else
{
get_performance().nb_consecutive_maximum_step_taken = 0;
}
return termcode;
}
// ============================== //
// //
// Residuals and Jacobian //
// ---------------------- //
// //
// ============================== //
// Residuals
// =========
template <class program_t>
void MiCPParabolicSolver<program_t>::setup_residuals(const ParabolicVariables& x)
{
compute_residuals(x, m_residuals);
reformulate_residuals(x);
}
template <class program_t>
void MiCPParabolicSolver<program_t>::compute_residuals(const ParabolicVariables& x,
Eigen::VectorXd& residuals)
{
m_program->copy_cp_variables(x, m_cp_variables);
get_program()->get_residuals(x, residuals);
get_performance().nb_call_residuals += 1;
}
// reformulation
// -------------
template <class program_t>
void MiCPParabolicSolver<program_t>::reformulate_residuals(const ParabolicVariables &x)
{
const std::vector<bool>& is_cp = m_program->micp_index();
m_phi_residuals.resizeLike(m_residuals);
m_phi_residuals.setZero();
for (int i=0; i<m_program->get_neq(); ++i)
{
if (is_cp[i])
{
m_phi_residuals(i) = specmicp::micpsolver::penalized_fisher_burmeister(
m_cp_variables(i),
m_residuals(i),
get_options().penalization_factor
);
}
else
{
m_phi_residuals(i) = m_residuals(i);
}
}
}
// this is mainly used in the linesearch
template <class program_t>
void MiCPParabolicSolver<program_t>::reformulate_residuals_inplace(const ParabolicVariables& x,
Eigen::VectorXd& residual)
{
const std::vector<bool>& is_cp = m_program->micp_index();
for (int i=0; i<m_program->get_neq(); ++i)
{
if (is_cp[i])
{
residual.coeffRef(i) = specmicp::micpsolver::penalized_fisher_burmeister(
m_cp_variables(i),
residual(i),
get_options().penalization_factor
);
}
}
}
// Jacobian
// ========
template <class program_t>
void MiCPParabolicSolver<program_t>::setup_jacobian(
ParabolicVariables& x,
const double dt,
Eigen::SparseMatrix<double, Eigen::RowMajor>& jacobian)
{
compute_jacobian(x, dt, jacobian);
reformulate_jacobian(x, jacobian, dt);
jacobian.makeCompressed();
}
template <class program_t>
void MiCPParabolicSolver<program_t>::compute_jacobian(ParabolicVariables& x,
const double dt,
Eigen::SparseMatrix<double, Eigen::RowMajor> &jacobian)
{
get_program()->get_jacobian(x, m_residuals, jacobian, get_options().alpha*dt);
get_performance().nb_call_jacobian += 1;
}
// Reformulation
// -------------
template <class program_t>
void MiCPParabolicSolver<program_t>::reformulate_jacobian(const ParabolicVariables& x,
Eigen::SparseMatrix<double, Eigen::RowMajor> &jacobian,
double dt)
{
// see Facchinei and Pang for more details
// adapted to sparse matrix in ROW major
// i.e. the outer direction is a row (the algorithm proceed row by row)
// Iterating over the non zero element :
// see Eigen doc http://eigen.tuxfamily.org/dox/group__TutorialSparse.html
// and https://forum.kde.org/viewtopic.php?f=74&t=122606&sid=a5b017a2e6dad7bab6f09e90680275b5
jacobian.makeCompressed(); // should be the case, but just to be sure
//std::cout << "residuals" << std::endl;
//std::cout << m_phi_residuals << std::endl;
//std::cout << "Jacobian " << std::endl;
//std::cout << jacobian.toDense() << std::endl;
const std::vector<bool>& is_cp = m_program->micp_index();
const double lambda = get_options().penalization_factor;
for (int i=0; i<get_neq(); ++i)
{
if (not is_cp[i]) continue;
double c, d;
const double z = 1.0;
if (m_cp_variables(i) == 0.0 and m_residuals(i) == 0.0)
{
std::cout << "is this path taken ?" << std::endl;
ERROR << "This part of the code is not correct (reformulation jacobian with x_i=0=F_i)";
// correct that or we will run into trouble
double gpdotz = 0; // grad(Fi)^t z
// for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(jacobian,i); it; ++it)
// {
// if (is_cp[it.col()] and m_cp_variables(it.col()) == 0.0 and m_residuals(it.col()) == 0.0)
// {
// gpdotz += z*it.value();
// }
// }
double s = z + std::abs(gpdotz);
s = s * std::sqrt(z*z/(s*s) + gpdotz*gpdotz/(s*s));
c = lambda*(z/s - 1);
d = lambda*(gpdotz/s -1);
}
else
{
double s = std::abs(m_cp_variables(i)) + std::abs( m_residuals(i));
s = s * std::sqrt((m_cp_variables(i)*m_cp_variables(i))/(s*s) + (m_residuals(i)*m_residuals(i))/(s*s));
c = lambda*(m_cp_variables(i)/s - 1.0);
d = lambda*( m_residuals(i)/s - 1.0);
if ((lambda < 1.0) and ( m_residuals(i) > 0.0) and (m_cp_variables(i) > 0.0))
{
c -= (1.0-lambda) * m_residuals(i);
d -= (1.0-lambda) * m_cp_variables(i);
}
}
c *= get_options().alpha*dt;
// update the jacobian
for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(jacobian,i); it; ++it)
{
it.valueRef() *= d;
if (it.row() == it.col()) it.valueRef() += c;
}
}
}
// ================================= //
// //
// Solving the linear system //
// ------------------------- //
// //
// ================================= //
template <class program_t>
MiCPParabolicSolverReturnCode MiCPParabolicSolver<program_t>::compute_search_direction(Eigen::VectorXd& update,
ParabolicVariables& x,
double dt)
{
Eigen::SparseMatrix<double, Eigen::RowMajor> jacobian;
setup_jacobian(x, dt, jacobian);
Eigen::SparseMatrix<double, Eigen::ColMajor> new_jacobian(jacobian);
Eigen::SparseQR<Eigen::SparseMatrix<double, Eigen::ColMajor>,
Eigen::COLAMDOrdering<int>> solver(new_jacobian);
//std::cout << new_jacobian.toDense() << std::endl;
m_gradient_step_taken = false;
if (solver.info() != Eigen::Success)
{
ERROR << "Failed to factorize the jacobian !";
return MiCPParabolicSolverReturnCode::FailedDecomposition;
}
m_gradient = jacobian.transpose()*m_phi_residuals;
update = solver.solve(-m_phi_residuals);
//std::cout << update << std::endl;
if (solver.info() != Eigen::Success)
{
ERROR << "Failed to solve the system !";
return MiCPParabolicSolverReturnCode::FailedDecomposition;
}
WARNING << "Linear system error : " << (jacobian*update + m_phi_residuals).norm();
//const double condition = - (get_options().factor_descent_direction *
// std::pow(update.norm(), get_options().power_descent_direction));
//WARNING << "Gradient step condition : " << m_gradient.dot(update) << " compared to : " << condition;
//std::cout << update << std::endl;
return MiCPParabolicSolverReturnCode::NotConvergedYet;
}
template <class program_t>
double MiCPParabolicSolver<program_t>::is_step_too_long(Eigen::VectorXd& update)
{
double steplength = update.norm();
//std::cout << "update : " << steplength << std::endl;
if (steplength > get_options().maximum_step_length)
{
get_performance().maximum_step_taken = true;
update = get_options().maximum_step_length / steplength * update;
steplength = get_options().maximum_step_length;
}
//0std::cout << "update : " << steplength << std::endl;
return steplength;
}
// ================== //
// //
// Linesearch //
// ---------- //
// //
// ================== //
template <class program_t>
double MiCPParabolicSolver<program_t>::compute_residuals_linesearch(
const Eigen::VectorXd& update,
ParabolicVariables& vars,
const Eigen::VectorXd &predictor,
double dt,
double lambda
)
{
Eigen::VectorXd residuals(get_neq());
get_program()->update_variables(vars, predictor, update, lambda, get_options().alpha*dt);
compute_residuals(vars, residuals);
reformulate_residuals_inplace(vars, residuals);
return 0.5*residuals.squaredNorm();
}
template <class program_t>
int MiCPParabolicSolver<program_t>::linesearch(Eigen::VectorXd& update,
ParabolicVariables& x,
const Eigen::VectorXd &predictor,
double dt)
{
// References
// ----------
// - Algo A6.3.1 : Dennis and Schnabel (1983)
// - Munson et al. (2001)
// - Facchinei (2003)
// - Nocedal & Wrigth (2006)
DEBUG << "Linesearch";
ParabolicVariables xp(x); // copy
double fcp;
WARNING << "Norm update : " << update.norm();
get_performance().maximum_step_taken = false;
int retcode = 2;
const double alpha = 1e-4;
double newtlen = is_step_too_long(update);
double init_slope = m_gradient.dot(update);
//std::cout << "slope : " << init_slope << std::endl;
double rellength = update.cwiseAbs().maxCoeff();
const double minlambda = get_options().step_tolerance / rellength;
//WARNING << "Minimum lambda : " << minlambda;
const double alphadt = get_options().alpha*dt;
double lambda = 1.0;
//double lambda = get_program()->maximum_lambda(x, update, alphadt);
double lambda_prev = lambda;
//std::cout << "Max lambda : " << lambda << std::endl;
// non monotone linesearch
// =======================
double merit_value = 0.5*m_phi_residuals.squaredNorm();
std::cout << "Beginning linesearch : " << merit_value << std::endl;
// new residual
fcp = compute_residuals_linesearch(update, xp, predictor, dt, lambda);
//xp.velocity = x.velocity;
// Skip linesearch if enough progress is done
// ------------------------------------------
if (fcp < get_options().coeff_accept_newton_step *merit_value)
{
WARNING << "Skip linesearch ";
if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value;
else m_max_merits.push_back(merit_value);
get_program()->update_variables(x, predictor, update, lambda, alphadt);
return 0;
}
// Gradient step
// -------------
const double condition = - (get_options().factor_descent_direction *
std::pow(update.norm(), get_options().power_descent_direction));
WARNING << "Gradient step condition : " << m_gradient.dot(update) << " compared to : " << condition;
// if (m_gradient.dot(update) > condition)
// {
// WARNING << "Gradient step taken !";
// get_performance().gradient_step_taken = true;
// update = - m_gradient;
// fcp = compute_residuals_linesearch(update, xp, predictor, dt, lambda, new_res);
// }
// Select the merit value of reference
// -----------------------------------
// double mmax = merit_value;
// if (m_max_merits.size() > 0)
// {
// mmax = m_max_merits[m_max_merits.size()-1];
// // check for cycling
// // - - - - - - - - -
//// if ( m_max_merits.size() ==4
//// && std::fabs(mmax - merit_value) < 1e-3*get_options().residuals_tolerance)
//// {
//// WARNING << "Cycling has been detected by the linesearch - Taking the full Newton step";
//// x = xp;
//// update = lambda*update;
//// return 3;
//// }
// }
// if (m_max_merits.size() < 4)
// {
// m_max_merits.push_back(merit_value);
// if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4;
// }
// else if (merit_value < mmax)
// {
// m_max_merits[3] = merit_value;
// merit_value = mmax;
// }
//if (get_performance().gradient_step_taken)
//{
// merit_value *= 100;
//}
// The linesearch
// --------------
double fc = merit_value;
double fcp_prev;
int cnt = 0;
do
{
WARNING << "cnt : " <<cnt << " - lambda : " << lambda << " # " << fc << " # " << fcp << " # " << init_slope;
if (fcp <= fc + alpha*lambda*init_slope) //pg760 Fachinei2003
{
retcode = 0;
if (lambda ==1 and (newtlen > 0.99 * get_options().maximum_step_length)) {
get_performance().maximum_step_taken = true;
}
break;
}
else if (lambda < minlambda)
{
retcode = 1;
break;
}
else
{
//WARNING << "denom : " << fcp - fc -init_slope;
// Select a new step length
// - - - - - - - - - - - -
double lambdatmp;
if (cnt == 0) { // only a quadratic at the first
lambdatmp = - init_slope / (2*(fcp - fc -init_slope));
}
else
{
const double factor = 1 /(lambda - lambda_prev);
const double x1 = fcp - fc - lambda*init_slope;
const double x2 = fcp_prev - fc - lambda_prev*init_slope;
const double a = factor * ( x1/(lambda*lambda) - x2/(lambda_prev*lambda_prev));
const double b = factor * ( -x1*lambda_prev/(lambda*lambda) + x2*lambda/(lambda_prev*lambda_prev));
if (a == 0)
{ // cubic interpolation is in fact a quadratic interpolation
lambdatmp = - init_slope/(2*b);
}
else
{
const double disc = b*b-3*a*init_slope;
lambdatmp = (-b+std::sqrt(disc))/(3*a);
}
if (lambdatmp > 0.5*lambda ) lambdatmp = 0.5*lambda;
}
//WARNING << "lambdatmp : " << lambdatmp;
lambda_prev = lambda;
fcp_prev = fcp;
if (lambdatmp < 0.1*lambda) {
lambda = 0.1 * lambda;
} else {
lambda = lambdatmp;
}
}
if (not std::isfinite(lambda))
{
ERROR << "Lambda is non-finite : " << lambda;
const int ndf = 13;
for (int i=0; i<ndf; ++i)
{
for (int node=0; node<5; ++node)
{
std::cout << xp.displacement(i+ndf*node) << "\t";
}
std::cout << std::endl;
}
throw std::runtime_error("Lambda is nonfinite : "+std::to_string(lambda));
}
xp = x;
fcp = compute_residuals_linesearch(update, xp, predictor, dt, lambda);
//xp.velocity = x.velocity;
++cnt;
} while(retcode == 2 and cnt < 100);
//WARNING << "Lambda : " << lambda << " - iterations : " << cnt;
if (cnt == 100)
{
ERROR << "Too much linesearch iterations ! We stop";
}
get_program()->update_variables(x, predictor, update, lambda, alphadt);
return retcode;
}
} // end namespace micpsolver
} // end namespace reactmicp
} // end namespace specmicp
c4science · Help