From f5979379040fa0fdf68b517aadf54743e2852def Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?St=C3=A9phane=20Del=20Pino?= <stephane.delpino44@gmail.com>
Date: Tue, 18 Jul 2023 17:51:38 +0200
Subject: [PATCH] Add symmetry boundary conditions

Still requires a lot improvements
---
 src/language/modules/SchemeModule.cpp |  42 +++
 src/mesh/MeshSmootherEscobar.cpp      | 501 ++++++++++++++++++--------
 2 files changed, 397 insertions(+), 146 deletions(-)

diff --git a/src/language/modules/SchemeModule.cpp b/src/language/modules/SchemeModule.cpp
index fb6e96002..81549c403 100644
--- a/src/language/modules/SchemeModule.cpp
+++ b/src/language/modules/SchemeModule.cpp
@@ -319,6 +319,48 @@ SchemeModule::SchemeModule()
 
                               ));
 
+  this->_addBuiltinFunction("smoothMeshEscobar",
+                            std::function(
+
+                              [](std::shared_ptr<const IMesh> p_mesh,
+                                 const std::vector<std::shared_ptr<const IBoundaryConditionDescriptor>>&
+                                   bc_descriptor_list,
+                                 const FunctionSymbolId& function_symbol_id) -> std::shared_ptr<const IMesh> {
+                                MeshSmootherEscobarHandler handler;
+                                return handler.getSmoothedMesh(p_mesh, bc_descriptor_list, function_symbol_id);
+                              }
+
+                              ));
+
+  this->_addBuiltinFunction("smoothMeshEscobar",
+                            std::function(
+
+                              [](std::shared_ptr<const IMesh> p_mesh,
+                                 const std::vector<std::shared_ptr<const IBoundaryConditionDescriptor>>&
+                                   bc_descriptor_list,
+                                 const std::vector<std::shared_ptr<const IZoneDescriptor>>& smoothing_zone_list)
+                                -> std::shared_ptr<const IMesh> {
+                                MeshSmootherEscobarHandler handler;
+                                return handler.getSmoothedMesh(p_mesh, bc_descriptor_list, smoothing_zone_list);
+                              }
+
+                              ));
+
+  this->_addBuiltinFunction("smoothMeshEscobar",
+                            std::function(
+
+                              [](std::shared_ptr<const IMesh> p_mesh,
+                                 const std::vector<std::shared_ptr<const IBoundaryConditionDescriptor>>&
+                                   bc_descriptor_list,
+                                 const std::vector<std::shared_ptr<const DiscreteFunctionVariant>>&
+                                   discrete_function_variant_list) -> std::shared_ptr<const IMesh> {
+                                MeshSmootherEscobarHandler handler;
+                                return handler.getSmoothedMesh(p_mesh, bc_descriptor_list,
+                                                               discrete_function_variant_list);
+                              }
+
+                              ));
+
   this->_addBuiltinFunction("qualityEscobar",
                             std::function(
 #warning REMOVE BEFORE MERGING
diff --git a/src/mesh/MeshSmootherEscobar.cpp b/src/mesh/MeshSmootherEscobar.cpp
index c1ff9e69f..412f459b1 100644
--- a/src/mesh/MeshSmootherEscobar.cpp
+++ b/src/mesh/MeshSmootherEscobar.cpp
@@ -2,6 +2,7 @@
 
 #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>
@@ -30,6 +31,7 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
   using MeshType         = Mesh<ConnectivityType>;
 
   const MeshType& m_given_mesh;
+  std::shared_ptr<const IMesh> m_p_given_mesh;
 
   class AxisBoundaryCondition;
   class FixedBoundaryCondition;
@@ -41,6 +43,7 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
   BoundaryConditionList m_boundary_condition_list;
 
   NodeValue<const bool> m_is_fixed_node;
+  NodeValue<const bool> m_is_smoothed_node;
 
   BoundaryConditionList
   _getBCList(const MeshType& mesh,
@@ -168,10 +171,6 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
 
             const auto node_list = bc.nodeList();
 
-            const Rdxd I   = identity;
-            const Rdxd nxn = tensorProduct(n, n);
-            const Rdxd P   = I - nxn;
-
             const ConnectivityType& connectivity = m_given_mesh.connectivity();
 
             auto is_boundary_node = connectivity.isBoundaryNode();
@@ -183,25 +182,30 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
               auto cell_to_node_matrix        = connectivity.cellToNodeMatrix();
               auto node_to_cell_matrix        = connectivity.nodeToCellMatrix();
               auto node_number_in_their_cells = connectivity.nodeLocalNumbersInTheirCells();
+              auto node_to_edge_matrix        = connectivity.nodeToEdgeMatrix();
+              auto edge_to_node_matrix        = connectivity.edgeToNodeMatrix();
 
-              NodeValue<double> quality{connectivity};
-
-              constexpr double eps = 1E-15;
-              quality.fill(2);
+              constexpr double eps = 1E-14;
 
               auto smooth = [=](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 = std::acos(-1) / cell_list.size();
-                const TinyMatrix<Dimension> W{1, std::cos(alpha),   //
-                                              0, std::sin(alpha)};
+
+                const double cos_alpha     = std::cos(alpha);
+                const double sin_alpha     = std::sin(alpha);
+                const double inv_sin_alpha = 1. / sin_alpha;
+                const double inv_tan_alpha = 1. / std::tan(alpha);
+
+                const TinyMatrix<Dimension> W{1, cos_alpha,   //
+                                              0, sin_alpha};
 
                 const TinyMatrix<Dimension> inv_W = inverse(W);
 
                 TinyMatrix<Dimension, Dimension> dtheta_S =
-                  TinyMatrix<Dimension>{t[0], t[0] * (1. / std::sin(alpha) - 1. / std::tan(alpha)),   //
-                                        t[1], t[1] * (1. / std::sin(alpha) - 1. / std::tan(alpha))};
+                  TinyMatrix<Dimension>{t[0], t[0] * (inv_sin_alpha - inv_tan_alpha),   //
+                                        t[1], t[1] * (inv_sin_alpha - inv_tan_alpha)};
 
                 SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size());
                 for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
@@ -223,17 +227,17 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
                   sigma_list[i_cell] = det(S_list[i_cell]);
                 }
 
-                const double sigma_min = min(sigma_list);
+                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;
+                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)); };
 
                 double final_f = 0;
 
-                for (size_t i_iter = 0; i_iter < 3; ++i_iter) {
+                for (size_t i_iter = 0; i_iter < 200; ++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];
@@ -254,6 +258,16 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
                     sigma_list[i_cell] = det(S_list[i_cell]);
                   }
 
+                  {
+                    double current_sigma_min = min(sigma_list);
+                    if (current_sigma_min < sigma_min) {
+                      sigma_min = current_sigma_min;
+                      delta     = (sigma_min < eps)
+                                ? std::max(std::sqrt(eps * (eps - sigma_min)), std::sqrt(eps) * std::abs(sigma_min))
+                                : 0;
+                    }
+                  }
+
                   double f         = 0;
                   double dtheta_f  = 0;
                   double d2theta_f = 0;
@@ -262,7 +276,15 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
                     const double sigma            = sigma_list[i_cell];
                     const TinyMatrix<Dimension> S = S_list[i_cell];
 
-                    const TinyMatrix<Dimension> Sigma = sigma * inverse(S);
+                    const TinyMatrix<Dimension> Sigma = [&S] {
+                      TinyMatrix<Dimension> transposed_comS;
+                      for (size_t i = 0; i < Dimension; ++i) {
+                        for (size_t j = 0; j < Dimension; ++j) {
+                          transposed_comS(i, j) = cofactor(S, j, i);
+                        }
+                      }
+                      return transposed_comS;
+                    }();
 
                     const double S_norm      = frobenius(S);
                     const double Sigma_norm  = frobenius(Sigma);
@@ -276,8 +298,8 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
                     const double dtheta_sigma = trace(Sigma * dtheta_S);
 
                     TinyMatrix<Dimension, Dimension> dtheta_Sigma =
-                      TinyMatrix<Dimension>{t[1] * (1. / std::sin(alpha) - 1. / std::tan(alpha)),
-                                            -t[0] * (1. / std::sin(alpha) - 1. / std::tan(alpha)),   //
+                      TinyMatrix<Dimension>{t[1] * (inv_sin_alpha - inv_tan_alpha),
+                                            -t[0] * (inv_sin_alpha - inv_tan_alpha),   //
                                             -t[1], t[0]};
 
                     const double g = trace(transpose(S) * dtheta_S) / S_norm2                 //
@@ -302,10 +324,38 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
                     dtheta_f += dtheta_f_jr;
                     d2theta_f += dtheta2_f_jr;
                   }
-                  if (std::abs(dtheta_f) < 1E-6) {
+                  if (std::abs(dtheta_f) < 1E-12) {
+                    break;
+                  }
+
+                  const double max_edge_lenght = [=] {
+                    auto node_edge_list = node_to_edge_matrix[node_id];
+                    double max_lenght   = 0;
+                    for (size_t i_edge = 0; i_edge < node_edge_list.size(); ++i_edge) {
+                      const EdgeId edge_id            = node_edge_list[i_edge];
+                      auto edge_node_list             = edge_to_node_matrix[edge_id];
+                      const TinyVector<Dimension>& x0 = old_xr[edge_node_list[0]];
+                      const TinyVector<Dimension>& x1 = old_xr[edge_node_list[1]];
+                      const double lenght             = l2Norm(x0 - x1);
+                      if (lenght > max_lenght) {
+                        max_lenght = lenght;
+                      }
+                    }
+                    return max_lenght;
+                  }();
+
+                  const double delta_x_candidate = dtheta_f / std::abs(d2theta_f);
+                  const double delta_x_norm      = std::abs(delta_x_candidate);
+
+                  if (delta_x_norm > 0.3 * max_edge_lenght) {
+                    x += (0.3 * max_edge_lenght / delta_x_norm) * delta_x_candidate * t;
+                  } else {
+                    x += delta_x_candidate * t;
+                  }
+
+                  if (delta_x_norm < 1E-2 * max_edge_lenght) {
                     break;
                   }
-                  x += dtheta_f / d2theta_f * t;
 
                   final_f = f;
                 }
@@ -315,7 +365,7 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
               parallel_for(
                 node_list.size(), PUGS_LAMBDA(const size_t i_node) {
                   const NodeId node_id = node_list[i_node];
-                  if (not m_is_fixed_node[node_id] and node_is_owned[node_id]) {
+                  if (m_is_smoothed_node[node_id] and node_is_owned[node_id]) {
                     smooth(node_id, new_xr[node_id]);
                   }
                 });
@@ -347,6 +397,59 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
     }
   }
 
+  CellValue<const int>
+  _getNonConvexCellTag(const NodeValue<const Rd>& xr) const
+  {
+    CellValue<int> non_convex_cell_tag{m_given_mesh.connectivity()};
+
+    auto cell_to_node_matrix = m_given_mesh.connectivity().cellToNodeMatrix();
+
+    if constexpr (Dimension == 2) {
+      parallel_for(
+        m_given_mesh.numberOfCells(), PUGS_LAMBDA(const CellId cell_id) {
+          auto cell_node_list = cell_to_node_matrix[cell_id];
+          bool is_convex      = true;
+          for (size_t i_node = 0; i_node < cell_node_list.size(); ++i_node) {
+            const TriangleTransformation<Dimension> T{xr[cell_node_list[i_node]],
+                                                      xr[cell_node_list[(i_node + 1) % cell_node_list.size()]],
+                                                      xr[cell_node_list[(i_node + 2) % cell_node_list.size()]]};
+            if (T.jacobianDeterminant() <= 0) {
+              is_convex = false;
+              break;
+            }
+          }
+          non_convex_cell_tag[cell_id] = (is_convex == false);
+        });
+    } else {
+      throw NotImplementedError("non convex cell calculation in dimension != 2");
+    }
+
+    return non_convex_cell_tag;
+  }
+
+  NodeValue<const bool>
+  _getSmoothedNode(CellValue<const int>& non_convex_cell_tag, const NodeValue<const bool>& m_is_fixed_node) const
+  {
+    auto node_to_cell_matrix = m_given_mesh.connectivity().nodeToCellMatrix();
+    NodeValue<bool> is_smoothed{m_given_mesh.connectivity()};
+    is_smoothed.fill(false);
+    parallel_for(
+      m_given_mesh.numberOfNodes(), PUGS_LAMBDA(const NodeId node_id) {
+        if (not m_is_fixed_node[node_id]) {
+          auto node_cell_list = node_to_cell_matrix[node_id];
+          for (size_t i_cell = 0; i_cell < node_cell_list.size(); ++i_cell) {
+            const CellId cell_id = node_cell_list[i_cell];
+            if (non_convex_cell_tag[cell_id]) {
+              is_smoothed[node_id] = true;
+              break;
+            }
+          }
+        }
+      });
+
+    return is_smoothed;
+  }
+
  public:
   std::shared_ptr<const ItemValueVariant>
   getQuality() const
@@ -365,28 +468,35 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
     if constexpr (Dimension == 2) {
       auto cell_to_node_matrix        = connectivity.cellToNodeMatrix();
       auto node_to_cell_matrix        = connectivity.nodeToCellMatrix();
+      auto node_to_edge_matrix        = connectivity.nodeToEdgeMatrix();
+      auto edge_to_node_matrix        = connectivity.edgeToNodeMatrix();
       auto node_number_in_their_cells = connectivity.nodeLocalNumbersInTheirCells();
 
       NodeValue<double> quality{connectivity};
 
-      constexpr double eps = 1E-15;
+      constexpr double eps = 1E-14;
       quality.fill(2);
 
       auto smooth = [=](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 double alpha         = 2 * std::acos(-1) / cell_list.size();
+        const double sin_alpha     = std::sin(alpha);
+        const double cos_alpha     = std::cos(alpha);
+        const double inv_sin_alpha = 1. / sin_alpha;
+        const double inv_tan_alpha = 1. / std::tan(alpha);
+
+        const TinyMatrix<Dimension> W{1, cos_alpha,   //
+                                      0, sin_alpha};
 
         const TinyMatrix<Dimension> inv_W = inverse(W);
 
         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)}};
+          {TinyMatrix<Dimension>{-1, -inv_sin_alpha + inv_tan_alpha,   //
+                                 +0, +0},                              //
+           TinyMatrix<Dimension>{+0, +0,                               //
+                                 -1, -inv_sin_alpha + inv_tan_alpha}};
 
         SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size());
         for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
@@ -408,16 +518,20 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
           sigma_list[i_cell] = det(S_list[i_cell]);
         }
 
-        const double sigma_min = min(sigma_list);
+        double sigma_min = min(sigma_list);
 
-        const double delta =
+        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)); };
 
         double final_f = 0;
 
-        for (size_t i_iter = 0; i_iter < 100; ++i_iter) {
+        for (size_t i_iter = 0; i_iter < 200; ++i_iter) {
+          double f                         = 0;
+          TinyVector<Dimension> f_gradient = zero;
+          TinyMatrix<Dimension> f_hessian  = zero;
+
           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];
@@ -438,32 +552,46 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
             sigma_list[i_cell] = det(S_list[i_cell]);
           }
 
-          double f                         = 0;
-          TinyVector<Dimension> f_gradient = zero;
-          TinyMatrix<Dimension> f_hessian  = zero;
+          {
+            double current_sigma_min = min(sigma_list);
+            if (current_sigma_min < sigma_min) {
+              sigma_min = current_sigma_min;
+              delta     = (sigma_min < eps)
+                        ? std::max(std::sqrt(eps * (eps - sigma_min)), std::sqrt(eps) * std::abs(sigma_min))
+                        : 0;
+            }
+          }
 
           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 TinyMatrix<Dimension> Sigma = [&S] {
+              TinyMatrix<Dimension> transposed_comS;
+              for (size_t i = 0; i < Dimension; ++i) {
+                for (size_t j = 0; j < Dimension; ++j) {
+                  transposed_comS(i, j) = cofactor(S, j, i);
+                }
+              }
+              return transposed_comS;
+            }();
 
             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;
 
-            const double h = sigma + std::sqrt(sigma * sigma + 4 * delta * delta);
+            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{TinyMatrix<Dimension>{0, 1. / std::sin(alpha - 1. / std::tan(alpha)),   //
+            const std::array<TinyMatrix<Dimension>, Dimension>                         //
+              Sigma_gradient{TinyMatrix<Dimension>{0, inv_sin_alpha - inv_tan_alpha,   //
                                                    0, -1},
-                             TinyMatrix<Dimension>{-1. / std::sin(alpha) + 1. / std::tan(alpha), 0,   //
+                             TinyMatrix<Dimension>{-inv_sin_alpha + inv_tan_alpha, 0,   //
                                                    1, 0}};
 
             TinyVector<Dimension> g{trace(transpose(S) * S_gradient[0]) / S_norm2                   //
@@ -486,7 +614,7 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
                    + 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]) *
@@ -499,149 +627,230 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
             f_hessian += f_jr_hessian;
           }
 
-          if (l2Norm(f_gradient) < 1E-6) {
+          if (l2Norm(f_gradient) < 1E-12) {
             break;
           }
 
-          x -= inverse(f_hessian) * f_gradient;
+          const double max_edge_lenght = [=] {
+            auto node_edge_list = node_to_edge_matrix[node_id];
+            double max_lenght   = 0;
+            for (size_t i_edge = 0; i_edge < node_edge_list.size(); ++i_edge) {
+              const EdgeId edge_id            = node_edge_list[i_edge];
+              auto edge_node_list             = edge_to_node_matrix[edge_id];
+              const TinyVector<Dimension>& x0 = old_xr[edge_node_list[0]];
+              const TinyVector<Dimension>& x1 = old_xr[edge_node_list[1]];
+              const double lenght             = l2Norm(x0 - x1);
+              if (lenght > max_lenght) {
+                max_lenght = lenght;
+              }
+            }
+            return max_lenght;
+          }();
+
+          const TinyVector delta_x_candidate = -inverse(f_hessian) * f_gradient;
+
+          const double delta_x_norm = l2Norm(delta_x_candidate);
+
+          if (delta_x_norm > 0.3 * max_edge_lenght) {
+            x += (0.3 * max_edge_lenght / delta_x_norm) * delta_x_candidate;
+          } else {
+            x += delta_x_candidate;
+          }
+
+          if (delta_x_norm < 1E-2 * max_edge_lenght) {
+            break;
+          }
 
           final_f = f;
         }
+
         return final_f;
       };
 
       parallel_for(
         connectivity.numberOfNodes(), PUGS_LAMBDA(NodeId node_id) {
-          if (node_is_owned[node_id] and not is_boundary_node[node_id]) {
+          if (node_is_owned[node_id] and m_is_smoothed_node[node_id] and not is_boundary_node[node_id]) {
             quality[node_id] = smooth(node_id, new_xr[node_id]);
           }
         });
     } else {
       throw NotImplementedError("Dimension != 2");
     }
-
     this->_applyBC(old_xr, new_xr);
   }
 
   std::shared_ptr<const IMesh>
-  getSmoothedMesh() const
+  _smoothedMesh()
   {
-    NodeValue<const Rd> given_xr = m_given_mesh.xr();
-    NodeValue<Rd> new_xr         = copy(given_xr);
+    NodeValue<const Rd> xr = m_given_mesh.xr();
+
+    size_t nb_non_convex_cells = 0;
+    size_t i_convexify         = 1;
+    do {
+      std::cout << "- convexfication step " << i_convexify++ << '\n';
+      NodeValue<Rd> new_xr = copy(xr);
+      this->_getNewPositions(xr, new_xr);
+      xr                  = new_xr;
+      nb_non_convex_cells = sum(this->_getNonConvexCellTag(xr));
+      std::cout << "  remaining non convex cells: " << nb_non_convex_cells << '\n';
+    } while ((nb_non_convex_cells > 0) and (i_convexify < 120));
+
+    return std::make_shared<MeshType>(m_given_mesh.shared_connectivity(), xr);
+  }
 
-    this->_getNewPositions(given_xr, new_xr);
+  std::shared_ptr<const IMesh>
+  getSmoothedMesh()
+  {
+    NodeValue<bool> is_smoothed_node(m_given_mesh.connectivity());
+    parallel_for(
+      m_given_mesh.numberOfNodes(),
+      PUGS_LAMBDA(const NodeId node_id) { is_smoothed_node[node_id] = not m_is_fixed_node[node_id]; });
+
+    m_is_smoothed_node = is_smoothed_node;
 
-    return std::make_shared<MeshType>(m_given_mesh.shared_connectivity(), new_xr);
+    return _smoothedMesh();
   }
 
   std::shared_ptr<const IMesh>
-  getSmoothedMesh(const FunctionSymbolId& function_symbol_id) const
+  getSmoothedMesh(const FunctionSymbolId& function_symbol_id)
   {
-    // NodeValue<const Rd> given_xr = m_given_mesh.xr();
+    NodeValue<const Rd> given_xr = m_given_mesh.xr();
+
+    NodeValue<const bool> is_displaced =
+      InterpolateItemValue<bool(const Rd)>::interpolate(function_symbol_id, given_xr);
 
-    // NodeValue<const bool> is_displaced =
-    //   InterpolateItemValue<bool(const Rd)>::interpolate(function_symbol_id, given_xr);
+    NodeValue<bool> is_fixed_node(m_given_mesh.connectivity());
+    parallel_for(
+      m_given_mesh.numberOfNodes(), PUGS_LAMBDA(const NodeId node_id) {
+        is_fixed_node[node_id] = m_is_fixed_node[node_id] or not is_displaced[node_id];
+      });
 
-    // NodeValue<Rd> xr = this->_getDisplacement();
+    synchronize(is_fixed_node);
 
-    // parallel_for(
-    //   m_given_mesh.numberOfNodes(),
-    //   PUGS_LAMBDA(const NodeId node_id) { xr[node_id] = is_displaced[node_id] * xr[node_id] + given_xr[node_id];
-    //   });
+    m_is_fixed_node = is_fixed_node;
 
-    // return std::make_shared<MeshType>(m_given_mesh.shared_connectivity(), xr);
-    return nullptr;
+    return _smoothedMesh();
   }
 
   std::shared_ptr<const IMesh>
-  getSmoothedMesh(const std::vector<std::shared_ptr<const IZoneDescriptor>>& zone_descriptor_list) const
+  getSmoothedMesh(const std::vector<std::shared_ptr<const IZoneDescriptor>>& zone_descriptor_list)
   {
-    // NodeValue<const Rd> given_xr = m_given_mesh.xr();
-
-    // auto node_to_cell_matrix = m_given_mesh.connectivity().nodeToCellMatrix();
-
-    // NodeValue<bool> is_displaced{m_given_mesh.connectivity()};
-    // is_displaced.fill(false);
-
-    // for (size_t i_zone = 0; i_zone < zone_descriptor_list.size(); ++i_zone) {
-    //   MeshCellZone<Dimension> cell_zone = getMeshCellZone(m_given_mesh, *zone_descriptor_list[i_zone]);
-    //   const auto cell_list              = cell_zone.cellList();
-    //   CellValue<bool> is_zone_cell{m_given_mesh.connectivity()};
-    //   is_zone_cell.fill(false);
-    //   parallel_for(
-    //     cell_list.size(), PUGS_LAMBDA(const size_t i_cell) { is_zone_cell[cell_list[i_cell]] = true; });
-    //   parallel_for(
-    //     m_given_mesh.numberOfNodes(), PUGS_LAMBDA(const NodeId node_id) {
-    //       auto node_cell_list = node_to_cell_matrix[node_id];
-    //       bool displace       = true;
-    //       for (size_t i_node_cell = 0; i_node_cell < node_cell_list.size(); ++i_node_cell) {
-    //         const CellId cell_id = node_cell_list[i_node_cell];
-    //         displace &= is_zone_cell[cell_id];
-    //       }
-    //       if (displace) {
-    //         is_displaced[node_id] = true;
-    //       }
-    //     });
-    // }
-    // synchronize(is_displaced);
-    // NodeValue<Rd> xr = this->_getDisplacement();
-
-    // parallel_for(
-    //   m_given_mesh.numberOfNodes(),
-    //   PUGS_LAMBDA(const NodeId node_id) { xr[node_id] = is_displaced[node_id] * xr[node_id] + given_xr[node_id];
-    //   });
-
-    // return std::make_shared<MeshType>(m_given_mesh.shared_connectivity(), xr);
-    return nullptr;
+    NodeValue<const Rd> given_xr = m_given_mesh.xr();
+
+    auto node_to_cell_matrix = m_given_mesh.connectivity().nodeToCellMatrix();
+
+    NodeValue<bool> is_displaced{m_given_mesh.connectivity()};
+    is_displaced.fill(false);
+
+    for (size_t i_zone = 0; i_zone < zone_descriptor_list.size(); ++i_zone) {
+      MeshCellZone<Dimension> cell_zone = getMeshCellZone(m_given_mesh, *zone_descriptor_list[i_zone]);
+      const auto cell_list              = cell_zone.cellList();
+      CellValue<bool> is_zone_cell{m_given_mesh.connectivity()};
+      is_zone_cell.fill(false);
+      parallel_for(
+        cell_list.size(), PUGS_LAMBDA(const size_t i_cell) { is_zone_cell[cell_list[i_cell]] = true; });
+      parallel_for(
+        m_given_mesh.numberOfNodes(), PUGS_LAMBDA(const NodeId node_id) {
+          auto node_cell_list = node_to_cell_matrix[node_id];
+          bool displace       = true;
+          for (size_t i_node_cell = 0; i_node_cell < node_cell_list.size(); ++i_node_cell) {
+            const CellId cell_id = node_cell_list[i_node_cell];
+            displace &= is_zone_cell[cell_id];
+          }
+          if (displace) {
+            is_displaced[node_id] = true;
+          }
+        });
+    }
+    synchronize(is_displaced);
+
+    NodeValue<bool> is_fixed_node(m_given_mesh.connectivity());
+    parallel_for(
+      m_given_mesh.numberOfNodes(), PUGS_LAMBDA(const NodeId node_id) {
+        is_fixed_node[node_id] = m_is_fixed_node[node_id] or not is_displaced[node_id];
+      });
+
+    synchronize(is_fixed_node);
+
+    m_is_fixed_node = is_fixed_node;
+
+    return _smoothedMesh();
   }
 
   std::shared_ptr<const IMesh>
-  getSmoothedMesh(
-    const std::vector<std::shared_ptr<const DiscreteFunctionVariant>>& discrete_function_variant_list) const
+  getSmoothedMesh(const std::vector<std::shared_ptr<const DiscreteFunctionVariant>>& discrete_function_variant_list)
   {
-    // NodeValue<const Rd> given_xr = m_given_mesh.xr();
+    NodeValue<const Rd> given_xr = m_given_mesh.xr();
+
+    auto node_to_cell_matrix = m_given_mesh.connectivity().nodeToCellMatrix();
+
+    NodeValue<bool> is_displaced{m_given_mesh.connectivity()};
+    is_displaced.fill(false);
 
-    // auto node_to_cell_matrix = m_given_mesh.connectivity().nodeToCellMatrix();
+    CellValue<bool> is_zone_cell{m_given_mesh.connectivity()};
+    is_zone_cell.fill(false);
 
-    // NodeValue<bool> is_displaced{m_given_mesh.connectivity()};
-    // is_displaced.fill(false);
+    for (size_t i_zone = 0; i_zone < discrete_function_variant_list.size(); ++i_zone) {
+      auto characteristic_function =
+        discrete_function_variant_list[i_zone]->get<DiscreteFunctionP0<Dimension, const double>>();
+      parallel_for(
+        m_given_mesh.numberOfCells(), PUGS_LAMBDA(const CellId cell_id) {
+          if (characteristic_function[cell_id] != 0) {
+            is_zone_cell[cell_id] = true;
+          }
+        });
+    }
+
+    CellValue<const int> non_convex_cell_tag = this->_getNonConvexCellTag(given_xr);
+
+    const bool has_non_convex_cell = max(non_convex_cell_tag) > 0;
+
+    if (not has_non_convex_cell) {
+      return m_p_given_mesh;
+    }
+
+    for (size_t i_zone = 0; i_zone < discrete_function_variant_list.size(); ++i_zone) {
+      auto characteristic_function =
+        discrete_function_variant_list[i_zone]->get<DiscreteFunctionP0<Dimension, const double>>();
+
+      parallel_for(
+        m_given_mesh.numberOfNodes(), PUGS_LAMBDA(const NodeId node_id) {
+          auto node_cell_list = node_to_cell_matrix[node_id];
+          bool displace       = true;
+          for (size_t i_node_cell = 0; i_node_cell < node_cell_list.size(); ++i_node_cell) {
+            const CellId cell_id = node_cell_list[i_node_cell];
+            displace &= (characteristic_function[cell_id] != 0);
+          }
+          if (displace) {
+            is_displaced[node_id] = true;
+          }
+        });
+    }
+    synchronize(is_displaced);
 
-    // for (size_t i_zone = 0; i_zone < discrete_function_variant_list.size(); ++i_zone) {
-    //   auto is_zone_cell = discrete_function_variant_list[i_zone]->get<DiscreteFunctionP0<Dimension, const
-    //   double>>();
+    NodeValue<bool> is_fixed_node(m_given_mesh.connectivity());
+    parallel_for(
+      m_given_mesh.numberOfNodes(), PUGS_LAMBDA(const NodeId node_id) {
+        is_fixed_node[node_id] = m_is_fixed_node[node_id] or not is_displaced[node_id];
+      });
 
-    //   parallel_for(
-    //     m_given_mesh.numberOfNodes(), PUGS_LAMBDA(const NodeId node_id) {
-    //       auto node_cell_list = node_to_cell_matrix[node_id];
-    //       bool displace       = true;
-    //       for (size_t i_node_cell = 0; i_node_cell < node_cell_list.size(); ++i_node_cell) {
-    //         const CellId cell_id = node_cell_list[i_node_cell];
-    //         displace &= (is_zone_cell[cell_id] != 0);
-    //       }
-    //       if (displace) {
-    //         is_displaced[node_id] = true;
-    //       }
-    //     });
-    // }
-    // synchronize(is_displaced);
-    // NodeValue<Rd> xr = this->_getDisplacement();
+    synchronize(is_fixed_node);
 
-    // parallel_for(
-    //   m_given_mesh.numberOfNodes(),
-    //   PUGS_LAMBDA(const NodeId node_id) { xr[node_id] = is_displaced[node_id] * xr[node_id] + given_xr[node_id];
-    //   });
+    m_is_fixed_node = is_fixed_node;
 
-    // return std::make_shared<MeshType>(m_given_mesh.shared_connectivity(), xr);
+    m_is_smoothed_node = _getSmoothedNode(non_convex_cell_tag, m_is_fixed_node);
 
-    return nullptr;
+    return _smoothedMesh();
   }
 
   MeshSmootherEscobar(const MeshSmootherEscobar&) = delete;
   MeshSmootherEscobar(MeshSmootherEscobar&&)      = delete;
 
   MeshSmootherEscobar(const MeshType& given_mesh,
+                      const std::shared_ptr<const IMesh>& p_mesh,
                       const std::vector<std::shared_ptr<const IBoundaryConditionDescriptor>>& bc_descriptor_list)
     : m_given_mesh{given_mesh},
+      m_p_given_mesh{p_mesh},
       m_boundary_condition_list{this->_getBCList(given_mesh, bc_descriptor_list)},
       m_is_fixed_node{this->_getFixedNodes()}
   {}
@@ -746,19 +955,19 @@ MeshSmootherEscobarHandler::getQuality(
   case 1: {
     constexpr size_t Dimension = 1;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getQuality();
   }
   case 2: {
     constexpr size_t Dimension = 2;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getQuality();
   }
   case 3: {
     constexpr size_t Dimension = 3;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getQuality();
   }
   default: {
@@ -776,19 +985,19 @@ MeshSmootherEscobarHandler::getSmoothedMesh(
   case 1: {
     constexpr size_t Dimension = 1;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh();
   }
   case 2: {
     constexpr size_t Dimension = 2;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh();
   }
   case 3: {
     constexpr size_t Dimension = 3;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh();
   }
   default: {
@@ -807,19 +1016,19 @@ MeshSmootherEscobarHandler::getSmoothedMesh(
   case 1: {
     constexpr size_t Dimension = 1;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(function_symbol_id);
   }
   case 2: {
     constexpr size_t Dimension = 2;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(function_symbol_id);
   }
   case 3: {
     constexpr size_t Dimension = 3;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(function_symbol_id);
   }
   default: {
@@ -838,19 +1047,19 @@ MeshSmootherEscobarHandler::getSmoothedMesh(
   case 1: {
     constexpr size_t Dimension = 1;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(smoothing_zone_list);
   }
   case 2: {
     constexpr size_t Dimension = 2;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(smoothing_zone_list);
   }
   case 3: {
     constexpr size_t Dimension = 3;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(smoothing_zone_list);
   }
   default: {
@@ -879,19 +1088,19 @@ MeshSmootherEscobarHandler::getSmoothedMesh(
   case 1: {
     constexpr size_t Dimension = 1;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(discrete_function_variant_list);
   }
   case 2: {
     constexpr size_t Dimension = 2;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(discrete_function_variant_list);
   }
   case 3: {
     constexpr size_t Dimension = 3;
     using MeshType             = Mesh<Connectivity<Dimension>>;
-    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), bc_descriptor_list);
+    MeshSmootherEscobar smoother(dynamic_cast<const MeshType&>(*mesh), mesh, bc_descriptor_list);
     return smoother.getSmoothedMesh(discrete_function_variant_list);
   }
   default: {
-- 
GitLab