diff --git a/src/mesh/MeshSmootherEscobar.cpp b/src/mesh/MeshSmootherEscobar.cpp index 5f608eb2e668d4a7c0d6914d40ca813c589fde69..6ef78cc3dd14ab57ee59f7d249979ef123b2abfd 100644 --- a/src/mesh/MeshSmootherEscobar.cpp +++ b/src/mesh/MeshSmootherEscobar.cpp @@ -263,8 +263,8 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar 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; + ? std::max(std::sqrt(eps * (eps - sigma_min)), std::sqrt(eps) * std::abs(sigma_min)) + : 0; } } @@ -367,6 +367,7 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar const NodeId node_id = node_list[i_node]; if (m_is_smoothed_node[node_id] and node_is_owned[node_id]) { smooth(node_id, new_xr[node_id]); + // new_xr[node_id] = 0.5 * (new_xr[node_id] + old_xr[node_id]); } }); @@ -428,7 +429,7 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar } NodeValue<const bool> - _getSmoothedNode(CellValue<const int>& non_convex_cell_tag, const NodeValue<const bool>& m_is_fixed_node) const + _getSmoothedNode(const 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()}; @@ -450,495 +451,593 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar return is_smoothed; } - public: - std::shared_ptr<const ItemValueVariant> - getQuality() const - { - return std::make_shared<ItemValueVariant>(m_given_mesh.xr()); - } - void - _getNewPositions(const NodeValue<const Rd>& old_xr, NodeValue<Rd>& new_xr) const + get_f_grad_hess(const NodeValue<const TinyVector<Dimension>>& old_xr, + double& sigma_min, + double& delta, + const NodeId node_id, + const TinyVector<Dimension>& x, + double& f, + TinyVector<Dimension>& f_gradient, + TinyMatrix<Dimension>& f_hessian) const { - constexpr bool multi_connection = false; + constexpr bool multi_connection = true; + constexpr double eps = 1E-14; - const ConnectivityType& connectivity = m_given_mesh.connectivity(); - - auto is_boundary_node = connectivity.isBoundaryNode(); - auto node_is_owned = connectivity.nodeIsOwned(); + auto frobenius = [](const TinyMatrix<Dimension>& M) { return std::sqrt(trace(transpose(M) * M)); }; - 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-14; - quality.fill(2); + const ConnectivityType& connectivity = m_given_mesh.connectivity(); - size_t maxiter = 0; + auto cell_to_node_matrix = connectivity.cellToNodeMatrix(); + auto node_to_cell_matrix = connectivity.nodeToCellMatrix(); + auto node_number_in_their_cells = connectivity.nodeLocalNumbersInTheirCells(); - auto smooth = [=, &maxiter](const NodeId node_id, TinyVector<Dimension>& x) -> double { - const bool print = (node_id == 4); + 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]; + 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 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> W{1, cos_alpha, // - 0, sin_alpha}; + const TinyMatrix<Dimension> inv_W = inverse(W); - const TinyMatrix<Dimension> inv_W = inverse(W); + std::array<TinyMatrix<Dimension>, Dimension> // + S_gradient = {TinyMatrix<Dimension>{-1, -1, // + +0, +0}, + TinyMatrix<Dimension>{+inv_tan_alpha, +inv_tan_alpha, // + -inv_sin_alpha, -inv_sin_alpha}}; - std::array<TinyMatrix<Dimension>, Dimension> // - S_gradient = {TinyMatrix<Dimension>{-1, -1, // - +0, +0}, - TinyMatrix<Dimension>{+inv_tan_alpha, +inv_tan_alpha, // - -inv_sin_alpha, -inv_sin_alpha}}; + f = 0; + f_gradient = zero; + f_hessian = zero; - SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size()); + 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(); - double sigma_min = std::numeric_limits<double>::max(); - double delta = 0; + const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]]; + const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]]; - auto frobenius = [](const TinyMatrix<Dimension>& M) { return std::sqrt(trace(transpose(M) * M)); }; + const TinyMatrix<Dimension> A{a[0] - x[0], b[0] - x[0], // + a[1] - x[1], b[1] - x[1]}; - double final_f = 0; + S_list[i_cell] = A * inv_W; + } - std::cout.precision(15); + 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]); + } - for (size_t i_iter = 0; i_iter < 5; ++i_iter) { - double sigma_min = std::numeric_limits<double>::max(); - delta = sigma_min; + { + double current_sigma_min = std::min(min(sigma_list), delta); - maxiter = std::max(i_iter + 1, maxiter); - if (print) { - std::cout << "------------------------------\n"; - std::cout << "iter = " << i_iter << '\n'; - std::cout << "x = " << x << '\n'; - } + if constexpr (multi_connection) { + size_t nb_medium_range_simplices = 0; + for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) { + const CellId cell_id = cell_list[i_cell]; + nb_medium_range_simplices += 2 * (cell_to_node_matrix[cell_id].size() - 3); + } - double f = 0; - TinyVector<Dimension> f_gradient = zero; - TinyMatrix<Dimension> f_hessian = zero; + if (nb_medium_range_simplices > 0) { + SmallArray<double> medium_range_sigma_list(nb_medium_range_simplices); - SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size()); for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) { + const CellId cell_id = cell_list[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 = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]]; - const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]]; + const double mr_alpha = 2 * std::acos(-1) / ((cell_nb_nodes - 2) * (cell_list.size())); + const double mr_sin_alpha = std::sin(mr_alpha); + const double mr_cos_alpha = std::cos(mr_alpha); + + const TinyMatrix<Dimension> mr_W{1, mr_cos_alpha, // + 0, mr_sin_alpha}; + + const TinyMatrix mr_inv_W = inverse(mr_W); + + { // first simplex + const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]]; + const TinyVector b = old_xr[cell_node_list[(i_cell_node + 2) % cell_nb_nodes]]; - const TinyMatrix<Dimension> A{a[0] - x[0], b[0] - x[0], // - a[1] - x[1], b[1] - x[1]}; + const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0], // + a[1] - x[1], b[1] - x[1]}; - S_list[i_cell] = A * inv_W; + const TinyMatrix mr_S = mr_A * mr_inv_W; + const double mr_sigma = det(mr_S); + if (mr_sigma < current_sigma_min) { + current_sigma_min = mr_sigma; + } + } + + { // second simplex + const TinyVector a = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 2) % cell_nb_nodes]]; + const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]]; - if (print) { - std::cout << "--- S --- cell = " << i_cell << " (" << node_to_cell_matrix[node_id][i_cell] << ")\n"; - std::cout << "a = " << a << '\n'; - std::cout << "b = " << b << '\n'; - std::cout << "x = " << x << '\n'; - std::cout << "A = " << A << '\n'; - std::cout << "S = " << S_list[i_cell] << '\n'; + const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0], // + a[1] - x[1], b[1] - x[1]}; + + const TinyMatrix mr_S = mr_A * mr_inv_W; + const double mr_sigma = det(mr_S); + if (mr_sigma < current_sigma_min) { + current_sigma_min = mr_sigma; + } } } + } + } + + 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; + } + } - 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]); + auto escobarSSigma = [&frobenius, delta](const double sigma, const TinyMatrix<Dimension>& S, + const TinyVector<Dimension>& sigma_gradient, + const std::array<TinyMatrix<Dimension>, Dimension>& S_gradient, + const std::array<TinyMatrix<Dimension>, Dimension>& Sigma_gradient, + double& f, TinyVector<Dimension>& f_gradient, + TinyMatrix<Dimension>& f_hessian) { + 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; + + double h = sigma + std::sqrt(sigma * sigma + 4 * delta * delta); + + const double f_jr = S_norm * Sigma_norm / h; + + 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) // + + - trace(Sigma_gradient[j] * S_gradient[i]) / (h - sigma) // + + sigma / (std::pow(h - sigma, 3)) * sigma_gradient[i] * sigma_gradient[j] // + + g[j] * g[i]) * + f_jr; + } + } - { - double current_sigma_min = std::min(min(sigma_list), delta); + f += f_jr; + f_gradient += f_jr_gradient; + f_hessian += f_jr_hessian; + }; - if constexpr (multi_connection) { - size_t nb_medium_range_simplices = 0; - for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) { - const CellId cell_id = cell_list[i_cell]; - nb_medium_range_simplices += 2 * (cell_to_node_matrix[cell_id].size() - 3); - } + auto escobar = escobarSSigma; - if (nb_medium_range_simplices > 0) { - SmallArray<double> medium_range_sigma_list(nb_medium_range_simplices); + 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]; - for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) { - const CellId cell_id = cell_list[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 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 mr_alpha = 2 * std::acos(-1) / ((cell_nb_nodes - 2) * (cell_list.size())); - const double mr_sin_alpha = std::sin(mr_alpha); - const double mr_cos_alpha = std::cos(mr_alpha); + TinyVector<Dimension> sigma_gradient{trace(Sigma * S_gradient[0]), // + trace(Sigma * S_gradient[1])}; - const TinyMatrix<Dimension> mr_W{1, mr_cos_alpha, // - 0, mr_sin_alpha}; + const std::array<TinyMatrix<Dimension>, Dimension> // + Sigma_gradient{TinyMatrix<Dimension>{+0, +1, // + +0, -1}, + TinyMatrix<Dimension>{-inv_sin_alpha, -inv_tan_alpha, // + +inv_sin_alpha, +inv_tan_alpha}}; - const TinyMatrix mr_inv_W = inverse(mr_W); + escobar(sigma, S, sigma_gradient, S_gradient, Sigma_gradient, f, f_gradient, f_hessian); + } - if (print) { - std::cout << "MC === i_cell = " << i_cell << '\n'; - } + if constexpr (multi_connection) { + for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) { + const CellId cell_id = cell_list[i_cell]; + auto cell_node_list = cell_to_node_matrix[cell_list[i_cell]]; + const size_t cell_nb_nodes = cell_node_list.size(); - { // first simplex - const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]]; - const TinyVector b = old_xr[cell_node_list[(i_cell_node + 2) % cell_nb_nodes]]; + if (cell_nb_nodes <= 3) { + continue; + } - const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0], // - a[1] - x[1], b[1] - x[1]}; + const size_t i_cell_node = node_number_in_cell[i_cell]; - const TinyMatrix mr_S = mr_A * mr_inv_W; - const double mr_sigma = det(mr_S); - if (mr_sigma < current_sigma_min) { - current_sigma_min = mr_sigma; - } + const double mr_alpha = 2 * std::acos(-1) / ((cell_nb_nodes - 2) * (cell_list.size())); - if (print) { - std::cout << "MC triangle 1 ===\n"; - std::cout << "MC a = " << a << '\n'; - std::cout << "MC b = " << b << '\n'; - std::cout << "MC x = " << x << '\n'; - } - } + const double mr_sin_alpha = std::sin(mr_alpha); + const double mr_cos_alpha = std::cos(mr_alpha); + const double mr_inv_sin_alpha = 1. / mr_sin_alpha; + const double mr_inv_tan_alpha = 1. / std::tan(mr_alpha); - { // second simplex - const TinyVector a = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 2) % cell_nb_nodes]]; - const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]]; + const TinyMatrix<Dimension> mr_W{1, mr_cos_alpha, // + 0, mr_sin_alpha}; - const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0], // - a[1] - x[1], b[1] - x[1]}; + const TinyMatrix mr_inv_W = inverse(mr_W); - const TinyMatrix mr_S = mr_A * mr_inv_W; - const double mr_sigma = det(mr_S); - if (mr_sigma < current_sigma_min) { - current_sigma_min = mr_sigma; - } + std::array<TinyMatrix<Dimension>, Dimension> // + mr_S_gradient = {TinyMatrix<Dimension>{-1, (mr_cos_alpha - 1) / mr_sin_alpha, // + +0, +0}, + TinyMatrix<Dimension>{+0, +0, // + -1, (mr_cos_alpha - 1) / mr_sin_alpha}}; - if (print) { - std::cout << "MC triangle 2 ===\n"; - std::cout << "MC a = " << a << '\n'; - std::cout << "MC b = " << b << '\n'; - std::cout << "MC x = " << x << '\n'; - } - } - } - } - } + { // first simplex + const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]]; + const TinyVector b = old_xr[cell_node_list[(i_cell_node + 2) % cell_nb_nodes]]; - 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; - } + const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0], // + a[1] - x[1], b[1] - x[1]}; - if (print) { - std::cout << " ==== \n"; - std::cout << "delta = " << delta << '\n'; - } - } + const TinyMatrix mr_S = mr_A * mr_inv_W; + const double mr_sigma = det(mr_S); - auto escobarSSigma = [&frobenius, delta, // - // - &print, &node_to_cell_matrix, - &node_id](const double sigma, const TinyMatrix<Dimension>& S, - const TinyVector<Dimension>& sigma_gradient, - const std::array<TinyMatrix<Dimension>, Dimension>& S_gradient, - const std::array<TinyMatrix<Dimension>, Dimension>& Sigma_gradient, double& f, - TinyVector<Dimension>& f_gradient, TinyMatrix<Dimension>& f_hessian) { - 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); - } + const TinyMatrix<Dimension> mr_Sigma = [&mr_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(mr_S, j, i); } - return transposed_comS; - }(); + } + return transposed_comS; + }(); + + TinyVector<Dimension> mr_sigma_gradient{trace(mr_Sigma * mr_S_gradient[0]), // + trace(mr_Sigma * mr_S_gradient[1])}; - 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 std::array<TinyMatrix<Dimension>, Dimension> // + mr_Sigma_gradient{TinyMatrix<Dimension>{+0, (1 - mr_cos_alpha) / mr_sin_alpha, // + +0, -1}, + TinyMatrix<Dimension>{(mr_cos_alpha - 1) / mr_sin_alpha, +0, // + +1, +0}}; - double h = sigma + std::sqrt(sigma * sigma + 4 * delta * delta); + escobar(mr_sigma, mr_S, mr_sigma_gradient, mr_S_gradient, mr_Sigma_gradient, f, f_gradient, f_hessian); + } - const double f_jr = S_norm * Sigma_norm / h; + { // second simplex + const TinyVector a = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 2) % cell_nb_nodes]]; + const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]]; - 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 TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0], // + a[1] - x[1], b[1] - x[1]}; - const TinyVector<Dimension> f_jr_gradient = f_jr * g; + const TinyMatrix mr_S = mr_A * mr_inv_W; + const double mr_sigma = det(mr_S); - TinyMatrix<Dimension> f_jr_hessian = zero; + const TinyMatrix<Dimension> mr_Sigma = [&mr_S] { + TinyMatrix<Dimension> transposed_comS; 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) // - - - trace(Sigma_gradient[j] * S_gradient[i]) / (h - sigma) // - + sigma / (std::pow(h - sigma, 3)) * sigma_gradient[i] * sigma_gradient[j] // - + g[j] * g[i]) * - f_jr; + transposed_comS(i, j) = cofactor(mr_S, j, i); } } + return transposed_comS; + }(); - if (print) { - std::cout << "S = " << S << '\n'; - std::cout << "Sigma = " << Sigma << '\n'; + TinyVector<Dimension> mr_sigma_gradient{trace(mr_Sigma * mr_S_gradient[0]), // + trace(mr_Sigma * mr_S_gradient[1])}; - std::cout << "normS = " << S_norm << '\n'; - std::cout << "normSigma = " << Sigma_norm << '\n'; + const std::array<TinyMatrix<Dimension>, Dimension> // + mr_Sigma_gradient{TinyMatrix<Dimension>{+0, (1 - mr_cos_alpha) / mr_sin_alpha, // + +0, -1}, + TinyMatrix<Dimension>{(mr_cos_alpha - 1) / mr_sin_alpha, +0, // + +1, +0}}; - std::cout << "sigma = " << sigma << '\n'; - std::cout << "f_jr = " << f_jr << '\n'; - std::cout << "g = " << g << '\n'; - std::cout << "grad_f_jr = " << f_jr_gradient << '\n'; - std::cout << "hess_f_jr = " << f_jr_hessian << '\n'; - } + escobar(mr_sigma, mr_S, mr_sigma_gradient, mr_S_gradient, mr_Sigma_gradient, f, f_gradient, f_hessian); + } + } + } + } - f += f_jr; - f_gradient += f_jr_gradient; - f_hessian += f_jr_hessian; - }; + NodeValue<const double> + _getNodeQuality(const NodeValue<const TinyVector<Dimension>>& xr) const + { + if constexpr (Dimension == 2) { + NodeValue<double> node_quality{m_given_mesh.connectivity()}; + for (NodeId node_id = 0; node_id < m_given_mesh.numberOfNodes(); ++node_id) { + if (not m_is_fixed_node[node_id]) { + double sigma_min = std::numeric_limits<double>::max(); + double delta = sigma_min; + double f; + TinyVector<Dimension> f_gradient; + TinyMatrix<Dimension> f_hessian; + get_f_grad_hess(xr, sigma_min, delta, node_id, xr[node_id], f, f_gradient, f_hessian); + node_quality[node_id] = f; + } else { + node_quality[node_id] = 1; + } + } + return node_quality; - auto escobar = escobarSSigma; + } else { + throw NotImplementedError("dimension != 2"); + } + } - for (size_t i_cell = 0; i_cell < S_list.size(); ++i_cell) { - if (print) { - std::cout << "------ cell = " << i_cell << '\n'; - } + public: + std::shared_ptr<const ItemValueVariant> + getQuality() const + { + return std::make_shared<ItemValueVariant>(this->_getNodeQuality(m_given_mesh.xr())); + } - const double sigma = sigma_list[i_cell]; - const TinyMatrix<Dimension> S = S_list[i_cell]; + void + _getNewPositions(const NodeValue<const Rd>& old_xr, NodeValue<Rd>& new_xr) const + { + constexpr bool use_newton = false; - 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 ConnectivityType& connectivity = m_given_mesh.connectivity(); - TinyVector<Dimension> sigma_gradient{trace(Sigma * S_gradient[0]), // - trace(Sigma * S_gradient[1])}; + auto is_boundary_node = connectivity.isBoundaryNode(); + auto node_is_owned = connectivity.nodeIsOwned(); - const std::array<TinyMatrix<Dimension>, Dimension> // - Sigma_gradient{TinyMatrix<Dimension>{+0, +1, // - +0, -1}, - TinyMatrix<Dimension>{-inv_sin_alpha, -inv_tan_alpha, // - +inv_sin_alpha, +inv_tan_alpha}}; + 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(); - escobar(sigma, S, sigma_gradient, S_gradient, Sigma_gradient, f, f_gradient, f_hessian); - } + NodeValue<double> quality{connectivity}; - if constexpr (multi_connection) { - for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) { - const CellId cell_id = cell_list[i_cell]; - auto cell_node_list = cell_to_node_matrix[cell_list[i_cell]]; - const size_t cell_nb_nodes = cell_node_list.size(); + quality.fill(2); - if (print) { - std::cout << "=== MC cell " << i_cell << " ===\n"; - } + size_t maxiter = 0; - if (cell_nb_nodes <= 3) { - continue; - } + auto smooth = [this, &maxiter, &old_xr](const NodeId node_id, TinyVector<Dimension>& x) -> double { + // SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size()); - const size_t i_cell_node = node_number_in_cell[i_cell]; + double delta = 0; - const double mr_alpha = 2 * std::acos(-1) / ((cell_nb_nodes - 2) * (cell_list.size())); - if (print) { - std::cout << "=== MC mr_alpha " << mr_alpha << " ===\n"; - } - const double mr_sin_alpha = std::sin(mr_alpha); - const double mr_cos_alpha = std::cos(mr_alpha); - const double mr_inv_sin_alpha = 1. / mr_sin_alpha; - const double mr_inv_tan_alpha = 1. / std::tan(mr_alpha); + double final_f = 0; - const TinyMatrix<Dimension> mr_W{1, mr_cos_alpha, // - 0, mr_sin_alpha}; + TinyMatrix<Dimension> H = identity; - const TinyMatrix mr_inv_W = inverse(mr_W); + const ConnectivityType& connectivity = m_given_mesh.connectivity(); - std::array<TinyMatrix<Dimension>, Dimension> // - mr_S_gradient = {TinyMatrix<Dimension>{-1, (mr_cos_alpha - 1) / mr_sin_alpha, // - +0, +0}, - TinyMatrix<Dimension>{+0, +0, // - -1, (mr_cos_alpha - 1) / mr_sin_alpha}}; + for (size_t i_iter = 0; i_iter < 200; ++i_iter) { + double sigma_min = std::numeric_limits<double>::max(); + delta = sigma_min; - { // first simplex - const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]]; - const TinyVector b = old_xr[cell_node_list[(i_cell_node + 2) % cell_nb_nodes]]; + double f = 0; + TinyVector<Dimension> f_gradient = zero; + TinyMatrix<Dimension> f_hessian = zero; - const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0], // - a[1] - x[1], b[1] - x[1]}; + auto node_to_edge_matrix = connectivity.nodeToEdgeMatrix(); + auto edge_to_node_matrix = connectivity.edgeToNodeMatrix(); - const TinyMatrix mr_S = mr_A * mr_inv_W; - const double mr_sigma = det(mr_S); + get_f_grad_hess(old_xr, sigma_min, delta, node_id, x, f, f_gradient, f_hessian); - const TinyMatrix<Dimension> mr_Sigma = [&mr_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(mr_S, j, i); - } - } - return transposed_comS; - }(); + 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; + }(); - TinyVector<Dimension> mr_sigma_gradient{trace(mr_Sigma * mr_S_gradient[0]), // - trace(mr_Sigma * mr_S_gradient[1])}; + std::cout.precision(15); - const std::array<TinyMatrix<Dimension>, Dimension> // - mr_Sigma_gradient{TinyMatrix<Dimension>{+0, (1 - mr_cos_alpha) / mr_sin_alpha, // - +0, -1}, - TinyMatrix<Dimension>{(mr_cos_alpha - 1) / mr_sin_alpha, +0, // - +1, +0}}; + if constexpr (use_newton) { + const TinyVector delta_x_candidate = -inverse(f_hessian) * f_gradient; - if (print) { - std::cout << "=== MC triangle 1 ===\n"; - std::cout << " A = " << mr_A << '\n'; - std::cout << " W = " << mr_W << '\n'; - std::cout << " inv(W) = " << mr_inv_W << '\n'; + const double delta_x_norm = l2Norm(delta_x_candidate); - std::cout << " A * inv(W) = " << mr_A * mr_inv_W << '\n'; + double factor = 1; + if (delta_x_norm > 0.2 * max_edge_lenght) { + factor *= (0.2 * max_edge_lenght / delta_x_norm); + } + x += factor * delta_x_candidate; - std::cout << " mr_S = " << mr_S << '\n'; - std::cout << " mr_Sigma = " << mr_Sigma << '\n'; + if (delta_x_norm < 1E-8 * max_edge_lenght) { + break; + } + } else { + constexpr double c1 = 1e-4; + constexpr double c2 = 0.7; - std::cout << " mr_S_gradient = " << mr_S_gradient[0] << " || " << mr_S_gradient[1] << '\n'; - std::cout << " mr_Sigma_gradient = " << mr_Sigma_gradient[0] << " || " << mr_Sigma_gradient[1] - << '\n'; - } + auto zoom = [&](const TinyVector<Dimension>& x, const TinyVector<Dimension>& n, const double& f0, + const TinyVector<Dimension>& f_gradient0, const double& df0_dn, double alpha_low, + double alpha_high) -> double { + double f_low = 0; + TinyVector<Dimension> f_gradient_low = zero; + TinyMatrix<Dimension> f_hessian_low = zero; - escobar(mr_sigma, mr_S, mr_sigma_gradient, mr_S_gradient, mr_Sigma_gradient, f, f_gradient, f_hessian); - } + double init_delta = alpha_high - alpha_low; - { // second simplex - const TinyVector a = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 2) % cell_nb_nodes]]; - const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]]; + get_f_grad_hess(old_xr, sigma_min, delta, node_id, x - alpha_low * f_gradient0, f_low, f_gradient_low, + f_hessian_low); - const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0], // - a[1] - x[1], b[1] - x[1]}; + double f_high = 0; + TinyVector<Dimension> f_gradient_high = zero; + TinyMatrix<Dimension> f_hessian_high = zero; - const TinyMatrix mr_S = mr_A * mr_inv_W; - const double mr_sigma = det(mr_S); + get_f_grad_hess(old_xr, sigma_min, delta, node_id, x - alpha_high * f_gradient0, f_high, f_gradient_high, + f_hessian_high); - const TinyMatrix<Dimension> mr_Sigma = [&mr_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(mr_S, j, i); - } - } - return transposed_comS; - }(); + // std::cout << "-- f_gradient =" << f_gradient0 << " n=" << n << " df0_n=" << df0_dn << '\n'; + // std::cout << " f_gradient_low =" << f_gradient_low << " n=" << n // + // << " df_low_n=" << dot(f_gradient_low, n) << '\n'; + // std::cout << " f_gradient_high=" << f_gradient_high << " n=" << n + // << " df_high_n=" << dot(f_gradient_high, n) << '\n'; - TinyVector<Dimension> mr_sigma_gradient{trace(mr_Sigma * mr_S_gradient[0]), // - trace(mr_Sigma * mr_S_gradient[1])}; + // std::cout << "starting box [" << alpha_low << ", " << alpha_high << "] --(f)--> [" << f_low << ", " + // << f_high << "]\n"; - const std::array<TinyMatrix<Dimension>, Dimension> // - mr_Sigma_gradient{TinyMatrix<Dimension>{+0, (1 - mr_cos_alpha) / mr_sin_alpha, // - +0, -1}, - TinyMatrix<Dimension>{(mr_cos_alpha - 1) / mr_sin_alpha, +0, // - +1, +0}}; + for (size_t i = 1; i <= 200; ++i) { + // std::cout << " box: " << alpha_low << " : " << alpha_high << '\n'; + if (alpha_high - alpha_low <= 1E-5 * init_delta) { + return alpha_low; + } + double alpha_i = 0.5 * (alpha_low + alpha_high); + // std::cout << i << ": alpha_i = " << alpha_i << "[alpha_low : alpha_max] = " << alpha_low << " : " + // << alpha_high << "diff = " << alpha_high - alpha_low << '\n'; - if (print) { - std::cout << "=== MC triangle 2 ===\n"; - std::cout << " A = " << mr_A << '\n'; - std::cout << " W = " << mr_W << '\n'; - std::cout << " inv(W) = " << mr_inv_W << '\n'; + double f_i = 0; + TinyVector<Dimension> f_gradient_i = zero; + TinyMatrix<Dimension> f_hessian_i = zero; - std::cout << " A * inv(W) = " << mr_A * mr_inv_W << '\n'; + get_f_grad_hess(old_xr, sigma_min, delta, node_id, x - alpha_i * f_gradient0, f_i, f_gradient_i, + f_hessian_i); - std::cout << " mr_S = " << mr_S << '\n'; - std::cout << " mr_Sigma = " << mr_Sigma << '\n'; + // std::cout << " f_gradient_low =" << f_gradient_low << " n=" << n // + // << " df_low_n=" << dot(f_gradient_low, n) << '\n'; - std::cout << " mr_S_gradient = " << mr_S_gradient[0] << " || " << mr_S_gradient[1] << '\n'; - std::cout << " mr_Sigma_gradient = " << mr_Sigma_gradient[0] << " || " << mr_Sigma_gradient[1] - << '\n'; - } + // std::cout << "f0 = " << f0 << " f_i = " << f_i << '\n'; - escobar(mr_sigma, mr_S, mr_sigma_gradient, mr_S_gradient, mr_Sigma_gradient, f, f_gradient, f_hessian); - } - } - } + if ((f_i > f0 + c1 * alpha_i * df0_dn) or (f_i >= f_low)) { + alpha_high = alpha_i; + f_high = f_i; + } else { + double dfi_dn = dot(f_gradient_i, n); + if (std::abs(dfi_dn) <= -c2 * df0_dn) { + return alpha_i; + } + if (dfi_dn * (alpha_high - alpha_low) >= 0) { + alpha_high = alpha_low; + f_high = f_low; + } - 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; + alpha_low = alpha_i; + f_low = f_i; + } + } + throw UnexpectedError("zoom did not converge!"); + }; + + auto line_search = [&](const double& f0, const TinyVector<Dimension>& f_gradient0, + const double alpha_max) -> double { + double alpha_i_1 = 0; + double alpha_i = alpha_max; + + const TinyVector<Dimension> n = -1 / l2Norm(f_gradient0) * f_gradient0; + + const double df0_dn = dot(f_gradient0, n); + + double f_i_1 = f0; + + double f_i = 0; + TinyVector<Dimension> f_gradient_i = zero; + TinyMatrix<Dimension> f_hessian_i = zero; + + for (size_t i = 1; i <= 100; ++i) { + get_f_grad_hess(old_xr, sigma_min, delta, node_id, x - alpha_i * f_gradient0, f_i, f_gradient_i, + f_hessian_i); + + const double dfi_dn = dot(f_gradient_i, n); + + // std::cout << "=== i=" << i << " alpha_i=" << alpha_i << " alpha_i_1=" << alpha_i_1 << " f0=" << f0 + // << " f_i=" << f_i << " f_i_1=" << f_i_1 << '\n'; + if ((f_i > f0 + c1 * alpha_i * df0_dn) or ((f_i >= f_i_1) and (i > 1))) { + // std::cout << "case 1: zoom[" << alpha_i << "," << alpha_i_1 << "] f_i=" << f_i << " f0=" << f0 + // << '\n'; + return zoom(x, n, f0, f_gradient0, df0_dn, alpha_i_1, alpha_i); + } else if (std::abs(dfi_dn) <= -c2 * df0_dn) { + // std::cout << "case 2: return alpha_i=" << alpha_i << '\n'; + return alpha_i; + } else if (dfi_dn >= 0) { + // std::cout << "case 3: zoom[" << alpha_i << "," << alpha_i_1 << "] f_i=" << f_i << " f0=" << f0 + // << '\n'; + return zoom(x, n, f0, f_gradient0, df0_dn, alpha_i, alpha_i_1); + } else { + alpha_i_1 = alpha_i; + f_i_1 = f_i; + alpha_i = 0.5 * (alpha_i + alpha_max); + } } + throw UnexpectedError("line search failed"); + }; + + if (l2Norm(f_gradient) < 1e-12 * f) { + continue; } - return max_lenght; - }(); - const TinyVector delta_x_candidate = -inverse(f_hessian) * f_gradient; + const double alpha_max = std::min(0.5 * max_edge_lenght / l2Norm(f_gradient), 1.); + // std::cout << rang::fgB::yellow << "i_iter=" << i_iter << rang::fg::reset << '\n'; + double alpha = line_search(f, f_gradient, alpha_max); - const double delta_x_norm = l2Norm(delta_x_candidate); + const TinyVector delta_x = -alpha * H * f_gradient; - if (print) { - std::cout << "==============\n"; - std::cout << "i_iter = " << i_iter << '\n'; - std::cout << "f_gradient = " << f_gradient << '\n'; - std::cout << "f_hessian = " << f_hessian << '\n'; - std::cout << "inv(f_hes) = " << inverse(f_hessian) << '\n'; - std::cout << "delta_x_candidate= " << delta_x_candidate << '\n'; - } + double new_f = 0; + TinyVector<Dimension> new_f_grad = zero; + get_f_grad_hess(old_xr, sigma_min, delta, node_id, x + delta_x, new_f, new_f_grad, f_hessian); - double factor = 1; - if (delta_x_norm > 0.2 * max_edge_lenght) { - factor *= (0.2 * max_edge_lenght / delta_x_norm); - } - x += factor * delta_x_candidate; + x += delta_x; - if (print) { - std::cout << "factor = " << factor << '\n'; - std::cout << "delta_x = " << factor * delta_x_candidate << '\n'; - } - if (l2Norm(f_gradient) < 1E-12) { - break; + // std::cout << "x -> " << x << '\n'; + // std::cout << rang::fgB::red << "f(x)=" << f << " f(x+dx)=" << new_f << " delta = " << delta_x + // << rang::fg::reset << '\n'; + // std::cout << rang::fgB::magenta << "l2Norm(delta_x) = " << l2Norm(delta_x) << rang::fg::reset << '\n'; + + if (l2Norm(delta_x) < 1E-8 * max_edge_lenght) { + break; + } else { + if (((i_iter + 1) % 5) == 0) { + H = identity; + } else { + TinyVector y = new_f_grad - f_gradient; + // std::cout << "l2Norm(y) = " << l2Norm(y) << '\n'; + const double rho = 1 / dot(delta_x, y); + + constexpr TinyMatrix<Dimension> I = identity; + + H = (I - rho * tensorProduct(delta_x, y)) * H * (I - rho * tensorProduct(y, delta_x)) + + rho * tensorProduct(delta_x, delta_x); + } + } } - if (delta_x_norm < 1E-8 * max_edge_lenght) { + if (l2Norm(f_gradient) < 1E-12) { break; } final_f = f; } + // std::exit(0); return final_f; }; @@ -954,6 +1053,9 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar for (NodeId node_id = 0; node_id < connectivity.numberOfNodes(); ++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]); + new_xr[node_id] = 0.5 * (new_xr[node_id] + old_xr[node_id]); + // TinyVector<Dimension> x0{2, 3}; + // quality[node_id] = smooth(node_id, x0); count++; } } @@ -975,15 +1077,21 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar do { std::cout << "- convexfication step " << i_convexify++ << '\n'; NodeValue<Rd> new_xr = copy(xr); - this->_getNewPositions(xr, new_xr); - xr = new_xr; + + auto node_quality = this->_getNodeQuality(xr); + + bool need_smooth = max(node_quality) > 50; + auto non_convex_cell_tag = this->_getNonConvexCellTag(xr); nb_non_convex_cells = sum(non_convex_cell_tag); + std::cout << " remaining non convex cells: " << nb_non_convex_cells << '\n'; if (nb_non_convex_cells > 0) { m_is_smoothed_node = _getSmoothedNode(non_convex_cell_tag, m_is_fixed_node); + this->_getNewPositions(xr, new_xr); + xr = new_xr; } - } while ((nb_non_convex_cells > 0) and (i_convexify < 0)); + } while ((nb_non_convex_cells > 0) and (i_convexify < 10000)); return std::make_shared<MeshType>(m_given_mesh.shared_connectivity(), xr); }