Commit a6b8eb89 authored by Martin Marinov's avatar Martin Marinov
Browse files

Merge commit 'dec5efd9' into marinom/REFORM-922-new-sanitization

parents 2f9a8a6e dec5efd9
......@@ -77,7 +77,7 @@ public:
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(COMISO_ARPACKSOLVER_C)
#define COMISO_ARPACKSOLVER_TEMPLATES
#include "ArpackSolver.cc"
//#include "ArpackSolver.cc"
#endif
//=============================================================================
#endif // COMISO_SUITESPARSE_AVAILABLE
......
......@@ -46,7 +46,7 @@ solve(NProblemGmmInterface* _problem)
// check for convergence
if( gmm::vect_norm2(g) < eps_)
{
DEB_line(2, "Newton Solver converged after " << i << " iterations");
DEB_line(verbosity_, "Newton Solver converged after " << i << " iterations");
_problem->store_result(P(x));
return true;
}
......@@ -80,7 +80,7 @@ solve(NProblemGmmInterface* _problem)
f = f_new;
improvement = true;
DEB_line(2, "energy improved to " << f);
DEB_line(verbosity_, "energy improved to " << f);
}
}
......@@ -97,7 +97,7 @@ solve(NProblemGmmInterface* _problem)
else
{
_problem->store_result(P(x));
DEB_line(2, "Newton solver reached max regularization but did not "
DEB_line(verbosity_, "Newton solver reached max regularization but did not "
"converge");
converged_ = false;
return false;
......@@ -105,7 +105,7 @@ solve(NProblemGmmInterface* _problem)
}
}
_problem->store_result(P(x));
DEB_line(2, "Newton Solver did not converge!!! after iterations.");
DEB_line(verbosity_, "Newton Solver did not converge!!! after iterations.");
converged_ = false;
return false;
......@@ -123,7 +123,7 @@ solve(NProblemGmmInterface* _problem)
int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
const VectorD& _b)
{
DEB_time_func_def;
DEB_time_func(verbosity_);
converged_ = false;
const double KKT_res_eps = 1e-6;
......@@ -136,7 +136,7 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
// number of constraints
size_t m = _b.size();
DEB_line(2, "optimize via Newton with " << n << " unknowns and " << m <<
DEB_line(verbosity_, "optimize via Newton with " << n << " unknowns and " << m <<
" linear constraints");
// initialize vectors of unknowns
......@@ -189,11 +189,14 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
if(!fact_ok || kkt_res2 > KKT_res_eps || constraint_res2 > max_allowed_constraint_violation2)
{
DEB_warning(2, "Numerical issues in KKT system");
DEB_warning(2, "Numerical issues in KKT system");
DEB_warning_if(!fact_ok, 2, "Factorization not ok");
DEB_warning_if(kkt_res2 > KKT_res_eps, 2, "KKT Residuum too high: " << kkt_res2);
DEB_warning_if(constraint_res2 > max_allowed_constraint_violation2, 2, "Constraint violation too high: " << constraint_res2);
// alternate hessian and constraints regularization
if(reg_iters % 2 == 0 || regularize_constraints >= regularize_constraints_limit)
{
DEB_line(2, "residual ^ 2 " << kkt_res2 << "->regularize hessian");
DEB_line(verbosity_, "residual ^ 2 " << kkt_res2 << "->regularize hessian");
if(regularize_hessian == 0.0)
regularize_hessian = 1e-6;
else
......@@ -201,7 +204,7 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
}
else
{
DEB_line(2, "residual^2 " << kkt_res2 << " -> regularize constraints");
DEB_line(verbosity_, "residual^2 " << kkt_res2 << " -> regularize constraints");
if(regularize_constraints == 0.0)
regularize_constraints = 1e-8;
else
......@@ -247,7 +250,7 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
}
DEB_line(2, "iter: " << iter
DEB_line(verbosity_, "iter: " << iter
<< ", f(x) = " << fx << ", t = " << t << " (tmax=" << t_max << ")"
<< (t < t_max ? " _clamped_" : "")
<< ", eps = [Newton decrement] = " << newton_decrement
......
......@@ -58,7 +58,7 @@ public:
/// Default constructor
NewtonSolver(const double _eps = 1e-6, const double _eps_line_search = 1e-9, const int _max_iters = 200, const double _alpha_ls=0.2, const double _beta_ls = 0.6)
: eps_(_eps), eps_ls_(_eps_line_search), max_iters_(_max_iters), alpha_ls_(_alpha_ls), beta_ls_(_beta_ls), solver_type_(LS_EigenLU), constant_hessian_structure_(false)
: eps_(_eps), eps_ls_(_eps_line_search), max_iters_(_max_iters), alpha_ls_(_alpha_ls), beta_ls_(_beta_ls), verbosity_(2), solver_type_(LS_EigenLU), constant_hessian_structure_(false)
{
//#if COMISO_SUITESPARSE_AVAILABLE
// solver_type_ = LS_Umfpack;
......@@ -84,8 +84,12 @@ public:
solver_type_ = _st;
}
// set verbosity level of the solver. Lower numbers are more verbose
void set_verbosity(int _verbosity) { verbosity_ = _verbosity; }
bool converged() { return converged_; }
protected:
bool factorize(NProblemInterface* _problem, const SMatrixD& _A,
......@@ -134,6 +138,7 @@ private:
int max_iters_;
double alpha_ls_;
double beta_ls_;
int verbosity_;
VectorD x_ls_;
......@@ -149,6 +154,7 @@ private:
Eigen::UmfPackLU<SMatrixD> umfpack_solver_;
#endif
// deprecated
bool constant_hessian_structure_;
......
......@@ -334,6 +334,218 @@ void SymmetricDirichletProblem::get_constraints(SMatrixD& _A, VectorD& _b)
_A.setFromTriplets(triplets.begin(), triplets.end());
}
SymmetricDirichletProblem::ReferencePositionVector2D SymmetricDirichletProblem::get_equilateral_refernce_positions(double _area)
{
ReferencePositionVector2D equilateral_reference;
equilateral_reference << 0.0, 0.0,
1.0, 0.0,
0.5, 0.5*std::sqrt(3.0);
equilateral_reference *= _area / 0.5*0.5*std::sqrt(3.0);
return equilateral_reference;
}
double SymmetricDirichletOneVertexElement::eval_f(const VecV& _x, const VecC& _c)
{
Matrix2x2d B;
B(0,0) = _c[0]-_x[0];
B(0,1) = _c[2]-_x[0];
B(1,0) = _c[1]-_x[1];
B(1,1) = _c[3]-_x[1];
Matrix2x2d Bin = B.inverse();
Matrix2x2d R;
R(0,0) = _c[4+2]-_c[4+0];
R(0,1) = _c[4+4]-_c[4+0];
R(1,0) = _c[4+3]-_c[4+1];
R(1,1) = _c[4+5]-_c[4+1];
Matrix2x2d Rin = R.inverse();
double area = 0.5 * R.determinant();
if (B.determinant() * area <= 0)
{
double res = std::numeric_limits<double>::max();
return res;
}
Matrix2x2d J = B * Rin;
Matrix2x2d Jin = R * Bin;
double res = J.squaredNorm() + Jin.squaredNorm();
return area * (res - 4);
}
void SymmetricDirichletOneVertexElement::eval_gradient(const VecV& _x, const VecC& _c, VecV& _g)
{
const double a_x = _x[0];
const double a_y = _x[1];
const double b_x = _c[0];
const double b_y = _c[1];
const double c_x = _c[2];
const double c_y = _c[3];
const double d_x = _c[4];
const double d_y = _c[5];
const double e_x = _c[6];
const double e_y = _c[7];
const double f_x = _c[8];
const double f_y = _c[9];
const double det = d_x*e_y - d_x*f_y - d_y*e_x + d_y*f_x + e_x*f_y - e_y*f_x;
_g[0] = (2*(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x))*(e_y - f_y) + 2*(-(a_x - c_x)*(e_x - f_x) + (b_x - c_x)*(d_x - f_x))*(-e_x + f_x))/
std::pow(d_x*e_y - d_x*f_y - d_y*e_x + d_y*f_x + e_x*f_y - e_y*f_x,2)
- 2*(std::pow(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x),2) +
std::pow(-(b_x - c_x)*(d_x - f_x) + (a_x - c_x)*(e_x - f_x),2) +
std::pow( (b_y - c_y)*(d_y - f_y) - (a_y - c_y)*(e_y - f_y),2) +
std::pow(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x),2)) *
( b_y - c_y)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,3)
+ (2*(-(b_x - c_x)*(d_x - f_x) + (a_x - c_x)*(e_x - f_x))*( e_x - f_x) +
2*(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x))*( e_y - f_y))/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,2);
_g[1] = (2*( (a_y - c_y)*(e_y - f_y) - (b_y - c_y)*(d_y - f_y))*(e_y - f_y) + 2*(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x))*(-e_x + f_x))/
std::pow(d_x*e_y - d_x*f_y - d_y*e_x + d_y*f_x + e_x*f_y - e_y*f_x,2)
- 2*(std::pow(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x),2) +
std::pow(-(b_x - c_x)*(d_x - f_x) + (a_x - c_x)*(e_x - f_x),2) +
std::pow( (b_y - c_y)*(d_y - f_y) - (a_y - c_y)*(e_y - f_y),2) +
std::pow(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x),2)) *
(-b_x + c_x)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,3)
+ (2*(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x))*(-e_x + f_x) +
2*( (b_y - c_y)*(d_y - f_y) - (a_y - c_y)*(e_y - f_y))*(-e_y + f_y))/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,2);
// area weight
_g[0] *= 0.5*det;
_g[1] *= 0.5*det;
}
void SymmetricDirichletOneVertexElement::eval_hessian(const VecV& _x, const VecC& _c, std::vector<Triplet>& _triplets)
{
const double a_x = _x[0];
const double a_y = _x[1];
const double b_x = _c[0];
const double b_y = _c[1];
const double c_x = _c[2];
const double c_y = _c[3];
const double d_x = _c[4];
const double d_y = _c[5];
const double e_x = _c[6];
const double e_y = _c[7];
const double f_x = _c[8];
const double f_y = _c[9];
const double det = d_x*e_y - d_x*f_y - d_y*e_x + d_y*f_x + e_x*f_y - e_y*f_x;
Eigen::MatrixXd H(2,2);
H(0,0) = (2*std::pow(e_y - f_y,2) + 2*std::pow(-e_x + f_x,2))/std::pow(d_x*e_y - d_x*f_y - d_y*e_x + d_y*f_x + e_x*f_y - e_y*f_x,2) + 6*(std::pow(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x),2) + std::pow(-(b_x - c_x)*(d_x - f_x) + (a_x - c_x)*(e_x - f_x),2) + std::pow((b_y - c_y)*(d_y - f_y) - (a_y - c_y)*(e_y - f_y),2) + std::pow(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x),2))*std::pow( b_y - c_y,2)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,4) - 4*(2*(-(b_x - c_x)*(d_x - f_x) + (a_x - c_x)*(e_x - f_x))*( e_x - f_x) + 2*(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x))*( e_y - f_y))*( b_y - c_y)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,3) + (2*std::pow( e_x - f_x,2) + 2*std::pow( e_y - f_y,2))/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,2);
H(1,1) = (2*std::pow(e_y - f_y,2) + 2*std::pow(-e_x + f_x,2))/std::pow(d_x*e_y - d_x*f_y - d_y*e_x + d_y*f_x + e_x*f_y - e_y*f_x,2) + 6*(std::pow(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x),2) + std::pow(-(b_x - c_x)*(d_x - f_x) + (a_x - c_x)*(e_x - f_x),2) + std::pow((b_y - c_y)*(d_y - f_y) - (a_y - c_y)*(e_y - f_y),2) + std::pow(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x),2))*std::pow(-b_x + c_x,2)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,4) - 4*(2*(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x))*(-e_x + f_x) + 2*( (b_y - c_y)*(d_y - f_y) - (a_y - c_y)*(e_y - f_y))*(-e_y + f_y))*(-b_x + c_x)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,3) + (2*std::pow(-e_x + f_x,2) + 2*std::pow(-e_y + f_y,2))/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,2);
H(1,0) = 6*(std::pow(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x),2) + std::pow(-(b_x - c_x)*(d_x - f_x) + (a_x - c_x)*(e_x - f_x),2) + std::pow((b_y - c_y)*(d_y - f_y) - (a_y - c_y)*(e_y - f_y),2) + std::pow(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x),2))*(b_y - c_y)*(-b_x + c_x)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,4) - 2*(2*(-(a_y - c_y)*(e_x - f_x) + (b_y - c_y)*(d_x - f_x))*(-e_x + f_x) + 2*((b_y - c_y)*(d_y - f_y) - (a_y - c_y)*(e_y - f_y))*(-e_y + f_y))*(b_y - c_y)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,3) - 2*(2*(-(b_x - c_x)*(d_x - f_x) + (a_x - c_x)*(e_x - f_x))*(e_x - f_x) + 2*(-(d_y - f_y)*(b_x - c_x) + (e_y - f_y)*(a_x - c_x))*(e_y - f_y))*(-b_x + c_x)/std::pow(a_x*b_y - a_x*c_y - a_y*b_x + a_y*c_x + b_x*c_y - b_y*c_x,3);
H(0,1) = H(1,0);
H *= 0.5 * det;
Eigen::MatrixXd Hspd(2,2);
project_hessian(H, Hspd, 1e-6);
_triplets.reserve(2*2);
for (int i = 0; i < 2; ++i)
for (int j = 0; j < 2; ++j)
_triplets.push_back(Triplet(i,j,Hspd(i,j)));
}
void SymmetricDirichletOneVertexElement::project_hessian(Eigen::MatrixXd& H_orig, Eigen::MatrixXd& H_spd, double eps)
{
// Compute eigen-decomposition (of symmetric matrix)
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(H_orig);
Eigen::MatrixXd V = eig.eigenvectors();
Eigen::MatrixXd D = eig.eigenvalues().asDiagonal();
// Clamp all eigenvalues to eps
for (int i = 0; i < H_orig.rows(); ++i)
D(i, i) = std::max(eps, D(i, i));
H_spd = V * D * V.inverse();
}
double SymmetricDirichletOneVertexElement::max_feasible_step(const VecV& _x, const VecV& _v, const VecC& _c)
{
// get quadratic coefficients (ax^2 + b^x + c)
auto U11 = _x[0];
auto U12 = _x[1];
auto U21 = _c[0];
auto U22 = _c[1];
auto U31 = _c[2];
auto U32 = _c[3];
auto V11 = _v[0];
auto V12 = _v[1];
const VecV::Scalar V21 = 0.0;
const VecV::Scalar V22 = 0.0;
const VecV::Scalar V31 = 0.0;
const VecV::Scalar V32 = 0.0;
double a = V11*V22 - V12*V21 - V11*V32 + V12*V31 + V21*V32 - V22*V31;
double b = U11*V22 - U12*V21 - U21*V12 + U22*V11 - U11*V32 + U12*V31 + U31*V12 - U32*V11 + U21*V32 - U22*V31 - U31*V22 + U32*V21;
double c = U11*U22 - U12*U21 - U11*U32 + U12*U31 + U21*U32 - U22*U31;
double delta_in = pow(b,2) - 4*a*c;
if (delta_in < 0) {
return std::numeric_limits<double>::max();
}
double delta = sqrt(delta_in);
double t1 = (-b + delta)/ (2*a);
double t2 = (-b - delta)/ (2*a);
double tmp_n = std::min(t1,t2);
t1 = std::max(t1,t2); t2 = tmp_n;
// return the smallest negative root if it exists, otherwise return infinity
if (t1 > 0)
{
if (t2 > 0)
{
return 0.999 * t2;
}
else
{
return 0.999 * t1;
}
}
else
{
return std::numeric_limits<double>::max();
}
}
SymmetricDirichletOneRingProblem::SymmetricDirichletOneRingProblem()
:
FiniteElementProblem(2)
{
FiniteElementProblem::add_set(&element_set);
}
void SymmetricDirichletOneRingProblem::add_triangle(const InputPositionVector2D& _current_positions, const SymmetricDirichletOneRingProblem::ReferencePositionVector2D& _reference_positions)
{
SymmetricDirichletOneVertexElement::VecI indices;
indices << 0,1;
SymmetricDirichletOneVertexElement::VecC constants;
constants << _current_positions (1, 0), _current_positions (1, 1),
_current_positions (2, 0), _current_positions (2, 1),
_reference_positions(0, 0), _reference_positions(0, 1),
_reference_positions(1, 0), _reference_positions(1, 1),
_reference_positions(2, 0), _reference_positions(2, 1);
element_set.instances().add_element(indices, constants);
}
SymmetricDirichletOneRingProblem::ReferencePositionVector2D SymmetricDirichletOneRingProblem::get_equilateral_refernce_positions(double _area)
{
ReferencePositionVector2D equilateral_reference;
equilateral_reference << 0.0, 0.0,
1.0, 0.0,
0.5, 0.5*std::sqrt(3.0);
equilateral_reference *= _area/(0.5*0.5*std::sqrt(3.0));
return equilateral_reference;
}
} // namespace COMISO
#endif //(COMISO_ADOLC_AVAILABLE && COMISO_EIGEN3_AVAILABLE)
......
......@@ -146,6 +146,9 @@ public:
/// remove all constraints
void clear_constraints() { fix_points.clear();}
/// get reference positions for an equilateral triangle with the given area
ReferencePositionVector2D get_equilateral_refernce_positions(double _area = 1.0);
private:
SymmetricDirichletElementSet element_set;
......@@ -153,6 +156,72 @@ private:
};
class SymmetricDirichletOneVertexElement
{
public:
// define dimensions
const static int NV = 2; // the one u and v coordinate of the single vertex that is optimized. u1, v1
const static int NC = 10; // the two other positions of the triangle u2, v2, u3, and v3, and the three reference positions of the triangle, x1, y1, x2, y2, x3, y3
typedef Eigen::Matrix<size_t,NV,1> VecI;
typedef Eigen::Matrix<double,NV,1> VecV;
typedef Eigen::Matrix<double,NC,1> VecC;
typedef Eigen::Triplet<double> Triplet;
typedef Eigen::Matrix<double,12,1> Vector12;
typedef Eigen::Matrix<adouble,2,2> Matrix2x2ad;
typedef Eigen::Matrix<double,2,2> Matrix2x2d;
typedef Eigen::Matrix<double,6,6> Matrix6x6;
SymmetricDirichletOneVertexElement(){}
double eval_f (const VecV& _x, const VecC& _c);
void eval_gradient(const VecV& _x, const VecC& _c, VecV& _g);
void eval_hessian (const VecV& _x, const VecC& _c, std::vector<Triplet>& _triplets);
void project_hessian(Eigen::MatrixXd& H_orig, Eigen::MatrixXd& H_spd, double eps);
double max_feasible_step(const VecV& _x, const VecV& _v, const VecC& /*_c*/);
};
/** \class SymmetricDirichletOneRingProblem
A problem that allows you to add triangles with reference positions for which
the symmetric dirichlet energy should be minimized. Secial case of SymmetricDirichletProblem
that optimizes only a single vertex for which the user inputs all adjacent triangles.
*/
class COMISODLLEXPORT SymmetricDirichletOneRingProblem : public FiniteElementProblem
{
public:
typedef FiniteElementSet<SymmetricDirichletOneVertexElement> SymmetricDirichletOneVertexElementSet;
typedef Eigen::Matrix<size_t,3,1> IndexVector;
typedef Eigen::Matrix<double,3,2> InputPositionVector2D;
typedef Eigen::Matrix<double,3,2> ReferencePositionVector2D;
typedef Eigen::Matrix<double,3,3> ReferencePositionVector3D;
typedef Eigen::VectorXd VectorD;
typedef Eigen::SparseMatrix<double> SMatrixD;
/// Default constructor
SymmetricDirichletOneRingProblem();
void add_triangle(const InputPositionVector2D& _current_positions, const ReferencePositionVector2D& _reference_positions);
static ReferencePositionVector2D get_equilateral_refernce_positions(double _area = 1.0);
private:
SymmetricDirichletOneVertexElementSet element_set;
std::vector<std::pair<int, double>> fix_points;
};
//=============================================================================
} // namespace COMISO
//=============================================================================
......
......@@ -751,11 +751,11 @@ void f(const gmm::row_matrix<GMM_VectorT>& _G, EIGEN_MatrixT& _E)
// convert a gmm col-sparse matrix into an eigen sparse matrix
template <class GMM_RealT, class EIGEN_MatrixT>
void f(const gmm::csc_matrix<GMM_RealT,0>& _G, EIGEN_MatrixT& _E)
void f(const gmm::csc_matrix<GMM_RealT>& _G, EIGEN_MatrixT& _E)
{
typedef typename EIGEN_MatrixT::Scalar Scalar;
typedef typename gmm::csc_matrix<GMM_RealT,0> GMM_MatrixT;
typedef typename gmm::csc_matrix<GMM_RealT> GMM_MatrixT;
typedef typename gmm::linalg_traits<GMM_MatrixT>::const_sub_col_type ColT;
typedef typename gmm::linalg_traits<ColT>::const_iterator CIter;
......
#include "ExactConstraintSatisfaction.hh"
#include <CoMISo/Config/config.hh>
#include <CoMISo/NSolver/NProblemInterface.hh>
#include <CoMISo/Utils/CoMISoError.hh>
#include <vector>
namespace COMISO {
ExactConstraintSatisfaction::ExactConstraintSatisfaction()
:
largest_exponent_(std::numeric_limits<int>::min())
{
}
// ------------------------Helpfull Methods----------------------------------------
//row1 belongs to vector b, row2 to a row in the matrix
void ExactConstraintSatisfaction::swap_rows(SP_Matrix_R& mat, int row1, int row2){
Eigen::SparseVector<int> row_2 = mat.row(row2);
Eigen::SparseVector<int> row_1 = mat.row(row1);
mat.row(row2) = row_1;
mat.row(row1) = row_2;
// mat.prune(0.0, 0);
// mat.makeCompressed();
// mat.finalize();
}
//We want to eliminate row1 in mat with the row corresponding to row2 in mat
//the row_2 has a pivot in (row2, col_p)
void ExactConstraintSatisfaction::eliminate_row(SP_Matrix_R& mat, Eigen::VectorXi& b, int row1, int row2, int pivot_column)
{
if(pivot_column < 0)
{
DEB_error("Pivot column is negative");
return;
}
int pivot_row1 = mat.coeff(row1, pivot_column); //the element under the pivot
if(pivot_row1 == 0)
return;
int pivot_row2 = mat.coeff(row2, pivot_column); //the pivot
b.coeffRef(row1) *= pivot_row2;
b.coeffRef(row1) -= pivot_row1 * b.coeffRef(row2);
mat.row(row1) *= pivot_row2;
mat.row(row1) -= pivot_row1 * mat.row(row2);
mat.row(row1) = mat.row(row1).pruned(0,0);
int gcdValue = gcd_row(mat, row1, b.coeff(row1));
mat.row(row1) /= gcdValue;
b.coeffRef(row1) /= gcdValue;
}
int ExactConstraintSatisfaction::gcd(const int a, const int b)
{
if(b == 0)
return std::abs(a);
return gcd(std::abs(b) , std::abs(a) % std::abs(b));
}
int ExactConstraintSatisfaction::gcd_row(const SP_Matrix_R& A, int row, const int b)
{
int gcdValue = b;
bool first = true;
bool is_negativ = 0;
for(SP_Matrix_R::InnerIterator it(A, row); it; ++it)
{
if(it.value() != 0 && first)
{
first = false;
is_negativ = it.value() < 0;
}
gcdValue = gcd(gcdValue, it.value());
}
if(gcdValue == 0)
return 1;
if(is_negativ)
gcdValue = std::abs(gcdValue) * -1;
return gcdValue;
}
void ExactConstraintSatisfaction::largest_exponent(const Eigen::VectorXd& x)
{
// only compute largest component if it was not set from the outside
if (largest_exponent_ == std::numeric_limits<int>::min())
set_largest_exponent(std::max(compute_largest_exponent(x)+2, -65));
}
void ExactConstraintSatisfaction::set_largest_exponent(int _exponent)
{
largest_exponent_ = _exponent;
delta_ = std::pow(2, largest_exponent_);
}
int ExactConstraintSatisfaction::compute_largest_exponent(const Eigen::VectorXd& x)
{
double max_elem = std::max(x.maxCoeff(), -x.minCoeff());
auto expo = static_cast<int>(std::ceil(std::log2(max_elem)));
return expo;
}
double ExactConstraintSatisfaction::F_delta(double x)
{
int sign = -1;
if(x >= 0)
sign = 1;
double x_of_F = (x + sign * delta_) - sign * delta_;
return x_of_F;