[ci-skip] Add jacobian and hessian (still buggy?)

src/mesh/MeshSmootherEscobar.cpp

@@ -2,7 +2,6 @@
 
 #include <algebra/TinyMatrix.hpp>
 #include <algebra/TinyVector.hpp>
-#include <geometry/TriangleTransformation.hpp>
 #include <language/utils/InterpolateItemValue.hpp>
 #include <mesh/Connectivity.hpp>
 #include <mesh/ItemValueUtils.hpp>
@@ -220,53 +219,157 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
       constexpr double eps = 1E-15;
       quality.fill(2);
 
-      auto f_inner = [=](const NodeId node_id, const TinyVector<Dimension>& x) -> double {
+      auto f_inner = [=](const NodeId node_id, TinyVector<Dimension>& x) -> double {
         auto cell_list           = node_to_cell_matrix[node_id];
         auto node_number_in_cell = node_number_in_their_cells[node_id];
 
         const double alpha = 2 * std::acos(-1) / cell_list.size();
-        const TinyMatrix<Dimension> W{1, std::cos(alpha), 0, std::sin(alpha)};
+        const TinyMatrix<Dimension> W{1, std::cos(alpha),   //
+                                      0, std::sin(alpha)};
 
         const TinyMatrix<Dimension> inv_W = inverse(W);
 
-        SmallArray<TinyMatrix<Dimension>> S(cell_list.size());
+        std::array<TinyMatrix<Dimension>, Dimension> S_gradient =
+          {TinyMatrix<Dimension>{-1, -1. / std::sin(alpha) + 1. / std::tan(alpha),   //
+                                 +0, +0},                                            //
+           TinyMatrix<Dimension>{+0, +0,                                             //
+                                 -1, -1. / std::sin(alpha) + 1. / std::tan(alpha)}};
+
+        SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size());
         for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
           const size_t i_cell_node   = node_number_in_cell[i_cell];
           auto cell_node_list        = cell_to_node_matrix[cell_list[i_cell]];
           const size_t cell_nb_nodes = cell_node_list.size();
 
-          TriangleTransformation<Dimension> T(x,   //
-                                              xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]],
-                                              xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]]);
-          S[i_cell] = T.jacobian() * inv_W;
+          const TinyVector a = xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]];
+          const TinyVector b = xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]];
+
+          const TinyMatrix<Dimension> A{a[0] - x[0], b[0] - x[0],   //
+                                        a[1] - x[1], b[1] - x[1]};
+
+          S_list[i_cell] = A * inv_W;
         }
 
-        SmallArray<double> sigma(S.size());
-        for (size_t j = 0; j < S.size(); ++j) {
-          sigma[j] = det(S[j]);
+        SmallArray<double> sigma_list(S_list.size());
+        for (size_t i_cell = 0; i_cell < S_list.size(); ++i_cell) {
+          sigma_list[i_cell] = det(S_list[i_cell]);
         }
 
-        const double sigma_min = min(sigma);
+        const double sigma_min = min(sigma_list);
 
         const double delta =
           (sigma_min < eps) ? std::max(std::sqrt(eps * (eps - sigma_min)), std::sqrt(eps) * std::abs(sigma_min)) : 0;
 
+        auto frobenius = [](const TinyMatrix<Dimension>& M) { return std::sqrt(trace(transpose(M) * M)); };
+
+        // TinyVector<Dimension> f_gradient = zero;
+        // TinyMatrix<Dimension> f_hessian  = zero;
+
+        double final_f = 0;
+
+        for (size_t i_iter = 0; i_iter < 15; ++i_iter) {
+          SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size());
+          for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
+            const size_t i_cell_node   = node_number_in_cell[i_cell];
+            auto cell_node_list        = cell_to_node_matrix[cell_list[i_cell]];
+            const size_t cell_nb_nodes = cell_node_list.size();
+
+            const TinyVector a = xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]];
+            const TinyVector b = xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]];
+
+            const TinyMatrix<Dimension> A{a[0] - x[0], b[0] - x[0],   //
+                                          a[1] - x[1], b[1] - x[1]};
+
+            S_list[i_cell] = A * inv_W;
+          }
+
+          SmallArray<double> sigma_list(S_list.size());
+          for (size_t i_cell = 0; i_cell < S_list.size(); ++i_cell) {
+            sigma_list[i_cell] = det(S_list[i_cell]);
+          }
+
           double f                         = 0;
-        for (size_t j = 0; j < S.size(); ++j) {
-          const TinyMatrix<Dimension> Sigma = sigma[j] * inverse(S[j]);
+          TinyVector<Dimension> f_gradient = zero;
+          TinyMatrix<Dimension> f_hessian  = zero;
+
+          for (size_t i_cell = 0; i_cell < S_list.size(); ++i_cell) {
+            const double sigma            = sigma_list[i_cell];
+            const TinyMatrix<Dimension> S = S_list[i_cell];
+
+            const TinyMatrix<Dimension> Sigma = sigma * inverse(S);
 
-          const double Sj_norm    = std::sqrt(trace(transpose(S[j]) * S[j]));
-          const double Sigma_norm = std::sqrt(trace(transpose(Sigma) * Sigma));
+            const double S_norm      = frobenius(S);
+            const double Sigma_norm  = frobenius(Sigma);
+            const double S_norm2     = S_norm * S_norm;
+            const double Sigma_norm2 = Sigma_norm * Sigma_norm;
 
-          f += Sj_norm * Sigma_norm / (sigma[j] + std::sqrt(sigma[j] * sigma[j] + 4 * delta * delta));
+            const double h = sigma + std::sqrt(sigma * sigma + 4 * delta * delta);
+
+            const double f_jr = S_norm * Sigma_norm / h;
+
+            TinyVector<Dimension> sigma_gradient{trace(Sigma * S_gradient[0]),   //
+                                                 trace(Sigma * S_gradient[1])};
+
+            const std::array<TinyMatrix<Dimension>, Dimension>   //
+              Sigma_gradient{sigma_gradient[0] * inverse(S) - Sigma * S_gradient[0] * Sigma,
+                             sigma_gradient[1] * inverse(S) - Sigma * S_gradient[1] * Sigma};
+
+            // TinyVector<Dimension> h_gradient = h / (h - sigma_list[i_cell]) * sigma_gradient;
+
+            TinyVector<Dimension> g{trace(transpose(S) * S_gradient[0]) / S_norm2                   //
+                                      + trace(transpose(Sigma) * Sigma_gradient[0]) / Sigma_norm2   //
+                                      - trace(Sigma * S_gradient[0]) / (h - sigma),
+                                    //
+                                    trace(transpose(S) * S_gradient[1]) / S_norm2                   //
+                                      + trace(transpose(Sigma) * Sigma_gradient[1]) / Sigma_norm2   //
+                                      - trace(Sigma * S_gradient[1]) / (h - sigma)};
+
+            const TinyVector<Dimension> f_jr_gradient = f_jr * g;
+            TinyMatrix<Dimension> f_jr_hessian        = zero;
+            for (size_t i = 0; i < Dimension; ++i) {
+              for (size_t j = 0; j < Dimension; ++j) {
+                f_jr_hessian(i, j) =                                           //
+                  (trace(transpose(S_gradient[j]) * S_gradient[i]) / S_norm2   //
+                   - 2 * trace(transpose(S) * S_gradient[j]) * trace(transpose(S) * S_gradient[i]) /
+                       (S_norm2 * S_norm2)                                                   //
+                                                                                             //
+                   + trace(transpose(Sigma_gradient[j]) * Sigma_gradient[i]) / Sigma_norm2   // + 0
+                   - 2 * trace(transpose(Sigma) * Sigma_gradient[j]) * trace(transpose(Sigma) * Sigma_gradient[i]) /
+                       (Sigma_norm2 * Sigma_norm2)                                                             //
+                                                                                                               //
+                   - 2 * trace(Sigma_gradient[j] * S_gradient[i]) / (h - sigma)                                //
+                   + 2 * trace(Sigma * S_gradient[i]) * sigma / (std::pow(h - sigma, 3)) * sigma_gradient[j]   //
+                   + g[j] * g[i]) *
+                  f_jr;
+              }
+            }
+
+            f += f_jr;
+            f_gradient += f_jr_gradient;
+            f_hessian += f_jr_hessian;
           }
-        return f;
+
+          std::cout << "f = " << f << '\n';
+          std::cout << "grad(f) = " << f_gradient << '\n';
+          std::cout << "hess(f) = " << f_hessian << " | hess(f)^T -hess(f) = " << transpose(f_hessian) - f_hessian
+                    << '\n';
+
+          std::cout << "inv(H)         = " << inverse(f_hessian) << '\n';
+          std::cout << "inv(H)*grad(f) = " << inverse(f_hessian) * f_gradient << '\n';
+
+          std::cout << "x = " << x << " -> " << x - inverse(f_hessian) * f_gradient << '\n';
+
+          x -= inverse(f_hessian) * f_gradient;
+
+          final_f = f;
+        }
+        return final_f;
       };
 
       parallel_for(
         connectivity.numberOfNodes(), PUGS_LAMBDA(NodeId node_id) {
-          auto cell_list           = node_to_cell_matrix[node_id];
-          auto node_number_in_cell = node_number_in_their_cells[node_id];
+          // auto cell_list           = node_to_cell_matrix[node_id];
+          // auto node_number_in_cell = node_number_in_their_cells[node_id];
 
           if (is_boundary_node[node_id]) {
             quality[node_id] = 1;
@@ -274,7 +377,9 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
             TinyVector x     = xr[node_id];
             quality[node_id] = f_inner(node_id, x);
 
-            TinyMatrix<Dimension> B = identity;
+            std::exit(0);
+
+            // TinyMatrix<Dimension> B = identity;
           }
         });