From 3f8eb9030bd03bee8d2374060b1f22d359181813 Mon Sep 17 00:00:00 2001 From: federico Date: Mon, 26 Oct 2026 10:34:25 -0400 Subject: [PATCH] prototype of a ESP improvement/fix --- python/src/barrier/barrier.cpp | 7 ++ python/src/potentials/barrier_potential.cpp | 1 + src/ipc/esp/esp_local_transfer.cpp | 27 ++++-- src/ipc/esp/esp_parameters.hpp | 4 +- src/ipc/esp/esp_potential.cpp | 35 +++++++- src/ipc/esp/esp_tangential_collisions.cpp | 4 +- src/ipc/gcp/distance/mollifier.cpp | 32 +++++++ src/ipc/gcp/distance/mollifier.tpp | 45 +++++++++- src/ipc/gcp/distance/mollifier_config.hpp | 94 +++++++++++++++++++++ 9 files changed, 236 insertions(+), 13 deletions(-) create mode 100644 src/ipc/gcp/distance/mollifier_config.hpp diff --git a/python/src/barrier/barrier.cpp b/python/src/barrier/barrier.cpp index d6abf56da..2d06642f6 100644 --- a/python/src/barrier/barrier.cpp +++ b/python/src/barrier/barrier.cpp @@ -107,6 +107,13 @@ void define_barrier(py::module_& m) )ipc_Qu8mg5v7") .def(py::init()); + // PROTOTYPE: needed to test the per-barrier mollifier power from Python. + py::class_< + InversePowerBarrier<>, Barrier, std::shared_ptr>>( + m, "InversePowerBarrier", "Inverse power barrier h(d) / d^p.") + .def(py::init(), py::arg("power")) + .def_property_readonly("power", &InversePowerBarrier<>::power); + py::class_, Barrier, std::shared_ptr>>( m, "CubicBarrier", R"ipc_Qu8mg5v7( Cubic barrier function from [Ando 2024]. diff --git a/python/src/potentials/barrier_potential.cpp b/python/src/potentials/barrier_potential.cpp index 763246559..a445ceb3a 100644 --- a/python/src/potentials/barrier_potential.cpp +++ b/python/src/potentials/barrier_potential.cpp @@ -277,6 +277,7 @@ void define_esp_potential(py::module& m) .def_readonly("quad_order", &ESPParameters::quad_order) .def_readonly("area_weights", &ESPParameters::area_weights) .def_readwrite("local_quadrature_transfer", &ESPParameters::local_quadrature_transfer) + .def_readwrite("barrier", &ESPParameters::barrier) // PROTOTYPE .def_readonly("integration_type", &ESPParameters::integration_type); py::class_(m, "ESPPotential") diff --git a/src/ipc/esp/esp_local_transfer.cpp b/src/ipc/esp/esp_local_transfer.cpp index cc5d9bf98..0286923a2 100644 --- a/src/ipc/esp/esp_local_transfer.cpp +++ b/src/ipc/esp/esp_local_transfer.cpp @@ -78,7 +78,15 @@ namespace { struct Element { std::vector vertices, edges; double weight; + double ee_scale = 1.0; // PROTOTYPE: area-consistent EE weight factor }; + // PROTOTYPE: reference length for area-consistent EE weights (0 = off). + double area_ref(const ESPParameters& p) + { + const auto& cfg = ee_mollifier_config(); + return cfg.area_ref_dbar ? p.dbar : cfg.area_ref; + } + Element element(const CollisionMesh& mesh, const ESPParameters& p, size_t k) { Element out; @@ -88,11 +96,15 @@ namespace { out.edges.push_back(mesh.faces_to_edges()(k, i)); } out.weight = p.area_weights ? mesh.face_areas()(k) / 9.0 : 1.0; + out.ee_scale = ee_area_scale_factor( + mesh.face_areas()(k) / 3.0, area_ref(p), /*face=*/true); } else if ((k -= mesh.num_faces()) < mesh.num_codim_edges()) { const auto e = mesh.codim_edges()[k]; out.vertices = { mesh.edges()(e, 0), mesh.edges()(e, 1) }; out.edges = { e }; out.weight = p.area_weights ? mesh.edge_area(e) / 4.0 : 1.0; + out.ee_scale = ee_area_scale_factor( + mesh.edge_area(e) / 2.0, area_ref(p), /*face=*/false); } else { const auto v = mesh.codim_vertices()[k - mesh.num_codim_edges()]; out.vertices = { v }; @@ -231,12 +243,16 @@ namespace { if (const auto* b = dynamic_cast*>( params.barrier.get())) power = std::max(1, int(std::lround(b->power())) + 1); + if (ee_mollifier_config().order_override > 0) // PROTOTYPE + power = ee_mollifier_config().order_override; for (const auto& pair : pairs) { - const std::array prim = { mesh.edges()(pair.ea, 0), - mesh.edges()(pair.ea, 1), - mesh.edges()(pair.eb, 0), - mesh.edges()(pair.eb, 1) }; + const std::array prim = { { + mesh.edges()(pair.ea, 0), + mesh.edges()(pair.ea, 1), + mesh.edges()(pair.eb, 0), + mesh.edges()(pair.eb, 1), + } }; Eigen::Matrix positions; if constexpr (Order == 0) { for (int i = 0; i < 4; ++i) @@ -257,7 +273,7 @@ namespace { X.row(prim[0]), X.row(prim[1]), X.row(prim[2]), X.row(prim[3]), value(d2)); T m = - Math::cubic_spline(sqrt(d2) / params.ee_support()) * 1.5 + Math::cubic_spline(sqrt(d2) / params.ee_support()) * ee_mollifier_config().ee_weight /* PROTOTYPE */ * edge_edge_mollifier( positions.row(0).transpose(), positions.row(1).transpose(), positions.row(2).transpose(), positions.row(3).transpose(), @@ -265,6 +281,7 @@ namespace { const T initial = m; for (int k = 1; k < power; ++k) m = m * initial; + m = m * elem.ee_scale; // PROTOTYPE const Eigen::Vector3 q = (positions.row(0) + u * (positions.row(1) - positions.row(0))) .transpose(); diff --git a/src/ipc/esp/esp_parameters.hpp b/src/ipc/esp/esp_parameters.hpp index dedc9f3d9..abc3f8171 100644 --- a/src/ipc/esp/esp_parameters.hpp +++ b/src/ipc/esp/esp_parameters.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -95,7 +96,8 @@ struct ESPParameters { /// Requires use_near_far=true and the default vertex quadrature rule. /// Currently supports frictionless contact only. /// Has no effect in 2D or when dbar_factor=0. Disabled by default. - bool local_quadrature_transfer = false; + bool local_quadrature_transfer = + ee_mollifier_config().local_transfer_default; // PROTOTYPE env default const IntegrationType integration_type; /// @brief Near/far split parameter dbar / dhat, in [0, 2]. diff --git a/src/ipc/esp/esp_potential.cpp b/src/ipc/esp/esp_potential.cpp index bdcf36ee1..71dacc940 100644 --- a/src/ipc/esp/esp_potential.cpp +++ b/src/ipc/esp/esp_potential.cpp @@ -31,6 +31,9 @@ namespace { // - InversePowerBarrier(p): order = round(p) + 1, so p=1 -> 2, p=2 -> 3. int mollifier_order_for_barrier(const std::shared_ptr& barrier) { + if (ee_mollifier_config().order_override > 0) { // PROTOTYPE + return ee_mollifier_config().order_override; + } if (const auto* ip = dynamic_cast*>(barrier.get())) { const int p = static_cast(std::lround(ip->power())); @@ -125,6 +128,26 @@ namespace { elem.weight = params.area_weights ? mesh.vertex_area(v) : 1.; return elem; } + + // PROTOTYPE: make an EE sample stand for about one r x r patch (length r + // on rods) instead of a fixed fraction of its element. Rest geometry only. + double ee_area_scale( + const CollisionMesh& mesh, + const IntegrationElement& elem, + const ESPParameters& params) + { + const auto& cfg = ee_mollifier_config(); + const double r = cfg.area_ref_dbar ? params.dbar : cfg.area_ref; + if (elem.face >= 0) { + return ee_area_scale_factor( + mesh.face_areas()(elem.face) / 3.0, r, /*face=*/true); + } + if (elem.num_edges == 1) { + return ee_area_scale_factor( + mesh.edge_area(elem.edges[0]) / 2.0, r, /*face=*/false); + } + return 1.0; + } } // namespace double ESPPotential::operator()( @@ -275,7 +298,7 @@ double ESPPotential::operator()( double mollifier = Math::cubic_spline( dist / m_params.ee_support()) - * 1.5; + * ee_mollifier_config().ee_weight /* PROTOTYPE */; mollifier *= edge_edge_mollifier( X.row(ea).transpose(), X.row(eb).transpose(), @@ -285,6 +308,8 @@ double ESPPotential::operator()( mollifier, mollifier_order_for_barrier( m_params.barrier)); + mollifier = mollifier + * ee_area_scale(mesh, elem, m_params); // PROTOTYPE if (use_nf) { const double p_near = PointPotentialHelper:: @@ -593,7 +618,7 @@ Eigen::VectorXd ESPPotential::gradient( T mollifier = Math::cubic_spline( dist / m_params.ee_support()) - * 1.5; + * ee_mollifier_config().ee_weight /* PROTOTYPE */; mollifier *= edge_edge_mollifier( positionsT.row(0).transpose(), positionsT.row(1).transpose(), @@ -604,6 +629,8 @@ Eigen::VectorXd ESPPotential::gradient( mollifier, mollifier_order_for_barrier( m_params.barrier)); + mollifier = mollifier + * ee_area_scale(mesh, elem, m_params); // PROTOTYPE const ESPCollisionDict& dict = *(iter->second); @@ -1022,7 +1049,7 @@ Eigen::SparseMatrix ESPPotential::hessian( T mollifier = Math::cubic_spline( dist / m_params.ee_support()) - * 1.5; + * ee_mollifier_config().ee_weight /* PROTOTYPE */; mollifier *= edge_edge_mollifier( positionsT.row(0).transpose(), positionsT.row(1).transpose(), @@ -1033,6 +1060,8 @@ Eigen::SparseMatrix ESPPotential::hessian( mollifier, mollifier_order_for_barrier( m_params.barrier)); + mollifier = mollifier + * ee_area_scale(mesh, elem, m_params); // PROTOTYPE const ESPCollisionDict& dict = *(iter->second); diff --git a/src/ipc/esp/esp_tangential_collisions.cpp b/src/ipc/esp/esp_tangential_collisions.cpp index 8177622bb..b98cabf1e 100644 --- a/src/ipc/esp/esp_tangential_collisions.cpp +++ b/src/ipc/esp/esp_tangential_collisions.cpp @@ -305,7 +305,7 @@ void TangentialCollisions::build( vertices.row(e11).transpose(), dist_sqr); double mol = Math::cubic_spline(dist / params.ee_support()) - * 1.5; + * ee_mollifier_config().ee_weight /* PROTOTYPE */; mol *= edge_edge_mollifier( vertices.row(e00).transpose(), vertices.row(e01).transpose(), @@ -461,7 +461,7 @@ void TangentialCollisions::build( vertices.row(e10).transpose(), vertices.row(e11).transpose(), dist_sqr_ee); double mollifier = - Math::cubic_spline(dist_ee / params.ee_support()) * 1.5; + Math::cubic_spline(dist_ee / params.ee_support()) * ee_mollifier_config().ee_weight /* PROTOTYPE */; mollifier *= edge_edge_mollifier( vertices.row(e00).transpose(), vertices.row(e01).transpose(), vertices.row(e10).transpose(), vertices.row(e11).transpose(), diff --git a/src/ipc/gcp/distance/mollifier.cpp b/src/ipc/gcp/distance/mollifier.cpp index e7485b23c..51a524663 100644 --- a/src/ipc/gcp/distance/mollifier.cpp +++ b/src/ipc/gcp/distance/mollifier.cpp @@ -296,6 +296,38 @@ std::array edge_edge_mollifier_type( const double dist_sqr) { std::array mtypes {}; + if (ee_mollifier_config().smoothstep) { + const double ell2 = ee_mollifier_config().rel_width + ? dist_sqr * MOLLIFIER_THRESHOLD_EPS + : ee_mollifier_config().sqrt_width + ? std::sqrt(dist_sqr) * MOLLIFIER_THRESHOLD_EPS + * ee_mollifier_config().L + : sqr(ee_mollifier_config().ell); + const auto t = [&](const double pe) { + return pe - dist_sqr >= ell2 ? HeavisideType::ONE + : HeavisideType::VARIANT; + }; + mtypes[0] = t(point_edge_distance(ea0, eb0, eb1)); + mtypes[1] = t(point_edge_distance(ea1, eb0, eb1)); + mtypes[2] = t(point_edge_distance(eb0, ea0, ea1)); + mtypes[3] = t(point_edge_distance(eb1, ea0, ea1)); + return mtypes; + } + if (ee_mollifier_config().fixed || ee_mollifier_config().sqrt_width) { + const double denom = ee_mollifier_config().fixed + ? sqr(ee_mollifier_config().ell) + : std::sqrt(dist_sqr) * MOLLIFIER_THRESHOLD_EPS + * ee_mollifier_config().L; + const auto t = [&](const double pe) { + return pe - dist_sqr >= denom ? HeavisideType::ONE + : HeavisideType::VARIANT; + }; + mtypes[0] = t(point_edge_distance(ea0, eb0, eb1)); + mtypes[1] = t(point_edge_distance(ea1, eb0, eb1)); + mtypes[2] = t(point_edge_distance(eb0, ea0, ea1)); + mtypes[3] = t(point_edge_distance(eb1, ea0, ea1)); + return mtypes; + } mtypes[0] = (point_edge_distance(ea0, eb0, eb1) - dist_sqr) >= dist_sqr * MOLLIFIER_THRESHOLD_EPS ? HeavisideType::ONE diff --git a/src/ipc/gcp/distance/mollifier.tpp b/src/ipc/gcp/distance/mollifier.tpp index 353b9e2e9..5a8b35d2b 100644 --- a/src/ipc/gcp/distance/mollifier.tpp +++ b/src/ipc/gcp/distance/mollifier.tpp @@ -3,6 +3,7 @@ #include "mollifier.hpp" #include +#include namespace ipc { template @@ -28,8 +29,48 @@ scalar edge_edge_mollifier( const std::array& mtypes, const scalar& dist_sqr) { - const scalar da = dist_sqr * MOLLIFIER_THRESHOLD_EPS; - const scalar db = dist_sqr * MOLLIFIER_THRESHOLD_EPS; + if (ee_mollifier_config().smoothstep) { + // PROTOTYPE: H(c / l) with H(t) = 10t^3 - 15t^4 + 6t^5 (C2), where + // c = sqrt(d_PE^2 - d^2) is the endpoint clearance and l is fixed. + using std::sqrt; + const scalar ell2 = ee_mollifier_config().rel_width + ? scalar(dist_sqr * MOLLIFIER_THRESHOLD_EPS) + : ee_mollifier_config().sqrt_width + ? scalar(sqrt(dist_sqr) + * (MOLLIFIER_THRESHOLD_EPS * ee_mollifier_config().L)) + : scalar(ee_mollifier_config().ell * ee_mollifier_config().ell); + const auto H = [&](const scalar& c2) -> scalar { + if (c2 <= 0) { + return scalar(0.); + } + if (c2 >= ell2) { + return scalar(1.); + } + const scalar t = sqrt(c2 / ell2); + return t * t * t * (10. + t * (-15. + 6. * t)); + }; + using PE = PointEdgeDistance; + scalar w(1.); + if (mtypes[0] == HeavisideType::VARIANT) + w = w * H(PE::point_edge_sqr_distance(ea0, eb0, eb1) - dist_sqr); + if (mtypes[1] == HeavisideType::VARIANT) + w = w * H(PE::point_edge_sqr_distance(ea1, eb0, eb1) - dist_sqr); + if (mtypes[2] == HeavisideType::VARIANT) + w = w * H(PE::point_edge_sqr_distance(eb0, ea0, ea1) - dist_sqr); + if (mtypes[3] == HeavisideType::VARIANT) + w = w * H(PE::point_edge_sqr_distance(eb1, ea0, ea1) - dist_sqr); + return w; + } + scalar denom = dist_sqr * MOLLIFIER_THRESHOLD_EPS; + if (ee_mollifier_config().fixed) { // PROTOTYPE + denom = scalar(ee_mollifier_config().ell * ee_mollifier_config().ell); + } else if (ee_mollifier_config().sqrt_width) { // PROTOTYPE + using std::sqrt; + denom = sqrt(dist_sqr) + * (MOLLIFIER_THRESHOLD_EPS * ee_mollifier_config().L); + } + const scalar da = denom; + const scalar db = denom; scalar a = (mtypes[0] == HeavisideType::VARIANT) ? Math::mollifier( (PointEdgeDistance::point_edge_sqr_distance( diff --git a/src/ipc/gcp/distance/mollifier_config.hpp b/src/ipc/gcp/distance/mollifier_config.hpp new file mode 100644 index 000000000..970afe3f2 --- /dev/null +++ b/src/ipc/gcp/distance/mollifier_config.hpp @@ -0,0 +1,94 @@ +// PROTOTYPE ONLY: runtime switches for comparing edge-edge mollifiers. +#pragma once + +#include +#include +#include + +namespace ipc { + +/// Read once per process from the environment: +/// ESP_EE_MOLLIFIER=gcp|smoothstep|gcp-fixed|gcp-sqrt (default gcp) +/// gcp x = c^2 / (eps d^2) current (width sqrt(eps) d) +/// gcp-fixed x = c^2 / l^2 same shape, fixed width l +/// gcp-sqrt x = c^2 / (eps d L) same shape, width sqrt(eps d L) +/// smoothstep H(c / l), H = 10t^3 - 15t^4 + 6t^5 +/// smoothstep-sqrt H(c / w), w^2 = eps d L +/// smoothstep-rel H(c / w), w^2 = eps d^2 (current width, C2 shape) +/// with c^2 = d_PE^2 - d^2 the endpoint clearance. +/// ESP_EE_MOLLIFIER_ELL= (fixed width l, default 150e-6) +/// ESP_EE_MOLLIFIER_L= (gcp-sqrt length L, default 1.5e-3) +/// ESP_MOLLIFIER_ORDER= (override the per-barrier power; 0 = keep) +/// ESP_EE_AREA_REF=|dbar (0 = off: scale each EE weight by the +/// smooth min (1 + (a/rho)^2)^(-1/2), with +/// a = rest face area / 3, rho = r^2 (faces) +/// a = rest rod length / 2, rho = r (rods); +/// "dbar" uses each ESPParameters' dbar) +/// ESP_LOCAL_TRANSFER=1 (default for +/// ESPParameters::local_quadrature_transfer) +/// ESP_EE_WEIGHT= (EE sample weight factor, default 1.5: +/// 1.5 * cubic_spline(0) = 1 vertex sample) +struct EEMollifierConfig { + bool smoothstep = false; + bool fixed = false; // gcp-fixed + bool sqrt_width = false; // gcp-sqrt, smoothstep-sqrt + bool rel_width = false; // smoothstep-rel + double ell = 150e-6; + double L = 1.5e-3; + int order_override = 0; + double ee_weight = 1.5; + double area_ref = 0; + bool area_ref_dbar = false; // ESP_EE_AREA_REF=dbar + bool local_transfer_default = false; +}; + +/// Area-consistent EE weight factor for an element: smooth min(1, rho / a). +/// `a` is the rest area (faces: A/3) or length (rods: L/2) per vertex sample, +/// `rho` is r^2 (faces) or r (rods). Returns 1 when the feature is off. +inline double ee_area_scale_factor(double a, double r, bool face) +{ + if (r <= 0) { + return 1.0; + } + const double t = a / (face ? r * r : r); + return 1.0 / std::sqrt(1.0 + t * t); +} + +inline const EEMollifierConfig& ee_mollifier_config() +{ + static const EEMollifierConfig config = [] { + EEMollifierConfig c; + if (const char* s = std::getenv("ESP_EE_MOLLIFIER")) { + c.smoothstep = std::strcmp(s, "smoothstep") == 0 + || std::strcmp(s, "smoothstep-sqrt") == 0 + || std::strcmp(s, "smoothstep-rel") == 0; + c.rel_width = std::strcmp(s, "smoothstep-rel") == 0; + c.fixed = std::strcmp(s, "gcp-fixed") == 0; + c.sqrt_width = std::strcmp(s, "gcp-sqrt") == 0 + || std::strcmp(s, "smoothstep-sqrt") == 0; + } + if (const char* s = std::getenv("ESP_EE_MOLLIFIER_L")) { + c.L = std::atof(s); + } + if (const char* s = std::getenv("ESP_EE_MOLLIFIER_ELL")) { + c.ell = std::atof(s); + } + if (const char* s = std::getenv("ESP_EE_AREA_REF")) { + c.area_ref_dbar = std::strcmp(s, "dbar") == 0; + c.area_ref = c.area_ref_dbar ? 0.0 : std::atof(s); + } + if (const char* s = std::getenv("ESP_LOCAL_TRANSFER")) { + c.local_transfer_default = std::strcmp(s, "1") == 0; + } + if (const char* s = std::getenv("ESP_EE_WEIGHT")) { + c.ee_weight = std::atof(s); + } + if (const char* s = std::getenv("ESP_MOLLIFIER_ORDER")) { + c.order_override = std::atoi(s); + } + return c; + }(); + return config; +} + +} // namespace ipc