Working minimization.

1 files changed, 3 insertions, 5 deletions

diff --git a/dynamics.hpp b/dynamics.hpp
index 639803b..21afd30 100644
--- a/dynamics.hpp
+++ b/dynamics.hpp
@@ -55,13 +55,13 @@ Vector<Real> findMinimum(const Tensor<Real, p>& J, const Vector<Real>& z0, Real
   Matrix<Real> M = ddH - (dH * z.transpose() + z.dot(dH) * Matrix<Real>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size() + 2.0 * z * z.transpose();
 
   while (g.norm() / z.size() > ε && λ < 1e8) {
-    Vector<Real> dz = (M + λ * Matrix<Real>::Identity(z.size(), z.size())).partialPivLu().solve(g);
+    Vector<Real> dz = (M + λ * (Matrix<Real>)abs(M.diagonal().array()).matrix().asDiagonal()).partialPivLu().solve(g);
     dz -= z.dot(dz) * z / (Real)z.size();
     Vector<Real> zNew = normalize(z - dz);
 
     auto [HNew, dHNew, ddHNew] = hamGradHess(J, zNew);
 
-    if (HNew <= H * 1.01) {
+    if (HNew * 1.0001 <= H) {
       z = zNew;
       H = HNew;
       dH = dHNew;
@@ -70,12 +70,10 @@ Vector<Real> findMinimum(const Tensor<Real, p>& J, const Vector<Real>& z0, Real
       g = dH - z.dot(dH) * z / (Real)z.size();
       M = ddH - (dH * z.transpose() + z.dot(dH) * Matrix<Real>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size() + 2.0 * z * z.transpose();
 
-      λ /= 1.001;
+      λ /= 2;
     } else {
       λ *= 1.5;
     }
-
-    std::cout << "error : " << H << " " << g.norm() / z.size() << " " << λ << std::endl;
   }
 
   return z;