diff --git a/CMakeLists.txt b/CMakeLists.txt index 7f6c06c5d..3f190f539 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -324,6 +324,10 @@ if(IPC_TOOLKIT_WITH_FILIB) target_link_libraries(ipc_toolkit PUBLIC filib::filib) endif() +# Exact arithmetic (geogram's MultiPrecision PSM) +include(geogram_multi_precision) +target_link_libraries(ipc_toolkit PRIVATE geogram::multi_precision) + # Block-accelerated Hessian assembly if(IPC_TOOLKIT_WITH_MESHFEM_SPARSE) include(meshfem_sparse) diff --git a/cmake/find/FindSIMD.cmake b/cmake/find/FindSIMD.cmake index 11db9a522..8bb70ebc4 100644 --- a/cmake/find/FindSIMD.cmake +++ b/cmake/find/FindSIMD.cmake @@ -339,10 +339,10 @@ function(test_avx_availability) elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") set(AVX_FLAGS "-xHost") elseif(MSVC) + # /arch: is single-valued on MSVC: emitting both downgrades to AVX. if(DETECTED_AVX_20) SET(AVX_FLAGS "${AVX_FLAGS} /arch:AVX2") - endif() - if(DETECTED_AVX_10) + elseif(DETECTED_AVX_10) SET(AVX_FLAGS "${AVX_FLAGS} /arch:AVX") endif() endif() diff --git a/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake b/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake index 9c53bc956..b378828d8 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake @@ -30,7 +30,7 @@ else() SOURCE_DIR ${IPC_TOOLKIT_TESTS_DATA_DIR} GIT_REPOSITORY https://github.com/ipc-sim/ipc-toolkit-tests-data.git - GIT_TAG c7eba549d9a80d15569a013c473f0aff104ac44a + GIT_TAG 05696a21b8662f6cd12f8469eeb35ee9a5cc0493 CONFIGURE_COMMAND "" BUILD_COMMAND "" diff --git a/cmake/recipes/geogram_multi_precision.cmake b/cmake/recipes/geogram_multi_precision.cmake new file mode 100644 index 000000000..6b0f79c76 --- /dev/null +++ b/cmake/recipes/geogram_multi_precision.cmake @@ -0,0 +1,90 @@ +# Geogram's MultiPrecision Pluggable Software Module +# (https://github.com/BrunoLevy/geogram) +# License: BSD 3-Clause License +if(TARGET geogram::multi_precision) + return() +endif() + +message(STATUS "Third-party: creating target 'geogram::multi_precision'") + +# The PSM is a standalone extract of geogram's exact arithmetic +# (expansion_nt), so we download its release archive instead of building all +# of geogram. +set(GEOGRAM_MULTI_PRECISION_VERSION "1.10.1") + +include(CPM) +CPMAddPackage( + NAME geogram_multi_precision + URL "https://github.com/BrunoLevy/geogram/releases/download/v${GEOGRAM_MULTI_PRECISION_VERSION}/MultiPrecision_psm_${GEOGRAM_MULTI_PRECISION_VERSION}.zip" + URL_HASH SHA256=1c4fa0ef6422547f08b11891a0f934b90a2bc91359dcfaeb89a490b3878c0e18 + DOWNLOAD_ONLY YES +) + +# The archive also ships an optional CGAL example that carries an LGPL-3.0 +# header. We never build it, so we delete it to keep the build tree free of +# non-permissive files. +file(REMOVE_RECURSE "${geogram_multi_precision_SOURCE_DIR}/CGAL") + +# Downstream projects (e.g., PolyFEM) may link their own full copy of geogram, +# which defines the same GEO:: symbols as the PSM. Linking both would either +# fail with duplicate symbols (static) or silently mix two geogram versions +# (shared). Instead, we compile a copy of the PSM with its namespace renamed +# from GEO to ipc_geogram, so the two can never collide. We write the copy to +# the build tree so a shared CPM source cache is never modified. +set(GEOGRAM_MULTI_PRECISION_DIR + "${CMAKE_CURRENT_BINARY_DIR}/geogram_multi_precision") +foreach(file IN ITEMS MultiPrecision_psm.h MultiPrecision_psm.cpp) + file(READ "${geogram_multi_precision_SOURCE_DIR}/${file}" content) + # GEO only appears as the namespace name ("namespace GEO", "GEO::", and + # "using namespace GEO"). Matching it as a whole word leaves macros such + # as GEO_OS_WINDOWS untouched. + string(REGEX REPLACE "([^A-Za-z0-9_])GEO([^A-Za-z0-9_])" + "\\1ipc_geogram\\2" content "${content}") + # If a future PSM spells the namespace in a way the pattern above misses, + # the copy would silently keep GEO:: symbols and could clash again, so we + # fail loudly instead. + string(REGEX MATCH "(^|[^A-Za-z0-9_])GEO([^A-Za-z0-9_]|$)" + leftover "${content}") + if(leftover) + message(FATAL_ERROR + "Failed to rename namespace GEO to ipc_geogram in ${file}.") + endif() + file(WRITE "${GEOGRAM_MULTI_PRECISION_DIR}/tmp/${file}" "${content}") + # Copy only when the content changed, so we don't trigger rebuilds. + configure_file("${GEOGRAM_MULTI_PRECISION_DIR}/tmp/${file}" + "${GEOGRAM_MULTI_PRECISION_DIR}/${file}" COPYONLY) +endforeach() + +add_library(geogram_multi_precision STATIC + "${GEOGRAM_MULTI_PRECISION_DIR}/MultiPrecision_psm.cpp" +) +add_library(geogram::multi_precision ALIAS geogram_multi_precision) + +target_include_directories(geogram_multi_precision SYSTEM PUBLIC + "$" +) + +# The PSM header assumes geogram is a shared library unless told otherwise. On +# MSVC that marks every symbol __declspec(dllimport), which breaks linking +# against our static build, so we define GEO_STATIC_LIBS for the library and +# everything that includes its header. +target_compile_definitions(geogram_multi_precision PUBLIC GEO_STATIC_LIBS) + +target_compile_features(geogram_multi_precision PUBLIC cxx_std_17) +set_target_properties(geogram_multi_precision PROPERTIES + POSITION_INDEPENDENT_CODE ON +) + +# The expansion arithmetic relies on error-free transforms, which break if the +# compiler fuses a*b+c into an FMA, so we turn off contraction. We also silence +# the third-party warnings. +if(MSVC) + target_compile_options(geogram_multi_precision PRIVATE "/fp:precise" "/w") +else() + target_compile_options(geogram_multi_precision PRIVATE + "-ffp-contract=off" "-w" + ) +endif() + +# Folder name for IDE +set_target_properties(geogram_multi_precision PROPERTIES FOLDER "ThirdParty") \ No newline at end of file diff --git a/docs/source/_static/graphviz/dependencies.dot b/docs/source/_static/graphviz/dependencies.dot index f76aba605..c516dab3d 100644 --- a/docs/source/_static/graphviz/dependencies.dot +++ b/docs/source/_static/graphviz/dependencies.dot @@ -98,4 +98,6 @@ digraph "IPC Toolkit Dependencies" { // MeshFEMSparse -> Eigen3_Eigen "node17" -> "node9" [color = "#8FB976";]; // MeshFEMSparse -> tbb + "node18" [label = "geogram_multi_precision\n(geogram::multi_precision)";shape = box;style = "rounded,filled";fillcolor = "#D5E8D4";color = "#8FB976";]; + "node5" -> "node18" [color = "#BE6562";]; } \ No newline at end of file diff --git a/docs/source/_static/graphviz/dependencies.svg b/docs/source/_static/graphviz/dependencies.svg index 6e9d89f80..17eb7de7e 100644 --- a/docs/source/_static/graphviz/dependencies.svg +++ b/docs/source/_static/graphviz/dependencies.svg @@ -4,8 +4,8 @@ - + IPC Toolkit Dependencies @@ -16,87 +16,87 @@ legendNode0 - -Static Library + +Static Library legendNode1 - -Shared Library + +Shared Library legendNode0->legendNode1 - - -Public + + +Public legendNode2 - -Interface Library + +Interface Library legendNode1->legendNode2 - - -Private + + +Private legendNode2->legendNode0 - - -Interface + + +Interface legendNode3 - -Optional Dependency + +Optional Dependency legendNode2->legendNode3 - - -Optional + + +Optional node5 - -ipc_toolkit -(ipc::toolkit) + +ipc_toolkit +(ipc::toolkit) node0 - -Eigen3_Eigen -(Eigen3::Eigen) + +Eigen3_Eigen +(Eigen3::Eigen) node5->node0 - - + + node1 - -filib -(filib::filib) + +filib +(filib::filib) node5->node1 - - + + @@ -108,8 +108,8 @@ node5->node2 - - + + @@ -121,137 +121,150 @@ node5->node3 - + node15 - -xsimd -(xsimd::xsimd) + +xsimd +(xsimd::xsimd) node5->node15 - - + + node6 - -robin_map -(tsl::robin_map) + +robin_map +(tsl::robin_map) node5->node6 - - + + node7 - -scalable_ccd -(scalable_ccd::scalable_ccd) + +scalable_ccd +(scalable_ccd::scalable_ccd) node5->node7 - - + + node8 - -spdlog -(spdlog::spdlog) + +spdlog +(spdlog::spdlog) node5->node8 - - + + node9 - -tbb -(TBB::tbb) + +tbb +(TBB::tbb) node5->node9 - - + + node11 - -tight_inclusion -(tight_inclusion::tight_inclusion) + +tight_inclusion +(tight_inclusion::tight_inclusion) node5->node11 - - + + node12 - -absl_hash -(absl::hash) + +absl_hash +(absl::hash) node5->node12 - - + + node13 - -TinyAD -(TinyAD::TinyAD) + +TinyAD +(TinyAD::TinyAD) node5->node13 - - + + node17 - -MeshFEMSparse -(MeshFEM::Sparse) + +MeshFEMSparse +(MeshFEM::Sparse) node5->node17 - - + + + + + +node18 + +geogram_multi_precision +(geogram::multi_precision) + + + +node5->node18 + + node2->node0 - - + + node3->node2 - - + + @@ -269,56 +282,56 @@ node7->node0 - - + + node7->node8 - - + + node7->node9 - - + + node11->node0 - - + + node11->node8 - - + + node13->node0 - - + + node13->node9 - - + + node17->node0 - - + + node17->node9 - - + + diff --git a/docs/source/about/dependencies.rst b/docs/source/about/dependencies.rst index 8205991ce..990cbdd04 100644 --- a/docs/source/about/dependencies.rst +++ b/docs/source/about/dependencies.rst @@ -48,6 +48,10 @@ These dependencies are all `permissively licensed `_, and we list - Automatic differentiation for testing and in non-performance critical smooth contact functions - MIT - `github.com/microsoft/TinyAD `_ + * - geogram (MultiPrecision PSM) + - Exact arithmetic for the exact distance-type classification used by the ESP potential. We only use the lightweight, two-file MultiPrecision PSM, not the full geogram library. + - BSD-3-Clause + - `github.com/BrunoLevy/geogram `_ Optional Dependencies --------------------- @@ -128,6 +132,12 @@ Some of these libraries are enabled by default, and some are not. You can enable .. note:: ``MeshFEMSparse`` (and its transitive dependency ``MeshFEMCore``) is downloaded source-only and compiled into a minimal static library (matrix data structures and assembly routines; no sparse direct solvers). When enabled (the default), :cpp:func:`ipc::Potential::hessian` assembles through the block-CSC backend — several times faster than the triplet-based assembly, with identical results up to floating-point summation order — and a :cpp:class:`ipc::MeshFEMHessianAssembler` held across :cpp:func:`ipc::Potential::assemble_hessian` calls additionally reuses the sparsity pattern between assemblies. It requires ``IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT=RowMajor`` (the default; the option is automatically disabled otherwise). +.. note:: + ``geogram`` makes the distance-type classifiers in ``ipc/distance/distance_type_exact.hpp`` (used by the ESP potential) exact. We only download geogram's standalone MultiPrecision Pluggable Software Module (its exact ``expansion_nt`` arithmetic, two source files with no dependencies) from the geogram release and compile it into a small static library, rather than building all of geogram. We compile it with its namespace renamed from ``GEO`` to ``ipc_geogram``, so it can coexist with a full copy of geogram linked by a downstream project (e.g., PolyFEM). The archive's optional CGAL example is deleted after download and never built. + +.. note:: + The Gauss–Lobatto quadrature used by the ESP potential (``ipc/esp/collisions/esp_quadrature.hpp``) is adapted from John Burkardt's `quad_rule `_ library, which is distributed under the MIT license. + .. warning:: ``filib`` is licensed under `LGPL-2.1 `_ and as such it is required to be dynamically linked. Doing so automatically is a challenge, so by default we use static linkage. Enabling dynamic linkage requires copying the ``.so``/``.dylib``/``.dll`` file to the binary directory or system path. To enable this, set the CMake option ``FILIB_BUILD_SHARED_LIBS`` to ``ON`` and add this CMake code to copy the shared library object to the binary directory: diff --git a/docs/source/cpp-api/barrier/InversePowerBarrier.rst b/docs/source/cpp-api/barrier/InversePowerBarrier.rst new file mode 100644 index 000000000..664876280 --- /dev/null +++ b/docs/source/cpp-api/barrier/InversePowerBarrier.rst @@ -0,0 +1,5 @@ +Inverse-Power Barrier +===================== + +.. doxygenclass:: ipc::InversePowerBarrier + :allow-dot-graphs: diff --git a/docs/source/cpp-api/barrier/NearFarBarrier.rst b/docs/source/cpp-api/barrier/NearFarBarrier.rst new file mode 100644 index 000000000..cd89147eb --- /dev/null +++ b/docs/source/cpp-api/barrier/NearFarBarrier.rst @@ -0,0 +1,5 @@ +Near-Far Barrier +================ + +.. doxygenclass:: ipc::NearFarBarrier + :allow-dot-graphs: diff --git a/docs/source/cpp-api/barrier/index.rst b/docs/source/cpp-api/barrier/index.rst index 78ccbcd5b..44770c454 100644 --- a/docs/source/cpp-api/barrier/index.rst +++ b/docs/source/cpp-api/barrier/index.rst @@ -11,6 +11,8 @@ Barrier ClampedLogSqBarrier.rst CubicBarrier.rst TwoStageBarrier.rst + InversePowerBarrier.rst + NearFarBarrier.rst Barrier functions and functionals. @@ -34,6 +36,10 @@ Types - Normalized barrier function from [Li et al.]. * - :cpp:class:`ipc::TwoStageBarrier` - Two-stage barrier function from [Chen et al. 2025]. + * - :cpp:class:`ipc::InversePowerBarrier` + - Inverse-power barrier :math:`h(d,\hat{d})/d^p` with a C² cubic B-spline window. + * - :cpp:class:`ipc::NearFarBarrier` + - Splits a base barrier into near and far parts (used by the ESP potential). Functions diff --git a/docs/source/cpp-api/distance.rst b/docs/source/cpp-api/distance.rst index bbb08a0e1..37164f17a 100644 --- a/docs/source/cpp-api/distance.rst +++ b/docs/source/cpp-api/distance.rst @@ -12,6 +12,17 @@ Distance Type .. doxygenfunction:: ipc::edge_edge_distance_type .. doxygenfunction:: ipc::point_triangle_distance_type +Exact Distance Type +------------------- + +.. doxygenclass:: ipc::DistanceTypeConfig + :allow-dot-graphs: + +.. doxygenfunction:: ipc::point_edge_distance_type_exact +.. doxygenfunction:: ipc::edge_edge_distance_type_exact +.. doxygenfunction:: ipc::point_triangle_distance_type_exact +.. doxygenfunction:: ipc::is_parallel_edge_edge + Edge-Edge Mollifier ------------------- diff --git a/docs/source/cpp-api/esp.rst b/docs/source/cpp-api/esp.rst new file mode 100644 index 000000000..4ab6a859b --- /dev/null +++ b/docs/source/cpp-api/esp.rst @@ -0,0 +1,26 @@ +Extremum-Sum Potential +====================== + +ESP Parameters +-------------- + +.. doxygenstruct:: ipc::ESPParameters + :allow-dot-graphs: + +ESP Collisions +-------------- + +.. doxygenclass:: ipc::ESPCollisions + :allow-dot-graphs: + +ESP Potential +------------- + +.. doxygenclass:: ipc::ESPPotential + :allow-dot-graphs: + +Arbitrary Point ESP +------------------- + +.. doxygenclass:: ipc::ArbitraryPointESP + :allow-dot-graphs: diff --git a/docs/source/cpp-api/index.rst b/docs/source/cpp-api/index.rst index b5b157833..729a6fe68 100644 --- a/docs/source/cpp-api/index.rst +++ b/docs/source/cpp-api/index.rst @@ -20,6 +20,8 @@ C++ API - Data structures representing collision meshes. * - `Distance `__ - Functions for computing distances between geometric entities. + * - `Extremum-Sum Potential `__ + - The Extremum-Sum Proximity Potential (ESP). * - `Friction `__ - Models and functions for frictional contact. * - `Intersections `__ diff --git a/docs/source/index.rst b/docs/source/index.rst index 9e782e745..3a64abb99 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -43,6 +43,7 @@ cpp-api/ccd.rst cpp-api/collision_mesh.rst cpp-api/distance.rst + cpp-api/esp.rst cpp-api/friction.rst cpp-api/intersections.rst cpp-api/interval.rst @@ -64,6 +65,7 @@ python-api/ccd.rst python-api/collision_mesh.rst python-api/distance.rst + python-api/esp.rst python-api/friction.rst python-api/intersections.rst python-api/interval.rst diff --git a/docs/source/python-api/esp.rst b/docs/source/python-api/esp.rst new file mode 100644 index 000000000..970106b5e --- /dev/null +++ b/docs/source/python-api/esp.rst @@ -0,0 +1,23 @@ +Extremum-Sum Potential +====================== + +ESP Parameters +-------------- + +.. autoclass:: ipctk.ESPParameters + + .. autoclasstoc:: + +ESP Collisions +-------------- + +.. autoclass:: ipctk.ESPCollisions + + .. autoclasstoc:: + +ESP Potential +------------- + +.. autoclass:: ipctk.ESPPotential + + .. autoclasstoc:: diff --git a/docs/source/python-api/index.rst b/docs/source/python-api/index.rst index 651e7528b..664eb4fb2 100644 --- a/docs/source/python-api/index.rst +++ b/docs/source/python-api/index.rst @@ -20,6 +20,8 @@ Python API - Data structures representing collision meshes. * - `Distance `__ - Functions for computing distances between geometric entities. + * - `Extremum-Sum Potential `__ + - The Extremum-Sum Proximity Potential (ESP). * - `Friction `__ - Models and functions for frictional contact. * - `Intersections `__ diff --git a/docs/source/tutorials/gcp.rst b/docs/source/tutorials/gcp.rst index c95d1bb41..746115cf8 100644 --- a/docs/source/tutorials/gcp.rst +++ b/docs/source/tutorials/gcp.rst @@ -297,17 +297,17 @@ GCP is implemented as separate collision and potential classes. A basic example double beta_n = 0.0; // exterior direction constraint offset int r = 2; // barrier exponent (dimension - 1) - ipc::SmoothContactParameters params(dhat, alpha_t, beta_t, alpha_n, beta_n, r); + ipc::GCPParameters params(dhat, alpha_t, beta_t, alpha_n, beta_n, r); // Build collision set bool use_adaptive_dhat = true; - ipc::SmoothCollisions collisions; + ipc::GCPCollisions collisions; if (use_adaptive_dhat) collisions.compute_adaptive_dhat(collision_mesh, vertices, params); collisions.build(collision_mesh, vertices, params, use_adaptive_dhat); // Compute potential - ipc::SmoothContactPotential barrier_potential(params); + ipc::GCPPotential barrier_potential(params); double b = barrier_potential(collisions, collision_mesh, vertices); // Compute gradient @@ -331,17 +331,17 @@ GCP is implemented as separate collision and potential classes. A basic example beta_n = 0.0 # exterior direction constraint offset r = 2 # barrier exponent (dimension - 1) - params = ipctk.SmoothContactParameters(dhat, alpha_t, beta_t, alpha_n, beta_n, r) + params = ipctk.GCPParameters(dhat, alpha_t, beta_t, alpha_n, beta_n, r) # Build collision set use_adaptive_dhat = True - collisions = ipctk.SmoothCollisions() + collisions = ipctk.GCPCollisions() if use_adaptive_dhat: collisions.compute_adaptive_dhat(collision_mesh, vertices, params) collisions.build(collision_mesh, vertices, params, use_adaptive_dhat) # Compute potential - barrier_potential = ipctk.SmoothContactPotential(params) + barrier_potential = ipctk.GCPPotential(params) b = barrier_potential(collisions, collision_mesh, vertices) # Compute gradient @@ -351,15 +351,15 @@ GCP is implemented as separate collision and potential classes. A basic example hess = barrier_potential.hessian(collisions, collision_mesh, vertices) .. important:: - If ``use_adaptive_dhat`` is true, make sure to call ``SmoothCollisions::compute_adaptive_dhat()`` **before** ``SmoothCollisions::build()``. Adaptive :math:`\hat{d}` computes per-element barrier extents based on the rest configuration to guarantee zero potential (and zero forces) in the undeformed state. + If ``use_adaptive_dhat`` is true, make sure to call ``GCPCollisions::compute_adaptive_dhat()`` **before** ``GCPCollisions::build()``. Adaptive :math:`\hat{d}` computes per-element barrier extents based on the rest configuration to guarantee zero potential (and zero forces) in the undeformed state. .. note:: - Unlike ``NormalCollisions`` in IPC, ``SmoothCollisions`` must be rebuilt whenever vertex positions change, because the interaction set depends on the current geometry (normals, tangents, and distances). + Unlike ``NormalCollisions`` in IPC, ``GCPCollisions`` must be rebuilt whenever vertex positions change, because the interaction set depends on the current geometry (normals, tangents, and distances). Parameter Choices ----------------- -The ``SmoothContactParameters`` structure contains the following parameters: +The ``GCPParameters`` structure contains the following parameters: .. list-table:: :header-rows: 1 @@ -392,7 +392,7 @@ As :math:`\alpha + \beta` decreases, the support of the Heaviside function shrin Additional internal parameters that may affect behavior: -- **Adaptive dhat ratio** (default ``0.5``): Controls the ratio :math:`\epsilon(x) / d_c(x, f_0)` in the adaptive barrier localization. Set it via ``SmoothContactParameters::set_adaptive_dhat_ratio()`` in C++ or the ``SmoothContactParameters.adaptive_dhat_ratio`` property in Python. +- **Adaptive dhat ratio** (default ``0.5``): Controls the ratio :math:`\epsilon(x) / d_c(x, f_0)` in the adaptive barrier localization. Set it via ``GCPParameters::set_adaptive_dhat_ratio()`` in C++ or the ``GCPParameters.adaptive_dhat_ratio`` property in Python. - **Element measure** :math:`L`: For vertices, this is set to the average edge length around the vertex; for edges, to the edge length; for faces, :math:`L` is not needed. This determines the strength of the potential for low-dimensional contact (edge–edge, edge–vertex, vertex–vertex). Friction diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index 914db38e8..88642b749 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -1,6 +1,9 @@ #include #include +#include +#include + PYBIND11_MODULE(ipctk, m) { // py::options options; @@ -100,6 +103,7 @@ PYBIND11_MODULE(ipctk, m) define_smooth_mu(m); define_smooth_potential(m); + define_esp_potential(m); // geometry define_angle(m); @@ -135,4 +139,31 @@ PYBIND11_MODULE(ipctk, m) // root define_collision_mesh(m); define_ipc(m); + + // Deprecated names from before the smooth_contact -> gcp rename. We resolve + // them in a module-level __getattr__ (PEP 562) instead of plain aliases, so + // using an old name raises a DeprecationWarning. Python only calls this + // for names the module does not define, so every other lookup is + // unaffected. + m.def("__getattr__", [](const std::string& name) -> py::object { + static const std::map renamed = { + { "SmoothCollision2", "GCPCollision2" }, + { "SmoothCollisions", "GCPCollisions" }, + { "SmoothContactParameters", "GCPParameters" }, + { "SmoothContactPotential", "GCPPotential" }, + }; + const auto it = renamed.find(name); + if (it == renamed.end()) { + throw py::attribute_error( + "module 'ipctk' has no attribute '" + name + "'"); + } + const std::string message = "ipctk." + it->first + + " is deprecated. Use ipctk." + it->second + " instead."; + // A stack level of 1 points at the caller's line, since this function + // has no Python frame of its own. + if (PyErr_WarnEx(PyExc_DeprecationWarning, message.c_str(), 1) < 0) { + throw py::error_already_set(); // warnings configured as errors + } + return py::module_::import("ipctk").attr(it->second.c_str()); + }); } diff --git a/python/src/candidates/candidates.cpp b/python/src/candidates/candidates.cpp index be82f62cc..872665c2e 100644 --- a/python/src/candidates/candidates.cpp +++ b/python/src/candidates/candidates.cpp @@ -12,7 +12,7 @@ void define_candidates(py::module_& m) "build", py::overload_cast< const CollisionMesh&, Eigen::ConstRef, - const double, BroadPhase*>(&Candidates::build), + const double, BroadPhase*, const bool>(&Candidates::build), R"ipc_Qu8mg5v7( Initialize the set of discrete collision detection candidates. @@ -21,9 +21,10 @@ void define_candidates(py::module_& m) vertices: Surface vertex positions (rowwise). inflation_radius: Amount to inflate the bounding boxes. broad_phase: Broad phase to use. + all_types: Also detect the candidate types only ESP needs. )ipc_Qu8mg5v7", "mesh"_a, "vertices"_a, "inflation_radius"_a = 0, - "broad_phase"_a = nullptr) + "broad_phase"_a = nullptr, "all_types"_a = false) .def( "build", py::overload_cast< @@ -268,5 +269,8 @@ void define_candidates(py::module_& m) .def_readwrite("ev_candidates", &Candidates::ev_candidates) .def_readwrite("ee_candidates", &Candidates::ee_candidates) .def_readwrite("fv_candidates", &Candidates::fv_candidates) - .def_readwrite("pv_candidates", &Candidates::pv_candidates); + .def_readwrite("pv_candidates", &Candidates::pv_candidates) + .def_readwrite( + "has_all_types", &Candidates::has_all_types, + "Whether these candidates include the types only ESP needs (set by build(all_types=...))."); } diff --git a/python/src/collisions/normal/normal_collisions.cpp b/python/src/collisions/normal/normal_collisions.cpp index d394cbc98..d590d2b9e 100644 --- a/python/src/collisions/normal/normal_collisions.cpp +++ b/python/src/collisions/normal/normal_collisions.cpp @@ -1,7 +1,8 @@ #include #include -#include +#include +#include using namespace ipc; @@ -17,10 +18,10 @@ void define_smooth_collision_template(py::module_& m, const std::string& name) void define_smooth_collisions(py::module_& m, const std::string& name) { - py::class_(m, name.c_str()) + py::class_(m, name.c_str()) .def(py::init()) .def( - "compute_adaptive_dhat", &SmoothCollisions::compute_adaptive_dhat, + "compute_adaptive_dhat", &GCPCollisions::compute_adaptive_dhat, R"ipc_Qu8mg5v7( Compute the per-element adaptive dhat from the rest configuration. @@ -30,7 +31,7 @@ void define_smooth_collisions(py::module_& m, const std::string& name) Parameters: mesh: The collision mesh. vertices: Vertices of the collision mesh. - params: SmoothContactParameters. + params: GCPParameters. broad_phase: Broad phase method. )ipc_Qu8mg5v7", "mesh"_a, "vertices"_a, "params"_a, "broad_phase"_a = nullptr) @@ -38,15 +39,15 @@ void define_smooth_collisions(py::module_& m, const std::string& name) "build", py::overload_cast< const CollisionMesh&, Eigen::ConstRef, - const SmoothContactParameters, const bool, BroadPhase*>( - &SmoothCollisions::build), + const GCPParameters, const bool, BroadPhase*>( + &GCPCollisions::build), R"ipc_Qu8mg5v7( Initialize the set of collisions used to compute the barrier potential. Parameters: mesh: The collision mesh. vertices: Vertices of the collision mesh. - param: SmoothContactParameters. + param: GCPParameters. use_adaptive_dhat: If the adaptive dhat should be used. broad_phase: Broad phase method. )ipc_Qu8mg5v7", @@ -54,7 +55,7 @@ void define_smooth_collisions(py::module_& m, const std::string& name) "broad_phase"_a = nullptr) .def( "compute_minimum_distance", - &SmoothCollisions::compute_minimum_distance, + &GCPCollisions::compute_minimum_distance, R"ipc_Qu8mg5v7( Computes the minimum distance between any non-adjacent elements. @@ -66,16 +67,15 @@ void define_smooth_collisions(py::module_& m, const std::string& name) The minimum distance between any non-adjacent elements. )ipc_Qu8mg5v7", "mesh"_a, "vertices"_a) + .def("__len__", &GCPCollisions::size, "Get the number of collisions.") .def( - "__len__", &SmoothCollisions::size, "Get the number of collisions.") - .def( - "empty", &SmoothCollisions::empty, + "empty", &GCPCollisions::empty, "Get if the collision set is empty.") - .def("clear", &SmoothCollisions::clear, "Clear the collision set.") + .def("clear", &GCPCollisions::clear, "Clear the collision set.") .def( "__getitem__", - [](SmoothCollisions& self, size_t i) -> - typename SmoothCollisions::value_type& { return self[i]; }, + [](GCPCollisions& self, size_t i) -> + typename GCPCollisions::value_type& { return self[i]; }, py::return_value_policy::reference, R"ipc_Qu8mg5v7( Get a reference to collision at index i. @@ -88,10 +88,58 @@ void define_smooth_collisions(py::module_& m, const std::string& name) )ipc_Qu8mg5v7", "i"_a) .def( - "to_string", &SmoothCollisions::to_string, "mesh"_a, "vertices"_a, + "to_string", &GCPCollisions::to_string, "mesh"_a, "vertices"_a, "param"_a) .def( - "n_candidates", &SmoothCollisions::n_candidates, + "n_candidates", &GCPCollisions::n_candidates, + "Get the number of candidates."); +} + +void define_esp_collisions(py::module_& m) +{ + py::class_(m, "ESPCollisions") + .def(py::init()) + .def( + "build", + py::overload_cast< + const CollisionMesh&, Eigen::ConstRef, + const ESPParameters&, BroadPhase*>(&ESPCollisions::build), + R"ipc_Qu8mg5v7( + Initialize the set of collisions used to compute the potential. + + Parameters: + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + param: ESPParameters. + broad_phase: Broad phase method. + )ipc_Qu8mg5v7", + py::arg("mesh"), py::arg("vertices"), py::arg("param"), + py::arg("broad_phase") = nullptr) + .def( + "compute_minimum_distance", + &ESPCollisions::compute_minimum_distance, + R"ipc_Qu8mg5v7( + Computes the minimum squared distance over the collision candidates + found by build(). + + Parameters: + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + + Returns: + The minimum squared distance, or inf if there are no candidates. + )ipc_Qu8mg5v7", + py::arg("mesh"), py::arg("vertices")) + .def("__len__", &ESPCollisions::size, "Get the number of collisions.") + .def( + "empty", &ESPCollisions::empty, + "Get if the collision set is empty.") + .def("clear", &ESPCollisions::clear, "Clear the collision set.") + .def( + "to_string", &ESPCollisions::to_string, py::arg("mesh"), + py::arg("vertices"), py::arg("param")) + .def( + "n_candidates", &ESPCollisions::n_candidates, "Get the number of candidates."); } @@ -147,6 +195,9 @@ void define_normal_collisions(py::module_& m) vertices: Vertices of the collision mesh. dhat: The activation distance of the barrier. dmin: Minimum distance. + + Raises: + ValueError: In 3D, if the candidates were built with all_types=True. )ipc_Qu8mg5v7", "candidates"_a, "mesh"_a, "vertices"_a, "dhat"_a, "dmin"_a = 0) .def( @@ -272,10 +323,10 @@ void define_normal_collisions(py::module_& m) .def_readwrite("fv_collisions", &NormalCollisions::fv_collisions) .def_readwrite("pv_collisions", &NormalCollisions::pv_collisions); - py::class_(m, "SmoothCollision2") - .def("n_dofs", &SmoothCollision::n_dofs, "Get the degree of freedom") + py::class_(m, "GCPCollision2") + .def("n_dofs", &GCPCollision::n_dofs, "Get the degree of freedom") .def( - "__call__", &SmoothCollision::operator(), + "__call__", &GCPCollision::operator(), R"ipc_Qu8mg5v7( Compute the potential. @@ -289,7 +340,7 @@ void define_normal_collisions(py::module_& m) "positions"_a, "params"_a) .def( "__getitem__", - [](SmoothCollision& self, size_t i) -> long { return self[i]; }, + [](GCPCollision& self, size_t i) -> long { return self[i]; }, R"ipc_Qu8mg5v7( Get primitive id. @@ -302,11 +353,13 @@ void define_normal_collisions(py::module_& m) "i"_a); define_smooth_collision_template< - SmoothCollisionTemplate, SmoothCollision>( + GCPCollisionTemplate, GCPCollision>( m, "Edge2Point2Collision"); define_smooth_collision_template< - SmoothCollisionTemplate, SmoothCollision>( + GCPCollisionTemplate, GCPCollision>( m, "Point2Point2Collision"); - define_smooth_collisions(m, "SmoothCollisions"); + define_smooth_collisions(m, "GCPCollisions"); + + define_esp_collisions(m); } diff --git a/python/src/potentials/barrier_potential.cpp b/python/src/potentials/barrier_potential.cpp index 25224fc9c..a08695cf7 100644 --- a/python/src/potentials/barrier_potential.cpp +++ b/python/src/potentials/barrier_potential.cpp @@ -1,7 +1,8 @@ #include #include -#include +#include +#include using namespace ipc; @@ -83,7 +84,7 @@ void define_barrier_potential(py::module_& m) void define_smooth_potential(py::module_& m) { - py::class_(m, "SmoothContactParameters") + py::class_(m, "GCPParameters") .def( py::init< const double, const double, const double, const double, @@ -104,22 +105,21 @@ void define_smooth_potential(py::module_& m) dhat, alpha_t, beta_t, r )ipc_Qu8mg5v7", "dhat"_a, "alpha_t"_a, "beta_t"_a, "r"_a) - .def_readonly("dhat", &SmoothContactParameters::dhat) - .def_readonly("alpha_t", &SmoothContactParameters::alpha_t) - .def_readonly("beta_t", &SmoothContactParameters::beta_t) - .def_readonly("alpha_n", &SmoothContactParameters::alpha_n) - .def_readonly("beta_n", &SmoothContactParameters::beta_n) - .def_readonly("r", &SmoothContactParameters::r) + .def_readonly("dhat", &GCPParameters::dhat) + .def_readonly("alpha_t", &GCPParameters::alpha_t) + .def_readonly("beta_t", &GCPParameters::beta_t) + .def_readonly("alpha_n", &GCPParameters::alpha_n) + .def_readonly("beta_n", &GCPParameters::beta_n) + .def_readonly("r", &GCPParameters::r) .def_property( - "adaptive_dhat_ratio", - &SmoothContactParameters::adaptive_dhat_ratio, - &SmoothContactParameters::set_adaptive_dhat_ratio, + "adaptive_dhat_ratio", &GCPParameters::adaptive_dhat_ratio, + &GCPParameters::set_adaptive_dhat_ratio, "Ratio of the distance to the interaction set in the rest " "configuration used as the per-element adaptive dhat."); - py::class_(m, "SmoothContactPotential") + py::class_(m, "GCPPotential") .def( - py::init(), + py::init(), R"ipc_Qu8mg5v7( Construct a smooth barrier potential. @@ -130,9 +130,9 @@ void define_smooth_potential(py::module_& m) .def( "__call__", py::overload_cast< - const SmoothCollisions&, const CollisionMesh&, + const GCPCollisions&, const CollisionMesh&, Eigen::ConstRef>( - &ipc::SmoothContactPotential::operator(), py::const_), + &ipc::GCPPotential::operator(), py::const_), R"ipc_Qu8mg5v7( Compute the barrier potential for a set of collisions. @@ -148,9 +148,9 @@ void define_smooth_potential(py::module_& m) .def( "gradient", py::overload_cast< - const SmoothCollisions&, const CollisionMesh&, + const GCPCollisions&, const CollisionMesh&, Eigen::ConstRef>( - &ipc::SmoothContactPotential::gradient, py::const_), + &ipc::GCPPotential::gradient, py::const_), R"ipc_Qu8mg5v7( Compute the gradient of the barrier potential. @@ -166,9 +166,9 @@ void define_smooth_potential(py::module_& m) .def( "hessian", py::overload_cast< - const SmoothCollisions&, const CollisionMesh&, + const GCPCollisions&, const CollisionMesh&, Eigen::ConstRef, const PSDProjectionMethod>( - &ipc::SmoothContactPotential::hessian, py::const_), + &ipc::GCPPotential::hessian, py::const_), R"ipc_Qu8mg5v7( Compute the hessian of the barrier potential. @@ -186,8 +186,8 @@ void define_smooth_potential(py::module_& m) .def( "__call__", py::overload_cast< - const SmoothCollision&, Eigen::ConstRef>( - &ipc::SmoothContactPotential::operator(), py::const_), + const GCPCollision&, Eigen::ConstRef>( + &ipc::GCPPotential::operator(), py::const_), R"ipc_Qu8mg5v7( Compute the potential for a single collision. @@ -202,8 +202,8 @@ void define_smooth_potential(py::module_& m) .def( "gradient", py::overload_cast< - const SmoothCollision&, Eigen::ConstRef>( - &SmoothContactPotential::gradient, py::const_), + const GCPCollision&, Eigen::ConstRef>( + &GCPPotential::gradient, py::const_), R"ipc_Qu8mg5v7( Compute the gradient of the potential for a single collision. @@ -218,9 +218,8 @@ void define_smooth_potential(py::module_& m) .def( "hessian", py::overload_cast< - const SmoothCollision&, Eigen::ConstRef, - const PSDProjectionMethod>( - &SmoothContactPotential::hessian, py::const_), + const GCPCollision&, Eigen::ConstRef, + const PSDProjectionMethod>(&GCPPotential::hessian, py::const_), R"ipc_Qu8mg5v7( Compute the hessian of the potential for a single collision. @@ -234,3 +233,116 @@ void define_smooth_potential(py::module_& m) "collision"_a, "x"_a, "project_hessian_to_psd"_a = PSDProjectionMethod::NONE); } + +void define_esp_potential(py::module& m) +{ + py::class_ esp_parameters(m, "ESPParameters"); + + py::enum_(esp_parameters, "IntegrationType") + .value( + "BRUTE_FORCE", ESPParameters::IntegrationType::BRUTE_FORCE, + "Integrate all pairs with no obstacle filtering") + .value( + "NORMAL", ESPParameters::IntegrationType::NORMAL, + "Filter obstacle-obstacle pairs; skip primitives with only " + "obstacle candidates") + .value( + "NO_OBST", ESPParameters::IntegrationType::NO_OBST, + "Skip obstacle sources entirely, may miss collisions!"); + + esp_parameters + .def( + py::init< + const double, const double, const int, bool, + ESPParameters::IntegrationType>(), + R"ipc_Qu8mg5v7( + Construct parameter set for ESP contact. + + Parameters: + dhat: Activation distance of the barrier. + dbar_factor: Near/far split parameter dbar / dhat, in [0, 2]. The far part vanishes identically at 2; the edge-edge weights use the support min(dbar, dhat); 0 disables edge-edge contact. + quad_order: Gauss-Lobatto edge quadrature order (2D). + area_weights: Whether to weight the quadrature by area. + integration_type: Which obstacle primitives to integrate. + + Raises: + ValueError: If dhat is not positive and finite, dbar_factor is not in [0, 2], or quad_order is negative. + )ipc_Qu8mg5v7", + py::arg("dhat"), py::arg("dbar_factor") = 0.2, + py::arg("quad_order") = 1, py::arg("area_weights") = true, + py::arg("integration_type") = + ESPParameters::IntegrationType::NORMAL) + .def_readonly("dhat", &ESPParameters::dhat) + .def_readonly("dbar", &ESPParameters::dbar) + .def_readonly("quad_order", &ESPParameters::quad_order) + .def_readonly("area_weights", &ESPParameters::area_weights) + .def_readonly("integration_type", &ESPParameters::integration_type); + + py::class_(m, "ESPPotential") + .def( + py::init(), + R"ipc_Qu8mg5v7( + Construct an Extremum Sum Potential (ESP). + + Parameters: + param: A set of parameters. + use_near_far: Whether to normalize the quadrature weights so they sum to 1, splitting near and far contributions when 0 < dbar_factor <= 2 (the far part vanishes at 2). + )ipc_Qu8mg5v7", + py::arg("param"), py::arg("use_near_far") = true) + .def( + "__call__", + py::overload_cast< + const ESPCollisions&, const CollisionMesh&, + Eigen::ConstRef>( + &ipc::ESPPotential::operator(), py::const_), + R"ipc_Qu8mg5v7( + Compute the barrier potential for a set of collisions. + + Parameters: + collisions: The set of collisions. + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + + Returns: + The sum of all barrier potentials (not scaled by the barrier stiffness). + )ipc_Qu8mg5v7", + py::arg("collisions"), py::arg("mesh"), py::arg("vertices")) + .def( + "gradient", + py::overload_cast< + const ESPCollisions&, const CollisionMesh&, + Eigen::ConstRef>( + &ipc::ESPPotential::gradient, py::const_), + R"ipc_Qu8mg5v7( + Compute the gradient of the barrier potential. + + Parameters: + collisions: The set of collisions. + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + + Returns: + The gradient of all barrier potentials (not scaled by the barrier stiffness). This will have a size of |vertices|. + )ipc_Qu8mg5v7", + py::arg("collisions"), py::arg("mesh"), py::arg("vertices")) + .def( + "hessian", + py::overload_cast< + const ESPCollisions&, const CollisionMesh&, + Eigen::ConstRef, const PSDProjectionMethod>( + &ipc::ESPPotential::hessian, py::const_), + R"ipc_Qu8mg5v7( + Compute the hessian of the barrier potential. + + Parameters: + collisions: The set of collisions. + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + project_hessian_to_psd: Make sure the hessian is positive semi-definite. With normalized weights and edge-edge terms, each face's block is projected as a whole. + + Returns: + The hessian of all barrier potentials (not scaled by the barrier stiffness). This will have a size of |vertices|x|vertices|. + )ipc_Qu8mg5v7", + py::arg("collisions"), py::arg("mesh"), py::arg("vertices"), + py::arg("project_hessian_to_psd") = PSDProjectionMethod::NONE); +} diff --git a/python/src/potentials/bindings.hpp b/python/src/potentials/bindings.hpp index f440ee258..fc8162bac 100644 --- a/python/src/potentials/bindings.hpp +++ b/python/src/potentials/bindings.hpp @@ -4,6 +4,7 @@ void define_barrier_potential(py::module_& m); void define_smooth_potential(py::module_& m); +void define_esp_potential(py::module_& m); void define_friction_potential(py::module_& m); void define_normal_adhesion_potential(py::module_& m); void define_normal_potential(py::module_& m); diff --git a/python/tests/test_potentials.py b/python/tests/test_potentials.py index 4c13e4900..e374d808a 100644 --- a/python/tests/test_potentials.py +++ b/python/tests/test_potentials.py @@ -1,7 +1,8 @@ """Tests for the potential bindings. Covers the Python-side input validation (which the C++ API only enforces with -assert(), compiled out under NDEBUG) and the smooth-contact/GCP bindings. +assert(), compiled out under NDEBUG) and the smooth-contact/GCP and ESP +bindings. Uses unittest.TestCase so that assertRaises is available under both nose2 (the runner used in CI) and pytest, without adding a pytest dependency. @@ -172,13 +173,13 @@ def test_use_physical_barrier_changes_result(self): self.assertNotEqual(off(*args), on(*args)) -class TestSmoothContactParameters(unittest.TestCase): +class TestGCPParameters(unittest.TestCase): def test_adaptive_dhat_ratio_default(self): - params = ipctk.SmoothContactParameters(DHAT, 0.5, 0.0, 0.1, 0.0, 2) + params = ipctk.GCPParameters(DHAT, 0.5, 0.0, 0.1, 0.0, 2) self.assertEqual(params.adaptive_dhat_ratio, 0.5) def test_adaptive_dhat_ratio_roundtrip(self): - params = ipctk.SmoothContactParameters(DHAT, 0.5, 0.0, 0.1, 0.0, 2) + params = ipctk.GCPParameters(DHAT, 0.5, 0.0, 0.1, 0.0, 2) for ratio in (0.1, 0.25, 0.9): params.adaptive_dhat_ratio = ratio self.assertEqual(params.adaptive_dhat_ratio, ratio) @@ -196,9 +197,9 @@ def test_adaptive_dhat_ratio_affects_adaptive_dhat(self): counts = [] for ratio in (0.1, 0.5, 0.9): - params = ipctk.SmoothContactParameters(DHAT, 0.5, 0.0, 0.1, 0.0, 2) + params = ipctk.GCPParameters(DHAT, 0.5, 0.0, 0.1, 0.0, 2) params.adaptive_dhat_ratio = ratio - collisions = ipctk.SmoothCollisions() + collisions = ipctk.GCPCollisions() collisions.compute_adaptive_dhat(mesh, rest, params) collisions.build(mesh, deformed, params, True) counts.append(len(collisions)) @@ -208,16 +209,16 @@ def test_adaptive_dhat_ratio_affects_adaptive_dhat(self): self.assertLess(counts[1], counts[2], f"not monotonic: {counts}") -class TestSmoothCollisionsAdaptiveDhat(unittest.TestCase): +class TestGCPCollisionsAdaptiveDhat(unittest.TestCase): @classmethod def setUpClass(cls): cls.mesh, cls.rest = two_cubes() - cls.params = ipctk.SmoothContactParameters( + cls.params = ipctk.GCPParameters( DHAT, 0.5, 0.0, 0.1, 0.0, 2) - cls.potential = ipctk.SmoothContactPotential(cls.params) + cls.potential = ipctk.GCPPotential(cls.params) def _build(self, use_adaptive_dhat): - collisions = ipctk.SmoothCollisions() + collisions = ipctk.GCPCollisions() if use_adaptive_dhat: collisions.compute_adaptive_dhat(self.mesh, self.rest, self.params) collisions.build(self.mesh, self.rest, self.params, use_adaptive_dhat) @@ -244,20 +245,54 @@ def test_adaptive_dhat_eliminates_spurious_rest_forces(self): np.testing.assert_array_equal(gradient, np.zeros_like(gradient)) def test_broad_phase_argument_accepted(self): - collisions = ipctk.SmoothCollisions() + collisions = ipctk.GCPCollisions() collisions.compute_adaptive_dhat( self.mesh, self.rest, self.params, ipctk.LBVH()) -class TestSmoothContactPotentialNaming(unittest.TestCase): - """The Python class was previously exposed as "SmoothPotential", which did - not match the C++ name. Guard the rename in both directions.""" +class TestGCPAliases(unittest.TestCase): + def test_smooth_names_alias_gcp(self): + for old, new in [ + ("SmoothCollision2", "GCPCollision2"), + ("SmoothCollisions", "GCPCollisions"), + ("SmoothContactParameters", "GCPParameters"), + ("SmoothContactPotential", "GCPPotential"), + ]: + with self.assertWarns(DeprecationWarning): + self.assertIs(getattr(ipctk, old), getattr(ipctk, new)) - def test_matches_cpp_name(self): - self.assertTrue(hasattr(ipctk, "SmoothContactPotential")) - def test_old_name_removed(self): - self.assertFalse(hasattr(ipctk, "SmoothPotential")) +class TestESPPotential(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.mesh, cls.rest = two_cubes() + cls.params = ipctk.ESPParameters(DHAT) + cls.potential = ipctk.ESPPotential(cls.params) + cls.collisions = ipctk.ESPCollisions() + cls.collisions.build(cls.mesh, cls.rest, cls.params) + + def test_parameter_defaults(self): + self.assertTrue(self.params.area_weights) + self.assertEqual( + self.params.integration_type, + ipctk.ESPParameters.IntegrationType.NORMAL) + + def test_gradient_matches_finite_differences(self): + args = (self.collisions, self.mesh) + self.assertGreater(self.potential(*args, self.rest), 0.0) + + direction = np.random.default_rng(0).standard_normal(self.rest.shape) + h = 1e-7 # central differences are O(h^2) and the barrier is stiff + fd = (self.potential(*args, self.rest + h * direction) + - self.potential(*args, self.rest - h * direction)) / (2 * h) + gradient = self.potential.gradient(*args, self.rest) + self.assertAlmostEqual( + gradient @ direction.ravel() / fd, 1.0, places=5) + + def test_hessian_shape(self): + hessian = self.potential.hessian( + self.collisions, self.mesh, self.rest) + self.assertEqual(hessian.shape, (self.rest.size, self.rest.size)) if __name__ == "__main__": diff --git a/src/ipc/CMakeLists.txt b/src/ipc/CMakeLists.txt index fd8b5be2f..676d818f6 100644 --- a/src/ipc/CMakeLists.txt +++ b/src/ipc/CMakeLists.txt @@ -25,6 +25,7 @@ add_subdirectory(geometry) add_subdirectory(math) add_subdirectory(ogc) add_subdirectory(potentials) -add_subdirectory(smooth_contact) +add_subdirectory(gcp) +add_subdirectory(esp) add_subdirectory(tangent) -add_subdirectory(utils) \ No newline at end of file +add_subdirectory(utils) diff --git a/src/ipc/barrier/barrier.cpp b/src/ipc/barrier/barrier.cpp index 5538430f2..2dd6b4631 100644 --- a/src/ipc/barrier/barrier.cpp +++ b/src/ipc/barrier/barrier.cpp @@ -211,6 +211,245 @@ T TwoStageBarrier::second_derivative(const T d, const T dhat) const [&] { return T(0); }); } +// ============================================================================ + +template +void InversePowerBarrier::h_and_derivs( + const T d, const T dhat, T& h, T& dh, T& ddh) +{ + const T t = T(2) * d / dhat; + const T s = T(2) - t; // only meaningful on the second piece, 1 <= t < 2 + const T B = select_lazy( + t < T(1), [&] { return T(2) / T(3) - sqr(t) + T(0.5) * cubic(t); }, + t < T(2), [&] { return cubic(s) / T(6); }, // + [&] { return T(0); }); + const T dB = select_lazy( + t < T(1), [&] { return T(-2) * t + T(1.5) * sqr(t); }, t < T(2), + [&] { return -sqr(s) / T(2); }, // + [&] { return T(0); }); + const T ddB = select_lazy( + t < T(1), [&] { return T(-2) + T(3) * t; }, // + t < T(2), [&] { return s; }, // + [&] { return T(0); }); + // h = 2*B(t), dh/dd = 2*B'(t)*(dt/dd) = 2*B'*(2/dhat) = 4/dhat * B' + h = T(2) * B; + dh = T(4) / dhat * dB; + ddh = T(8) / sqr(dhat) * ddB; +} + +template +T InversePowerBarrier::operator()(const T d, const T dhat) const +{ + using namespace ipc::numext; // pow + return select_lazy( + d <= T(0), [&] { return infinity(); }, // + d < dhat, + [&] { + T h, dh, ddh; + h_and_derivs(d, dhat, h, dh, ddh); + return h / pow(d, literal(m_power)); + }, + [&] { return T(0); }); +} + +template +T InversePowerBarrier::first_derivative(const T d, const T dhat) const +{ + using namespace ipc::numext; // pow + return select_lazy( + d <= T(0), [&] { return T(0); }, // + d < dhat, + [&] { + const T p = literal(m_power); + T h, dh, ddh; + h_and_derivs(d, dhat, h, dh, ddh); + // b'(d) = (dh·d − p·h) / d^(p+1) + return (dh * d - p * h) / pow(d, p + T(1)); + }, + [&] { return T(0); }); +} + +template +T InversePowerBarrier::second_derivative(const T d, const T dhat) const +{ + using namespace ipc::numext; // pow + return select_lazy( + d <= T(0), [&] { return T(0); }, // + d < dhat, + [&] { + const T p = literal(m_power); + T h, dh, ddh; + h_and_derivs(d, dhat, h, dh, ddh); + // b''(d) = (ddh·d² − 2p·dh·d + p(p+1)·h) / d^(p+2) + return (ddh * sqr(d) - T(2) * p * dh * d + p * (p + T(1)) * h) + / pow(d, p + T(2)); + }, + [&] { return T(0); }); +} + +template T InversePowerBarrier::units(const T dhat) const +{ + using namespace ipc::numext; // pow + return T(1) / pow(dhat, literal(m_power)); +} + +// ============================================================================ + +namespace { + // The C² piecewise-cubic step behind NearFarBarrier's far weight, rising + // from 0 at x = -3 to 1 at x = 0, and its first two derivatives. These are + // the same polynomials as Math::smooth_heaviside, but we write them out as + // select_lazy cascades because Math branches with `if`, which a batch + // cannot do since each lane may land in a different piece. + + template T smooth_step(const T x) + { + return select_lazy( + x <= T(-3), [&] { return T(0); }, // + x <= T(-2), [&] { return cubic(T(3) + x) / T(6); }, // + x <= T(-1), + [&] { return (((T(-2) * x - T(9)) * x - T(9)) * x + T(3)) / T(6); }, + x < T(0), [&] { return cubic(x) / T(6) + T(1); }, // + [&] { return T(1); }); + } + + template T smooth_step_first_derivative(const T x) + { + return select_lazy( + x <= T(-3), [&] { return T(0); }, // + x <= T(-2), [&] { return sqr(T(3) + x) / T(2); }, // + x <= T(-1), [&] { return -(sqr(x) + T(3) * x + T(1.5)); }, x < T(0), + [&] { return sqr(x) / T(2); }, // + [&] { return T(0); }); + } + + template T smooth_step_second_derivative(const T x) + { + return select_lazy( + x <= T(-3), [&] { return T(0); }, // + x <= T(-2), [&] { return T(3) + x; }, // + x <= T(-1), [&] { return T(-3) - T(2) * x; }, // + x < T(0), [&] { return x; }, // + [&] { return T(0); }); + } +} // namespace + +// We map the far weight's ramp [alpha * dhat / 2, alpha * dhat] onto the +// standard step's [-3, 0] with x = (d - alpha * dhat) * scale, where +// scale = 6 / (alpha * dhat). Each derivative of H then picks up one factor of +// that scale from the chain rule. + +template +auto NearFarBarrier::is_purely_near(const T d, const T dhat) const -> Mask +{ + // The far weight H and all its derivatives are exactly zero at or below + // alpha * dhat / 2, so near = b and far = 0 there. We return those values + // directly instead of multiplying through, because the base barrier is +inf + // at d <= 0 (and InversePowerBarrier overflows to +inf for tiny d > 0), and + // inf * 0 would turn an exact contact into NaN. + return d <= literal(0.5 * m_alpha) * dhat; +} + +template +T NearFarBarrier::far_weight(const T d, const T dhat) const +{ + const T end = literal(m_alpha) * dhat; + return smooth_step((d - end) * (T(6) / end)); +} + +template +T NearFarBarrier::far_weight_first_derivative(const T d, const T dhat) const +{ + const T end = literal(m_alpha) * dhat; + const T scale = T(6) / end; + return smooth_step_first_derivative((d - end) * scale) * scale; +} + +template +T NearFarBarrier::far_weight_second_derivative(const T d, const T dhat) const +{ + const T end = literal(m_alpha) * dhat; + const T scale = T(6) / end; + return smooth_step_second_derivative((d - end) * scale) * sqr(scale); +} + +template +T NearFarBarrier::near_value(const T d, const T dhat) const +{ + const T b = (*m_base_barrier)(d, dhat); + return select_lazy( + is_purely_near(d, dhat), [&] { return b; }, + [&] { return b * (T(1) - far_weight(d, dhat)); }); +} + +template +T NearFarBarrier::far_value(const T d, const T dhat) const +{ + return select_lazy( + is_purely_near(d, dhat), [&] { return T(0); }, + [&] { return (*m_base_barrier)(d, dhat) * far_weight(d, dhat); }); +} + +template +T NearFarBarrier::first_derivative_near(const T d, const T dhat) const +{ + const T bp = m_base_barrier->first_derivative(d, dhat); + return select_lazy( + is_purely_near(d, dhat), [&] { return bp; }, + [&] { + const T b = (*m_base_barrier)(d, dhat); + const T w = far_weight(d, dhat); + const T wp = far_weight_first_derivative(d, dhat); + return bp * (T(1) - w) - b * wp; + }); +} + +template +T NearFarBarrier::first_derivative_far(const T d, const T dhat) const +{ + return select_lazy( + is_purely_near(d, dhat), [&] { return T(0); }, + [&] { + const T b = (*m_base_barrier)(d, dhat); + const T bp = m_base_barrier->first_derivative(d, dhat); + const T w = far_weight(d, dhat); + const T wp = far_weight_first_derivative(d, dhat); + return bp * w + b * wp; + }); +} + +template +T NearFarBarrier::second_derivative_near(const T d, const T dhat) const +{ + const T bpp = m_base_barrier->second_derivative(d, dhat); + return select_lazy( + is_purely_near(d, dhat), [&] { return bpp; }, + [&] { + const T b = (*m_base_barrier)(d, dhat); + const T bp = m_base_barrier->first_derivative(d, dhat); + const T w = far_weight(d, dhat); + const T wp = far_weight_first_derivative(d, dhat); + const T wpp = far_weight_second_derivative(d, dhat); + return bpp * (T(1) - w) - T(2) * bp * wp - b * wpp; + }); +} + +template +T NearFarBarrier::second_derivative_far(const T d, const T dhat) const +{ + return select_lazy( + is_purely_near(d, dhat), [&] { return T(0); }, + [&] { + const T b = (*m_base_barrier)(d, dhat); + const T bp = m_base_barrier->first_derivative(d, dhat); + const T bpp = m_base_barrier->second_derivative(d, dhat); + const T w = far_weight(d, dhat); + const T wp = far_weight_first_derivative(d, dhat); + const T wpp = far_weight_second_derivative(d, dhat); + return bpp * w + T(2) * bp * wp + b * wpp; + }); +} + // ============================================================================ // Explicit template instantiations /// @cond DOXYGEN_SKIP @@ -224,6 +463,10 @@ template class CubicBarrier; template class CubicBarrier; template class TwoStageBarrier; template class TwoStageBarrier; +template class InversePowerBarrier; +template class InversePowerBarrier; +template class NearFarBarrier; +template class NearFarBarrier; #ifdef IPC_TOOLKIT_WITH_SIMD template class BarrierBase>; template class BarrierBase>; @@ -235,6 +478,10 @@ template class CubicBarrier>; template class CubicBarrier>; template class TwoStageBarrier>; template class TwoStageBarrier>; +template class InversePowerBarrier>; +template class InversePowerBarrier>; +template class NearFarBarrier>; +template class NearFarBarrier>; #endif /// @endcond // ============================================================================ diff --git a/src/ipc/barrier/barrier.hpp b/src/ipc/barrier/barrier.hpp index de7456e32..34c356547 100644 --- a/src/ipc/barrier/barrier.hpp +++ b/src/ipc/barrier/barrier.hpp @@ -5,6 +5,13 @@ #pragma once #include +#include + +#include +#include +#include +#include +#include namespace ipc { @@ -399,4 +406,229 @@ template class TwoStageBarrier : public BarrierBase { } }; +// ============================================================================ +// Inverse-power barrier +// ============================================================================ + +/// @brief Inverse-power barrier with smooth compact support. +/// +/// \f\[ +/// b(d) = \frac{h(d,\hat{d})}{d^p}, \quad +/// h(d,\hat{d}) = 2\,B\!\left(\frac{2d}{\hat{d}}\right) +/// \f\] +/// +/// where \f$B\f$ is the standard cubic B-spline basis function and \f$p > 0\f$ +/// is the power parameter. The window \f$h\f$ vanishes smoothly at +/// \f$d = \hat{d}\f$ (C² contact), ensuring \f$b(d)=0\f$ for \f$d\ge\hat{d}\f$, +/// while \f$b(d)\to+\infty\f$ as \f$d\to 0^+\f$. +template +class InversePowerBarrier : public BarrierBase { +public: + /// @param power The power \f$p > 0\f$ controlling the singularity at d = 0. + /// @throws std::invalid_argument if power is not positive and finite. With + /// p = 0 the barrier stays bounded at contact (allowing + /// interpenetration), and with p < 0 it attracts instead of repels. + explicit InversePowerBarrier(const double power) : m_power(power) + { + if (!std::isfinite(power) || power <= 0) { + log_and_throw_error( + "InversePowerBarrier power {} must be positive and finite.", + power); + } + } + + /// @brief b(d) = h(d, d̂) / d^p. + /// @param d Distance (must be > 0 for a finite value). + /// @param dhat Activation distance of the barrier. + /// @return The value of the barrier function at d. + T operator()(const T d, const T dhat) const override; + + /// @brief First derivative b'(d) = (h'·d − p·h) / d^(p+1). + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The first derivative of the barrier function at d. + T first_derivative(const T d, const T dhat) const override; + + /// @brief Second derivative b''(d) = (h''·d² − 2p·h'·d + p(p+1)·h) / d^(p+2). + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The second derivative of the barrier function at d. + T second_derivative(const T d, const T dhat) const override; + + /// @brief Get the units of the barrier function (d̂^{-p}). + /// @param dhat The activation distance of the barrier. + /// @return The units of the barrier function. + T units(const T dhat) const override; + + /// @brief The power p used in the barrier. + double power() const { return m_power; } + +private: + /// @brief p > 0. We keep it as a double, whatever T is, because it is a + /// parameter of the barrier rather than a per-lane input. + double m_power; + + /// @brief Evaluate the B-spline window h(d, dhat) and its first two + /// derivatives with respect to d. + /// + /// h(d) = 2 * B(2d/dhat) where B is the cubic B-spline: + /// B(t) = 2/3 - t² + t³/2 for 0 ≤ t < 1 + /// B(t) = (2-t)³ / 6 for 1 ≤ t < 2 + /// B(t) = 0 for t ≥ 2 + static void h_and_derivs(T d, T dhat, T& h, T& dh, T& ddh); +}; + +/// @brief Near-Far barrier function. +/// This barrier function takes another "base" barrier object as argument. +/// All operations defined in BarrierBase defer the computation to its base +/// barrier. In addition, it splits the base barrier \f$b\f$ into a near and a +/// far part, +/// +/// \f\[ +/// b_\text{near}(d) = b(d)\,(1 - H(d)), \quad +/// b_\text{far}(d) = b(d)\,H(d), \quad +/// b_\text{near} + b_\text{far} = b, +/// \f\] +/// +/// where \f$H\f$ is a C² piecewise-cubic step rising from 0 at +/// \f$d = \alpha\hat{d}/2\f$ to 1 at \f$d = \alpha\hat{d}\f$. Below +/// \f$\alpha\hat{d}/2\f$ the near part is exactly \f$b\f$ and the far part +/// exactly 0, so the identity also holds at contact, where the log barriers +/// give \f$b_\text{near} = +\infty\f$. For \f$\alpha \ge 2\f$ the far part +/// vanishes identically, since \f$b = 0\f$ for \f$d \ge \hat{d}\f$. +/// ESPPotential uses \f$\alpha =\f$ dbar_factor. +template class NearFarBarrier : public BarrierBase { +public: + /// @brief Construct a NearFarBarrier that borrows its base barrier. + /// @param base_barrier The base barrier function to use. This does not take + /// ownership, so the base barrier must outlive this object. + /// @param alpha Split parameter: the far weight H rises from 0 at + /// d = alpha * dhat / 2 to 1 at d = alpha * dhat. + /// @throws std::invalid_argument if alpha is not positive and finite. At + /// alpha = 0 the step H collapses to a jump and its derivatives + /// are NaN. + NearFarBarrier(const BarrierBase* const base_barrier, const double alpha) + // We use the aliasing constructor with an empty owner, so this + // shared_ptr points at base_barrier without owning it or allocating a + // control block. That lets both constructors share one member, and + // destroying this object never deletes the borrowed barrier. + : m_base_barrier(std::shared_ptr(), base_barrier) + , m_alpha(alpha) + { + validate_alpha(); + } + + /// @brief Construct a NearFarBarrier holding shared ownership of the base + /// barrier. Prefer this over the raw-pointer overload whenever the caller + /// already has a shared_ptr — it pins the base barrier for the lifetime of + /// this object, eliminating dangling-pointer risk. + NearFarBarrier( + std::shared_ptr> base_barrier, const double alpha) + : m_base_barrier(std::move(base_barrier)) + , m_alpha(alpha) + { + validate_alpha(); + } + + /// @brief Evaluate the barrier function. + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The value of the barrier function at d. + T operator()(const T d, const T dhat) const override + { + return (*m_base_barrier)(d, dhat); + } + + /// @brief Evaluate the first derivative of the barrier function wrt d. + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The value of the first derivative of the barrier function at d. + T first_derivative(const T d, const T dhat) const override + { + return m_base_barrier->first_derivative(d, dhat); + } + + /// @brief Evaluate the second derivative of the barrier function wrt d. + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The value of the second derivative of the barrier function at d. + T second_derivative(const T d, const T dhat) const override + { + return m_base_barrier->second_derivative(d, dhat); + } + + /// @brief Get the units of the barrier function. + /// @param dhat The activation distance of the barrier. + /// @return The units of the barrier function. + T units(const T dhat) const override { return m_base_barrier->units(dhat); } + + /// @brief Evaluate the near part b(d) (1 - H(d)). + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The value of the near part at d. + T near_value(const T d, const T dhat) const; + + /// @brief Evaluate the far part b(d) H(d). + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The value of the far part at d. + T far_value(const T d, const T dhat) const; + + /// @brief Evaluate the first derivative of the near part wrt d. + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The first derivative of the near part at d. + T first_derivative_near(const T d, const T dhat) const; + + /// @brief Evaluate the first derivative of the far part wrt d. + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The first derivative of the far part at d. + T first_derivative_far(const T d, const T dhat) const; + + /// @brief Evaluate the second derivative of the near part wrt d. + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The second derivative of the near part at d. + T second_derivative_near(const T d, const T dhat) const; + + /// @brief Evaluate the second derivative of the far part wrt d. + /// @param d Distance. + /// @param dhat Activation distance of the barrier. + /// @return The second derivative of the far part at d. + T second_derivative_far(const T d, const T dhat) const; + +private: + /// @brief The result of comparing two T: a bool for a scalar, and a + /// per-lane mask for a batch. + using Mask = decltype(std::declval() <= std::declval()); + + void validate_alpha() const + { + if (!std::isfinite(m_alpha) || m_alpha <= 0) { + log_and_throw_error( + "NearFarBarrier alpha {} must be positive and finite.", + m_alpha); + } + } + + /// @brief Whether d lies where the far weight H vanishes identically + /// (d <= alpha * dhat / 2), so near = b and far = 0. + Mask is_purely_near(const T d, const T dhat) const; + + /// @brief The far weight H(d). + T far_weight(const T d, const T dhat) const; + + /// @brief The first derivative of the far weight H wrt d. + T far_weight_first_derivative(const T d, const T dhat) const; + + /// @brief The second derivative of the far weight H wrt d. + T far_weight_second_derivative(const T d, const T dhat) const; + + /// @brief The base barrier. It owns the barrier when built from a + /// shared_ptr and only aliases it when built from a raw pointer. + const std::shared_ptr> m_base_barrier; + const double m_alpha; +}; + } // namespace ipc diff --git a/src/ipc/broad_phase/broad_phase.cpp b/src/ipc/broad_phase/broad_phase.cpp index 5f00b3dda..d7db95b4e 100644 --- a/src/ipc/broad_phase/broad_phase.cpp +++ b/src/ipc/broad_phase/broad_phase.cpp @@ -77,7 +77,8 @@ void BroadPhase::clear() dim = 0; // reset dimension } -void BroadPhase::detect_collision_candidates(Candidates& candidates) const +void BroadPhase::detect_collision_candidates( + Candidates& candidates, bool all_types) const { candidates.clear(); assert(dim == 2 || dim == 3); @@ -88,6 +89,14 @@ void BroadPhase::detect_collision_candidates(Candidates& candidates) const // These are not needed for 2D detect_edge_edge_candidates(candidates.ee_candidates); detect_face_vertex_candidates(candidates.fv_candidates); + + // These are needed for ESP contact + if (all_types) { + detect_vertex_vertex_candidates(candidates.vv_candidates); + detect_edge_face_candidates(candidates.ef_candidates); + detect_face_face_candidates(candidates.ff_candidates); + detect_edge_vertex_candidates(candidates.ev_candidates); + } } } diff --git a/src/ipc/broad_phase/broad_phase.hpp b/src/ipc/broad_phase/broad_phase.hpp index 5f333f6c6..0168b8f83 100644 --- a/src/ipc/broad_phase/broad_phase.hpp +++ b/src/ipc/broad_phase/broad_phase.hpp @@ -64,7 +64,11 @@ class BroadPhase { /// @brief Detect all collision candidates needed for a given dimensional simulation. /// @param candidates The detected collision candidates (cleared first). - void detect_collision_candidates(Candidates& candidates) const; + /// @param all_types Also detect the candidate types that only ESP contact + /// needs (vertex-vertex, edge-vertex, edge-face, and + /// face-face in 3D). + void detect_collision_candidates( + Candidates& candidates, bool all_types = false) const; /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisions (cleared first). diff --git a/src/ipc/broad_phase/lbvh.cpp b/src/ipc/broad_phase/lbvh.cpp index 3182be40d..56e14cdc1 100644 --- a/src/ipc/broad_phase/lbvh.cpp +++ b/src/ipc/broad_phase/lbvh.cpp @@ -557,4 +557,63 @@ bool LBVH::can_faces_collide(size_t fai, size_t fbi) const fa0i, fa1i, fa2i, fb0i, fb1i, fb2i, can_vertices_collide); } -} // namespace ipc \ No newline at end of file +namespace { + /// @brief Build a query node whose AABB is a cube of half-width `radius` + /// centered at p, for use with LBVH::Node::intersects(). Only the AABB is + /// meaningful. + /// + /// We pad a 2D point to (x, y, 0), which is what AABB does to 2D + /// primitives (it zero-fills the unused component), so the query's z range + /// [-radius, radius] straddles the tree's. + LBVH::Node + point_query_node(Eigen::ConstRef p, const double radius) + { + Eigen::Array3d c = Eigen::Array3d::Zero(); + c.head(p.size()) = p.transpose().array(); + + LBVH::Node n; + n.aabb_min = (c - radius).cast(); + n.aabb_max = (c + radius).cast(); + n.primitive_id = -1; + n.is_inner_marker = 0; + return n; + } + + /// @brief Collect the primitive ids of every leaf of one tree that + /// intersects the query node. We reuse the descent of the pairwise + /// queries, whose fixed-size stack is sized for the deepest tree a + /// Morton-code build can produce and throws instead of overflowing. + void query_tree( + const LBVH::Nodes& bvh, + const LBVH::Node& query, + std::vector& hits) + { + hits.clear(); + if (bvh.empty()) { + return; + } + + details::traverse_lbvh( + /*query_leaf_idx=*/-1, bvh.data(), static_cast(bvh.size()), + /*target_rightmost=*/nullptr, + [&](const LBVH::Node& node) { return node.intersects(query); }, + [&](const LBVH::Node& leaf, int /*leaf_idx*/, bool /*mask*/) { + hits.push_back(leaf.primitive_id); + }); + } +} // namespace + +void LBVH::query_point( + Eigen::ConstRef q, + const double radius, + std::vector& vertex_ids, + std::vector& edge_ids, + std::vector& face_ids) const +{ + const Node query = point_query_node(q, radius); + query_tree(vertex_bvh, query, vertex_ids); + query_tree(edge_bvh, query, edge_ids); + query_tree(face_bvh, query, face_ids); +} + +} // namespace ipc diff --git a/src/ipc/broad_phase/lbvh.hpp b/src/ipc/broad_phase/lbvh.hpp index 4274e61d8..f8e0fc5b8 100644 --- a/src/ipc/broad_phase/lbvh.hpp +++ b/src/ipc/broad_phase/lbvh.hpp @@ -155,6 +155,26 @@ class LBVH : public BroadPhase { void detect_face_face_candidates( std::vector& candidates) const override; + /// @brief Find the primitives whose AABB is within a radius of a point. + /// + /// We test the built boxes against a box of half-width `radius` centered + /// at `q`, so this is a broad phase only: callers still need an exact + /// distance check. The tree is typically built with zero inflation so one + /// build serves queries at any radius, since we apply the radius to the + /// query box instead. + /// @param q Query point (2D or 3D, matching the mesh). + /// @param radius Half-width of the query box around q. + /// @param[out] vertex_ids Ids of the nearby vertices. + /// @param[out] edge_ids Ids of the nearby edges. + /// @param[out] face_ids Ids of the nearby faces. Empty in 2D, where no + /// face tree is built. + void query_point( + Eigen::ConstRef q, + double radius, + std::vector& vertex_ids, + std::vector& edge_ids, + std::vector& face_ids) const; + const Nodes& vertex_nodes() const { return vertex_bvh; } const Nodes& edge_nodes() const { return edge_bvh; } const Nodes& face_nodes() const { return face_bvh; } diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index 6d8bc9685..b9da17d8e 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -18,8 +19,12 @@ #include #include +#include +#include #include +#include #include +#include namespace ipc { @@ -44,7 +49,8 @@ void Candidates::build( const CollisionMesh& mesh, Eigen::ConstRef vertices, const double inflation_radius, - BroadPhase* broad_phase) + BroadPhase* broad_phase, + const bool all_types) { IPC_TOOLKIT_PROFILE_BLOCK("Candidates::build(static)"); @@ -60,10 +66,18 @@ void Candidates::build( broad_phase->can_vertices_collide = mesh.can_collide; broad_phase->build(vertices, mesh.edges(), mesh.faces(), inflation_radius); - broad_phase->detect_collision_candidates(*this); + broad_phase->detect_collision_candidates(*this, all_types); + has_all_types = all_types; + + // In 3D, all_types already detects vertex-vertex and edge-vertex + // candidates over the whole mesh, which includes every codim. primitive. + // We skip the codim. passes below in that case because their detectors + // clear their output first, so they would replace the whole-mesh lists + // with the codim.-only ones. + const bool detect_codim = !all_types || dim != 3; // Codim. vertices to codim. vertices: - if (mesh.num_codim_vertices()) { + if (detect_codim && mesh.num_codim_vertices()) { broad_phase->clear(); broad_phase->build( vertices(mesh.codim_vertices(), Eigen::all), // @@ -80,7 +94,8 @@ void Candidates::build( // Only need this in 3D because in 2D, the codim. edges are the same as the // edges of the boundary. Only need codim. edge to codim. vertex because // codim. edge to non-codim. vertex is the same as edge-edge or face-vertex. - if (dim == 3 && mesh.num_codim_vertices() && mesh.num_codim_edges()) { + if (detect_codim && dim == 3 && mesh.num_codim_vertices() + && mesh.num_codim_edges()) { // Extract the vertices of the codim. edges Eigen::MatrixXd CE_V; // vertices of codim. edges Eigen::MatrixXi CE; // codim. edges (indices into CEV) @@ -493,11 +508,18 @@ void Candidates::clear() ev_candidates.clear(); ee_candidates.clear(); fv_candidates.clear(); + ef_candidates.clear(); + ff_candidates.clear(); pv_candidates.clear(); + has_all_types = false; + m_sets.reset(); } CollisionStencil& Candidates::operator[](size_t i) { + // We keep the requested index for the error message, since i is reduced + // below as we walk past each list of candidates. + const size_t requested = i; if (i < vv_candidates.size()) { return vv_candidates[i]; } @@ -517,11 +539,16 @@ CollisionStencil& Candidates::operator[](size_t i) if (i < pv_candidates.size()) { return pv_candidates[i]; } - throw std::out_of_range("Candidate index is out of range!"); + log_and_throw_error( + "Candidate index {} is out of range! Total candidates: {}", requested, + size()); } const CollisionStencil& Candidates::operator[](size_t i) const { + // We keep the requested index for the error message, since i is reduced + // below as we walk past each list of candidates. + const size_t requested = i; if (i < vv_candidates.size()) { return vv_candidates[i]; } @@ -541,7 +568,9 @@ const CollisionStencil& Candidates::operator[](size_t i) const if (i < pv_candidates.size()) { return pv_candidates[i]; } - throw std::out_of_range("Candidate index is out of range!"); + log_and_throw_error( + "Candidate index {} is out of range! Total candidates: {}", requested, + size()); } bool Candidates::is_vertex_vertex(size_t i) const @@ -717,4 +746,348 @@ bool Candidates::save_obj( return true; } +// ============================================================================ +// Adjacency Sets +// ============================================================================ + +namespace { + /// @brief The nine ordered relations between primitive types. For example, + /// EV maps an edge id to the vertices near it, and VE maps a vertex id to + /// the edges near it. + enum Relation : char { VV, VE, VF, EV, EE, EF, FV, FE, FF, NUM_RELATIONS }; + + using IdPairs = std::vector>; + + /// @brief Neighbor lists for ids 0 to n - 1, stored back to back. + /// + /// We use this compressed sparse row layout instead of a hash map of + /// std::set because every query only iterates a row. A row is then a + /// contiguous sorted range with no per-id allocation, and ids are dense + /// mesh indices, so a lookup is two array reads instead of a hash. + struct AdjacencyCSR { + std::vector offsets { 0 }; + std::vector ids; + + /// @brief The sorted neighbors of id, or an empty range if id has no + /// row. + std::pair row(const index_t id) const + { + if (id < 0 || size_t(id) + 1 >= offsets.size()) { + return { nullptr, nullptr }; + } + return { ids.data() + offsets[size_t(id)], + ids.data() + offsets[size_t(id) + 1] }; + } + + /// @brief Group (row, neighbor) pairs by row, with each row sorted and + /// free of duplicates. + /// + /// Instead of sorting all the pairs at once, we count the pairs per + /// row, scatter them into place, and sort each row on its own. Rows are + /// short, so this builds several times faster than one global sort + /// (we measured 780k candidates on cloth-ball) and is linear apart + /// from the tiny per-row sorts. + static AdjacencyCSR from_pairs(const IdPairs& pairs) + { + index_t num_rows = 0; + for (const auto& [row, id] : pairs) { + num_rows = std::max(num_rows, row + 1); + } + + std::vector start(size_t(num_rows) + 1, 0); + for (const auto& [row, id] : pairs) { + ++start[size_t(row) + 1]; + } + std::partial_sum(start.begin(), start.end(), start.begin()); + + std::vector scattered(pairs.size()); + std::vector cursor(start.begin(), start.end() - 1); + for (const auto& [row, id] : pairs) { + scattered[size_t(cursor[size_t(row)]++)] = id; + } + + AdjacencyCSR csr; + csr.offsets.assign(size_t(num_rows) + 1, 0); + csr.ids.reserve(scattered.size()); + for (size_t r = 0; r < size_t(num_rows); ++r) { + const auto first = scattered.begin() + start[r]; + auto last = scattered.begin() + start[r + 1]; + std::sort(first, last); + last = std::unique(first, last); + csr.ids.insert(csr.ids.end(), first, last); + csr.offsets[r + 1] = index_t(csr.ids.size()); + } + return csr; + } + }; + + void sort_unique(std::vector& ids) + { + std::sort(ids.begin(), ids.end()); + ids.erase(std::unique(ids.begin(), ids.end()), ids.end()); + } + + /// @brief Remove id from a sorted, duplicate-free list if it is there. + void erase_sorted(std::vector& ids, const index_t id) + { + const auto it = std::lower_bound(ids.begin(), ids.end(), id); + if (it != ids.end() && *it == id) { + ids.erase(it); + } + } + + void append( + std::vector& ids, + const std::pair range) + { + ids.insert(ids.end(), range.first, range.second); + } + + void append(std::vector& ids, const std::vector& more) + { + ids.insert(ids.end(), more.begin(), more.end()); + } +} // namespace + +// Definition of the pimpl declared in candidates.hpp. Kept in the .cpp so the +// public header does not expose the storage layout. +struct Candidates::AdjacencySets { + explicit AdjacencySets(const Candidates& candidates); + + /// @brief The neighbors of a primitive. + /// + /// The neighbors of a vertex among edges (VE) and faces (VF) are exactly + /// the stored candidates, so we return a view of them. Every other relation + /// also merges in the neighbors that come from the mesh topology, which + /// builds a new list. The mesh is only needed for those, so it may be null + /// for VE and VF. + AdjacencyList + query(Relation relation, const CollisionMesh* mesh, index_t id) const; + + /// @brief The sorted neighbors of a primitive, merged with mesh topology. + std::vector + merged(Relation relation, const CollisionMesh& mesh, index_t id) const; + + /// @brief The candidate partners of each primitive, per relation. + std::array stored; +}; + +Candidates::AdjacencySets::AdjacencySets(const Candidates& candidates) +{ + // We build one relation at a time so only one pair list is alive at once. + const auto build = [&](const Relation relation, const auto& fill) { + IdPairs pairs; + fill(pairs); + stored[relation] = AdjacencyCSR::from_pairs(pairs); + }; + + build(VV, [&](IdPairs& pairs) { + pairs.reserve(2 * candidates.vv_candidates.size()); + for (const auto& vv : candidates.vv_candidates) { + pairs.emplace_back(vv.vertex0_id, vv.vertex1_id); + pairs.emplace_back(vv.vertex1_id, vv.vertex0_id); + } + }); + build(EE, [&](IdPairs& pairs) { + pairs.reserve(2 * candidates.ee_candidates.size()); + for (const auto& ee : candidates.ee_candidates) { + pairs.emplace_back(ee.edge0_id, ee.edge1_id); + pairs.emplace_back(ee.edge1_id, ee.edge0_id); + } + }); + build(FF, [&](IdPairs& pairs) { + pairs.reserve(2 * candidates.ff_candidates.size()); + for (const auto& ff : candidates.ff_candidates) { + pairs.emplace_back(ff.face0_id, ff.face1_id); + pairs.emplace_back(ff.face1_id, ff.face0_id); + } + }); + build(EV, [&](IdPairs& pairs) { + pairs.reserve(candidates.ev_candidates.size()); + for (const auto& ev : candidates.ev_candidates) { + pairs.emplace_back(ev.edge_id, ev.vertex_id); + } + }); + build(VE, [&](IdPairs& pairs) { + pairs.reserve(candidates.ev_candidates.size()); + for (const auto& ev : candidates.ev_candidates) { + pairs.emplace_back(ev.vertex_id, ev.edge_id); + } + }); + build(FV, [&](IdPairs& pairs) { + pairs.reserve(candidates.fv_candidates.size()); + for (const auto& fv : candidates.fv_candidates) { + pairs.emplace_back(fv.face_id, fv.vertex_id); + } + }); + build(VF, [&](IdPairs& pairs) { + pairs.reserve(candidates.fv_candidates.size()); + for (const auto& fv : candidates.fv_candidates) { + pairs.emplace_back(fv.vertex_id, fv.face_id); + } + }); + build(EF, [&](IdPairs& pairs) { + pairs.reserve(candidates.ef_candidates.size()); + for (const auto& ef : candidates.ef_candidates) { + pairs.emplace_back(ef.edge_id, ef.face_id); + } + }); + build(FE, [&](IdPairs& pairs) { + pairs.reserve(candidates.ef_candidates.size()); + for (const auto& ef : candidates.ef_candidates) { + pairs.emplace_back(ef.face_id, ef.edge_id); + } + }); +} + +AdjacencyList Candidates::AdjacencySets::query( + const Relation relation, const CollisionMesh* mesh, const index_t id) const +{ + if (relation == VE || relation == VF) { + const auto [first, last] = stored[relation].row(id); + return AdjacencyList::view(first, size_t(last - first)); + } + assert(mesh != nullptr); + return AdjacencyList(merged(relation, *mesh, id)); +} + +std::vector Candidates::AdjacencySets::merged( + const Relation relation, const CollisionMesh& mesh, const index_t id) const +{ + // Every case starts from the candidate partners, adds the primitives that + // share a vertex (or an edge) with this one in the mesh, and then removes + // whatever should not be its own neighbor. + // The mesh topology adds a handful of ids, so we reserve room for them up + // front. Otherwise the vector would regrow (and reallocate) while merging. + const auto partners = stored[relation].row(id); + std::vector ids; + ids.reserve(size_t(partners.second - partners.first) + 16); + append(ids, partners); + + switch (relation) { + case VV: + // In 2D, a vertex near an edge is near both of its endpoints. + if (mesh.dim() == 2) { + const auto [first, last] = stored[VE].row(id); + for (const index_t* ej = first; ej != last; ++ej) { + ids.push_back(mesh.edges()(*ej, 0)); + ids.push_back(mesh.edges()(*ej, 1)); + } + } + sort_unique(ids); + erase_sorted(ids, id); + break; + + case EV: + for (index_t lv = 0; lv < 2; ++lv) { + ids.push_back(mesh.edges()(id, lv)); + } + sort_unique(ids); + break; + + case EE: + for (index_t lv = 0; lv < 2; ++lv) { + append(ids, mesh.vertices_to_edges()[mesh.edges()(id, lv)]); + } + // In 2D the broad phase never builds edge-edge candidates, so we + // reconstruct them from the edge-vertex ones, in both directions: + // (a) the edges at vertices that are near this edge, and + // (b) the edges near this edge's own endpoints. + if (mesh.dim() == 2) { + for (const index_t vj : merged(EV, mesh, id)) { + append(ids, mesh.vertices_to_edges()[vj]); + } + for (index_t lv = 0; lv < 2; ++lv) { + append(ids, stored[VE].row(mesh.edges()(id, lv))); + } + } + sort_unique(ids); + erase_sorted(ids, id); + break; + + case EF: + for (index_t lv = 0; lv < 2; ++lv) { + append(ids, mesh.vertices_to_faces()[mesh.edges()(id, lv)]); + } + sort_unique(ids); + for (const index_t fid : mesh.edges_to_faces()[id]) { + erase_sorted(ids, fid); + } + break; + + case FV: + for (index_t lv = 0; lv < 3; ++lv) { + ids.push_back(mesh.faces()(id, lv)); + } + sort_unique(ids); + break; + + case FE: + for (index_t lv = 0; lv < 3; ++lv) { + append(ids, mesh.vertices_to_edges()[mesh.faces()(id, lv)]); + } + sort_unique(ids); + break; + + case FF: + for (index_t lv = 0; lv < 3; ++lv) { + append(ids, mesh.vertices_to_faces()[mesh.faces()(id, lv)]); + } + sort_unique(ids); + erase_sorted(ids, id); + break; + + case VE: + case VF: + default: + // These are the stored lists, which are already sorted and unique. + break; + } + return ids; +} + +void Candidates::convert_candidates_to_sets() +{ + m_sets = std::make_shared(*this); +} + +AdjacencyList Candidates::vv_set(const CollisionMesh& mesh, index_t id) const +{ + return m_sets ? m_sets->query(VV, &mesh, id) : AdjacencyList(); +} +AdjacencyList Candidates::ve_set(index_t id) const +{ + return m_sets ? m_sets->query(VE, nullptr, id) : AdjacencyList(); +} +AdjacencyList Candidates::vf_set(index_t id) const +{ + return m_sets ? m_sets->query(VF, nullptr, id) : AdjacencyList(); +} + +AdjacencyList Candidates::ev_set(const CollisionMesh& mesh, index_t id) const +{ + return m_sets ? m_sets->query(EV, &mesh, id) : AdjacencyList(); +} +AdjacencyList Candidates::ee_set(const CollisionMesh& mesh, index_t id) const +{ + return m_sets ? m_sets->query(EE, &mesh, id) : AdjacencyList(); +} +AdjacencyList Candidates::ef_set(const CollisionMesh& mesh, index_t id) const +{ + return m_sets ? m_sets->query(EF, &mesh, id) : AdjacencyList(); +} + +AdjacencyList Candidates::fv_set(const CollisionMesh& mesh, index_t id) const +{ + return m_sets ? m_sets->query(FV, &mesh, id) : AdjacencyList(); +} +AdjacencyList Candidates::fe_set(const CollisionMesh& mesh, index_t id) const +{ + return m_sets ? m_sets->query(FE, &mesh, id) : AdjacencyList(); +} +AdjacencyList Candidates::ff_set(const CollisionMesh& mesh, index_t id) const +{ + return m_sets ? m_sets->query(FF, &mesh, id) : AdjacencyList(); +} + } // namespace ipc diff --git a/src/ipc/candidates/candidates.hpp b/src/ipc/candidates/candidates.hpp index a52ae6bb0..839c3f7c5 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -9,10 +9,17 @@ #include +#include +#include +#include +#include #include namespace ipc { +// Forward declarations +class AdjacencyList; + /// @brief A class for storing and managing collision candidates. class Candidates { public: @@ -23,14 +30,20 @@ class Candidates { /// @param vertices Surface vertex positions (rowwise). /// @param inflation_radius Amount to inflate the bounding boxes. /// @param broad_phase Broad phase method to use. + /// @param all_types Also detect the candidate types only ESP needs (see + /// BroadPhase::detect_collision_candidates). Recorded in + /// has_all_types. void build( const CollisionMesh& mesh, Eigen::ConstRef vertices, const double inflation_radius = 0, - BroadPhase* broad_phase = nullptr); + BroadPhase* broad_phase = nullptr, + const bool all_types = false); /// @brief Initialize the set of continuous collision detection candidates. /// @note Assumes the trajectory is linear. + /// @note These candidates never include the ESP-only types, so + /// has_all_types is false afterwards. /// @param mesh The surface of the collision mesh. /// @param vertices_t0 Surface vertex starting positions (rowwise). /// @param vertices_t1 Surface vertex ending positions (rowwise). @@ -236,6 +249,34 @@ class Candidates { Eigen::ConstRef edges, Eigen::ConstRef faces) const; + /// @brief Build per-primitive adjacency lists from the candidates. + /// @note Required before calling any of the *_set() queries below, which + /// return empty lists until then. + /// @note The ve_set() and vf_set() results view storage inside this + /// object, so calling this again or clear() invalidates them. + void convert_candidates_to_sets(); + + /// @brief Vertices near vertex id (2D: also the endpoints of nearby edges). + AdjacencyList vv_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Edges near vertex id. + AdjacencyList ve_set(index_t id) const; + /// @brief Faces near vertex id. + AdjacencyList vf_set(index_t id) const; + + /// @brief Vertices near edge id, including its endpoints. + AdjacencyList ev_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Edges near or adjacent to edge id, excluding id. + AdjacencyList ee_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Faces near or adjacent to edge id, excluding faces containing id. + AdjacencyList ef_set(const CollisionMesh& mesh, index_t id) const; + + /// @brief Vertices near face id, including its corners. + AdjacencyList fv_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Edges near or adjacent to face id. + AdjacencyList fe_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Faces near or adjacent to face id, excluding id. + AdjacencyList ff_set(const CollisionMesh& mesh, index_t id) const; + public: std::vector vv_candidates; std::vector ev_candidates; @@ -243,8 +284,75 @@ class Candidates { std::vector fv_candidates; std::vector pv_candidates; + std::vector ef_candidates; + std::vector ff_candidates; + + /// @brief Whether these candidates include the types only ESP needs. + /// + /// In 3D, all_types adds whole-mesh vertex-vertex, edge-vertex, edge-face, + /// and face-face candidates. ESPCollisions needs them and NormalCollisions + /// would double count them, so both check this flag in 3D and throw on a + /// mismatch. build() and clear() set it; if you fill the candidate lists + /// by hand, set it to match what you filled. + bool has_all_types = false; + private: static bool default_is_active(double candidate) { return true; } + + /// @brief Adjacency lists built by convert_candidates_to_sets(). + /// + /// Held behind a shared pointer so this public header does not expose the + /// storage layout, and so copying a Candidates shares the lists instead of + /// copying their arrays. + struct AdjacencySets; + std::shared_ptr m_sets; +}; + +/// @brief A sorted list of unique primitive ids, as returned by the adjacency +/// queries of Candidates. +/// +/// The list either owns its ids or views ids stored inside the Candidates it +/// came from. We use this instead of std::set because every caller only +/// iterates, and a contiguous sorted range gives the same ascending order +/// without allocating a tree node per id. The tradeoff is that a view is +/// invalidated by clear() or convert_candidates_to_sets() on its Candidates. +class AdjacencyList { +public: + AdjacencyList() = default; + + /// @brief Take ownership of ids that are already sorted and unique. + explicit AdjacencyList(std::vector sorted_unique_ids) + : m_owned(std::move(sorted_unique_ids)) + { + } + + /// @brief View ids owned elsewhere; they must stay alive and unchanged for + /// as long as the list is used. + static AdjacencyList view(const index_t* data, const size_t size) + { + AdjacencyList list; + list.m_view = data; + list.m_view_size = size; + list.m_is_view = true; + return list; + } + + const index_t* begin() const { return m_is_view ? m_view : m_owned.data(); } + const index_t* end() const { return begin() + size(); } + size_t size() const { return m_is_view ? m_view_size : m_owned.size(); } + bool empty() const { return size() == 0; } + + /// @brief Whether id is in the list (a binary search, since it is sorted). + bool contains(const index_t id) const + { + return std::binary_search(begin(), end(), id); + } + +private: + std::vector m_owned; + const index_t* m_view = nullptr; + size_t m_view_size = 0; + bool m_is_view = false; }; } // namespace ipc diff --git a/src/ipc/candidates/collision_stencil.hpp b/src/ipc/candidates/collision_stencil.hpp index aca2156a6..71731675c 100644 --- a/src/ipc/candidates/collision_stencil.hpp +++ b/src/ipc/candidates/collision_stencil.hpp @@ -46,6 +46,8 @@ class CollisionStencil { /// @brief Get the number of vertices in the collision stencil. virtual int num_vertices() const = 0; + virtual std::string name() const { return "unknown"; } + /// @brief Get the dimension of the collision stencil. /// @param ndof Number of degrees of freedom in the stencil. /// @return The dimension of the collision stencil. diff --git a/src/ipc/candidates/edge_edge.hpp b/src/ipc/candidates/edge_edge.hpp index 0e174fb14..8660f63e4 100644 --- a/src/ipc/candidates/edge_edge.hpp +++ b/src/ipc/candidates/edge_edge.hpp @@ -19,6 +19,8 @@ class EdgeEdgeCandidate : virtual public CollisionStencil { int num_vertices() const override { return 4; } + std::string name() const override { return "ee"; } + /// @brief Get the vertex IDs for the edge-edge pair /// @param edges The edge connectivity matrix /// @param faces The face connectivity matrix diff --git a/src/ipc/candidates/edge_vertex.hpp b/src/ipc/candidates/edge_vertex.hpp index b489fbe49..b48f30012 100644 --- a/src/ipc/candidates/edge_vertex.hpp +++ b/src/ipc/candidates/edge_vertex.hpp @@ -19,6 +19,8 @@ class EdgeVertexCandidate : virtual public CollisionStencil { int num_vertices() const override { return 3; } + std::string name() const override { return "ev"; } + /// @brief Get the vertex IDs for the edge-vertex pair /// @param edges The edge connectivity matrix /// @param faces The face connectivity matrix diff --git a/src/ipc/candidates/face_vertex.hpp b/src/ipc/candidates/face_vertex.hpp index ef8cd755f..05b3ac544 100644 --- a/src/ipc/candidates/face_vertex.hpp +++ b/src/ipc/candidates/face_vertex.hpp @@ -19,6 +19,8 @@ class FaceVertexCandidate : virtual public CollisionStencil { int num_vertices() const override { return 4; } + std::string name() const override { return "fv"; } + /// @brief Get the vertex IDs for the face-vertex pair /// @param edges The edge connectivity matrix /// @param faces The face connectivity matrix diff --git a/src/ipc/candidates/vertex_vertex.hpp b/src/ipc/candidates/vertex_vertex.hpp index a2a440b6f..ec1cb80bb 100644 --- a/src/ipc/candidates/vertex_vertex.hpp +++ b/src/ipc/candidates/vertex_vertex.hpp @@ -19,6 +19,8 @@ class VertexVertexCandidate : virtual public CollisionStencil { int num_vertices() const override { return 2; } + std::string name() const override { return "vv"; } + /// @brief Get the indices of the vertices /// @param edges edge matrix of mesh /// @param faces face matrix of mesh diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index 7c609ee93..11874f8e8 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -11,6 +11,7 @@ #include #include +#include namespace ipc { @@ -32,6 +33,7 @@ CollisionMesh::CollisionMesh( CollisionMesh::CollisionMesh( const std::vector& include_vertex, const std::vector& orient_vertex, + const std::vector& obstacle_vertex, Eigen::ConstRef full_rest_positions, Eigen::ConstRef edges, Eigen::ConstRef faces, @@ -99,6 +101,18 @@ CollisionMesh::CollisionMesh( m_is_orient_vertex[i] = orient_vertex[m_vertex_to_full_vertex[i]]; } + // An assert is not enough here: a mask sized to the collision vertices + // instead of the full mesh would be read out of bounds in Release builds. + if (obstacle_vertex.size() != full_rest_positions.rows()) { + log_and_throw_error( + "obstacle_vertex has {} entries, but the full mesh has {} vertices.", + obstacle_vertex.size(), full_rest_positions.rows()); + } + m_is_obstacle_vertex.assign(m_rest_positions.rows(), false); + for (int i = 0; i < m_is_obstacle_vertex.size(); i++) { + m_is_obstacle_vertex[i] = obstacle_vertex[m_vertex_to_full_vertex[i]]; + } + // Map faces and edges to only included vertices if (!include_all_vertices) { for (int i = 0; i < m_edges.rows(); i++) { @@ -342,9 +356,10 @@ void CollisionMesh::init_areas() Eigen::VectorXd vertex_face_areas = Eigen::VectorXd::Constant(num_vertices(), -1); m_edge_areas.setConstant(m_edges.rows(), -1); + m_face_areas.setConstant(m_faces.rows(), -1); if (dim() == 3) { for (int i = 0; i < m_faces.rows(); i++) { - double face_area = triangle_area( + m_face_areas(i) = triangle_area( m_rest_positions.row(m_faces(i, 0)), m_rest_positions.row(m_faces(i, 1)), m_rest_positions.row(m_faces(i, 2))); @@ -352,11 +367,11 @@ void CollisionMesh::init_areas() for (int j = 0; j < m_faces.cols(); ++j) { vertex_face_areas[m_faces(i, j)] = std::max(vertex_face_areas[m_faces(i, j)], 0.0); - vertex_face_areas[m_faces(i, j)] += face_area / 3.0; + vertex_face_areas[m_faces(i, j)] += m_face_areas(i) / 3.0; m_edge_areas[m_faces_to_edges(i, j)] = std::max(m_edge_areas[m_faces_to_edges(i, j)], 0.0); - m_edge_areas[m_faces_to_edges(i, j)] += face_area / 3.0; + m_edge_areas[m_faces_to_edges(i, j)] += m_face_areas(i) / 3.0; } } } @@ -383,14 +398,13 @@ namespace { const char* name) { if (areas.size() != expected_size) { - throw std::invalid_argument( - fmt::format( - "{} has size {} but expected {}", name, areas.size(), - expected_size)); + log_and_throw_error( + "{} has size {} but expected {}", name, areas.size(), + expected_size); } if (!areas.allFinite() || (areas.array() < 0).any()) { - throw std::invalid_argument( - fmt::format("{} must be finite and non-negative", name)); + log_and_throw_error( + "{} must be finite and non-negative", name); } } } // namespace @@ -400,7 +414,7 @@ void CollisionMesh::set_vertex_areas( { check_areas(vertex_areas, num_vertices(), "vertex_areas"); if (are_area_jacobians_initialized()) { - throw std::runtime_error( + log_and_throw_error( "Cannot set custom vertex areas after init_area_jacobians(): the area Jacobians would no longer match."); } m_vertex_areas = vertex_areas; @@ -411,7 +425,7 @@ void CollisionMesh::set_edge_areas(Eigen::ConstRef edge_areas) { check_areas(edge_areas, num_edges(), "edge_areas"); if (are_area_jacobians_initialized()) { - throw std::runtime_error( + log_and_throw_error( "Cannot set custom edge areas after init_area_jacobians(): the area Jacobians would no longer match."); } m_edge_areas = edge_areas; @@ -421,7 +435,7 @@ void CollisionMesh::set_edge_areas(Eigen::ConstRef edge_areas) void CollisionMesh::init_area_jacobians() { if (m_has_custom_areas) { - throw std::runtime_error( + log_and_throw_error( "Cannot initialize area Jacobians after setting custom areas: shape derivatives are not supported with custom areas."); } @@ -609,7 +623,7 @@ Eigen::MatrixXi CollisionMesh::construct_faces_to_edges( if (search != edge_map.end()) { faces_to_edges(fi, fj) = search->second; } else { - throw std::runtime_error("Unable to find edge!"); + log_and_throw_error("Unable to find edge!"); } } } diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index 26636fe8b..e5559abf7 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -3,6 +3,7 @@ #include #include #include +#include namespace ipc { @@ -38,6 +39,35 @@ class CollisionMesh { Eigen::ConstRef full_rest_positions, Eigen::ConstRef edges = Eigen::MatrixXi(), Eigen::ConstRef faces = Eigen::MatrixXi(), + const Eigen::SparseMatrix& displacement_map = + Eigen::SparseMatrix()) + : CollisionMesh( + include_vertex, + orient_vertex, + std::vector(full_rest_positions.rows(), false), + full_rest_positions, + edges, + faces, + displacement_map) + { + } + + /// @brief Construct a new Collision Mesh object from a full mesh vertices. + /// @param include_vertex Vector of bools indicating whether each vertex should be included in the collision mesh. + /// @param orient_vertex Vector of bools indicating whether each vertex is orientable. + /// @param obstacle_vertex Vector of bools indicating whether each vertex comes from an obstacle. Like include_vertex, it is indexed by full mesh vertex (|V|). + /// @param full_rest_positions The vertices of the full mesh at rest (|V| × dim). + /// @param edges The edges of the collision mesh indexed into the full mesh vertices (|E| × 2). + /// @param faces The faces of the collision mesh indexed into the full mesh vertices (|F| × 3). + /// @param displacement_map The displacement mapping from displacements on the full mesh to the collision mesh. + /// @throws std::invalid_argument if obstacle_vertex does not have one entry per full mesh vertex. + CollisionMesh( + const std::vector& include_vertex, + const std::vector& orient_vertex, + const std::vector& obstacle_vertex, + Eigen::ConstRef full_rest_positions, + Eigen::ConstRef edges = Eigen::MatrixXi(), + Eigen::ConstRef faces = Eigen::MatrixXi(), const Eigen::SparseMatrix& displacement_map = Eigen::SparseMatrix()); @@ -110,6 +140,33 @@ class CollisionMesh { return m_is_orient_vertex[i]; } + /// @brief Check if vertex i is from an obstacle. + bool is_obstacle_vertex(const index_t i) const + { + return m_is_obstacle_vertex[i]; + } + + /// @brief Check if edge i is from an obstacle. + /// @note An edge is an obstacle only if both of its vertices are obstacle + /// vertices. An edge with one dynamic vertex still moves, so we treat it + /// as dynamic (e.g., a rope whose end is anchored to a static obstacle). + bool is_obstacle_edge(const index_t i) const + { + return is_obstacle_vertex(edges()(i, 0)) + && is_obstacle_vertex(edges()(i, 1)); + } + + /// @brief Check if face i is from an obstacle. + /// @note A face is an obstacle only if all three of its vertices are + /// obstacle vertices. As with edges, a face touching a dynamic vertex + /// moves, so we treat it as dynamic. + bool is_obstacle_face(const index_t i) const + { + return is_obstacle_vertex(faces()(i, 0)) + && is_obstacle_vertex(faces()(i, 1)) + && is_obstacle_vertex(faces()(i, 2)); + } + /// @brief Get the indices of codimensional edges of the collision mesh (|CE| × 1). const Eigen::VectorXi& codim_edges() const { return m_codim_edges; } @@ -228,7 +285,7 @@ class CollisionMesh { const std::vector>& vertex_vertex_adjacencies() const { if (!are_adjacencies_initialized()) { - throw std::runtime_error( + log_and_throw_error( "Vertex-vertex adjacencies not initialized. Call init_adjacencies() first."); } return m_vertex_vertex_adjacencies; @@ -238,7 +295,7 @@ class CollisionMesh { const std::vector>& vertex_edge_adjacencies() const { if (!are_adjacencies_initialized()) { - throw std::runtime_error( + log_and_throw_error( "Vertex-edge adjacencies not initialized. Call init_adjacencies() first."); } return m_vertex_edge_adjacencies; @@ -248,7 +305,7 @@ class CollisionMesh { const std::vector>& vertex_face_adjacencies() const { if (!are_adjacencies_initialized()) { - throw std::runtime_error( + log_and_throw_error( "Vertex-face adjacencies not initialized. Call init_adjacencies() first."); } return m_vertex_face_adjacencies; @@ -258,7 +315,7 @@ class CollisionMesh { const std::vector>& edge_vertex_adjacencies() const { if (!are_adjacencies_initialized()) { - throw std::runtime_error( + log_and_throw_error( "Edge-vertex adjacencies not initialized. Call init_adjacencies() first."); } return m_edge_vertex_adjacencies; @@ -288,6 +345,9 @@ class CollisionMesh { /// @brief Get the barycentric area of the vertices. const Eigen::VectorXd& vertex_areas() const { return m_vertex_areas; } + /// @brief Get the area of the faces (3D). + const Eigen::VectorXd& face_areas() const { return m_face_areas; } + /// @brief Override the vertex areas used as collision weights. /// @note Use this when the quadrature weight of a vertex is not its barycentric area (e.g., axisymmetric models where each vertex stands for a ring of contact). Collisions built before this call keep their old weights. /// @note Custom areas do not support shape derivatives: this throws if the area Jacobians were already initialized, and init_area_jacobians() throws afterwards. @@ -301,7 +361,7 @@ class CollisionMesh { vertex_area_gradient(const size_t vi) const { if (!are_area_jacobians_initialized()) { - throw std::runtime_error( + log_and_throw_error( "Vertex area Jacobian not initialized. Call init_area_jacobians() first."); } return m_vertex_area_jacobian[vi]; @@ -326,7 +386,7 @@ class CollisionMesh { const Eigen::SparseVector& edge_area_gradient(const size_t ei) const { if (!are_area_jacobians_initialized()) { - throw std::runtime_error( + log_and_throw_error( "Edge area Jacobian not initialized. Call init_area_jacobians() first."); } return m_edge_area_jacobian[ei]; @@ -402,6 +462,8 @@ class CollisionMesh { std::vector m_is_codim_vertex; /// @brief The mask of orientable vertices (|V|). std::vector m_is_orient_vertex; + /// @brief The mask of obstacle vertices (|V|). + std::vector m_is_obstacle_vertex; /// @brief The indices of codimensional vertices (|CV| × 1). Eigen::VectorXi m_codim_vertices; /// @brief The mask of codimensional edges (|E|). @@ -465,6 +527,10 @@ class CollisionMesh { /// @brief Were the areas overridden by the user (so the Jacobians no longer match)? bool m_has_custom_areas = false; + /// @brief Face areas + /// 3D: per-face area + Eigen::VectorXd m_face_areas; + // Stored as a std::vector so it is easier to access the rows directly. /// @brief The rows of the Jacobian of the vertex areas vector. std::vector> m_vertex_area_jacobian; diff --git a/src/ipc/collisions/normal/normal_collisions.cpp b/src/ipc/collisions/normal/normal_collisions.cpp index 607657c68..89c8382ac 100644 --- a/src/ipc/collisions/normal/normal_collisions.cpp +++ b/src/ipc/collisions/normal/normal_collisions.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -10,7 +11,7 @@ #include #include -#include // std::out_of_range +#include // std::out_of_range, std::invalid_argument namespace ipc { @@ -42,6 +43,16 @@ void NormalCollisions::build( assert(vertices.rows() == mesh.num_vertices()); IPC_TOOLKIT_PROFILE_BLOCK("NormalCollisions::build(candidates)"); + // In 3D, all_types adds whole-mesh vertex-vertex and edge-vertex + // candidates. The edge-edge and face-vertex candidates already produce + // those contacts, and the builder merges matching collisions by adding + // their weights, so the barrier would be over-counted. We reject these + // candidates instead of silently returning the wrong energy. + if (mesh.dim() == 3 && candidates.has_all_types) { + log_and_throw_error( + "NormalCollisions::build: 3D candidates must be built with all_types = false"); + } + clear(); // Cull the candidates by measuring the distance and dropping those that are @@ -52,7 +63,7 @@ void NormalCollisions::build( tbb::enumerable_thread_specific storage( use_area_weighting(), enable_shape_derivatives(), - collision_set_type() == CollisionSetType::OGC); + collision_set_type() == CollisionSetType::OGC, m_skip_obstacles); tbb::parallel_for( size_t(0), candidates.vv_candidates.size(), [&](size_t i) { @@ -129,6 +140,9 @@ void NormalCollisions::build( NormalCollisionsBuilder::merge(storage, *this); for (const auto& [plane, vertex_id] : candidates.pv_candidates) { + if (m_skip_obstacles && mesh.is_obstacle_vertex(vertex_id)) { + continue; + } const double d = plane.signedDistance(vertices.row(vertex_id)); if (d < dhat + dmin) { const double weight = @@ -274,7 +288,7 @@ NormalCollision& NormalCollisions::operator[](size_t i) if (i < pv_collisions.size()) { return pv_collisions[i]; } - throw std::out_of_range("Collision index is out of range!"); + log_and_throw_error("Collision index is out of range!"); } const NormalCollision& NormalCollisions::operator[](size_t i) const @@ -298,7 +312,7 @@ const NormalCollision& NormalCollisions::operator[](size_t i) const if (i < pv_collisions.size()) { return pv_collisions[i]; } - throw std::out_of_range("Collision index is out of range!"); + log_and_throw_error("Collision index is out of range!"); } bool NormalCollisions::is_vertex_vertex(size_t i) const diff --git a/src/ipc/collisions/normal/normal_collisions.hpp b/src/ipc/collisions/normal/normal_collisions.hpp index 47347e04d..7e82868a4 100644 --- a/src/ipc/collisions/normal/normal_collisions.hpp +++ b/src/ipc/collisions/normal/normal_collisions.hpp @@ -55,6 +55,8 @@ class NormalCollisions { /// @param vertices Vertices of the collision mesh. /// @param dhat The activation distance of the barrier. /// @param dmin Minimum distance. + /// @throws std::invalid_argument In 3D, if the candidates were built with + /// all_types = true (see Candidates::has_all_types). void build( const Candidates& candidates, const CollisionMesh& mesh, @@ -188,10 +190,31 @@ class NormalCollisions { /// @brief Plane-vertex normal collisions. std::vector pv_collisions; + /// @brief Get if the collision set skips obstacle surfaces. + bool skip_obstacles() const { return m_skip_obstacles; } + + /// @brief Set if the collision set skips obstacle surfaces. + /// Obstacles are marked with CollisionMesh's obstacle_vertex. When set, + /// collisions are integrated over non-obstacle surfaces only: edge-vertex, + /// face-vertex, and plane-vertex pairs whose vertex is an obstacle are + /// dropped, as are vertex-vertex and edge-edge pairs between two + /// obstacles. A vertex-vertex or edge-edge pair with one obstacle keeps + /// only the non-obstacle primitive's share of the weight: with area + /// weighting, the obstacle's area no longer contributes; without it, the + /// weight is halved. The IMPROVED_MAX_APPROX corrections follow the same + /// rule, so the net weights stay non-negative. + /// @warning This must be set before the collisions are built. + /// @param skip_obstacles If the collision set should skip obstacle surfaces. + void set_skip_obstacles(const bool skip_obstacles) + { + m_skip_obstacles = skip_obstacles; + } + protected: CollisionSetType m_collision_set_type = CollisionSetType::IPC; bool m_use_area_weighting = false; bool m_enable_shape_derivatives = false; + bool m_skip_obstacles = false; }; } // namespace ipc diff --git a/src/ipc/collisions/normal/normal_collisions_builder.cpp b/src/ipc/collisions/normal/normal_collisions_builder.cpp index bf5889b1a..94e98b749 100644 --- a/src/ipc/collisions/normal/normal_collisions_builder.cpp +++ b/src/ipc/collisions/normal/normal_collisions_builder.cpp @@ -14,10 +14,12 @@ namespace ipc { NormalCollisionsBuilder::NormalCollisionsBuilder( const bool _use_area_weighting, const bool _enable_shape_derivatives, - const bool _use_ogc) + const bool _use_ogc, + const bool _skip_obstacles) : use_area_weighting(_use_area_weighting) , enable_shape_derivatives(_enable_shape_derivatives) , use_ogc(_use_ogc) + , skip_obstacles(_skip_obstacles) { } @@ -31,6 +33,12 @@ void NormalCollisionsBuilder::add_vertex_vertex_collision( { const auto& [vi, vj] = candidate; + const bool is_obstacle_vi = skip_obstacles && mesh.is_obstacle_vertex(vi); + const bool is_obstacle_vj = skip_obstacles && mesh.is_obstacle_vertex(vj); + if (is_obstacle_vi && is_obstacle_vj) { + return; + } + const double distance = point_point_distance(vertices.row(vi), vertices.row(vj)); if (!is_active(distance)) { @@ -38,18 +46,25 @@ void NormalCollisionsBuilder::add_vertex_vertex_collision( } // ÷ 2 to handle double counting. Sum vertex areas because duplicate - // vertex-vertex candidates were removed. - const double weight = use_area_weighting - ? (0.5 * (mesh.vertex_area(vi) + mesh.vertex_area(vj))) - : 1; + // vertex-vertex candidates were removed. When we skip obstacles, we drop + // the obstacle vertex's half of the integral, so only the non-obstacle + // vertex's area contributes. + double weight = is_obstacle_vi || is_obstacle_vj ? 0.5 : 1; + if (use_area_weighting) { + weight = 0.5 + * ((is_obstacle_vi ? 0 : mesh.vertex_area(vi)) + + (is_obstacle_vj ? 0 : mesh.vertex_area(vj))); + } Eigen::SparseVector weight_gradient; if (enable_shape_derivatives) { - weight_gradient = use_area_weighting - ? (0.5 - * (mesh.vertex_area_gradient(vi) - + mesh.vertex_area_gradient(vj))) - : Eigen::SparseVector(vertices.size()); + weight_gradient = Eigen::SparseVector(vertices.size()); + if (use_area_weighting && !is_obstacle_vi) { + weight_gradient += 0.5 * mesh.vertex_area_gradient(vi); + } + if (use_area_weighting && !is_obstacle_vj) { + weight_gradient += 0.5 * mesh.vertex_area_gradient(vj); + } } VertexVertexNormalCollision vv(vi, vj, weight, weight_gradient); @@ -64,6 +79,11 @@ void NormalCollisionsBuilder::add_edge_vertex_collision( const std::function& is_active) { const auto& [ei, vi] = candidate; + + if (skip_obstacles && mesh.is_obstacle_vertex(vi)) { + return; + } + const auto [v, e0, e1, _] = candidate.vertices(vertices, mesh.edges(), mesh.faces()); @@ -145,6 +165,12 @@ void NormalCollisionsBuilder::add_edge_edge_collision( { const auto& [eai, ebi] = candidate; + const bool is_obstacle_ea = skip_obstacles && mesh.is_obstacle_edge(eai); + const bool is_obstacle_eb = skip_obstacles && mesh.is_obstacle_edge(ebi); + if (is_obstacle_ea && is_obstacle_eb) { + return; + } + const auto [ea0i, ea1i, eb0i, eb1i] = candidate.vertex_ids(mesh.edges(), mesh.faces()); @@ -177,17 +203,27 @@ void NormalCollisionsBuilder::add_edge_edge_collision( ee_cross_norm_sqr < eps_x ? EdgeEdgeDistanceType::EA_EB : actual_dtype; // ÷ 4 to handle double counting and PT + EE for correct integration. - // Sum edge areas because duplicate edge candidates were removed. - const double weight = use_area_weighting - ? (0.25 * (mesh.edge_area(eai) + mesh.edge_area(ebi))) - : 1; + // Sum edge areas because duplicate edge candidates were removed. When we + // skip obstacles, we drop the obstacle edge's half of the integral, so + // only the non-obstacle edge's area contributes. Halving the summed weight + // instead would keep part of the obstacle's area, making the contact + // stiffer whenever the obstacle is coarser than the deformable surface. + double weight = is_obstacle_ea || is_obstacle_eb ? 0.5 : 1; + if (use_area_weighting) { + weight = 0.25 + * ((is_obstacle_ea ? 0 : mesh.edge_area(eai)) + + (is_obstacle_eb ? 0 : mesh.edge_area(ebi))); + } Eigen::SparseVector weight_gradient; if (enable_shape_derivatives) { - weight_gradient = use_area_weighting - ? (0.25 - * (mesh.edge_area_gradient(eai) + mesh.edge_area_gradient(ebi))) - : Eigen::SparseVector(vertices.size()); + weight_gradient = Eigen::SparseVector(vertices.size()); + if (use_area_weighting && !is_obstacle_ea) { + weight_gradient += 0.25 * mesh.edge_area_gradient(eai); + } + if (use_area_weighting && !is_obstacle_eb) { + weight_gradient += 0.25 * mesh.edge_area_gradient(ebi); + } } switch (dtype) { @@ -243,6 +279,10 @@ void NormalCollisionsBuilder::add_face_vertex_collision( const std::function& is_active) { const auto& [fi, vi] = candidate; + if (skip_obstacles && mesh.is_obstacle_vertex(vi)) { + return; + } + const index_t f0i = mesh.faces()(fi, 0), f1i = mesh.faces()(fi, 1), f2i = mesh.faces()(fi, 2); @@ -345,6 +385,12 @@ void NormalCollisionsBuilder::add_edge_vertex_negative_vertex_vertex_collision( const auto add_weight = [&](const size_t vi, const size_t vj, double& weight, Eigen::SparseVector& weight_gradient) { + // This corrects the edge-vertex terms integrated over vi, which we + // dropped if vi is an obstacle. + if (skip_obstacles && mesh.is_obstacle_vertex(vi)) { + return; + } + const auto& incident_vertices = mesh.vertex_vertex_adjacencies()[vj]; const bool is_vi_incident = std::binary_search( incident_vertices.begin(), incident_vertices.end(), vi); @@ -388,6 +434,12 @@ void NormalCollisionsBuilder::add_face_vertex_positive_vertex_vertex_collision( const auto add_weight = [&](const size_t vi, const size_t vj, double& weight, Eigen::SparseVector& weight_gradient) { + // This corrects the face-vertex terms integrated over vi, which we + // dropped if vi is an obstacle. + if (skip_obstacles && mesh.is_obstacle_vertex(vi)) { + return; + } + const auto& incident_vertices = mesh.vertex_vertex_adjacencies()[vj]; assert( std::is_sorted(incident_vertices.begin(), incident_vertices.end())); @@ -430,6 +482,12 @@ void NormalCollisionsBuilder::add_face_vertex_negative_edge_vertex_collision( const auto& [ei, vi] = candidate; assert(vi != mesh.edges()(ei, 0) && vi != mesh.edges()(ei, 1)); + // This corrects the face-vertex terms integrated over vi, which we dropped + // if vi is an obstacle. + if (skip_obstacles && mesh.is_obstacle_vertex(vi)) { + return; + } + const auto& incident_vertices = mesh.edge_vertex_adjacencies()[ei]; assert(std::is_sorted(incident_vertices.begin(), incident_vertices.end())); const bool is_vi_incident = std::binary_search( @@ -470,9 +528,20 @@ void NormalCollisionsBuilder::add_edge_edge_negative_edge_vertex_collision( const index_t ea0 = mesh.edges()(ea, 0), ea1 = mesh.edges()(ea, 1); assert(p != ea0 && p != ea1); - // ÷ 4 to handle double counting and PT + EE for correct integration - const double weight = - use_area_weighting ? (-0.25 * mesh.edge_area(ea)) : -1; + // This corrects the edge-edge terms integrated over ea, which we dropped + // if ea is an obstacle. + if (skip_obstacles && mesh.is_obstacle_edge(ea)) { + return; + } + + // ÷ 4 to handle double counting and PT + EE for correct integration. + // Without area weighting, edge-edge pairs with one obstacle edge have half + // weight, so we halve their correction too. Otherwise, the k edges incident + // to an obstacle p would sum to k/2 - (k - 1) < 0 for k ≥ 3. + double weight = use_area_weighting ? (-0.25 * mesh.edge_area(ea)) : -1; + if (!use_area_weighting && skip_obstacles && mesh.is_obstacle_vertex(p)) { + weight = -0.5; + } Eigen::SparseVector weight_gradient; if (enable_shape_derivatives) { weight_gradient = use_area_weighting diff --git a/src/ipc/collisions/normal/normal_collisions_builder.hpp b/src/ipc/collisions/normal/normal_collisions_builder.hpp index cc2eef4bd..56815ee13 100644 --- a/src/ipc/collisions/normal/normal_collisions_builder.hpp +++ b/src/ipc/collisions/normal/normal_collisions_builder.hpp @@ -19,7 +19,8 @@ class NormalCollisionsBuilder { NormalCollisionsBuilder( const bool use_area_weighting, const bool enable_shape_derivatives, - const bool use_ogc); + const bool use_ogc, + const bool skip_obstacles = false); void add_vertex_vertex_collision( const CollisionMesh& mesh, @@ -158,6 +159,7 @@ class NormalCollisionsBuilder { const bool use_area_weighting; const bool enable_shape_derivatives; const bool use_ogc; + const bool skip_obstacles; // turns off obstacle integration }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/collisions/tangential/tangential_collision.hpp b/src/ipc/collisions/tangential/tangential_collision.hpp index ce25bdaaf..d5cf7376b 100644 --- a/src/ipc/collisions/tangential/tangential_collision.hpp +++ b/src/ipc/collisions/tangential/tangential_collision.hpp @@ -1,8 +1,8 @@ #pragma once #include +#include #include -#include #include #include @@ -93,8 +93,8 @@ class TangentialCollision : virtual public CollisionStencil { /// @brief Normal force magnitude double normal_force_magnitude = 0; - /// @brief SmoothCollision instance to compute normal force magnitude and its derivatives - std::shared_ptr smooth_collision; + /// @brief GCPCollision instance to compute normal force magnitude and its derivatives + std::shared_ptr gcp_collision; /// @brief Ratio between normal and static tangential forces (e.g., friction coefficient) double mu_s = 0; diff --git a/src/ipc/collisions/tangential/tangential_collisions.cpp b/src/ipc/collisions/tangential/tangential_collisions.cpp index 1946afcc3..65ff9e5f3 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.cpp +++ b/src/ipc/collisions/tangential/tangential_collisions.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -173,8 +174,8 @@ void TangentialCollisions::build( void TangentialCollisions::build( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const SmoothCollisions& collisions, - const SmoothContactParameters& params, + const GCPCollisions& collisions, + const GCPParameters& params, const double normal_stiffness, Eigen::ConstRef mu_s, Eigen::ConstRef mu_k, @@ -201,8 +202,9 @@ void TangentialCollisions::build( if (mesh.dim() == 3) { TangentialCollision* ptr = nullptr; - if (const auto* const cvv = dynamic_cast< - const SmoothCollisionTemplate*>(&cc)) { + if (const auto* const cvv = + dynamic_cast*>( + &cc)) { Eigen::VectorXd collision_points = cvv->core_dof(vertices); FC_vv.emplace_back( VertexVertexNormalCollision( @@ -216,7 +218,7 @@ void TangentialCollisions::build( ptr = &(FC_vv.back()); } else if ( const auto* const cev = - dynamic_cast*>( + dynamic_cast*>( &cc)) { Eigen::VectorXd collision_points = cev->core_dof(vertices); collision_points = @@ -242,7 +244,7 @@ void TangentialCollisions::build( ptr = &(FC_ev.back()); } else if ( const auto* const cee = - dynamic_cast*>( + dynamic_cast*>( &cc)) { Eigen::VectorXd collision_points = cee->core_dof(vertices); const auto vert_ids = cee->core_vertex_ids(); @@ -281,7 +283,7 @@ void TangentialCollisions::build( ptr = &(FC_ee.back()); } else if ( const auto* const cfv = - dynamic_cast*>( + dynamic_cast*>( &cc)) { Eigen::VectorXd collision_points = cfv->core_dof(vertices); collision_points = @@ -307,12 +309,13 @@ void TangentialCollisions::build( ptr = &(FC_fv.back()); } if (ptr) { - ptr->smooth_collision = collisions.collisions[i]; + ptr->gcp_collision = collisions.collisions[i]; } } else { TangentialCollision* ptr = nullptr; - if (const auto* const cvv = dynamic_cast< - const SmoothCollisionTemplate*>(&cc)) { + if (const auto* const cvv = + dynamic_cast*>( + &cc)) { Eigen::VectorXd collision_points = cvv->core_dof(vertices); FC_vv.emplace_back( VertexVertexNormalCollision( @@ -326,7 +329,7 @@ void TangentialCollisions::build( ptr = &(FC_vv.back()); } else if ( const auto* const cev = - dynamic_cast*>( + dynamic_cast*>( &cc)) { Eigen::VectorXd collision_points = cev->core_dof(vertices); collision_points = @@ -352,7 +355,7 @@ void TangentialCollisions::build( ptr = &(FC_ev.back()); } if (ptr) { - ptr->smooth_collision = collisions.collisions[i]; + ptr->gcp_collision = collisions.collisions[i]; } } } @@ -465,7 +468,8 @@ TangentialCollision& TangentialCollisions::operator[](size_t i) if (i < pv_collisions.size()) { return pv_collisions[i]; } - throw std::out_of_range("Friction collision index is out of range!"); + log_and_throw_error( + "Friction collision index is out of range!"); } const TangentialCollision& TangentialCollisions::operator[](size_t i) const @@ -489,7 +493,8 @@ const TangentialCollision& TangentialCollisions::operator[](size_t i) const if (i < pv_collisions.size()) { return pv_collisions[i]; } - throw std::out_of_range("Friction collision index is out of range!"); + log_and_throw_error( + "Friction collision index is out of range!"); } } // namespace ipc diff --git a/src/ipc/collisions/tangential/tangential_collisions.hpp b/src/ipc/collisions/tangential/tangential_collisions.hpp index e3d9ff8ed..9d230dfa0 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.hpp +++ b/src/ipc/collisions/tangential/tangential_collisions.hpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include #include @@ -16,6 +16,9 @@ namespace ipc { +class ESPCollisions; +struct ESPParameters; + class TangentialCollisions { public: /// @brief The type of the collisions. @@ -91,14 +94,42 @@ class TangentialCollisions { void build( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const SmoothCollisions& collisions, - const SmoothContactParameters& params, + const GCPCollisions& collisions, + const GCPParameters& params, const double normal_stiffness, Eigen::ConstRef mu_s, Eigen::ConstRef mu_k, const std::function& blend_mu = default_blend_mu); + /// @brief Build the tangential collisions for ESP contact. + /// @param mesh The collision mesh. + /// @param vertices The vertices of the mesh. + /// @param collisions The set of ESP collisions. + /// @param params Parameters of Extremum Sum Potential (ESP). + /// @param normal_stiffness Stiffness of the normal potential. + /// @param mu_s The static friction coefficient per vertex. + /// @param mu_k The kinetic friction coefficient per vertex. + /// @param blend_mu Function to blend vertex-based coefficients of friction. Defaults to average. + /// @param normalize_weights Divide each face's weight by its total weight + /// (vertex or face-rule weights plus the edge-edge mollifier + /// weights). Unlike ESPPotential with use_near_far = true, near and + /// far parts are not normalized separately. 3D only. + /// @throws std::invalid_argument if params' quadrature differs from the + /// one collisions were built with (see + /// ESPCollisions::check_quadrature()). + void build( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const ESPCollisions& collisions, + const ESPParameters& params, + const double normal_stiffness, + Eigen::ConstRef mu_s, + Eigen::ConstRef mu_k, + const std::function& blend_mu = + default_blend_mu, + const bool normalize_weights = true); + /// @brief Set lagged effective μ to scalar mu_s/mu_k on every collision (after build). void reset_lagged_anisotropic_friction_coefficients(); diff --git a/src/ipc/distance/CMakeLists.txt b/src/ipc/distance/CMakeLists.txt index 9ede9e1e7..b769bb4a4 100644 --- a/src/ipc/distance/CMakeLists.txt +++ b/src/ipc/distance/CMakeLists.txt @@ -1,7 +1,10 @@ set(SOURCES distance_type.hpp + distance_type_exact.cpp + distance_type_exact.hpp edge_edge.hpp edge_edge_mollifier.hpp + exact_vector.hpp line_line.hpp point_edge.hpp point_line.hpp diff --git a/src/ipc/distance/distance_type.cpp b/src/ipc/distance/distance_type.cpp index f5bf59d84..215cadd3b 100644 --- a/src/ipc/distance/distance_type.cpp +++ b/src/ipc/distance/distance_type.cpp @@ -250,19 +250,18 @@ void warn_degenerate_point_edge_host() noexcept void throw_invalid_distance_type_host(const char* function) { - throw std::invalid_argument( - fmt::format("{}: invalid distance type", function)); + log_and_throw_error( + "{}: invalid distance type", function); } void throw_auto_requires_explicit_dtype_host(const char* function) { - throw std::invalid_argument( - fmt::format( - "{}: an explicit distance type is required for non-floating-point " - "scalars; resolving AUTO means comparing single ordered values, " - "which an autodiff, SIMD batch, or interval scalar does not " - "provide", - function)); + log_and_throw_error( + "{}: an explicit distance type is required for non-floating-point " + "scalars; resolving AUTO means comparing single ordered values, " + "which an autodiff, SIMD batch, or interval scalar does not " + "provide", + function); } #endif // !__CUDACC__ diff --git a/src/ipc/distance/distance_type_exact.cpp b/src/ipc/distance/distance_type_exact.cpp new file mode 100644 index 000000000..7ecb61d60 --- /dev/null +++ b/src/ipc/distance/distance_type_exact.cpp @@ -0,0 +1,299 @@ +#include "distance_type_exact.hpp" + +#include + +#include + +// fp_filters.h must come first: the PSM #defines FPG_UNCERTAIN_VALUE (to the +// same value), which would otherwise clobber the constant's declaration. +// clang-format off +#include "fp_filters.h" +#include "exact_vector.hpp" +// clang-format on + +namespace ipc { + +namespace { + + // The predicates below take raw coordinate pointers and live in this + // translation unit, so a classification's whole chain of filtered + // predicates inlines into its public entry point. The entry point converts + // its arguments, checks DistanceTypeConfig, and initializes the exact + // arithmetic once, instead of once per nested point-edge test. Only the + // exact fallbacks, taken when a filter is inconclusive, stay out of line. + + using ExReal = ExactReal; + using ExVec3 = ExactVector3; + + void init_predicates() { init_exact_arithmetic(); } + + /// @brief Exact copy of a 2D or 3D point (z = 0 in 2D). + ExVec3 make_exact(const double* v, const int dim) + { + return ExVec3::from(v, dim); + } + + int exact_sign(const ExReal& s) { return int(s.sign()); } + + // -- Exact fallbacks ------------------------------------------------------ + + /// @brief Sign of dot(p1-p0, p2-p0) in exact arithmetic. + int dot3_exact( + const double* _p0, const double* _p1, const double* _p2, const int dim) + { + logger().trace("dot3 filter uncertain - fallback to exact arithmetic"); + const ExVec3 p0 = make_exact(_p0, dim); + const ExVec3 p1 = make_exact(_p1, dim); + const ExVec3 p2 = make_exact(_p2, dim); + return exact_sign((p1 - p0).dot(p2 - p0)); + } + + /// @brief Sign of dot(cross(p1-p0, p2-p0), cross(p3-p0, p1-p0)) in exact + /// arithmetic. + int cross_dot_cross_1_exact( + const double* _p0, + const double* _p1, + const double* _p2, + const double* _p3) + { + logger().trace( + "cross_dot_cross_1 filter uncertain - fallback to exact arithmetic"); + const ExVec3 p0 = make_exact(_p0, 3); + const ExVec3 p1 = make_exact(_p1, 3); + const ExVec3 p2 = make_exact(_p2, 3); + const ExVec3 p3 = make_exact(_p3, 3); + return exact_sign( + (p1 - p0).cross(p2 - p0).dot((p3 - p0).cross(p1 - p0))); + } + + /// @brief Sign of dot(cross(p1-p0, p2-p0), cross(p3-p0, p1-p2)) in exact + /// arithmetic. + int cross_dot_cross_2_exact( + const double* _p0, + const double* _p1, + const double* _p2, + const double* _p3) + { + logger().trace( + "cross_dot_cross_2 filter uncertain - fallback to exact arithmetic"); + const ExVec3 p0 = make_exact(_p0, 3); + const ExVec3 p1 = make_exact(_p1, 3); + const ExVec3 p2 = make_exact(_p2, 3); + const ExVec3 p3 = make_exact(_p3, 3); + return exact_sign( + (p1 - p0).cross(p2 - p0).dot((p3 - p0).cross(p1 - p2))); + } + + // -- Filtered predicates -------------------------------------------------- + + /// @brief Sign of dot(p1-p0, p2-p0). + template + inline int dot3(const double* p0, const double* p1, const double* p2) + { + int s; + if constexpr (dim == 2) { + s = dot3_2d_filter(p0, p1, p2); + } else { + s = dot3_3d_filter(p0, p1, p2); + } + return s != FPG_UNCERTAIN_VALUE ? s : dot3_exact(p0, p1, p2, dim); + } + + /// @brief Sign of dot(cross(p1-p0, p2-p0), cross(p3-p0, p1-p0)) + /// = dot(p1-p0, p3-p0) * dot(p1-p0, p2-p0) + /// - dot(p1-p0, p1-p0) * dot(p2-p0, p3-p0). + inline int cross_dot_cross_1( + const double* p0, const double* p1, const double* p2, const double* p3) + { + const int s = cross_dot_cross_1_3d_filter(p0, p1, p2, p3); + return s != FPG_UNCERTAIN_VALUE + ? s + : cross_dot_cross_1_exact(p0, p1, p2, p3); + } + + /// @brief Sign of dot(cross(p1-p0, p2-p0), cross(p3-p0, p1-p2)) + /// = dot(p1-p0, p3-p0) * dot(p1-p2, p2-p0) + /// - dot(p1-p0, p1-p2) * dot(p2-p0, p3-p0). + inline int cross_dot_cross_2( + const double* p0, const double* p1, const double* p2, const double* p3) + { + const int s = cross_dot_cross_2_3d_filter(p0, p1, p2, p3); + return s != FPG_UNCERTAIN_VALUE + ? s + : cross_dot_cross_2_exact(p0, p1, p2, p3); + } + + // -- Classifiers ---------------------------------------------------------- + + template + inline PointEdgeDistanceType + point_edge_type(const double* p, const double* e0, const double* e1) + { + if (dot3(e0, p, e1) <= 0) { + return PointEdgeDistanceType::P_E0; + } else if (dot3(e1, p, e0) <= 0) { + return PointEdgeDistanceType::P_E1; + } else { + return PointEdgeDistanceType::P_E; + } + } + + inline PointTriangleDistanceType point_triangle_type( + const double* p, const double* t0, const double* t1, const double* t2) + { + const int dot01 = dot3<3>(t0, p, t1); + const int dot02 = dot3<3>(t0, p, t2); + if (dot01 <= 0 && dot02 <= 0) { + return PointTriangleDistanceType::P_T0; + } + const int dot12 = dot3<3>(t1, p, t2); + const int dot10 = dot3<3>(t1, p, t0); + if (dot12 <= 0 && dot10 <= 0) { + return PointTriangleDistanceType::P_T1; + } + const int dot20 = dot3<3>(t2, p, t0); + const int dot21 = dot3<3>(t2, p, t1); + if (dot20 <= 0 && dot21 <= 0) { + return PointTriangleDistanceType::P_T2; + } + + if (cross_dot_cross_1(t0, t1, t2, p) >= 0 && dot01 > 0 && dot10 > 0) { + return PointTriangleDistanceType::P_E0; + } + if (cross_dot_cross_1(t1, t2, t0, p) >= 0 && dot12 > 0 && dot21 > 0) { + return PointTriangleDistanceType::P_E1; + } + if (cross_dot_cross_1(t2, t0, t1, p) >= 0 && dot20 > 0 && dot02 > 0) { + return PointTriangleDistanceType::P_E2; + } + + return PointTriangleDistanceType::P_T; + } + + inline EdgeEdgeDistanceType edge_edge_type( + const double* ea0, + const double* ea1, + const double* eb0, + const double* eb1) + { + using PE = PointEdgeDistanceType; + + const PE dt_ea0 = point_edge_type<3>(ea0, eb0, eb1); + const PE dt_ea1 = point_edge_type<3>(ea1, eb0, eb1); + + if (dt_ea0 == PE::P_E0 && dot3<3>(ea0, eb0, ea1) <= 0) { + return EdgeEdgeDistanceType::EA0_EB0; + } + if (dt_ea0 == PE::P_E1 && dot3<3>(ea0, eb1, ea1) <= 0) { + return EdgeEdgeDistanceType::EA0_EB1; + } + if (dt_ea1 == PE::P_E0 && dot3<3>(ea1, eb0, ea0) <= 0) { + return EdgeEdgeDistanceType::EA1_EB0; + } + if (dt_ea1 == PE::P_E1 && dot3<3>(ea1, eb1, ea0) <= 0) { + return EdgeEdgeDistanceType::EA1_EB1; + } + + const PE dt_eb0 = point_edge_type<3>(eb0, ea0, ea1); + const PE dt_eb1 = point_edge_type<3>(eb1, ea0, ea1); + + if (dt_eb0 == PE::P_E && cross_dot_cross_2(eb0, ea0, ea1, eb1) >= 0) { + return EdgeEdgeDistanceType::EA_EB0; + } + if (dt_eb1 == PE::P_E && cross_dot_cross_2(eb1, ea0, ea1, eb0) >= 0) { + return EdgeEdgeDistanceType::EA_EB1; + } + if (dt_ea0 == PE::P_E && cross_dot_cross_2(ea0, eb0, eb1, ea1) >= 0) { + return EdgeEdgeDistanceType::EA0_EB; + } + if (dt_ea1 == PE::P_E && cross_dot_cross_2(ea1, eb0, eb1, ea0) >= 0) { + return EdgeEdgeDistanceType::EA1_EB; + } + + return EdgeEdgeDistanceType::EA_EB; + } + +} // namespace + +PointEdgeDistanceType point_edge_distance_type_exact( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + if (!DistanceTypeConfig::instance().use_standard()) { + assert(p.size() == e0.size() && p.size() == e1.size()); + init_predicates(); + return p.size() == 2 + ? point_edge_type<2>(p.data(), e0.data(), e1.data()) + : point_edge_type<3>(p.data(), e0.data(), e1.data()); + } + return point_edge_distance_type(p, e0, e1); +} + +PointTriangleDistanceType point_triangle_distance_type_exact( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2) +{ + if (!DistanceTypeConfig::instance().use_standard()) { + init_predicates(); + return point_triangle_type(p.data(), t0.data(), t1.data(), t2.data()); + } + return point_triangle_distance_type(p, t0, t1, t2); +} + +bool is_almost_parallel_edge_edge( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + const Eigen::Vector3d u = ea1 - ea0; + const Eigen::Vector3d v = eb1 - eb0; + const double cross_norm_sqr = u.cross(v).squaredNorm(); + const double a = u.squaredNorm(); + const double c = v.squaredNorm(); + // Relative sin² test: parallel when sin²(θ) < PARALLEL_THRESHOLD. + // Scaling by a*c (rather than max(1, a*c)) keeps this scale-invariant. + return cross_norm_sqr < a * c * PARALLEL_THRESHOLD; +} + +bool is_parallel_edge_edge( + Eigen::ConstRef _ea0, + Eigen::ConstRef _ea1, + Eigen::ConstRef _eb0, + Eigen::ConstRef _eb1) +{ + if constexpr (PARALLEL_THRESHOLD == 0.0) { + init_predicates(); + // TODO use a zero filter? + const int s = cross_null_3d_filter( + _ea0.data(), _ea1.data(), _eb0.data(), _eb1.data()); + if (s != FPG_UNCERTAIN_VALUE) { + return false; + } + const ExVec3 ea0 = make_exact(_ea0.data(), 3); + const ExVec3 ea1 = make_exact(_ea1.data(), 3); + const ExVec3 eb0 = make_exact(_eb0.data(), 3); + const ExVec3 eb1 = make_exact(_eb1.data(), 3); + return exact_sign((ea1 - ea0).cross(eb1 - eb0).squared_norm()) == 0; + } else { + return is_almost_parallel_edge_edge(_ea0, _ea1, _eb0, _eb1); + } +} + +EdgeEdgeDistanceType edge_edge_distance_type_exact( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + if (!DistanceTypeConfig::instance().use_standard()) { + init_predicates(); + return edge_edge_type(ea0.data(), ea1.data(), eb0.data(), eb1.data()); + } + return edge_edge_distance_type(ea0, ea1, eb0, eb1); +} + +} // namespace ipc diff --git a/src/ipc/distance/distance_type_exact.hpp b/src/ipc/distance/distance_type_exact.hpp new file mode 100644 index 000000000..f50592166 --- /dev/null +++ b/src/ipc/distance/distance_type_exact.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +constexpr double PARALLEL_THRESHOLD { 2.5e-16 }; // TODO set to zero eventually + +/// @brief Runtime switch between the standard analytic distance-type routines +/// and the predicate-based implementations. Controls +/// point_edge_distance_type_exact, point_triangle_distance_type_exact, and +/// edge_edge_distance_type_exact. Defaults to predicate-based. +/// +/// The predicates are exact (floating-point filters with an exact fallback), so +/// the three classifications are mutually consistent (e.g., a point-triangle +/// P_E0 implies a point-edge P_E on that edge); the standard routines do not +/// guarantee this. +class DistanceTypeConfig { +public: + static DistanceTypeConfig& instance() + { + static DistanceTypeConfig cfg; + return cfg; + } + + bool use_standard() const { return m_use_standard; } + void set_use_standard(bool v) { m_use_standard = v; } + + DistanceTypeConfig(const DistanceTypeConfig&) = delete; + DistanceTypeConfig& operator=(const DistanceTypeConfig&) = delete; + +private: + DistanceTypeConfig() = default; + bool m_use_standard = false; +}; + +/// @brief Determine the closest pair between a point and edge, using exact sign +/// predicates, or the standard analytic implementation when +/// DistanceTypeConfig prefers it. +/// @param p The point. +/// @param e0 The first vertex of the edge. +/// @param e1 The second vertex of the edge. +/// @return The distance type of the point-edge pair. +PointEdgeDistanceType point_edge_distance_type_exact( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1); + +/// @brief Determine the closest pair between a point and triangle, using exact +/// sign predicates (see DistanceTypeConfig). +/// @param p The point. +/// @param t0 The first vertex of the triangle. +/// @param t1 The second vertex of the triangle. +/// @param t2 The third vertex of the triangle. +/// @return The distance type of the point-triangle pair. +PointTriangleDistanceType point_triangle_distance_type_exact( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2); + +/// @brief Determine the closest pair between two edges, using exact sign +/// predicates (see DistanceTypeConfig). +/// @param ea0 The first vertex of the first edge. +/// @param ea1 The second vertex of the first edge. +/// @param eb0 The first vertex of the second edge. +/// @param eb1 The second vertex of the second edge. +/// @return The distance type of the edge-edge pair. +EdgeEdgeDistanceType edge_edge_distance_type_exact( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); + +/// @brief Determine whether two edges are (nearly) parallel. +/// @param ea0 The first vertex of the first edge. +/// @param ea1 The second vertex of the first edge. +/// @param eb0 The first vertex of the second edge. +/// @param eb1 The second vertex of the second edge. +bool is_parallel_edge_edge( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); + +} // namespace ipc diff --git a/src/ipc/distance/exact_vector.hpp b/src/ipc/distance/exact_vector.hpp new file mode 100644 index 000000000..fe28a270d --- /dev/null +++ b/src/ipc/distance/exact_vector.hpp @@ -0,0 +1,62 @@ +#pragma once + +// This header depends on geogram's MultiPrecision PSM, a private dependency of +// ipc_toolkit, so only our own sources and tests include it. Our copy of the +// PSM lives in namespace ipc_geogram instead of GEO, so it never collides with +// a full geogram linked by a downstream project. +#include + +namespace ipc { + +/// @brief Exact scalar type (an adaptive-precision floating-point expansion). +using ExactReal = ipc_geogram::expansion_nt; + +/// @brief Minimal exact 3D vector for sign predicates. +/// +/// The MultiPrecision PSM only ships the scalar type, not geogram's vec3E, so +/// we provide the handful of operations the exact distance-type predicates +/// need. Every operation is exact, so the sign of any result is exact too. +struct ExactVector3 { + ExactReal x, y, z; + + /// @brief Exact copy of a 2D or 3D point (z = 0 in 2D). + static ExactVector3 from(const double* v, const int dim) + { + return { ExactReal(v[0]), ExactReal(v[1]), + ExactReal(dim == 3 ? v[2] : 0.0) }; + } + + ExactVector3 operator-(const ExactVector3& o) const + { + return { x - o.x, y - o.y, z - o.z }; + } + + ExactVector3 operator-() const { return { -x, -y, -z }; } + + ExactReal dot(const ExactVector3& o) const + { + return x * o.x + y * o.y + z * o.z; + } + + ExactVector3 cross(const ExactVector3& o) const + { + return { y * o.z - z * o.y, z * o.x - x * o.z, x * o.y - y * o.x }; + } + + ExactReal squared_norm() const { return dot(*this); } +}; + +/// @brief Initialize the expansion arithmetic once (thread-safe). +/// +/// geogram's MultiPrecision PSM computes its splitter and machine epsilon at +/// run time, so we must call this before the first exact computation. +inline void init_exact_arithmetic() +{ + static const bool initialized = [] { + ipc_geogram::expansion::initialize(); + return true; + }(); + (void)initialized; +} + +} // namespace ipc diff --git a/src/ipc/distance/fp_filters.h b/src/ipc/distance/fp_filters.h new file mode 100644 index 000000000..67bfb6da2 --- /dev/null +++ b/src/ipc/distance/fp_filters.h @@ -0,0 +1,446 @@ +#pragma once +/* Automatically generated code, do not edit the functions! */ + +/* +The first filter evaluates the sign of this expression: +dot(cross(p1-p0, p2-p0), cross(p3-p0, p1-p0)) = += dot(p1-p0, p3-p0) * dot(p1-p0, p2-p0) - dot(p1-p0, p1-p0) * dot(p2-p0, p3-p0) + +Explanation: +let t0,t1,t2,p be 3D points. +e0 := t1 - t0; +e1 := t2 - t1; +e2 := t0 - t2; +r0 := p - t0; +r1 := p - t1; +r2 := p - t2; +n := cross(e0, -e1) = cross(e1, -e2) = cross(e2, -e0) up to rescaling + +for any i=0,1,2, call j=i+1 mod 3 and k=i+2 mod 3. + +dot(n, cross(ri, ei)) += dot(cross(ej, -ek), cross(ri, ei)) += dot(cross(tj-ti, tk-ti), cross(p-ti, tj-ti)) += dot(tj-ti, p-ti) * dot(tk-ti, tj-ti) - dot(tj-ti, tj-ti) * dot(tk-ti, p-ti) +so we evaluate the predicate with +p0 = ti +p1 = tj +p2 = tk +p3 = p +*/ + +constexpr int FPG_UNCERTAIN_VALUE = 0; + +inline int cross_dot_cross_1_3d_filter( + const double* p0, const double* p1, const double* p2, const double* p3) +{ + double d1_0; + d1_0 = (p1[0] - p0[0]); + double d1_1; + d1_1 = (p1[1] - p0[1]); + double d1_2; + d1_2 = (p1[2] - p0[2]); + double d2_0; + d2_0 = (p2[0] - p0[0]); + double d2_1; + d2_1 = (p2[1] - p0[1]); + double d2_2; + d2_2 = (p2[2] - p0[2]); + double d3_0; + d3_0 = (p3[0] - p0[0]); + double d3_1; + d3_1 = (p3[1] - p0[1]); + double d3_2; + d3_2 = (p3[2] - p0[2]); + double m11; + m11 = (((d1_0 * d1_0) + (d1_1 * d1_1)) + (d1_2 * d1_2)); + double m12; + m12 = (((d1_0 * d2_0) + (d1_1 * d2_1)) + (d1_2 * d2_2)); + double m13; + m13 = (((d1_0 * d3_0) + (d1_1 * d3_1)) + (d1_2 * d3_2)); + double m23; + m23 = (((d2_0 * d3_0) + (d2_1 * d3_1)) + (d2_2 * d3_2)); + double r; + r = ((m12 * m13) - (m11 * m23)); + int int_tmp_result; + double eps; + double max1 = fabs(d1_0); + if ((max1 < fabs(d1_1))) { + max1 = fabs(d1_1); + } + if ((max1 < fabs(d1_2))) { + max1 = fabs(d1_2); + } + double max2 = fabs(d2_0); + if ((max2 < fabs(d2_1))) { + max2 = fabs(d2_1); + } + if ((max2 < fabs(d2_2))) { + max2 = fabs(d2_2); + } + double max3 = fabs(d3_0); + if ((max3 < fabs(d3_1))) { + max3 = fabs(d3_1); + } + if ((max3 < fabs(d3_2))) { + max3 = fabs(d3_2); + } + double lower_bound_1; + double upper_bound_1; + lower_bound_1 = max1; + upper_bound_1 = max1; + if ((max2 < lower_bound_1)) { + lower_bound_1 = max2; + } else { + if ((max2 > upper_bound_1)) { + upper_bound_1 = max2; + } + } + if ((max3 < lower_bound_1)) { + lower_bound_1 = max3; + } else { + if ((max3 > upper_bound_1)) { + upper_bound_1 = max3; + } + } + if ((lower_bound_1 < 3.14773426688569445494e-74)) { + return FPG_UNCERTAIN_VALUE; + } else { + if ((upper_bound_1 > 7.23700557733225900010e+75)) { + return FPG_UNCERTAIN_VALUE; + } + eps = (2.26648152760393650857e-14 * (((max1 * max2) * max1) * max3)); + if ((r > eps)) { + int_tmp_result = 1; + } else { + if ((r < -eps)) { + int_tmp_result = -1; + } else { + return FPG_UNCERTAIN_VALUE; + } + } + } + return int_tmp_result; +} + +/* +The second filter evaluates the sign of this expression: +dot(cross(p1-p0, p2-p0), cross(p3-p0, p1-p2)) = += dot(p1-p0, p3-p0) * dot(p1-p2, p2-p0) - dot(p1-p0, p1-p2) * dot(p2-p0, p3-p0) +Explanation: +We separately determine if the closest point on A to B is at endpoint +0, endpoint 1, or the interior. Given one of the endpoints, we can +assess whether it is a local minimum of distance by checking if the other +endpoint is behind the plane defined by the closest direction from the first +endpoint to segment B, that is, the gradient of the distance function. +This predicate can be tested with dot_3. If neither endpoint of A is the +local minimum, then the minimum must be in the interior of A. +To determine the gradient of the distance function at a point, it is enough to +determine the point-edge distance type for that point, for which we have +another function. +If the distance type is point-point, the gradient points in the direction +connecting the two points. Otherwise, the gradient points in the direction +connecting the endpoint to the interior of B; up to changes in magnitude, +this is g=cross(cross(b1-ai, b0-ai), b1-b0). We must compute dot(g, aj-ai), +which is dot(cross(cross(b1-ai, b0-ai), b1-b0), aj-ai) = += dot(cross(b1-ai, b0-ai), cross(b1-b0, aj-ai)) = += dot(cross(b1-ai, b0-ai), cross(aj-ai, b0-b1)) = += dot(b1-ai, aj-ai) * dot(b0-ai, b0-b1) - dot(b1-ai, b0-b1) * dot(b0-ai, aj-ai) +*/ +inline int cross_dot_cross_2_3d_filter( + const double* p0, const double* p1, const double* p2, const double* p3) +{ + double d1_0; + d1_0 = (p1[0] - p0[0]); + double d1_1; + d1_1 = (p1[1] - p0[1]); + double d1_2; + d1_2 = (p1[2] - p0[2]); + double d2_0; + d2_0 = (p2[0] - p0[0]); + double d2_1; + d2_1 = (p2[1] - p0[1]); + double d2_2; + d2_2 = (p2[2] - p0[2]); + double d3_0; + d3_0 = (p3[0] - p0[0]); + double d3_1; + d3_1 = (p3[1] - p0[1]); + double d3_2; + d3_2 = (p3[2] - p0[2]); + double dX_0; + dX_0 = (p1[0] - p2[0]); + double dX_1; + dX_1 = (p1[1] - p2[1]); + double dX_2; + dX_2 = (p1[2] - p2[2]); + double mX1; + mX1 = (((dX_0 * d1_0) + (dX_1 * d1_1)) + (dX_2 * d1_2)); + double mX2; + mX2 = (((dX_0 * d2_0) + (dX_1 * d2_1)) + (dX_2 * d2_2)); + double m13; + m13 = (((d1_0 * d3_0) + (d1_1 * d3_1)) + (d1_2 * d3_2)); + double m23; + m23 = (((d2_0 * d3_0) + (d2_1 * d3_1)) + (d2_2 * d3_2)); + double r; + r = ((mX2 * m13) - (mX1 * m23)); + int int_tmp_result; + double eps; + double max1 = fabs(d1_0); + if ((max1 < fabs(d1_1))) { + max1 = fabs(d1_1); + } + if ((max1 < fabs(d1_2))) { + max1 = fabs(d1_2); + } + double max2 = fabs(d2_0); + if ((max2 < fabs(d2_1))) { + max2 = fabs(d2_1); + } + if ((max2 < fabs(d2_2))) { + max2 = fabs(d2_2); + } + double max3 = fabs(d3_0); + if ((max3 < fabs(d3_1))) { + max3 = fabs(d3_1); + } + if ((max3 < fabs(d3_2))) { + max3 = fabs(d3_2); + } + double max4 = fabs(dX_0); + if ((max4 < fabs(dX_1))) { + max4 = fabs(dX_1); + } + if ((max4 < fabs(dX_2))) { + max4 = fabs(dX_2); + } + double lower_bound_1; + double upper_bound_1; + lower_bound_1 = max1; + upper_bound_1 = max1; + if ((max2 < lower_bound_1)) { + lower_bound_1 = max2; + } else { + if ((max2 > upper_bound_1)) { + upper_bound_1 = max2; + } + } + if ((max3 < lower_bound_1)) { + lower_bound_1 = max3; + } else { + if ((max3 > upper_bound_1)) { + upper_bound_1 = max3; + } + } + if ((max4 < lower_bound_1)) { + lower_bound_1 = max4; + } else { + if ((max4 > upper_bound_1)) { + upper_bound_1 = max4; + } + } + if ((lower_bound_1 < 3.14773426688569445494e-74)) { + return FPG_UNCERTAIN_VALUE; + } else { + if ((upper_bound_1 > 7.23700557733225900010e+75)) { + return FPG_UNCERTAIN_VALUE; + } + eps = (2.26648152760393650857e-14 * (((max4 * max2) * max1) * max3)); + if ((r > eps)) { + int_tmp_result = 1; + } else { + if ((r < -eps)) { + int_tmp_result = -1; + } else { + return FPG_UNCERTAIN_VALUE; + } + } + } + return int_tmp_result; +} + +inline int dot3_2d_filter(const double* p0, const double* p1, const double* p2) +{ + double a11; + a11 = (p1[0] - p0[0]); + double a12; + a12 = (p1[1] - p0[1]); + double a21; + a21 = (p2[0] - p0[0]); + double a22; + a22 = (p2[1] - p0[1]); + double Delta; + Delta = ((a11 * a21) + (a12 * a22)); + int int_tmp_result; + double eps; + double max1 = fabs(a11); + if ((max1 < fabs(a12))) { + max1 = fabs(a12); + } + double max2 = fabs(a21); + if ((max2 < fabs(a22))) { + max2 = fabs(a22); + } + double lower_bound_1; + double upper_bound_1; + lower_bound_1 = max1; + upper_bound_1 = max1; + if ((max2 < lower_bound_1)) { + lower_bound_1 = max2; + } else { + if ((max2 > upper_bound_1)) { + upper_bound_1 = max2; + } + } + if ((lower_bound_1 < 5.00368081960964802120e-147)) { + return FPG_UNCERTAIN_VALUE; + } else { + if ((upper_bound_1 > 1.67597599124282389316e+153)) { + return FPG_UNCERTAIN_VALUE; + } + eps = (8.88720573725927779595e-16 * (max1 * max2)); + if ((Delta > eps)) { + int_tmp_result = 1; + } else { + if ((Delta < -eps)) { + int_tmp_result = -1; + } else { + return FPG_UNCERTAIN_VALUE; + } + } + } + return int_tmp_result; +} + +inline int dot3_3d_filter(const double* p0, const double* p1, const double* p2) +{ + double a11; + a11 = (p1[0] - p0[0]); + double a12; + a12 = (p1[1] - p0[1]); + double a13; + a13 = (p1[2] - p0[2]); + double a21; + a21 = (p2[0] - p0[0]); + double a22; + a22 = (p2[1] - p0[1]); + double a23; + a23 = (p2[2] - p0[2]); + double Delta; + Delta = (((a11 * a21) + (a12 * a22)) + (a13 * a23)); + int int_tmp_result; + double eps; + double max1 = fabs(a11); + if ((max1 < fabs(a12))) { + max1 = fabs(a12); + } + if ((max1 < fabs(a13))) { + max1 = fabs(a13); + } + double max2 = fabs(a21); + if ((max2 < fabs(a22))) { + max2 = fabs(a22); + } + if ((max2 < fabs(a23))) { + max2 = fabs(a23); + } + double lower_bound_1; + double upper_bound_1; + lower_bound_1 = max1; + upper_bound_1 = max1; + if ((max2 < lower_bound_1)) { + lower_bound_1 = max2; + } else { + if ((max2 > upper_bound_1)) { + upper_bound_1 = max2; + } + } + if ((lower_bound_1 < 3.78232824369468580207e-147)) { + return FPG_UNCERTAIN_VALUE; + } else { + if ((upper_bound_1 > 1.67597599124282389316e+153)) { + return FPG_UNCERTAIN_VALUE; + } + eps = (1.55534235888797938037e-15 * (max1 * max2)); + if ((Delta > eps)) { + int_tmp_result = 1; + } else { + if ((Delta < -eps)) { + int_tmp_result = -1; + } else { + return FPG_UNCERTAIN_VALUE; + } + } + } + return int_tmp_result; +} + +inline int cross_null_3d_filter( + const double* p0, const double* p1, const double* q0, const double* q1) +{ + double v_0; + v_0 = (p1[0] - p0[0]); + double v_1; + v_1 = (p1[1] - p0[1]); + double v_2; + v_2 = (p1[2] - p0[2]); + double w_0; + w_0 = (q1[0] - q0[0]); + double w_1; + w_1 = (q1[1] - q0[1]); + double w_2; + w_2 = (q1[2] - q0[2]); + double c_i; + c_i = ((v_1 * w_2) - (v_2 * w_1)); + double c_j; + c_j = ((v_2 * w_0) - (v_0 * w_2)); + double c_k; + c_k = ((v_0 * w_1) - (v_1 * w_0)); + double cross; + cross = (((c_i * c_i) + (c_j * c_j)) + (c_k * c_k)); + int int_tmp_result; + double eps; + double max1 = fabs(v_0); + if ((max1 < fabs(v_1))) { + max1 = fabs(v_1); + } + if ((max1 < fabs(v_2))) { + max1 = fabs(v_2); + } + double max2 = fabs(w_0); + if ((max2 < fabs(w_1))) { + max2 = fabs(w_1); + } + if ((max2 < fabs(w_2))) { + max2 = fabs(w_2); + } + double lower_bound_1; + double upper_bound_1; + lower_bound_1 = max1; + upper_bound_1 = max1; + if ((max2 < lower_bound_1)) { + lower_bound_1 = max2; + } else { + if ((max2 > upper_bound_1)) { + upper_bound_1 = max2; + } + } + if ((lower_bound_1 < 3.53675409555095779245e-74)) { + return FPG_UNCERTAIN_VALUE; + } else { + if ((upper_bound_1 > 1.44740111546645180002e+76)) { + return FPG_UNCERTAIN_VALUE; + } + eps = (1.42208308574965596077e-14 * (((max1 * max2) * max1) * max2)); + if ((cross > eps)) { + int_tmp_result = 1; + } else { + if ((cross < -eps)) { + int_tmp_result = -1; + } else { + return FPG_UNCERTAIN_VALUE; + } + } + } + return int_tmp_result; +} \ No newline at end of file diff --git a/src/ipc/esp/CMakeLists.txt b/src/ipc/esp/CMakeLists.txt new file mode 100644 index 000000000..06baf4094 --- /dev/null +++ b/src/ipc/esp/CMakeLists.txt @@ -0,0 +1,27 @@ +set(SOURCES + arbitrary_point_esp.cpp + arbitrary_point_esp.hpp + esp_collision_maps.hpp + esp_collisions.cpp + esp_collisions.hpp + esp_collisions_builder.cpp + esp_collisions_builder.hpp + esp_distance.hpp + esp_parameters.hpp + esp_potential.cpp + esp_potential.hpp + esp_tangential_collisions.cpp + inclusion_exclusion_weights.cpp + inclusion_exclusion_weights.hpp + quadrature_potential.cpp + quadrature_potential.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ + +add_subdirectory(collisions) diff --git a/src/ipc/esp/arbitrary_point_esp.cpp b/src/ipc/esp/arbitrary_point_esp.cpp new file mode 100644 index 000000000..a324ad2ea --- /dev/null +++ b/src/ipc/esp/arbitrary_point_esp.cpp @@ -0,0 +1,469 @@ +#include "arbitrary_point_esp.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace ipc { + +namespace { + + // The collision set at one query point, in per-thread lists that outlive + // the call: build_point_collisions() clears them and refills them, so a + // query allocates nothing once each list has grown to its working size. + // One list per collision type, in the order ESPCollisionDict + // evaluates them (vertex-vertex, edge-vertex, face-vertex), each holding + // the collisions by value -- they carry only fixed-size arrays of ids. + template struct PointCollisions { + using VV = typename ESPCollisionDict::VVType; + using EV = typename ESPCollisionDict::EVType; + using FV = ESPCollisionTemplate; + + std::vector vertex_ids, edge_ids, face_ids; // broad phase + std::vector vv; + std::vector ev; + std::vector fv; // unused in 2D + + template void for_each(F&& f) const + { + for (const VV& c : vv) { + f(c); + } + for (const EV& c : ev) { + f(c); + } + for (const FV& c : fv) { + f(c); + } + } + }; + + template PointCollisions& point_collisions_scratch() + { + thread_local PointCollisions scratch; + return scratch; + } + + // Merges collisions that resolve to the same underlying feature (same + // typed hash) by accumulating their weight, and drops the entry + // entirely if the accumulated weight is exactly zero. This is the + // symbolic-cancellation step: redundant +1/-1 contributions from + // different primitives converging on the same feature (e.g. two + // adjacent faces and their shared edge) cancel here, as integers, + // before any barrier value is ever computed -- never as a + // floating-point subtraction of two independently-rounded nearly-equal + // barrier values (which is numerically unstable near dhat -> 0, where + // the barrier and its derivatives blow up). + // + // The same merge the hash map of insert_pair() in quadrature_potential.cpp + // performs, as a linear search over one type's list: the typed hash + // starts with the type, so two collisions of different types never + // merge, and a query point has only a handful of collisions. + template + void insert_by_value( + std::vector& list, + Collision&& collision, + const double weight) + { + collision.weight = weight; + const std::array hash = collision.get_typed_hash(); + for (auto it = list.begin(); it != list.end(); ++it) { + if (it->get_typed_hash() == hash) { + it->weight += collision.weight; + if (it->weight == 0) { + list.erase(it); + } + return; + } + } + list.push_back(std::move(collision)); + } + + // ESPCollisionsBuilder<3>::reduce_point_triangle_collision() and + // reduce_point_edge_collision() (esp_collisions_builder.cpp), + // emitting into the by-value lists instead of a new shared_ptr: the same + // distance-type classification, the same dhat cut, the same collision + // type for each case. + void reduce_point_triangle( + PointCollisions<3>& s, + const index_t fi, + const index_t vid, + const double weight, + const ESPParameters& params, + const CollisionMesh& mesh, + const VertexMatrixView<3>& vertices) + { + using VV = PointCollisions<3>::VV; + using EV = PointCollisions<3>::EV; + using FV = PointCollisions<3>::FV; + + const index_t t0 = mesh.faces()(fi, 0); + const index_t t1 = mesh.faces()(fi, 1); + const index_t t2 = mesh.faces()(fi, 2); + + const index_t e0 = mesh.faces_to_edges()(fi, 0); + const index_t e1 = mesh.faces_to_edges()(fi, 1); + const index_t e2 = mesh.faces_to_edges()(fi, 2); + + assert(vid != t0 && vid != t1 && vid != t2); + + const PointTriangleDistanceType dtype = + point_triangle_distance_type_exact( + vertices(vid), vertices(t0), vertices(t1), vertices(t2)); + + const double dist_sqr = point_triangle_distance( + vertices(vid), vertices(t0), vertices(t1), vertices(t2), dtype); + if (dist_sqr >= params.dhat * params.dhat) { + return; + } + + switch (dtype) { + case PointTriangleDistanceType::P_T0: + insert_by_value(s.vv, VV(t0, vid, mesh), weight); + break; + case PointTriangleDistanceType::P_T1: + insert_by_value(s.vv, VV(t1, vid, mesh), weight); + break; + case PointTriangleDistanceType::P_T2: + insert_by_value(s.vv, VV(t2, vid, mesh), weight); + break; + case PointTriangleDistanceType::P_E0: + insert_by_value(s.ev, EV(e0, vid, mesh), weight); + break; + case PointTriangleDistanceType::P_E1: + insert_by_value(s.ev, EV(e1, vid, mesh), weight); + break; + case PointTriangleDistanceType::P_E2: + insert_by_value(s.ev, EV(e2, vid, mesh), weight); + break; + default: // point_triangle_distance_type_exact never returns AUTO + assert(dtype == PointTriangleDistanceType::P_T); + insert_by_value(s.fv, FV(fi, vid, mesh), weight); + break; + } + } + + void reduce_point_edge( + PointCollisions<3>& s, + const index_t ei, + const index_t vid, + const double weight, + const ESPParameters& params, + const CollisionMesh& mesh, + const VertexMatrixView<3>& vertices) + { + using VV = PointCollisions<3>::VV; + using EV = PointCollisions<3>::EV; + + const index_t t0 = mesh.edges()(ei, 0); + const index_t t1 = mesh.edges()(ei, 1); + + const PointEdgeDistanceType dtype = point_edge_distance_type_exact( + vertices(vid), vertices(t0), vertices(t1)); + + const double dist_sqr = point_edge_distance( + vertices(vid), vertices(t0), vertices(t1), dtype); + if (dist_sqr >= params.dhat * params.dhat) { + return; + } + + switch (dtype) { + case PointEdgeDistanceType::P_E0: + insert_by_value(s.vv, VV(t0, vid, mesh), weight); + break; + case PointEdgeDistanceType::P_E1: + insert_by_value(s.vv, VV(t1, vid, mesh), weight); + break; + case PointEdgeDistanceType::P_E: + default: + assert(dtype == PointEdgeDistanceType::P_E); + insert_by_value(s.ev, EV(ei, vid, mesh), weight); + break; + } + } + + // 2D counterpart of ESPCollisionsBuilder<3>:: + // reduce_point_edge_collision (esp_collisions_builder.cpp), which + // only exists on the <3> specialization: classify which sub-feature of + // edge ei the query's closest point falls on and emit the + // correspondingly-typed collision, so endpoint cases hash-merge with the + // direct vertex collisions instead of double-counting a corner. + // + // Query vertex first in both templates, matching the convention of the + // 2D edge-QP builder in quadrature_potential.cpp; Vertex2-Edge2P1's + // evaluators assume that layout ([q, e0, e1]). + void reduce_point_edge_2d( + PointCollisions<2>& s, + const index_t ei, + const index_t vid, + const double weight, + const ESPParameters& params, + const CollisionMesh& mesh, + const VertexMatrixView<2>& vertices) + { + using VV = PointCollisions<2>::VV; + using EV = PointCollisions<2>::EV; + + const index_t e0 = mesh.edges()(ei, 0); + const index_t e1 = mesh.edges()(ei, 1); + + const PointEdgeDistanceType dtype = point_edge_distance_type_exact( + vertices(vid), vertices(e0), vertices(e1)); + + const double dist_sqr = point_edge_distance( + vertices(vid), vertices(e0), vertices(e1), dtype); + if (dist_sqr >= params.dhat * params.dhat) { + return; + } + + switch (dtype) { + case PointEdgeDistanceType::P_E0: + insert_by_value(s.vv, VV(vid, e0, mesh), weight); + break; + case PointEdgeDistanceType::P_E1: + insert_by_value(s.vv, VV(vid, e1, mesh), weight); + break; + case PointEdgeDistanceType::P_E: + insert_by_value(s.ev, EV(vid, ei, mesh), weight); + break; + default: + assert(false); + break; + } + } + + // The collisions at query point q, in the calling thread's scratch. + template + const PointCollisions& build_point_collisions( + const LBVH& point_bvh, + const CollisionMesh& mesh, + const ESPParameters& params, + const std::vector& edge_weights, + const std::vector& vertex_weights, + Eigen::ConstRef V, + Eigen::ConstRef> q) + { + PointCollisions& s = point_collisions_scratch(); + s.vv.clear(); + s.ev.clear(); + s.fv.clear(); + + const index_t vid = static_cast(V.rows()); // virtual vertex id + const VertexMatrixView V_view(V, q); + + point_bvh.query_point( + q, params.dhat, s.vertex_ids, s.edge_ids, s.face_ids); + + // Inclusion-exclusion: every element whose offset region can contain q + // contributes a term with the element's own weight (see the + // constructor) -- on a closed mesh, in 3D faces +1, edges -1, vertices + // +1; in 2D edges +1, vertices -1. Each element is first *reduced* to + // the sub-feature its closest point to q actually lies on (never just + // "this face's interior" regardless of where the closest point falls), + // so redundant terms converging on the same feature share a typed hash + // and cancel as integers in insert_by_value() above. An element of + // weight zero (a boundary edge or vertex) contributes nothing. + if constexpr (dim == 3) { + for (const index_t fi : s.face_ids) { + reduce_point_triangle(s, fi, vid, +1, params, mesh, V_view); + } + for (const index_t ei : s.edge_ids) { + if (edge_weights[ei] != 0) { + reduce_point_edge( + s, ei, vid, edge_weights[ei], params, mesh, V_view); + } + } + } else { + // In 2D edges are the top-dimensional elements, so they take the + // weight faces take in 3D and there is no face loop (mesh.faces() + // is empty and the face BVH is never built). + for (const index_t ei : s.edge_ids) { + reduce_point_edge_2d( + s, ei, vid, edge_weights[ei], params, mesh, V_view); + } + } + + // Vertices, with their own weights. These merge (and symbolically + // cancel) with the vertex-typed collisions the reductions above emit + // when both resolve to the same corner. + // + // The (query, mesh vertex) argument order is load-bearing in 2D and + // only in 2D: get_typed_hash() is {type, primitive_a.id(), + // primitive_b.id()}, and Vertex2-Vertex2 goes through the generic + // constructor, which stores the ids as given -- so this has to match + // what reduce_point_edge_2d emits or the two never merge. + // Vertex3-Vertex3 has a specialized constructor that sorts its two ids + // (esp_collision_template.cpp), so 3D merges either way. + using VV = typename PointCollisions::VV; + for (const index_t vi : s.vertex_ids) { + if (vertex_weights[vi] == 0 + || (V.row(vi) - q).squaredNorm() >= params.dhat * params.dhat) { + continue; + } + insert_by_value(s.vv, VV(vid, vi, mesh), vertex_weights[vi]); + } + return s; + } + + // The collision's stencil positions, as ESPCollision::dof() gives + // them, on the stack rather than in a new Eigen::VectorXd. + template + VectorMax + stencil_positions(const Collision& cc, const VertexMatrixView& V_view) + { + VectorMax x( + cc.num_vertices() * dim); + for (int i = 0; i < cc.num_vertices(); i++) { + x.template segment(i * dim) = V_view(cc.vertex_id(i)); + } + return x; + } + + // Where the query vertex sits in the collision's stencil. Every collision + // at a query point has the query vertex as exactly one of its vertices, + // and only its block of a gradient or Hessian is ever returned, so that + // block is all that is accumulated: the same per-collision terms, added + // in the same collision order, as assembling the whole stencil and then + // extracting the query's block. + template + int query_slot(const Collision& cc, const index_t vid) + { + for (int i = 0; i < cc.num_vertices(); i++) { + if (cc.vertex_id(i) == vid) { + return i; + } + } + assert(false); + return -1; + } + +} // namespace + +template +ArbitraryPointESP::ArbitraryPointESP( + const CollisionMesh& _mesh, ESPParameters _params) + : m_mesh(_mesh) + , m_params(std::move(_params)) +{ + if (m_mesh.dim() != dim) { + log_and_throw_error( + "ArbitraryPointESP<{}> requires a {}D mesh (got {}D)!", dim, dim, + m_mesh.dim()); + } + + // Inclusion-exclusion weights from the mesh connectivity: the alternating + // signs on a closed mesh, zero on boundary edges and vertices, and one on + // edges in no face and isolated vertices. + compute_inclusion_exclusion_weights( + m_mesh, m_edge_weights, m_vertex_weights); +} + +template +void ArbitraryPointESP::update(Eigen::ConstRef V) +{ + // Zero inflation: LBVH::query_point applies the radius to the query box, so + // one build serves queries at any radius. + m_point_bvh.build( + V, m_mesh.edges(), m_mesh.faces(), /*inflation_radius=*/0.0); +} + +template +double ArbitraryPointESP::operator()( + Eigen::ConstRef V, Eigen::ConstRef q) const +{ + const PointCollisions& collisions = build_point_collisions( + m_point_bvh, m_mesh, m_params, m_edge_weights, m_vertex_weights, V, q); + const VertexMatrixView V_view(V, q); + + double value = 0.0; + collisions.for_each([&](const auto& cc) { + value += cc.weight * cc(stencil_positions(cc, V_view), m_params); + }); + return value; +} + +template +auto ArbitraryPointESP::gradient( + Eigen::ConstRef V, Eigen::ConstRef q) const + -> Gradient +{ + const PointCollisions& collisions = build_point_collisions( + m_point_bvh, m_mesh, m_params, m_edge_weights, m_vertex_weights, V, q); + const VertexMatrixView V_view(V, q); + const index_t vid = static_cast(V.rows()); + + Gradient grad = Gradient::Zero(); + collisions.for_each([&](const auto& cc) { + const VectorMax g = cc.weight + * cc.gradient(stencil_positions(cc, V_view), m_params); + grad += g.template segment(dim * query_slot(cc, vid)); + }); + return grad; +} + +template +auto ArbitraryPointESP::hessian( + Eigen::ConstRef V, Eigen::ConstRef q) const + -> Hessian +{ + const PointCollisions& collisions = build_point_collisions( + m_point_bvh, m_mesh, m_params, m_edge_weights, m_vertex_weights, V, q); + const VertexMatrixView V_view(V, q); + const index_t vid = static_cast(V.rows()); + + Hessian H = Hessian::Zero(); + collisions.for_each([&](const auto& cc) { + const int j = dim * query_slot(cc, vid); + // One Hessian temporary (ELEMENT_SIZE^2 doubles on the stack), read + // through a block of the scaled expression rather than copied. + H += (cc.hessian(stencil_positions(cc, V_view), m_params) + * cc.weight) + .template block(j, j); + }); + return H; +} + +template +auto ArbitraryPointESP::evaluate( + Eigen::ConstRef V, Eigen::ConstRef q) const + -> std::tuple +{ + const PointCollisions& collisions = build_point_collisions( + m_point_bvh, m_mesh, m_params, m_edge_weights, m_vertex_weights, V, q); + const VertexMatrixView V_view(V, q); + const index_t vid = static_cast(V.rows()); + + double value = 0.0; + Gradient grad = Gradient::Zero(); + Hessian H = Hessian::Zero(); + + collisions.for_each([&](const auto& cc) { + const VectorMax dof = + stencil_positions(cc, V_view); + const int j = dim * query_slot(cc, vid); + + value += cc.weight * cc(dof, m_params); + + const VectorMax g = + cc.weight * cc.gradient(dof, m_params); + grad += g.template segment(j); + + H += (cc.weight * cc.hessian(dof, m_params)) + .template block(j, j); + }); + + return { value, grad, H }; +} + +template class ArbitraryPointESP<2>; +template class ArbitraryPointESP<3>; + +} // namespace ipc diff --git a/src/ipc/esp/arbitrary_point_esp.hpp b/src/ipc/esp/arbitrary_point_esp.hpp new file mode 100644 index 000000000..8e49b41a7 --- /dev/null +++ b/src/ipc/esp/arbitrary_point_esp.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +namespace ipc { + +/// @brief Evaluate the Extremum Sum Potential (ESP) at an arbitrary +/// point in space, not restricted to the mesh's own vertices/edges/faces. +/// +/// Reuses the same collision-construction machinery as +/// PointPotential::build_collisions_at_vertex / build_collisions_at_edge_qp +/// (quadrature_potential.cpp): nearby primitives found by LBVH::query_point +/// are classified with exact point-primitive distance-type predicates -- +/// which of a primitive's sub-features (face/edge/vertex in 3D, +/// edge/vertex in 2D) the query's closest point actually falls on -- and +/// redundant contributions from different primitives resolving to the same +/// feature (e.g. two adjacent faces and their shared edge, or the two 2D +/// edges meeting at a corner) are merged and symbolically cancelled +/// (integer weight accumulation, dropped exactly at zero) before any +/// barrier value is computed. This avoids the catastrophic cancellation a +/// naive "sum every found primitive independently with a fixed sign" +/// approach suffers near shared features, where barrier derivatives blow up +/// as distance -> 0. +/// +/// Every element enters with its inclusion-exclusion weight, chosen so that +/// the weights of the elements containing any point of the mesh sum to one +/// (supplemental S2). A closed mesh gets the alternating signs; boundary edges +/// and vertices get zero (S4), and edges in no face and isolated vertices get +/// one, so open meshes and lower-dimensional pieces are handled too. +/// +/// Value/gradient/Hessian are computed by the same ESPCollision:: +/// operator()/gradient()/hessian() used by the production +/// ESPPotential, just evaluated for a virtual point +/// (id == V.rows()) instead of a real mesh vertex, via VertexMatrixView. +/// +/// @tparam dim Spatial dimension of the mesh, 2 or 3. +template class ArbitraryPointESP { + static_assert(dim == 2 || dim == 3, "dim must be 2 or 3"); + +public: + /// @brief A query point in space. + using Point = Eigen::RowVector; + /// @brief Gradient of the potential w.r.t. a query point. + using Gradient = Eigen::Vector; + /// @brief Hessian of the potential w.r.t. a query point. + using Hessian = Eigen::Matrix; + + /// @throws std::runtime_error if mesh.dim() != dim. + ArbitraryPointESP(const CollisionMesh& mesh, ESPParameters params); + + /// @brief Rebuild the underlying broad-phase index. O(n log n). Call + /// once per vertex configuration, before any operator()/gradient()/ + /// hessian() calls against that configuration. + void update(Eigen::ConstRef V); + + /// @brief Evaluate the potential at q. + double operator()( + Eigen::ConstRef V, Eigen::ConstRef q) const; + + /// @brief Gradient of the potential with respect to q. + Gradient gradient( + Eigen::ConstRef V, Eigen::ConstRef q) const; + + /// @brief Hessian of the potential with respect to q. + Hessian + hessian(Eigen::ConstRef V, Eigen::ConstRef q) const; + + /// @brief Value, gradient, and Hessian at q, computed together. + /// + /// Equivalent to calling operator()/gradient()/hessian() separately, but + /// builds the (BVH-queried, exact-predicate-classified, + /// symbolically-cancelled) collision dict for q only once instead of + /// three times -- collision construction, not the final per-collision + /// barrier evaluation, dominates cost (see the profiling that motivated + /// this), so calling operator()/gradient()/hessian() separately at the + /// same point does ~3x the necessary work. Prefer this whenever you need + /// more than one of the three at the same q (e.g. a Newton step). + std::tuple evaluate( + Eigen::ConstRef V, Eigen::ConstRef q) const; + +private: + const CollisionMesh& m_mesh; + ESPParameters m_params; + LBVH m_point_bvh; + /// Inclusion-exclusion weight of each edge and each vertex (every face + /// has weight one), from the mesh connectivity; see + /// compute_inclusion_exclusion_weights(). + std::vector m_edge_weights, m_vertex_weights; +}; + +extern template class ArbitraryPointESP<2>; +extern template class ArbitraryPointESP<3>; + +} // namespace ipc diff --git a/src/ipc/esp/collisions/CMakeLists.txt b/src/ipc/esp/collisions/CMakeLists.txt new file mode 100644 index 000000000..63da7ed27 --- /dev/null +++ b/src/ipc/esp/collisions/CMakeLists.txt @@ -0,0 +1,12 @@ +set(SOURCES + esp_collision.cpp + esp_collision.hpp + vertex_matrix_view.hpp + esp_collision_template.cpp + esp_collision_template.hpp + esp_collision_dict.cpp + esp_collision_dict.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/esp/collisions/esp_collision.cpp b/src/ipc/esp/collisions/esp_collision.cpp new file mode 100644 index 000000000..c0be9d2d3 --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision.cpp @@ -0,0 +1,57 @@ +#include "esp_collision.hpp" + +#include +#include +#include +#include + +namespace ipc { + +std::vector ESPCollision::vertex_ids() const +{ + std::vector ids; + ids.reserve(num_vertices()); + for (int i = 0; i < num_vertices(); ++i) { + ids.push_back(vertex_id(i)); + } + return ids; +} + +Eigen::VectorXd ESPCollision::dof(Eigen::ConstRef X) const +{ + const int DIM = X.cols(); + Eigen::VectorXd x(num_vertices() * DIM); + if (DIM == 2) { + for (int i = 0; i < num_vertices(); i++) { + x.segment<2>(i * 2) = X.row(vertex_id(i)); + } + } else if (DIM == 3) { + for (int i = 0; i < num_vertices(); i++) { + x.segment<3>(i * 3) = X.row(vertex_id(i)); + } + } else { + log_and_throw_error("Invalid dimension!"); + } + return x; +} + +Eigen::VectorXd ESPCollision::dof(VertexMatrixView<3> X_extended) const +{ + Eigen::VectorXd x(num_vertices() * 3); + for (int i = 0; i < num_vertices(); i++) { + assert(vertex_id(i) < X_extended.rows()); + x.segment<3>(i * 3) = X_extended(vertex_id(i)); + } + return x; +} + +Eigen::VectorXd ESPCollision::dof(VertexMatrixView<2> X_extended) const +{ + Eigen::VectorXd x(num_vertices() * 2); + for (int i = 0; i < num_vertices(); i++) { + assert(vertex_id(i) < X_extended.rows()); + x.segment<2>(i * 2) = X_extended(vertex_id(i)); + } + return x; +} +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/esp/collisions/esp_collision.hpp b/src/ipc/esp/collisions/esp_collision.hpp new file mode 100644 index 000000000..96fc3579d --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision.hpp @@ -0,0 +1,152 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +namespace ipc { + +enum class ESPCollisionType : uint8_t { + EDGE_VERTEX = 0, + VERTEX_VERTEX = 1, + FACE_VERTEX = 2, + EDGE_EDGE = 3, + EDGE_FACE = 4, + FACE_FACE = 5 +}; + +/// @brief Contact pair (stencil) of the Extremum Sum Potential (ESP). +/// @note Unlike NormalCollision, ESPCollision has to be reconstructed whenever vertices change position +class ESPCollision { +public: + /// @brief Most vertices in any ESP stencil. + /// Every stencil pairs one primitive with a single vertex, so the largest + /// is a face and a vertex (Face3P1-Vertex3). We size the stencil buffers + /// for that instead of reusing GCP's neighborhood limit, because MatrixMax + /// stores its full capacity inline: at 120 DOF each Hessian put 115 KB on + /// the stack, enough to overflow a 512 KiB thread stack. + static constexpr int MAX_VERTICES = 4; + /// @brief Most DOF in any ESP stencil (gradient and Hessian buffer size). + static constexpr int ELEMENT_SIZE = 3 * MAX_VERTICES; + + ESPCollision() = default; + + virtual ~ESPCollision() = default; + + /// @brief Name of the contact pair type + virtual std::string name() const = 0; + + /// @brief Number of vertices involved times the dimension + virtual int n_dofs() const = 0; + + /// @brief Contact pair type + virtual ESPCollisionType type() const = 0; + + virtual std::array get_typed_hash() const = 0; + + /// @brief Get the number of vertices in the collision stencil. + virtual int num_vertices() const = 0; + + /// @brief Get the number of vertices in primitive A's stencil. + virtual size_t n_vertices_a() const = 0; + + /// @brief Get the number of vertices in primitive B's stencil. + virtual size_t n_vertices_b() const = 0; + + /// @brief Get the vertex IDs of the collision stencil. + /// @return The num_vertices() vertex IDs of the collision stencil. + std::vector vertex_ids() const; + virtual index_t vertex_id(index_t i) const = 0; + + /// @brief Get the vertex attributes of the collision stencil. + /// @param vertices Vertex attributes + /// @return The vertex positions of the collision stencil, one row per + /// vertex (num_vertices() rows). + Eigen::MatrixXd vertices(Eigen::ConstRef vertices) const + { + const int DIM = vertices.cols(); + Eigen::MatrixXd stencil_vertices(num_vertices(), DIM); + for (int i = 0; i < num_vertices(); i++) { + stencil_vertices.row(i) = vertices.row(vertex_id(i)); + } + + return stencil_vertices; + } + + /// @brief Select this stencil's DOF from the full matrix of DOF. + /// @param X Full matrix of DOF (rowwise). + /// @return This stencil's DOF. + Eigen::VectorXd dof(Eigen::ConstRef X) const; + + /// @brief Select this stencil's DOF from the full matrix of DOF. + /// In 3D, some vertices may not be directly stored in the full matrix, e.g. + /// face quadrature points and edge-edge closest points. + Eigen::VectorXd dof(VertexMatrixView<3> X_extended) const; + + /// @brief Select this stencil's DOF from the full 2D matrix of DOF (with a virtual vertex appended). + Eigen::VectorXd dof(VertexMatrixView<2> X_extended) const; + + /// @brief Compute the distance of the stencil. + /// @param vertices Collision mesh vertices + /// @return Squared distance of the stencil. + virtual double + compute_distance(Eigen::ConstRef vertices) const = 0; + + /// @brief Compute the barrier (params.barrier) of this pair's distance. + virtual double operator()( + Eigen::ConstRef> positions, + const ESPParameters& params) const = 0; + + /// @brief Compute the gradient of the barrier wrt. the stencil's vertices. + virtual VectorMax gradient( + Eigen::ConstRef> positions, + const ESPParameters& params) const = 0; + + /// @brief Compute the Hessian of the barrier wrt. the stencil's vertices. + virtual MatrixMax hessian( + Eigen::ConstRef> positions, + const ESPParameters& params) const = 0; + + bool operator==(const ESPCollision& other) const + { + return ((*this)[0] == other[0] && (*this)[1] == other[1]); + } + + bool operator!=(const ESPCollision& other) const + { + return !(*this == other); + } + + virtual index_t operator[](int idx) const = 0; + + virtual std::pair get_hash() const = 0; + + virtual std::pair operator_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const = 0; + + virtual std:: + pair, VectorMax> + gradient_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const = 0; + + virtual std::pair< + MatrixMax, + MatrixMax> + hessian_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const = 0; + + double weight = 1; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/esp/collisions/esp_collision_dict.cpp b/src/ipc/esp/collisions/esp_collision_dict.cpp new file mode 100644 index 000000000..cace7ed51 --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision_dict.cpp @@ -0,0 +1,200 @@ +#include "esp_collision_dict.hpp" + +#include +#include + +#include +#include + +namespace ipc { +template +void ESPCollisionDict::initialize( + const std::vector& primitive_ids, + const std::vector& primary_vertex_ids, + const ESPCollisionPairMap& map, + const index_t n_total_vertices) +{ + assert(primary_vertex_ids.size() <= m_primary_vertex_ids.size()); + for (int i = 0; i < primary_vertex_ids.size(); i++) { + m_primary_vertex_ids[i] = primary_vertex_ids[i]; + } + + assert(primitive_ids.size() <= m_primitive_ids.size()); + for (int i = 0; i < primitive_ids.size(); i++) { + m_primitive_ids[i] = primitive_ids[i]; + } + + std::set vids; + for (const auto& [key, val] : map) { + for (const index_t vid : val->vertex_ids()) { + vids.insert(vid); + } + } + + // Erase virtual vertex id, which is the largest in all ids + if (pType != PointType::VERTEX && !map.empty()) { + auto iter = std::prev(vids.end()); + vids.erase(iter); + } + + // Insert primary ids + for (index_t vi : primary_vertex_ids) { + vids.insert(vi); + } + + m_vertex_ids.assign(vids.begin(), vids.end()); + assert(std::is_sorted(m_vertex_ids.begin(), m_vertex_ids.end())); + + for (int i = 0; i < m_vertex_ids.size(); i++) { + m_vertex_ids_inverse[m_vertex_ids[i]] = i; + } + + // Cache primary local ids + for (int i = 0; i < m_primary_vertex_ids.size(); i++) { + if (m_primary_vertex_ids[i] < 0) { + break; + } + m_primary_local_ids[i] = vertex_ids_inverse(m_primary_vertex_ids[i]); + } + + // We cache the global DOF indices in the same layout the potential + // assembles its gradient and Hessian in, so callers can scatter with them + // directly. + assert(n_total_vertices > 0); + const auto global_dof = [n_total_vertices](index_t vid, index_t d) { + (void)n_total_vertices; // Suppress unused parameter warning + assert(vid < n_total_vertices); + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + return vid * DIMENSION + d; + } else { + return d * n_total_vertices + vid; + } + }; + + // Cache dofs + m_dofs.resize(m_vertex_ids.size() * DIMENSION); + for (int i = 0; i < m_vertex_ids.size(); i++) { + for (int d = 0; d < DIMENSION; d++) { + m_dofs[i * DIMENSION + d] = global_dof(m_vertex_ids[i], d); + } + } + + // Cache primary dofs + m_primary_dofs.clear(); + m_primary_dofs.reserve(m_primary_vertex_ids.size() * DIMENSION); + for (index_t i : m_primary_vertex_ids) { + if (i < 0) { + break; + } + for (index_t d = 0; d < DIMENSION; d++) { + m_primary_dofs.push_back(global_dof(i, d)); + } + } + + // Convert unordered_map to typed vectors + for (const auto& [key, val] : map) { + switch (val->type()) { + case ESPCollisionType::VERTEX_VERTEX: + if constexpr (DIM == 2) { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + m_vv_collisions.push_back(*ptr); + } else { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + m_vv_collisions.push_back(*ptr); + } + break; + case ESPCollisionType::EDGE_VERTEX: + if constexpr (DIM == 2) { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + m_ev_collisions.push_back(*ptr); + } else { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + m_ev_collisions.push_back(*ptr); + } + break; + case ESPCollisionType::FACE_VERTEX: + if constexpr (DIM == 3) { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + m_fv_collisions.push_back(*ptr); + } else { + log_and_throw_error( + "FACE_VERTEX collision type not supported in 2D dict"); + } + break; + default: + log_and_throw_error("Invalid collision type!"); + } + } +} + +template +ESPCollision& ESPCollisionDict::operator[](int i) +{ + return const_cast( + static_cast(*this)[i]); +} + +template +const ESPCollision& ESPCollisionDict::operator[](int i) const +{ + if (i < m_vv_collisions.size()) { + return m_vv_collisions[i]; + } else { + i -= m_vv_collisions.size(); + if (i < m_ev_collisions.size()) { + return m_ev_collisions[i]; + } else { + i -= m_ev_collisions.size(); + if (i < m_fv_collisions.size()) { + return m_fv_collisions[i]; + } else { + log_and_throw_error("Invalid index!"); + } + } + } +} + +template +const std::vector& ESPCollisionDict::vertex_ids() const +{ + return m_vertex_ids; +} + +template +const std::vector& ESPCollisionDict::primary_dofs() const +{ + return m_primary_dofs; +} + +template +const std::vector& ESPCollisionDict::dofs() const +{ + return m_dofs; +} + +template +index_t ESPCollisionDict::vertex_ids_inverse(index_t id) const +{ + auto iter = m_vertex_ids_inverse.find(id); + if (iter == m_vertex_ids_inverse.end()) { + return -1; + } + return iter->second; +} + +template class ESPCollisionDict; +template class ESPCollisionDict; +template class ESPCollisionDict; +template class ESPCollisionDict; +template class ESPCollisionDict; +} // namespace ipc diff --git a/src/ipc/esp/collisions/esp_collision_dict.hpp b/src/ipc/esp/collisions/esp_collision_dict.hpp new file mode 100644 index 000000000..3a53cfaba --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision_dict.hpp @@ -0,0 +1,151 @@ +#pragma once +#include +#include + +#include +#include +#include +#include + +namespace ipc { + +/// @brief Defined in the internal header esp_collision_maps.hpp. +struct ESPCollisionPairMap; + +enum class PointType : std::uint8_t { VERTEX, EDGE, FACE }; + +/// @brief The collision pairs of one ESP query point with nearby primitives: +/// (point, vertex), (point, edge) or, in 3D, (point, face). In (point, edge) +/// and (point, face) pairs, the query point is primitive B in 3D and primitive +/// A in 2D; 3D (point, vertex) pairs order their two vertices by ID. It can be +/// 1. A real vertex +/// 2. A point on an edge, as the closest point between a pair of edges +/// 3. A face quadrature point (ESPParameters::set_quad_rule()) or, in 2D, +/// an edge quadrature point +/// In 2 and 3, the point is a virtual vertex that does not exist in the +/// CollisionMesh, the ID of a virtual vertex is always #n_verts, i.e. +/// immediately after all real vertices. +/// @tparam DIM Spatial dimension (2 or 3). Default is 3. +template class ESPCollisionDict { +public: + static constexpr int DIMENSION = DIM; + + // Collision pair types depend on dimension. + using VVType = std::conditional_t< + DIM == 2, + ESPCollisionTemplate, + ESPCollisionTemplate>; + using EVType = std::conditional_t< + DIM == 2, + ESPCollisionTemplate, + ESPCollisionTemplate>; + + ESPCollisionDict() = default; + ~ESPCollisionDict() = default; + + /// @brief Gather the collision pairs and cache the vertex and DOF indices. + /// @param primitive_ids IDs of the primitives the query point lies on. + /// @param primary_vertex_ids Vertices of those primitives. + /// @param map Collision pairs of the query point. + /// @param n_total_vertices Number of vertices in the CollisionMesh. We need + /// it to lay out the cached DOF indices in VERTEX_DERIVATIVE_LAYOUT + /// order, since a ColMajor DOF index is d * n_total_vertices + vid. + void initialize( + const std::vector& primitive_ids, + const std::vector& primary_vertex_ids, + const ESPCollisionPairMap& map, + index_t n_total_vertices); + + const std::array& primary_vertex_ids() const + { + return m_primary_vertex_ids; + } + /// @brief Local indices of primary vertices (i.e. vertex_ids_inverse(primary_vertex_ids()[i])) + const std::array& primary_local_ids() const + { + return m_primary_local_ids; + } + int size() const + { + return m_vv_collisions.size() + m_ev_collisions.size() + + m_fv_collisions.size(); + } + + ESPCollision& operator[](int i); + const ESPCollision& operator[](int i) const; + + template < + PointType T = pType, + typename = std::enable_if_t> + int primitive_id() const + { + return m_primitive_ids[0]; + } + + template < + PointType T = pType, + typename = std::enable_if_t> + std::array primitive_ids() const + { + return m_primitive_ids; + } + + template < + PointType T = pType, + typename = std::enable_if_t> + EdgeEdgeDistanceType ee_dtype() const + { + return m_ee_dtype; + } + + template < + PointType T = pType, + typename = std::enable_if_t> + void set_ee_dtype(EdgeEdgeDistanceType dtype) + { + m_ee_dtype = dtype; + } + + /* These functions are only available after calling initialize() */ + + // Global indices of DoFs, in VERTEX_DERIVATIVE_LAYOUT order. Entry + // i * DIM + d is coordinate d of vertex_ids()[i]. + const std::vector& dofs() const; + const std::vector& primary_dofs() const; + // Global indices of vertices + const std::vector& vertex_ids() const; + // Map from global vertex index to local vertex index + index_t vertex_ids_inverse(index_t id) const; + +private: + std::vector m_vv_collisions; + std::vector m_ev_collisions; + std::vector> + m_fv_collisions; // unused in DIM=2 + + std::array m_primitive_ids { { -1, -1 } }; + + /// @brief Cached edge-edge distance type (only meaningful for PointType::EDGE) + EdgeEdgeDistanceType m_ee_dtype = EdgeEdgeDistanceType::AUTO; + + /// @brief Primary vertices used to compute the virtual vertex + /// When the quadrature point q is + /// - a vertex, this is that vertex id + /// - an edge point, this is the four vertices of the edge-edge pair + /// - a face point, this is the three vertices of the face + /// When the size is smaller than 4, append -1 to entries not used. + std::array m_primary_vertex_ids { { -1, -1, -1, -1 } }; + /// @brief Cached local indices: m_primary_local_ids[i] = vertex_ids_inverse(m_primary_vertex_ids[i]) + std::array m_primary_local_ids { { -1, -1, -1, -1 } }; + + /// @brief Collection of all vertices in collision pairs, including the primary vertices, but not the virtual vertex + std::vector m_vertex_ids; + /// @brief Inverse of m_vertex_ids + std::map m_vertex_ids_inverse; + + /// @brief Cached global DOF indices (computed once in initialize()) + std::vector m_dofs; + /// @brief Cached primary DOF indices (computed once in initialize()) + std::vector m_primary_dofs; +}; +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/esp/collisions/esp_collision_template.cpp b/src/ipc/esp/collisions/esp_collision_template.cpp new file mode 100644 index 000000000..92d5c9e99 --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision_template.cpp @@ -0,0 +1,823 @@ +#include "esp_collision_template.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +namespace ipc { + +// ---- type ---- + +template <> +ESPCollisionType ESPCollisionTemplate::type() const +{ + return ESPCollisionType::VERTEX_VERTEX; +} +template <> +ESPCollisionType ESPCollisionTemplate::type() const +{ + return ESPCollisionType::EDGE_VERTEX; +} +template <> +ESPCollisionType ESPCollisionTemplate::type() const +{ + return ESPCollisionType::FACE_VERTEX; +} +template <> +ESPCollisionType ESPCollisionTemplate::type() const +{ + return ESPCollisionType::VERTEX_VERTEX; +} +template <> +ESPCollisionType ESPCollisionTemplate::type() const +{ + return ESPCollisionType::EDGE_VERTEX; +} + +// ---- name ---- + +template <> std::string ESPCollisionTemplate::name() const +{ + return "vv_3d"; +} +template <> std::string ESPCollisionTemplate::name() const +{ + return "ev_3d"; +} +template <> std::string ESPCollisionTemplate::name() const +{ + return "fv_3d"; +} +template <> std::string ESPCollisionTemplate::name() const +{ + return "vv_2d_pt"; +} +template <> std::string ESPCollisionTemplate::name() const +{ + return "ev_2d_pt"; +} + +// ---- constructors ---- + +template +ESPCollisionTemplate::ESPCollisionTemplate( + index_t _primitive0, index_t _primitive1, const CollisionMesh& mesh) + : m_primitive_a(_primitive0, mesh) + , m_primitive_b(_primitive1, mesh) +{ +} + +template <> +ESPCollisionTemplate::ESPCollisionTemplate( + index_t _primitive0, index_t _primitive1, const CollisionMesh& mesh) + : m_primitive_a(std::min(_primitive0, _primitive1), mesh) + , m_primitive_b(std::max(_primitive0, _primitive1), mesh) +{ +} + +// ---- vertex_id ---- + +template +index_t ESPCollisionTemplate::vertex_id(index_t i) const +{ + if (i < static_cast(m_primitive_a.n_vertices())) { + return m_primitive_a.vertex_id(i); + } + i -= m_primitive_a.n_vertices(); + assert(static_cast(m_primitive_b.n_vertices()) > i); + return m_primitive_b.vertex_id(i); +} + +// ---- generic stubs ---- + +template +double ESPCollisionTemplate::operator()( + Eigen::ConstRef> /*positions*/, + const ESPParameters& /*params*/) const +{ + return 0; +} + +template +auto ESPCollisionTemplate::gradient( + Eigen::ConstRef> /*positions*/, + const ESPParameters& /*params*/) const -> VectorMax +{ + return VectorMax::Zero(n_dofs()); +} + +template +auto ESPCollisionTemplate::hessian( + Eigen::ConstRef> /*positions*/, + const ESPParameters& /*params*/) const + -> MatrixMax +{ + return MatrixMax::Zero( + n_dofs(), n_dofs()); +} + +template +double ESPCollisionTemplate::compute_distance( + Eigen::ConstRef /*vertices*/) const +{ + log_and_throw_error("Not implemented"); + return 0; +} + +template +std::pair +ESPCollisionTemplate::operator_nearfar( + Eigen::ConstRef> /*positions*/, + const ESPParameters& /*params*/, + const NearFarBarrier<>* /*nf_barrier*/) const +{ + return { 0.0, 0.0 }; +} + +template +auto ESPCollisionTemplate::gradient_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& /*params*/, + const NearFarBarrier<>* /*nf_barrier*/) const -> std:: + pair, VectorMax> +{ + const auto n = positions.size(); + return { VectorMax::Zero(n), + VectorMax::Zero(n) }; +} + +template +auto ESPCollisionTemplate::hessian_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& /*params*/, + const NearFarBarrier<>* /*nf_barrier*/) const + -> std::pair< + MatrixMax, + MatrixMax> +{ + const auto n = positions.size(); + return { MatrixMax::Zero(n, n), + MatrixMax::Zero(n, n) }; +} + +// ---- 3D specializations ---- + +template <> +double ESPCollisionTemplate::compute_distance( + Eigen::ConstRef vertices) const +{ + const int n_verts = vertices.rows(); + if (n_verts > vertex_id(0) && n_verts > vertex_id(1)) { + return point_point_distance( + vertices.row(vertex_id(0)), + vertices.row(vertex_id(n_vertices_a()))); + } + return std::numeric_limits::max(); +} + +template <> +double ESPCollisionTemplate::compute_distance( + Eigen::ConstRef vertices) const +{ + const int n_verts = vertices.rows(); + if (n_verts > vertex_id(0) && n_verts > vertex_id(1) + && n_verts > vertex_id(2)) { + return point_edge_distance( + vertices.row(vertex_id(n_vertices_a())), vertices.row(vertex_id(0)), + vertices.row(vertex_id(1))); + } + return std::numeric_limits::max(); +} + +template <> +double ESPCollisionTemplate::compute_distance( + Eigen::ConstRef vertices) const +{ + const int n_verts = vertices.rows(); + if (n_verts > vertex_id(0) && n_verts > vertex_id(1) + && n_verts > vertex_id(2) && n_verts > vertex_id(3)) { + return point_triangle_distance( + vertices.row(vertex_id(3)), vertices.row(vertex_id(0)), + vertices.row(vertex_id(1)), vertices.row(vertex_id(2))); + } + return std::numeric_limits::max(); +} + +template <> +double ESPCollisionTemplate::operator()( + Eigen::ConstRef> positions, + const ESPParameters& params) const +{ + const double dist = + (positions.template head<3>() - positions.template segment<3>(3)) + .norm(); + const double eps = params.dhat; + return (*params.barrier)(dist, eps); +} + +template <> +double ESPCollisionTemplate::operator()( + Eigen::ConstRef> positions, + const ESPParameters& params) const +{ + assert( + point_edge_distance_type_exact( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3)) + == PointEdgeDistanceType::P_E); + const double eps = params.dhat; + // Edge3P1-Vertex3 is constructed only at interior P_E (see + // ESPCollisionsBuilder<3>::reduce_point_edge_collision). + const double dist = sqrt(point_edge_distance( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3))); + return (*params.barrier)(dist, eps); +} + +template <> +double ESPCollisionTemplate::operator()( + Eigen::ConstRef> positions, + const ESPParameters& params) const +{ + assert( + point_triangle_distance_type_exact( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6)) + == PointTriangleDistanceType::P_T); + const double eps = params.dhat; + // Face3P1-Vertex3 is constructed only at interior P_T (see + // ESPCollisionsBuilder<3>::reduce_point_triangle_collision). + const double dist = sqrt(point_triangle_distance( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6))); + return (*params.barrier)(dist, eps); +} + +template <> +auto ESPCollisionTemplate::gradient( + Eigen::ConstRef> positions, + const ESPParameters& params) const -> VectorMax +{ + assert(positions.size() == 6); + const double dist = + (positions.template head<3>() - positions.template tail<3>()).norm(); + const double eps = params.dhat; + const double deriv = + params.barrier->first_derivative(dist, eps) / (dist * 2.); + Vector6d grad = + deriv + * point_point_distance_gradient( + positions.template head<3>(), positions.template tail<3>()); + return grad; +} + +template <> +auto ESPCollisionTemplate::gradient( + Eigen::ConstRef> positions, + const ESPParameters& params) const -> VectorMax +{ + assert(positions.size() == 9); + assert( + point_edge_distance_type_exact( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3)) + == PointEdgeDistanceType::P_E); + // Edge3P1-Vertex3 is constructed only at interior P_E. + constexpr auto dtype = PointEdgeDistanceType::P_E; + const double dist = sqrt(point_edge_distance( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype)); + const double eps = params.dhat; + const double deriv = + params.barrier->first_derivative(dist, eps) / (dist * 2.); + Vector9d grad = point_edge_distance_gradient( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype); + grad *= deriv; + grad = grad({ 3, 4, 5, 6, 7, 8, 0, 1, 2 }).eval(); + return grad; +} + +template <> +auto ESPCollisionTemplate::gradient( + Eigen::ConstRef> positions, + const ESPParameters& params) const -> VectorMax +{ + assert(positions.size() == 12); + assert( + point_triangle_distance_type_exact( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6)) + == PointTriangleDistanceType::P_T); + // Face3P1-Vertex3 is constructed only at interior P_T. + constexpr auto dtype = PointTriangleDistanceType::P_T; + const double dist = sqrt(point_triangle_distance( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype)); + const double eps = params.dhat; + const double deriv = + params.barrier->first_derivative(dist, eps) / (dist * 2.); + Vector12d grad = point_triangle_distance_gradient( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype); + grad *= deriv; + grad = grad({ 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2 }).eval(); + return grad; +} + +template <> +auto ESPCollisionTemplate::hessian( + Eigen::ConstRef> positions, + const ESPParameters& params) const + -> MatrixMax +{ + assert(positions.size() == 6); + const double dist = + (positions.template head<3>() - positions.template tail<3>()).norm(); + const double eps = params.dhat; + double deriv1 = params.barrier->first_derivative(dist, eps); + double deriv2 = params.barrier->second_derivative(dist, eps); + deriv2 = deriv2 / (4 * dist * dist) - deriv1 / (4 * dist * dist * dist); + deriv1 /= (2 * dist); + const Vector6d g = point_point_distance_gradient( + positions.template head<3>(), positions.template tail<3>()); + const Matrix6d h = point_point_distance_hessian( + positions.template head<3>(), positions.template tail<3>()); + return g * deriv2 * g.transpose() + h * deriv1; +} + +template <> +auto ESPCollisionTemplate::hessian( + Eigen::ConstRef> positions, + const ESPParameters& params) const + -> MatrixMax +{ + assert(positions.size() == 9); + assert( + point_edge_distance_type_exact( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3)) + == PointEdgeDistanceType::P_E); + // Edge3P1-Vertex3 is constructed only at interior P_E. + constexpr auto dtype = PointEdgeDistanceType::P_E; + const double dist = sqrt(point_edge_distance( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype)); + const double eps = params.dhat; + double deriv1 = params.barrier->first_derivative(dist, eps); + double deriv2 = params.barrier->second_derivative(dist, eps); + deriv2 = deriv2 / (4 * dist * dist) - deriv1 / (4 * dist * dist * dist); + deriv1 /= (2 * dist); + const Vector9d g = point_edge_distance_gradient( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype); + const Matrix9d h = point_edge_distance_hessian( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype); + Matrix9d hess = g * deriv2 * g.transpose() + h * deriv1; + std::vector reorder { 3, 4, 5, 6, 7, 8, 0, 1, 2 }; + return hess(reorder, reorder); +} + +template <> +auto ESPCollisionTemplate::hessian( + Eigen::ConstRef> positions, + const ESPParameters& params) const + -> MatrixMax +{ + assert(positions.size() == 12); + assert( + point_triangle_distance_type_exact( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6)) + == PointTriangleDistanceType::P_T); + // Face3P1-Vertex3 is constructed only at interior P_T. + constexpr auto dtype = PointTriangleDistanceType::P_T; + const double dist = sqrt(point_triangle_distance( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype)); + const double eps = params.dhat; + double deriv1 = params.barrier->first_derivative(dist, eps); + double deriv2 = params.barrier->second_derivative(dist, eps); + deriv2 = deriv2 / (4 * dist * dist) - deriv1 / (4 * dist * dist * dist); + deriv1 /= (2 * dist); + const Vector12d g = point_triangle_distance_gradient( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype); + const Matrix12d h = point_triangle_distance_hessian( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype); + Matrix12d hess = g * deriv2 * g.transpose() + h * deriv1; + std::vector reorder { 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2 }; + return hess(reorder, reorder); +} + +// ---- NearFarBarrier specializations (3D only) ---- + +template <> +std::pair +ESPCollisionTemplate::operator_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + const double dist = + (positions.template head<3>() - positions.template segment<3>(3)) + .norm(); + const double eps = params.dhat; + return { nf_barrier->near_value(dist, eps), + nf_barrier->far_value(dist, eps) }; +} + +template <> +std::pair +ESPCollisionTemplate::operator_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + const double dist = sqrt(point_edge_distance( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3))); + const double eps = params.dhat; + return { nf_barrier->near_value(dist, eps), + nf_barrier->far_value(dist, eps) }; +} + +template <> +std::pair +ESPCollisionTemplate::operator_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + const double dist = sqrt(point_triangle_distance( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6))); + const double eps = params.dhat; + return { nf_barrier->near_value(dist, eps), + nf_barrier->far_value(dist, eps) }; +} + +template <> +std::pair< + VectorMax, + VectorMax> +ESPCollisionTemplate::gradient_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + assert(positions.size() == 6); + const double dist = + (positions.template head<3>() - positions.template tail<3>()).norm(); + const double eps = params.dhat; + const double deriv_near = + nf_barrier->first_derivative_near(dist, eps) / (dist * 2.); + const double deriv_far = + nf_barrier->first_derivative_far(dist, eps) / (dist * 2.); + Vector6d g = point_point_distance_gradient( + positions.template head<3>(), positions.template tail<3>()); + return { deriv_near * g, deriv_far * g }; +} + +template <> +std::pair< + VectorMax, + VectorMax> +ESPCollisionTemplate::gradient_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + assert(positions.size() == 9); + auto dtype = point_edge_distance_type_exact( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3)); + const double dist = sqrt(point_edge_distance( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype)); + const double eps = params.dhat; + const double deriv_near = + nf_barrier->first_derivative_near(dist, eps) / (dist * 2.); + const double deriv_far = + nf_barrier->first_derivative_far(dist, eps) / (dist * 2.); + Vector9d g = point_edge_distance_gradient( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype); + g = g({ 3, 4, 5, 6, 7, 8, 0, 1, 2 }).eval(); + return { deriv_near * g, deriv_far * g }; +} + +template <> +std::pair< + VectorMax, + VectorMax> +ESPCollisionTemplate::gradient_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + assert(positions.size() == 12); + auto dtype = point_triangle_distance_type_exact( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6)); + const double dist = sqrt(point_triangle_distance( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype)); + const double eps = params.dhat; + const double deriv_near = + nf_barrier->first_derivative_near(dist, eps) / (dist * 2.); + const double deriv_far = + nf_barrier->first_derivative_far(dist, eps) / (dist * 2.); + Vector12d g = point_triangle_distance_gradient( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype); + g = g({ 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2 }).eval(); + return { deriv_near * g, deriv_far * g }; +} + +template <> +std::pair< + MatrixMax, + MatrixMax> +ESPCollisionTemplate::hessian_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + assert(positions.size() == 6); + const double dist = + (positions.template head<3>() - positions.template tail<3>()).norm(); + const double eps = params.dhat; + double deriv1_near = nf_barrier->first_derivative_near(dist, eps); + double deriv2_near = nf_barrier->second_derivative_near(dist, eps); + double deriv1_far = nf_barrier->first_derivative_far(dist, eps); + double deriv2_far = nf_barrier->second_derivative_far(dist, eps); + deriv2_near = deriv2_near / (4 * dist * dist) + - deriv1_near / (4 * dist * dist * dist); + deriv2_far = + deriv2_far / (4 * dist * dist) - deriv1_far / (4 * dist * dist * dist); + deriv1_near /= (2 * dist); + deriv1_far /= (2 * dist); + const Vector6d g = point_point_distance_gradient( + positions.template head<3>(), positions.template tail<3>()); + const Matrix6d h = point_point_distance_hessian( + positions.template head<3>(), positions.template tail<3>()); + return { g * deriv2_near * g.transpose() + h * deriv1_near, + g * deriv2_far * g.transpose() + h * deriv1_far }; +} + +template <> +std::pair< + MatrixMax, + MatrixMax> +ESPCollisionTemplate::hessian_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + assert(positions.size() == 9); + auto dtype = point_edge_distance_type_exact( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3)); + const double dist = sqrt(point_edge_distance( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype)); + const double eps = params.dhat; + double deriv1_near = nf_barrier->first_derivative_near(dist, eps); + double deriv2_near = nf_barrier->second_derivative_near(dist, eps); + double deriv1_far = nf_barrier->first_derivative_far(dist, eps); + double deriv2_far = nf_barrier->second_derivative_far(dist, eps); + deriv2_near = deriv2_near / (4 * dist * dist) + - deriv1_near / (4 * dist * dist * dist); + deriv2_far = + deriv2_far / (4 * dist * dist) - deriv1_far / (4 * dist * dist * dist); + deriv1_near /= (2 * dist); + deriv1_far /= (2 * dist); + const Vector9d g = point_edge_distance_gradient( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype); + const Matrix9d h = point_edge_distance_hessian( + positions.template segment<3>(6), positions.template head<3>(), + positions.template segment<3>(3), dtype); + const Matrix9d hess_near = + g * deriv2_near * g.transpose() + h * deriv1_near; + const Matrix9d hess_far = g * deriv2_far * g.transpose() + h * deriv1_far; + const std::array reorder { { 3, 4, 5, 6, 7, 8, 0, 1, 2 } }; + return { hess_near(reorder, reorder), hess_far(reorder, reorder) }; +} + +template <> +std::pair< + MatrixMax, + MatrixMax> +ESPCollisionTemplate::hessian_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const +{ + assert(positions.size() == 12); + auto dtype = point_triangle_distance_type_exact( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6)); + const double dist = sqrt(point_triangle_distance( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype)); + const double eps = params.dhat; + double deriv1_near = nf_barrier->first_derivative_near(dist, eps); + double deriv2_near = nf_barrier->second_derivative_near(dist, eps); + double deriv1_far = nf_barrier->first_derivative_far(dist, eps); + double deriv2_far = nf_barrier->second_derivative_far(dist, eps); + deriv2_near = deriv2_near / (4 * dist * dist) + - deriv1_near / (4 * dist * dist * dist); + deriv2_far = + deriv2_far / (4 * dist * dist) - deriv1_far / (4 * dist * dist * dist); + deriv1_near /= (2 * dist); + deriv1_far /= (2 * dist); + const Vector12d g = point_triangle_distance_gradient( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype); + const Matrix12d h = point_triangle_distance_hessian( + positions.template segment<3>(9), positions.template head<3>(), + positions.template segment<3>(3), positions.template segment<3>(6), + dtype); + const Matrix12d hess_near = + g * deriv2_near * g.transpose() + h * deriv1_near; + const Matrix12d hess_far = g * deriv2_far * g.transpose() + h * deriv1_far; + const std::array reorder { { 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, + 2 } }; + return { hess_near(reorder, reorder), hess_far(reorder, reorder) }; +} + +// ---- 2D specializations ---- +// positions layout VV: [q_x, q_y, v_x, v_y] +// positions layout VE: [q_x, q_y, e0_x, e0_y, e1_x, e1_y] + +template <> +double ESPCollisionTemplate::compute_distance( + Eigen::ConstRef vertices) const +{ + const int n = vertices.rows(); + if (vertex_id(0) >= n || vertex_id(1) >= n) { + return std::numeric_limits::max(); + } + return point_point_distance( + vertices.row(vertex_id(0)), vertices.row(vertex_id(1))); +} + +template <> +double ESPCollisionTemplate::compute_distance( + Eigen::ConstRef vertices) const +{ + const int n = vertices.rows(); + if (vertex_id(0) >= n || vertex_id(1) >= n || vertex_id(2) >= n) { + return std::numeric_limits::max(); + } + return point_edge_distance( + vertices.row(vertex_id(0)), vertices.row(vertex_id(1)), + vertices.row(vertex_id(2))); +} + +template <> +double ESPCollisionTemplate::operator()( + Eigen::ConstRef> positions, + const ESPParameters& params) const +{ + const double dist = + (positions.template head<2>() - positions.template tail<2>()).norm(); + const double eps = params.dhat; + return (*params.barrier)(dist, eps); +} + +template <> +double ESPCollisionTemplate::operator()( + Eigen::ConstRef> positions, + const ESPParameters& params) const +{ + assert( + point_edge_distance_type_exact( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4)) + == PointEdgeDistanceType::P_E); + const double eps = params.dhat; + // Vertex2-Edge2P1 is constructed only at interior P_E (the 2D edge-QP + // builder routes endpoint cases to Vertex2-Vertex2). + const double dist = std::sqrt(point_edge_distance( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4))); + return (*params.barrier)(dist, eps); +} + +template <> +auto ESPCollisionTemplate::gradient( + Eigen::ConstRef> positions, + const ESPParameters& params) const -> VectorMax +{ + const double dist = + (positions.template head<2>() - positions.template tail<2>()).norm(); + const double eps = params.dhat; + const double deriv = + params.barrier->first_derivative(dist, eps) / (dist * 2.0); + const VectorMax6d g = point_point_distance_gradient( + positions.template head<2>(), positions.template tail<2>()); + return deriv * g; +} + +template <> +auto ESPCollisionTemplate::gradient( + Eigen::ConstRef> positions, + const ESPParameters& params) const -> VectorMax +{ + assert( + point_edge_distance_type_exact( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4)) + == PointEdgeDistanceType::P_E); + // Vertex2-Edge2P1 is constructed only at interior P_E. + constexpr auto dtype = PointEdgeDistanceType::P_E; + const double dist = std::sqrt(point_edge_distance( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4), dtype)); + const double eps = params.dhat; + const double deriv = + params.barrier->first_derivative(dist, eps) / (dist * 2.0); + const VectorMax9d g = point_edge_distance_gradient( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4), dtype); + return deriv * g; +} + +template <> +auto ESPCollisionTemplate::hessian( + Eigen::ConstRef> positions, + const ESPParameters& params) const + -> MatrixMax +{ + const double dist = + (positions.template head<2>() - positions.template tail<2>()).norm(); + const double eps = params.dhat; + double deriv1 = params.barrier->first_derivative(dist, eps); + double deriv2 = params.barrier->second_derivative(dist, eps); + deriv2 = deriv2 / (4 * dist * dist) - deriv1 / (4 * dist * dist * dist); + deriv1 /= (2 * dist); + const VectorMax6d g = point_point_distance_gradient( + positions.template head<2>(), positions.template tail<2>()); + const MatrixMax6d H = point_point_distance_hessian( + positions.template head<2>(), positions.template tail<2>()); + return g * deriv2 * g.transpose() + H * deriv1; +} + +template <> +auto ESPCollisionTemplate::hessian( + Eigen::ConstRef> positions, + const ESPParameters& params) const + -> MatrixMax +{ + assert( + point_edge_distance_type_exact( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4)) + == PointEdgeDistanceType::P_E); + // Vertex2-Edge2P1 is constructed only at interior P_E. + constexpr auto dtype = PointEdgeDistanceType::P_E; + const double dist = std::sqrt(point_edge_distance( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4), dtype)); + const double eps = params.dhat; + double deriv1 = params.barrier->first_derivative(dist, eps); + double deriv2 = params.barrier->second_derivative(dist, eps); + deriv2 = deriv2 / (4 * dist * dist) - deriv1 / (4 * dist * dist * dist); + deriv1 /= (2 * dist); + const VectorMax9d g = point_edge_distance_gradient( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4), dtype); + const MatrixMax9d H = point_edge_distance_hessian( + positions.template head<2>(), positions.template segment<2>(2), + positions.template segment<2>(4), dtype); + return g * deriv2 * g.transpose() + H * deriv1; +} + +// ---- explicit instantiations ---- + +template class ESPCollisionTemplate; +template class ESPCollisionTemplate; +template class ESPCollisionTemplate; +template class ESPCollisionTemplate; +template class ESPCollisionTemplate; + +} // namespace ipc diff --git a/src/ipc/esp/collisions/esp_collision_template.hpp b/src/ipc/esp/collisions/esp_collision_template.hpp new file mode 100644 index 000000000..f816ebe82 --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision_template.hpp @@ -0,0 +1,199 @@ +#pragma once +#include +#include +#include +#include + +#include + +namespace ipc { + +/// @brief Templated class for various types of contact pairs +template +class ESPCollisionTemplate : public ESPCollision { +public: + using Super = ESPCollision; + static constexpr int N_CORE_POINTS = + PrimitiveA::N_CORE_POINTS + PrimitiveB::N_CORE_POINTS; + static constexpr int DIM = PrimitiveA::DIM; + static constexpr int N_CORE_DOFS_A = PrimitiveA::N_CORE_POINTS * DIM; + static constexpr int N_CORE_DOFS_B = PrimitiveB::N_CORE_POINTS * DIM; + static constexpr int N_CORE_DOFS = N_CORE_POINTS * DIM; + static constexpr int ELEMENT_SIZE = Super::ELEMENT_SIZE; + static_assert( + PrimitiveA::N_DOFS + PrimitiveB::N_DOFS <= ELEMENT_SIZE, + "ESPCollision::ELEMENT_SIZE is too small for this stencil"); + + ESPCollisionTemplate( + index_t primitive0, index_t primitive1, const CollisionMesh& mesh); + + virtual ~ESPCollisionTemplate() = default; + + std::string name() const override; + + int n_dofs() const override + { + return m_primitive_a.n_dofs() + m_primitive_b.n_dofs(); + } + ESPCollisionType type() const override; + + std::pair get_hash() const override + { + return std::make_pair(m_primitive_a.id(), m_primitive_b.id()); + } + + std::array get_typed_hash() const override + { + return { { static_cast(type()), m_primitive_a.id(), + m_primitive_b.id() } }; + } + + index_t operator[](int idx) const override + { + if (idx == 0) { + return m_primitive_a.id(); + } else if (idx == 1) { + return m_primitive_b.id(); + } else { + log_and_throw_error("Invalid index in ESP collision!"); + } + } + + int num_vertices() const override + { + return m_primitive_a.n_vertices() + m_primitive_b.n_vertices(); + } + + index_t vertex_id(index_t i) const override; + + size_t n_vertices_a() const override { return m_primitive_a.n_vertices(); } + size_t n_vertices_b() const override { return m_primitive_b.n_vertices(); } + + double operator()( + Eigen::ConstRef> positions, + const ESPParameters& params) const override; + + VectorMax gradient( + Eigen::ConstRef> positions, + const ESPParameters& params) const override; + + MatrixMax hessian( + Eigen::ConstRef> positions, + const ESPParameters& params) const override; + + double + compute_distance(Eigen::ConstRef vertices) const override; + + // The near/far functions are only declared here. Their generic stubs and + // the 3D specializations are both defined in esp_collision_template.cpp: + // a generic definition in the class body can be instantiated (MSVC does) + // before the specializations are seen, putting the zero stub in the + // vtable. + + std::pair operator_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const override; + + std::pair, VectorMax> + gradient_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const override; + + std::pair< + MatrixMax, + MatrixMax> + hessian_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier<>* nf_barrier) const override; + +private: + PrimitiveA m_primitive_a; + PrimitiveB m_primitive_b; +}; + +// We define the members below as explicit specializations in +// esp_collision_template.cpp, and every translation unit that uses one must +// see its declaration first; otherwise the program is ill-formed (no +// diagnostic required). In particular, the vertex-vertex constructor sorts its +// two ids, which ArbitraryPointESP relies on. + +template <> +ESPCollisionTemplate::ESPCollisionTemplate( + index_t primitive0, index_t primitive1, const CollisionMesh& mesh); + +#define IPC_DECLARE_ESP_COLLISION_SPECIALIZATIONS(A, B) \ + template <> std::string ESPCollisionTemplate::name() const; \ + template <> ESPCollisionType ESPCollisionTemplate::type() const; \ + template <> \ + double ESPCollisionTemplate::compute_distance( \ + Eigen::ConstRef vertices) const; \ + template <> \ + double ESPCollisionTemplate::operator()( \ + Eigen::ConstRef> \ + positions, \ + const ESPParameters& params) const; \ + template <> \ + VectorMax \ + ESPCollisionTemplate::gradient( \ + Eigen::ConstRef> \ + positions, \ + const ESPParameters& params) const; \ + template <> \ + MatrixMax \ + ESPCollisionTemplate::hessian( \ + Eigen::ConstRef> \ + positions, \ + const ESPParameters& params) const + +#define IPC_DECLARE_ESP_COLLISION_NEARFAR_SPECIALIZATIONS(A, B) \ + template <> \ + std::pair ESPCollisionTemplate::operator_nearfar( \ + Eigen::ConstRef> \ + positions, \ + const ESPParameters& params, const NearFarBarrier<>* nf_barrier) \ + const; \ + template <> \ + std::pair< \ + VectorMax, \ + VectorMax> \ + ESPCollisionTemplate::gradient_nearfar( \ + Eigen::ConstRef> \ + positions, \ + const ESPParameters& params, const NearFarBarrier<>* nf_barrier) \ + const; \ + template <> \ + std::pair< \ + MatrixMax< \ + double, ESPCollision::ELEMENT_SIZE, ESPCollision::ELEMENT_SIZE>, \ + MatrixMax< \ + double, ESPCollision::ELEMENT_SIZE, ESPCollision::ELEMENT_SIZE>> \ + ESPCollisionTemplate::hessian_nearfar( \ + Eigen::ConstRef> \ + positions, \ + const ESPParameters& params, const NearFarBarrier<>* nf_barrier) const + +IPC_DECLARE_ESP_COLLISION_SPECIALIZATIONS(Vertex3, Vertex3); +IPC_DECLARE_ESP_COLLISION_SPECIALIZATIONS(Edge3P1, Vertex3); +IPC_DECLARE_ESP_COLLISION_SPECIALIZATIONS(Face3P1, Vertex3); +IPC_DECLARE_ESP_COLLISION_SPECIALIZATIONS(Vertex2, Vertex2); +IPC_DECLARE_ESP_COLLISION_SPECIALIZATIONS(Vertex2, Edge2P1); + +IPC_DECLARE_ESP_COLLISION_NEARFAR_SPECIALIZATIONS(Vertex3, Vertex3); +IPC_DECLARE_ESP_COLLISION_NEARFAR_SPECIALIZATIONS(Edge3P1, Vertex3); +IPC_DECLARE_ESP_COLLISION_NEARFAR_SPECIALIZATIONS(Face3P1, Vertex3); + +#undef IPC_DECLARE_ESP_COLLISION_SPECIALIZATIONS +#undef IPC_DECLARE_ESP_COLLISION_NEARFAR_SPECIALIZATIONS + +// Keep old name as alias for backward compatibility within this codebase +template +using ESPCollision3DTemplate = ESPCollisionTemplate; + +// 2D alias (for use with 2D primitives) +template +using ESPCollision2DTemplate = ESPCollisionTemplate; + +} // namespace ipc diff --git a/src/ipc/esp/collisions/esp_primitives.hpp b/src/ipc/esp/collisions/esp_primitives.hpp new file mode 100644 index 000000000..65082fe62 --- /dev/null +++ b/src/ipc/esp/collisions/esp_primitives.hpp @@ -0,0 +1,138 @@ +#pragma once + +#include +#include +#include + +#include + +namespace ipc { + +/** + * @brief Base class for primitives used in ESP contact models. + * + * This class defines the common interface for geometric primitives (like + * vertices and edges) involved in a ESP contact. Derived classes + * are responsible for implementing the specific logic for their geometry type. + */ +class ESPPrimitive { +public: + constexpr static int MAX_NUM_VERTS = 3; + ESPPrimitive(const index_t id) : m_id(id) { } + + virtual ~ESPPrimitive() = default; + + bool operator==(const ESPPrimitive& other) const + { + return id() == other.id(); + } + + /// @brief Get the ID of this primitive (e.g., vertex ID, edge ID). + index_t id() const { return m_id; } + + /// @brief Get the number of vertices in the primitive's stencil. + virtual int n_vertices() const = 0; + + /// @brief Get the number of degrees of freedom for this primitive. + virtual int n_dofs() const = 0; + + /// @brief Get the i-th vertex ID of the primitive's stencil. + index_t vertex_id(const index_t i) const + { + assert(i >= 0 && i < n_vertices()); + return m_vertex_ids[i]; + } + +protected: + /// @brief Vertex IDs of the stencil for this primitive. + std::array m_vertex_ids { { -1, -1, -1 } }; + /// @brief The ID of this primitive. + index_t m_id; +}; + +class Edge2P1 : public ESPPrimitive { +public: + static constexpr int N_CORE_POINTS = 2; + static constexpr int N_POINTS = 2; + static constexpr int DIM = 2; + static constexpr int N_DOFS = N_POINTS * DIM; + + Edge2P1(const index_t id, const CollisionMesh& mesh) : ESPPrimitive(id) + { + m_vertex_ids[0] = mesh.edges()(id, 0); + m_vertex_ids[1] = mesh.edges()(id, 1); + } + + int n_vertices() const override { return 2; } + int n_dofs() const override { return n_vertices() * DIM; } +}; + +/// @brief Simple 2D vertex primitive (single vertex, no neighbor storage). +class Vertex2 : public ESPPrimitive { +public: + static constexpr int N_CORE_POINTS = 1; + static constexpr int N_POINTS = 1; + static constexpr int DIM = 2; + static constexpr int N_DOFS = N_POINTS * DIM; + + Vertex2(const index_t id, const CollisionMesh& /*mesh*/) : ESPPrimitive(id) + { + m_vertex_ids[0] = id; + } + + int n_vertices() const override { return 1; } + int n_dofs() const override { return N_DOFS; } +}; + +class Vertex3 : public ESPPrimitive { +public: + static constexpr int N_CORE_POINTS = 1; + static constexpr int N_POINTS = 1; + static constexpr int DIM = 3; + static constexpr int N_DOFS = N_POINTS * DIM; + + Vertex3(const index_t id, const CollisionMesh& mesh) : ESPPrimitive(id) + { + m_vertex_ids[0] = id; + } + + int n_vertices() const override { return 1; } + int n_dofs() const override { return n_vertices() * DIM; } +}; + +class Edge3P1 : public ESPPrimitive { +public: + static constexpr int N_CORE_POINTS = 2; + static constexpr int N_POINTS = 2; + static constexpr int DIM = 3; + static constexpr int N_DOFS = N_POINTS * DIM; + + Edge3P1(const index_t id, const CollisionMesh& mesh) : ESPPrimitive(id) + { + m_vertex_ids[0] = mesh.edges()(id, 0); + m_vertex_ids[1] = mesh.edges()(id, 1); + } + + int n_vertices() const override { return 2; } + int n_dofs() const override { return n_vertices() * DIM; } +}; + +class Face3P1 : public ESPPrimitive { +public: + static constexpr int N_CORE_POINTS = 3; + static constexpr int N_POINTS = 3; + static constexpr int DIM = 3; + static constexpr int N_DOFS = N_POINTS * DIM; + + Face3P1(const index_t id, const CollisionMesh& mesh) : ESPPrimitive(id) + { + m_vertex_ids[0] = mesh.faces()(id, 0); + m_vertex_ids[1] = mesh.faces()(id, 1); + m_vertex_ids[2] = mesh.faces()(id, 2); + } + + int n_vertices() const override { return 3; } + int n_dofs() const override { return n_vertices() * DIM; } +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/esp/collisions/esp_quadrature.hpp b/src/ipc/esp/collisions/esp_quadrature.hpp new file mode 100644 index 000000000..1a8e429ca --- /dev/null +++ b/src/ipc/esp/collisions/esp_quadrature.hpp @@ -0,0 +1,784 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace ipc { + +/// Compute the nodes and weights for Gauss-Lobatto quadrature of order n. +void lobatto_compute(int n, std::vector& x, std::vector& w); + +/// A single edge quadrature point in [0, 1] with its weight. +/// Parallel to FaceQuadPoint for face quadrature. +struct EdgeQuadPoint { + double xi; ///< Abscissa in [0, 1] + double weight; +}; + +// Class to compute and cache nodes and weights for Gauss-Lobatto quadrature. +class GaussLobatto { +public: + using Rule = std::vector; + + // Get the quadrature rule for a given order n >= 1: the (n + 1)-point + // rule, exact for polynomials of degree up to 2n - 1. + static const Rule& get_rule(int n) + { + if (n < 1) { + log_and_throw_error("Order must be at least 1"); + } + + static std::map cache; + static std::mutex mtx; + + std::lock_guard lock(mtx); + auto it = cache.find(n); + if (it == cache.end()) { + it = cache.emplace(n, compute_rule(n)).first; + } + return it->second; + } + +private: + // Computes the nodes and weights for Gauss-Lobatto quadrature of order n. + static Rule compute_rule(int n) + { + std::vector nodes, weights; + lobatto_compute(n, nodes, weights); + + Rule res; + res.reserve(n); + for (int i = 0; i < nodes.size(); ++i) { + res.push_back({ nodes[i] / 2 + .5, weights[i] / 2 }); + } + return res; + } +}; + +/******************************************************************************/ + +inline void +lobatto_set(int n, std::vector& xtab, std::vector& weight) + +/******************************************************************************/ +/* + Purpose: + + LOBATTO_SET sets abscissas and weights for Lobatto quadrature. + + Discussion: + + The integral: + + Integral ( -1 <= X <= 1 ) F(X) dX + + The quadrature rule: + + Sum ( 1 <= I <= N ) WEIGHT[I] * F ( XTAB[I] ) + + The quadrature rule will integrate exactly all polynomials up to + X^(2*N-3). + + The Lobatto rule is distinguished by the fact that both endpoints + (-1 and 1) are always abscissas. + + Licensing: + + This code is distributed under the MIT license. + + Modified: + + 30 April 2006 + + Author: + + John Burkardt + + Source: + + https://people.sc.fsu.edu/~jburkardt/c_src/quad_rule/quad_rule.c + (adapted to std::vector and exceptions) + + Reference: + + Milton Abramowitz, Irene Stegun, + Handbook of Mathematical Functions, + National Bureau of Standards, 1964, + ISBN: 0-486-61272-4, + LC: QA47.A34. + + Arthur Stroud, Don Secrest, + Gaussian Quadrature Formulas, + Prentice Hall, 1966, + LC: QA299.4G3S7. + + Daniel Zwillinger, editor, + CRC Standard Mathematical Tables and Formulae, + 30th Edition, + CRC Press, 1996, + ISBN: 0-8493-2479-3. + + Parameters: + + Input, int N, the order. + N must be between 2 and 20. + + Output, double XTAB[N], the abscissas. + + Output, double WEIGHT[N], the weights. +*/ +{ + xtab.resize(n); + weight.resize(n); + switch (n) { + case 2: { + xtab[0] = -1.0E+00; + xtab[1] = 1.0E+00; + + weight[0] = 1.0E+00; + weight[1] = 1.0E+00; + } break; + case 3: { + xtab[0] = -1.0E+00; + xtab[1] = 0.0E+00; + xtab[2] = 1.0E+00; + + weight[0] = 1.0 / 3.0E+00; + weight[1] = 4.0 / 3.0E+00; + weight[2] = 1.0 / 3.0E+00; + } break; + case 4: { + xtab[0] = -1.0E+00; + xtab[1] = -0.447213595499957939281834733746E+00; + xtab[2] = 0.447213595499957939281834733746E+00; + xtab[3] = 1.0E+00; + + weight[0] = 1.0E+00 / 6.0E+00; + weight[1] = 5.0E+00 / 6.0E+00; + weight[2] = 5.0E+00 / 6.0E+00; + weight[3] = 1.0E+00 / 6.0E+00; + } break; + case 5: { + xtab[0] = -1.0E+00; + xtab[1] = -0.654653670707977143798292456247E+00; + xtab[2] = 0.0E+00; + xtab[3] = 0.654653670707977143798292456247E+00; + xtab[4] = 1.0E+00; + + weight[0] = 9.0E+00 / 90.0E+00; + weight[1] = 49.0E+00 / 90.0E+00; + weight[2] = 64.0E+00 / 90.0E+00; + weight[3] = 49.0E+00 / 90.0E+00; + weight[4] = 9.0E+00 / 90.0E+00; + } break; + case 6: { + xtab[0] = -1.0E+00; + xtab[1] = -0.765055323929464692851002973959E+00; + xtab[2] = -0.285231516480645096314150994041E+00; + xtab[3] = 0.285231516480645096314150994041E+00; + xtab[4] = 0.765055323929464692851002973959E+00; + xtab[5] = 1.0E+00; + + weight[0] = 0.066666666666666666666666666667E+00; + weight[1] = 0.378474956297846980316612808212E+00; + weight[2] = 0.554858377035486353016720525121E+00; + weight[3] = 0.554858377035486353016720525121E+00; + weight[4] = 0.378474956297846980316612808212E+00; + weight[5] = 0.066666666666666666666666666667E+00; + } break; + case 7: { + xtab[0] = -1.0E+00; + xtab[1] = -0.830223896278566929872032213967E+00; + xtab[2] = -0.468848793470714213803771881909E+00; + xtab[3] = 0.0E+00; + xtab[4] = 0.468848793470714213803771881909E+00; + xtab[5] = 0.830223896278566929872032213967E+00; + xtab[6] = 1.0E+00; + + weight[0] = 0.476190476190476190476190476190E-01; + weight[1] = 0.276826047361565948010700406290E+00; + weight[2] = 0.431745381209862623417871022281E+00; + weight[3] = 0.487619047619047619047619047619E+00; + weight[4] = 0.431745381209862623417871022281E+00; + weight[5] = 0.276826047361565948010700406290E+00; + weight[6] = 0.476190476190476190476190476190E-01; + } break; + case 8: { + xtab[0] = -1.0E+00; + xtab[1] = -0.871740148509606615337445761221E+00; + xtab[2] = -0.591700181433142302144510731398E+00; + xtab[3] = -0.209299217902478868768657260345E+00; + xtab[4] = 0.209299217902478868768657260345E+00; + xtab[5] = 0.591700181433142302144510731398E+00; + xtab[6] = 0.871740148509606615337445761221E+00; + xtab[7] = 1.0E+00; + + weight[0] = 0.357142857142857142857142857143E-01; + weight[1] = 0.210704227143506039382991065776E+00; + weight[2] = 0.341122692483504364764240677108E+00; + weight[3] = 0.412458794658703881567052971402E+00; + weight[4] = 0.412458794658703881567052971402E+00; + weight[5] = 0.341122692483504364764240677108E+00; + weight[6] = 0.210704227143506039382991065776E+00; + weight[7] = 0.357142857142857142857142857143E-01; + } break; + case 9: { + xtab[0] = -1.0E+00; + xtab[1] = -0.899757995411460157312345244418E+00; + xtab[2] = -0.677186279510737753445885427091E+00; + xtab[3] = -0.363117463826178158710752068709E+00; + xtab[4] = 0.0E+00; + xtab[5] = 0.363117463826178158710752068709E+00; + xtab[6] = 0.677186279510737753445885427091E+00; + xtab[7] = 0.899757995411460157312345244418E+00; + xtab[8] = 1.0E+00; + + weight[0] = 0.277777777777777777777777777778E-01; + weight[1] = 0.165495361560805525046339720029E+00; + weight[2] = 0.274538712500161735280705618579E+00; + weight[3] = 0.346428510973046345115131532140E+00; + weight[4] = 0.371519274376417233560090702948E+00; + weight[5] = 0.346428510973046345115131532140E+00; + weight[6] = 0.274538712500161735280705618579E+00; + weight[7] = 0.165495361560805525046339720029E+00; + weight[8] = 0.277777777777777777777777777778E-01; + } break; + case 10: { + xtab[0] = -1.0E+00; + xtab[1] = -0.919533908166458813828932660822E+00; + xtab[2] = -0.738773865105505075003106174860E+00; + xtab[3] = -0.477924949810444495661175092731E+00; + xtab[4] = -0.165278957666387024626219765958E+00; + xtab[5] = 0.165278957666387024626219765958E+00; + xtab[6] = 0.477924949810444495661175092731E+00; + xtab[7] = 0.738773865105505075003106174860E+00; + xtab[8] = 0.919533908166458813828932660822E+00; + xtab[9] = 1.0E+00; + + weight[0] = 0.222222222222222222222222222222E-01; + weight[1] = 0.133305990851070111126227170755E+00; + weight[2] = 0.224889342063126452119457821731E+00; + weight[3] = 0.292042683679683757875582257374E+00; + weight[4] = 0.327539761183897456656510527917E+00; + weight[5] = 0.327539761183897456656510527917E+00; + weight[6] = 0.292042683679683757875582257374E+00; + weight[7] = 0.224889342063126452119457821731E+00; + weight[8] = 0.133305990851070111126227170755E+00; + weight[9] = 0.222222222222222222222222222222E-01; + } break; + case 11: { + xtab[0] = -1.0E+00; + xtab[1] = -0.934001430408059134332274136099E+00; + xtab[2] = -0.784483473663144418622417816108E+00; + xtab[3] = -0.565235326996205006470963969478E+00; + xtab[4] = -0.295758135586939391431911515559E+00; + xtab[5] = 0.0E+00; + xtab[6] = 0.295758135586939391431911515559E+00; + xtab[7] = 0.565235326996205006470963969478E+00; + xtab[8] = 0.784483473663144418622417816108E+00; + xtab[9] = 0.934001430408059134332274136099E+00; + xtab[10] = 1.0E+00; + + weight[0] = 0.181818181818181818181818181818E-01; + weight[1] = 0.109612273266994864461403449580E+00; + weight[2] = 0.187169881780305204108141521899E+00; + weight[3] = 0.248048104264028314040084866422E+00; + weight[4] = 0.286879124779008088679222403332E+00; + weight[5] = 0.300217595455690693785931881170E+00; + weight[6] = 0.286879124779008088679222403332E+00; + weight[7] = 0.248048104264028314040084866422E+00; + weight[8] = 0.187169881780305204108141521899E+00; + weight[9] = 0.109612273266994864461403449580E+00; + weight[10] = 0.181818181818181818181818181818E-01; + } break; + case 12: { + xtab[0] = -1.0E+00; + xtab[1] = -0.944899272222882223407580138303E+00; + xtab[2] = -0.819279321644006678348641581717E+00; + xtab[3] = -0.632876153031869677662404854444E+00; + xtab[4] = -0.399530940965348932264349791567E+00; + xtab[5] = -0.136552932854927554864061855740E+00; + xtab[6] = 0.136552932854927554864061855740E+00; + xtab[7] = 0.399530940965348932264349791567E+00; + xtab[8] = 0.632876153031869677662404854444E+00; + xtab[9] = 0.819279321644006678348641581717E+00; + xtab[10] = 0.944899272222882223407580138303E+00; + xtab[11] = 1.0E+00; + + weight[0] = 0.151515151515151515151515151515E-01; + weight[1] = 0.916845174131961306683425941341E-01; + weight[2] = 0.157974705564370115164671062700E+00; + weight[3] = 0.212508417761021145358302077367E+00; + weight[4] = 0.251275603199201280293244412148E+00; + weight[5] = 0.271405240910696177000288338500E+00; + weight[6] = 0.271405240910696177000288338500E+00; + weight[7] = 0.251275603199201280293244412148E+00; + weight[8] = 0.212508417761021145358302077367E+00; + weight[9] = 0.157974705564370115164671062700E+00; + weight[10] = 0.916845174131961306683425941341E-01; + weight[11] = 0.151515151515151515151515151515E-01; + } break; + case 13: { + xtab[0] = -1.0E+00; + xtab[1] = -0.953309846642163911896905464755E+00; + xtab[2] = -0.846347564651872316865925607099E+00; + xtab[3] = -0.686188469081757426072759039566E+00; + xtab[4] = -0.482909821091336201746937233637E+00; + xtab[5] = -0.249286930106239992568673700374E+00; + xtab[6] = 0.0E+00; + xtab[7] = 0.249286930106239992568673700374E+00; + xtab[8] = 0.482909821091336201746937233637E+00; + xtab[9] = 0.686188469081757426072759039566E+00; + xtab[10] = 0.846347564651872316865925607099E+00; + xtab[11] = 0.953309846642163911896905464755E+00; + xtab[12] = 1.0E+00; + + weight[0] = 0.128205128205128205128205128205E-01; + weight[1] = 0.778016867468189277935889883331E-01; + weight[2] = 0.134981926689608349119914762589E+00; + weight[3] = 0.183646865203550092007494258747E+00; + weight[4] = 0.220767793566110086085534008379E+00; + weight[5] = 0.244015790306676356458578148360E+00; + weight[6] = 0.251930849333446736044138641541E+00; + weight[7] = 0.244015790306676356458578148360E+00; + weight[8] = 0.220767793566110086085534008379E+00; + weight[9] = 0.183646865203550092007494258747E+00; + weight[10] = 0.134981926689608349119914762589E+00; + weight[11] = 0.778016867468189277935889883331E-01; + weight[12] = 0.128205128205128205128205128205E-01; + } break; + case 14: { + xtab[0] = -1.0E+00; + xtab[1] = -0.959935045267260901355100162015E+00; + xtab[2] = -0.867801053830347251000220202908E+00; + xtab[3] = -0.728868599091326140584672400521E+00; + xtab[4] = -0.550639402928647055316622705859E+00; + xtab[5] = -0.342724013342712845043903403642E+00; + xtab[6] = -0.116331868883703867658776709736E+00; + xtab[7] = 0.116331868883703867658776709736E+00; + xtab[8] = 0.342724013342712845043903403642E+00; + xtab[9] = 0.550639402928647055316622705859E+00; + xtab[10] = 0.728868599091326140584672400521E+00; + xtab[11] = 0.867801053830347251000220202908E+00; + xtab[12] = 0.959935045267260901355100162015E+00; + xtab[13] = 1.0E+00; + + weight[0] = 0.109890109890109890109890109890E-01; + weight[1] = 0.668372844976812846340706607461E-01; + weight[2] = 0.116586655898711651540996670655E+00; + weight[3] = 0.160021851762952142412820997988E+00; + weight[4] = 0.194826149373416118640331778376E+00; + weight[5] = 0.219126253009770754871162523954E+00; + weight[6] = 0.231612794468457058889628357293E+00; + weight[7] = 0.231612794468457058889628357293E+00; + weight[8] = 0.219126253009770754871162523954E+00; + weight[9] = 0.194826149373416118640331778376E+00; + weight[10] = 0.160021851762952142412820997988E+00; + weight[11] = 0.116586655898711651540996670655E+00; + weight[12] = 0.668372844976812846340706607461E-01; + weight[13] = 0.109890109890109890109890109890E-01; + } break; + case 15: { + xtab[0] = -1.0E+00; + xtab[1] = -0.965245926503838572795851392070E+00; + xtab[2] = -0.885082044222976298825401631482E+00; + xtab[3] = -0.763519689951815200704118475976E+00; + xtab[4] = -0.606253205469845711123529938637E+00; + xtab[5] = -0.420638054713672480921896938739E+00; + xtab[6] = -0.215353955363794238225679446273E+00; + xtab[7] = 0.0E+00; + xtab[8] = 0.215353955363794238225679446273E+00; + xtab[9] = 0.420638054713672480921896938739E+00; + xtab[10] = 0.606253205469845711123529938637E+00; + xtab[11] = 0.763519689951815200704118475976E+00; + xtab[12] = 0.885082044222976298825401631482E+00; + xtab[13] = 0.965245926503838572795851392070E+00; + xtab[14] = 1.0E+00; + + weight[0] = 0.952380952380952380952380952381E-02; + weight[1] = 0.580298930286012490968805840253E-01; + weight[2] = 0.101660070325718067603666170789E+00; + weight[3] = 0.140511699802428109460446805644E+00; + weight[4] = 0.172789647253600949052077099408E+00; + weight[5] = 0.196987235964613356092500346507E+00; + weight[6] = 0.211973585926820920127430076977E+00; + weight[7] = 0.217048116348815649514950214251E+00; + weight[8] = 0.211973585926820920127430076977E+00; + weight[9] = 0.196987235964613356092500346507E+00; + weight[10] = 0.172789647253600949052077099408E+00; + weight[11] = 0.140511699802428109460446805644E+00; + weight[12] = 0.101660070325718067603666170789E+00; + weight[13] = 0.580298930286012490968805840253E-01; + weight[14] = 0.952380952380952380952380952381E-02; + } break; + case 16: { + xtab[0] = -1.0E+00; + xtab[1] = -0.969568046270217932952242738367E+00; + xtab[2] = -0.899200533093472092994628261520E+00; + xtab[3] = -0.792008291861815063931088270963E+00; + xtab[4] = -0.652388702882493089467883219641E+00; + xtab[5] = -0.486059421887137611781890785847E+00; + xtab[6] = -0.299830468900763208098353454722E+00; + xtab[7] = -0.101326273521949447843033005046E+00; + xtab[8] = 0.101326273521949447843033005046E+00; + xtab[9] = 0.299830468900763208098353454722E+00; + xtab[10] = 0.486059421887137611781890785847E+00; + xtab[11] = 0.652388702882493089467883219641E+00; + xtab[12] = 0.792008291861815063931088270963E+00; + xtab[13] = 0.899200533093472092994628261520E+00; + xtab[14] = 0.969568046270217932952242738367E+00; + xtab[15] = 1.0E+00; + + weight[0] = 0.833333333333333333333333333333E-02; + weight[1] = 0.508503610059199054032449195655E-01; + weight[2] = 0.893936973259308009910520801661E-01; + weight[3] = 0.124255382132514098349536332657E+00; + weight[4] = 0.154026980807164280815644940485E+00; + weight[5] = 0.177491913391704125301075669528E+00; + weight[6] = 0.193690023825203584316913598854E+00; + weight[7] = 0.201958308178229871489199125411E+00; + weight[8] = 0.201958308178229871489199125411E+00; + weight[9] = 0.193690023825203584316913598854E+00; + weight[10] = 0.177491913391704125301075669528E+00; + weight[11] = 0.154026980807164280815644940485E+00; + weight[12] = 0.124255382132514098349536332657E+00; + weight[13] = 0.893936973259308009910520801661E-01; + weight[14] = 0.508503610059199054032449195655E-01; + weight[15] = 0.833333333333333333333333333333E-02; + } break; + case 17: { + xtab[0] = -1.0E+00; + xtab[1] = -0.973132176631418314156979501874E+00; + xtab[2] = -0.910879995915573595623802506398E+00; + xtab[3] = -0.815696251221770307106750553238E+00; + xtab[4] = -0.691028980627684705394919357372E+00; + xtab[5] = -0.541385399330101539123733407504E+00; + xtab[6] = -0.372174433565477041907234680735E+00; + xtab[7] = -0.189511973518317388304263014753E+00; + xtab[8] = 0.0E+00; + xtab[9] = 0.189511973518317388304263014753E+00; + xtab[10] = 0.372174433565477041907234680735E+00; + xtab[11] = 0.541385399330101539123733407504E+00; + xtab[12] = 0.691028980627684705394919357372E+00; + xtab[13] = 0.815696251221770307106750553238E+00; + xtab[14] = 0.910879995915573595623802506398E+00; + xtab[15] = 0.973132176631418314156979501874E+00; + xtab[16] = 1.0E+00; + + weight[0] = 0.735294117647058823529411764706E-02; + weight[1] = 0.449219405432542096474009546232E-01; + weight[2] = 0.791982705036871191902644299528E-01; + weight[3] = 0.110592909007028161375772705220E+00; + weight[4] = 0.137987746201926559056201574954E+00; + weight[5] = 0.160394661997621539516328365865E+00; + weight[6] = 0.177004253515657870436945745363E+00; + weight[7] = 0.187216339677619235892088482861E+00; + weight[8] = 0.190661874753469433299407247028E+00; + weight[9] = 0.187216339677619235892088482861E+00; + weight[10] = 0.177004253515657870436945745363E+00; + weight[11] = 0.160394661997621539516328365865E+00; + weight[12] = 0.137987746201926559056201574954E+00; + weight[13] = 0.110592909007028161375772705220E+00; + weight[14] = 0.791982705036871191902644299528E-01; + weight[15] = 0.449219405432542096474009546232E-01; + weight[16] = 0.735294117647058823529411764706E-02; + } break; + case 18: { + xtab[0] = -1.0E+00; + xtab[1] = -0.976105557412198542864518924342E+00; + xtab[2] = -0.920649185347533873837854625431E+00; + xtab[3] = -0.835593535218090213713646362328E+00; + xtab[4] = -0.723679329283242681306210365302E+00; + xtab[5] = -0.588504834318661761173535893194E+00; + xtab[6] = -0.434415036912123975342287136741E+00; + xtab[7] = -0.266362652878280984167665332026E+00; + xtab[8] = -0.897490934846521110226450100886E-01; + xtab[9] = 0.897490934846521110226450100886E-01; + xtab[10] = 0.266362652878280984167665332026E+00; + xtab[11] = 0.434415036912123975342287136741E+00; + xtab[12] = 0.588504834318661761173535893194E+00; + xtab[13] = 0.723679329283242681306210365302E+00; + xtab[14] = 0.835593535218090213713646362328E+00; + xtab[15] = 0.920649185347533873837854625431E+00; + xtab[16] = 0.976105557412198542864518924342E+00; + xtab[17] = 1.0E+00; + + weight[0] = 0.653594771241830065359477124183E-02; + weight[1] = 0.399706288109140661375991764101E-01; + weight[2] = 0.706371668856336649992229601678E-01; + weight[3] = 0.990162717175028023944236053187E-01; + weight[4] = 0.124210533132967100263396358897E+00; + weight[5] = 0.145411961573802267983003210494E+00; + weight[6] = 0.161939517237602489264326706700E+00; + weight[7] = 0.173262109489456226010614403827E+00; + weight[8] = 0.179015863439703082293818806944E+00; + weight[9] = 0.179015863439703082293818806944E+00; + weight[10] = 0.173262109489456226010614403827E+00; + weight[11] = 0.161939517237602489264326706700E+00; + weight[12] = 0.145411961573802267983003210494E+00; + weight[13] = 0.124210533132967100263396358897E+00; + weight[14] = 0.990162717175028023944236053187E-01; + weight[15] = 0.706371668856336649992229601678E-01; + weight[16] = 0.399706288109140661375991764101E-01; + weight[17] = 0.653594771241830065359477124183E-02; + } break; + case 19: { + xtab[0] = -1.0E+00; + xtab[1] = -0.978611766222080095152634063110E+00; + xtab[2] = -0.928901528152586243717940258797E+00; + xtab[3] = -0.852460577796646093085955970041E+00; + xtab[4] = -0.751494202552613014163637489634E+00; + xtab[5] = -0.628908137265220497766832306229E+00; + xtab[6] = -0.488229285680713502777909637625E+00; + xtab[7] = -0.333504847824498610298500103845E+00; + xtab[8] = -0.169186023409281571375154153445E+00; + xtab[9] = 0.0E+00; + xtab[10] = 0.169186023409281571375154153445E+00; + xtab[11] = 0.333504847824498610298500103845E+00; + xtab[12] = 0.488229285680713502777909637625E+00; + xtab[13] = 0.628908137265220497766832306229E+00; + xtab[14] = 0.751494202552613014163637489634E+00; + xtab[15] = 0.852460577796646093085955970041E+00; + xtab[16] = 0.928901528152586243717940258797E+00; + xtab[17] = 0.978611766222080095152634063110E+00; + xtab[18] = 1.0E+00; + + weight[0] = 0.584795321637426900584795321637E-02; + weight[1] = 0.357933651861764771154255690351E-01; + weight[2] = 0.633818917626297368516956904183E-01; + weight[3] = 0.891317570992070844480087905562E-01; + weight[4] = 0.112315341477305044070910015464E+00; + weight[5] = 0.132267280448750776926046733910E+00; + weight[6] = 0.148413942595938885009680643668E+00; + weight[7] = 0.160290924044061241979910968184E+00; + weight[8] = 0.167556584527142867270137277740E+00; + weight[9] = 0.170001919284827234644672715617E+00; + weight[10] = 0.167556584527142867270137277740E+00; + weight[11] = 0.160290924044061241979910968184E+00; + weight[12] = 0.148413942595938885009680643668E+00; + weight[13] = 0.132267280448750776926046733910E+00; + weight[14] = 0.112315341477305044070910015464E+00; + weight[15] = 0.891317570992070844480087905562E-01; + weight[16] = 0.633818917626297368516956904183E-01; + weight[17] = 0.357933651861764771154255690351E-01; + weight[18] = 0.584795321637426900584795321637E-02; + } break; + case 20: { + xtab[0] = -1.0E+00; + xtab[1] = -0.980743704893914171925446438584E+00; + xtab[2] = -0.935934498812665435716181584931E+00; + xtab[3] = -0.866877978089950141309847214616E+00; + xtab[4] = -0.775368260952055870414317527595E+00; + xtab[5] = -0.663776402290311289846403322971E+00; + xtab[6] = -0.534992864031886261648135961829E+00; + xtab[7] = -0.392353183713909299386474703816E+00; + xtab[8] = -0.239551705922986495182401356927E+00; + xtab[9] = -0.805459372388218379759445181596E-01; + xtab[10] = 0.805459372388218379759445181596E-01; + xtab[11] = 0.239551705922986495182401356927E+00; + xtab[12] = 0.392353183713909299386474703816E+00; + xtab[13] = 0.534992864031886261648135961829E+00; + xtab[14] = 0.663776402290311289846403322971E+00; + xtab[15] = 0.775368260952055870414317527595E+00; + xtab[16] = 0.866877978089950141309847214616E+00; + xtab[17] = 0.935934498812665435716181584931E+00; + xtab[18] = 0.980743704893914171925446438584E+00; + xtab[19] = 1.0E+00; + + weight[0] = 0.526315789473684210526315789474E-02; + weight[1] = 0.322371231884889414916050281173E-01; + weight[2] = 0.571818021275668260047536271732E-01; + weight[3] = 0.806317639961196031447768461137E-01; + weight[4] = 0.101991499699450815683781205733E+00; + weight[5] = 0.120709227628674725099429705002E+00; + weight[6] = 0.136300482358724184489780792989E+00; + weight[7] = 0.148361554070916825814713013734E+00; + weight[8] = 0.156580102647475487158169896794E+00; + weight[9] = 0.160743286387845749007726726449E+00; + weight[10] = 0.160743286387845749007726726449E+00; + weight[11] = 0.156580102647475487158169896794E+00; + weight[12] = 0.148361554070916825814713013734E+00; + weight[13] = 0.136300482358724184489780792989E+00; + weight[14] = 0.120709227628674725099429705002E+00; + weight[15] = 0.101991499699450815683781205733E+00; + weight[16] = 0.806317639961196031447768461137E-01; + weight[17] = 0.571818021275668260047536271732E-01; + weight[18] = 0.322371231884889414916050281173E-01; + weight[19] = 0.526315789473684210526315789474E-02; + } break; + default: { + log_and_throw_error( + "Legal values for lobatto_set are between 2 and 20."); + } + } +} + +inline void +lobatto_compute(int n1, std::vector& x, std::vector& w) + +/******************************************************************************/ +/* + Purpose: + + LOBATTO_COMPUTE computes a Lobatto quadrature rule. + + Discussion: + + The integral: + + Integral ( -1 <= X <= 1 ) F(X) dX + + The quadrature rule: + + Sum ( 1 <= I <= N ) WEIGHT(I) * F ( XTAB(I) ) + + The quadrature rule will integrate exactly all polynomials up to + X^(2*N-3). + + The Lobatto rule is distinguished by the fact that both endpoints + (-1 and 1) are always abscissas. + + Licensing: + + This code is distributed under the MIT license. + + Modified: + + 28 May 2014 + + Author: + + Original MATLAB version by Greg von Winckel. + This version by John Burkardt. + + Source: + + https://people.sc.fsu.edu/~jburkardt/c_src/quad_rule/quad_rule.c + (adapted to std::vector and exceptions; orders up to 20 come from + lobatto_set) + + Reference: + + Milton Abramowitz, Irene Stegun, + Handbook of Mathematical Functions, + National Bureau of Standards, 1964, + ISBN: 0-486-61272-4, + LC: QA47.A34. + + Claudio Canuto, Yousuff Hussaini, Alfio Quarteroni, Thomas Zang, + Spectral Methods in Fluid Dynamics, + Springer, 1993, + ISNB13: 978-3540522058, + LC: QA377.S676. + + Arthur Stroud, Don Secrest, + Gaussian Quadrature Formulas, + Prentice Hall, 1966, + LC: QA299.4G3S7. + + Daniel Zwillinger, editor, + CRC Standard Mathematical Tables and Formulae, + 30th Edition, + CRC Press, 1996, + ISBN: 0-8493-2479-3. + + Parameters: + + Input, int N1, the order. The rule has N = N1 + 1 points, so N1 must + be at least 1. + + Output, double X[N], the abscissas. + + Output, double W[N], the weights. +*/ +{ + // M_PI is not standard C++: MSVC only defines it when _USE_MATH_DEFINES is + // set before , which a consumer of this header cannot be relied on + // to do. + static constexpr double pi_v = 3.14159265358979323846; + + int i; + int j; + double test; + double tolerance; + const int n = n1 + 1; + + if (n < 2) { + log_and_throw_error( + "Requested Gauss Lobatto rule with {} points, but n>=2 is required.", + n); + } + + if (n <= 20) { + // Use tabled weights and nodes + lobatto_set(n, x, w); + return; + } + + // Resize tables to correct length + x.resize(n); + w.resize(n); + + tolerance = 100.0 * DBL_EPSILON; + /* + Initial estimate for the abscissas is the Chebyshev-Gauss-Lobatto nodes. + */ + for (i = 0; i < n; i++) { + x[i] = cos(pi_v * static_cast(i) / static_cast(n - 1)); + } + + std::vector xold(n); + std::vector p(n * n); + + do { + for (i = 0; i < n; i++) { + xold[i] = x[i]; + } + for (i = 0; i < n; i++) { + p[i + 0 * n] = 1.0; + } + for (i = 0; i < n; i++) { + p[i + 1 * n] = x[i]; + } + + for (j = 2; j <= n - 1; j++) { + for (i = 0; i < n; i++) { + p[i + j * n] = + (static_cast(2 * j - 1) * x[i] * p[i + (j - 1) * n] + + static_cast(-j + 1) * p[i + (j - 2) * n]) + / static_cast(j); + } + } + + for (i = 0; i < n; i++) { + x[i] = xold[i] + - (x[i] * p[i + (n - 1) * n] - p[i + (n - 2) * n]) + / (static_cast(n) * p[i + (n - 1) * n]); + } + + test = 0.0; + for (i = 0; i < n; i++) { + test = std::fmax(test, std::fabs(x[i] - xold[i])); + } + + } while (tolerance < test); + + // Reverse order of x. + for (int ii = 0; ii < n / 2; ++ii) { + std::swap(x[ii], x[n - 1 - ii]); + } + + for (i = 0; i < n; i++) { + w[i] = 2.0 + / (static_cast((n - 1) * n) * pow(p[i + (n - 1) * n], 2)); + } +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/esp/collisions/vertex_matrix_view.hpp b/src/ipc/esp/collisions/vertex_matrix_view.hpp new file mode 100644 index 000000000..565e86b5c --- /dev/null +++ b/src/ipc/esp/collisions/vertex_matrix_view.hpp @@ -0,0 +1,131 @@ +#pragma once + +#include +#include + +#include + +#include +#include + +namespace ipc { + +/// @brief A non-owning view that presents one or two matrices (with the same +/// number of columns) as a single vertically concatenated matrix. +/// +/// The view reads each matrix through its own row and column strides, so any +/// directly accessible double expression works without a copy: plain +/// matrices of either storage order, blocks such as `V.topRows(n)`, strided +/// `Eigen::Map`s, and `Eigen::Ref`s bound to any of these. +/// +/// We take each argument as a template instead of an `Eigen::Ref` because a const Ref silently copies arguments it cannot map +/// (row-major matrices, arithmetic expressions) into a temporary that dies at +/// the end of the constructor call, which would leave the view dangling. +/// Instead, expressions without direct access fail to compile, and so do +/// temporary plain matrices, since a view of them would dangle the same way. +template class VertexMatrixView { +public: + /// @brief Construct a view concatenating two matrices vertically. + /// @param A The top matrix. + /// @param B The bottom matrix. + template + VertexMatrixView( + const Eigen::DenseBase& A, + const Eigen::DenseBase& B) + : m_n_a_rows(A.rows()) + , m_n_b_rows(B.rows()) + , m_a(A.derived().data()) + , m_b(B.derived().data()) + , m_a_row_stride(A.derived().rowStride()) + , m_a_col_stride(A.derived().colStride()) + , m_b_row_stride(B.derived().rowStride()) + , m_b_col_stride(B.derived().colStride()) + { + check_viewable(); + check_viewable(); + if (A.cols() != ncols || B.cols() != ncols) { + log_and_throw_error("Incompatible matrix columns!"); + } + } + + /// @brief Construct a view wrapping a single matrix (no concatenation). + template + explicit VertexMatrixView(const Eigen::DenseBase& A) + : m_n_a_rows(A.rows()) + , m_n_b_rows(0) + , m_a(A.derived().data()) + , m_b(nullptr) + , m_a_row_stride(A.derived().rowStride()) + , m_a_col_stride(A.derived().colStride()) + , m_b_row_stride(0) + , m_b_col_stride(0) + { + check_viewable(); + if (A.cols() != ncols) { + log_and_throw_error("Incompatible matrix columns!"); + } + } + + // A temporary matrix is destroyed at the end of the full-expression that + // creates it, so a view of one would dangle as soon as it is constructed. + template + explicit VertexMatrixView(const Eigen::PlainObjectBase&&) = + delete; + template + VertexMatrixView( + const Eigen::PlainObjectBase&&, + const Eigen::DenseBase&) = delete; + template + VertexMatrixView( + const Eigen::DenseBase&, + const Eigen::PlainObjectBase&&) = delete; + + /// @brief Access row i of the concatenated matrix. + Eigen::RowVector operator()(index_t i) const + { + assert(i < rows()); + const bool in_a = i < m_n_a_rows; + const double* src = in_a ? m_a : m_b; + const index_t li = in_a ? i : (i - m_n_a_rows); + const Eigen::Index row_stride = in_a ? m_a_row_stride : m_b_row_stride; + const Eigen::Index col_stride = in_a ? m_a_col_stride : m_b_col_stride; + Eigen::RowVector row; + for (int d = 0; d < ncols; ++d) { + row[d] = src[li * row_stride + d * col_stride]; + } + return row; + } + + /// @brief Total number of rows (A rows + B rows). + index_t rows() const { return m_n_a_rows + m_n_b_rows; } + + /// @brief Number of columns (compile-time constant). + index_t cols() const { return ncols; } + + const index_t m_n_a_rows; + const index_t m_n_b_rows; + const double* const m_a; + const double* const m_b; + +private: + template static constexpr void check_viewable() + { + static_assert( + std::is_same_v, + "VertexMatrixView requires a matrix of doubles"); + static_assert( + (int(Derived::Flags) & Eigen::DirectAccessBit) != 0, + "VertexMatrixView requires direct access to the matrix " + "coefficients; evaluate the expression into a matrix first"); + } + + // Element (i, j) of a matrix lives at data[i * row_stride + j * col_stride] + // which covers both storage orders and any outer stride. + const Eigen::Index m_a_row_stride; + const Eigen::Index m_a_col_stride; + const Eigen::Index m_b_row_stride; + const Eigen::Index m_b_col_stride; +}; + +} // namespace ipc diff --git a/src/ipc/esp/esp_collision_maps.hpp b/src/ipc/esp/esp_collision_maps.hpp new file mode 100644 index 000000000..97d243523 --- /dev/null +++ b/src/ipc/esp/esp_collision_maps.hpp @@ -0,0 +1,50 @@ +// NOTE: This is an internal header file, not meant to be used outside of the +// IPC Toolkit library. It includes ipc/utils/unordered_map_and_set.hpp, which +// pulls in Abseil, a private dependency of the IPC Toolkit library. + +#pragma once + +#include +#include + +#include +#include +#include +#include + +namespace ipc { + +/// @brief Collisions of one quadrature point, keyed by +/// ESPCollision::get_typed_hash(). +struct ESPCollisionPairMap + : unordered_map, std::shared_ptr> { }; + +struct ESPCollisions::Maps { + /// @brief collision sets for 3D quadrature + + // vertex_collisions[vi] provides the contact set for vertex vi + unordered_map>> + vertex_collisions; + // edge_edge_collisions[(ei, ej)] provides the contact set for the closest + // point on ei, between edge ei and ej. + unordered_map< + std::pair, + std::unique_ptr>> + edge_edge_collisions; + // face_collisions[fi][qi] provides the contact set for quadrature point qi + // of face fi + unordered_map< + index_t, + std::vector>>> + face_collisions; + + /// @brief collision sets for 2D quadrature + // edge_collisions_2d[ei][qi] provides the contact set for Gauss-Lobatto QP + // qi on edge ei + unordered_map< + index_t, + std::vector>>> + edge_collisions_2d; +}; + +} // namespace ipc diff --git a/src/ipc/esp/esp_collisions.cpp b/src/ipc/esp/esp_collisions.cpp new file mode 100644 index 000000000..4c8f7b947 --- /dev/null +++ b/src/ipc/esp/esp_collisions.cpp @@ -0,0 +1,342 @@ +#include "esp_collisions.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include // std::out_of_range, std::invalid_argument +#include + +namespace ipc { + +ESPCollisions::ESPCollisions() : m_maps(std::make_unique()) { } + +ESPCollisions::~ESPCollisions() = default; + +void ESPCollisions::build( + const Candidates& _candidates, + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const ESPParameters& params) +{ + assert(static_cast(vertices.rows()) == mesh.num_vertices()); + + if (mesh.dim() == 2 && params.quad_order < 1) { + log_and_throw_error( + "quad_order {} must be at least 1 in 2D.", params.quad_order); + } + + // In 3D, ESP corrects each point's face contacts by inclusion-exclusion + // over the nearby edges and vertices, which it finds through the + // vertex-vertex, edge-vertex, edge-face, and face-face candidates. Without + // them, the energy is still finite but wrong (over-counted, or even + // attractive), so we reject candidates that were not built with all_types. + if (mesh.dim() == 3 && !_candidates.has_all_types) { + log_and_throw_error( + "ESPCollisions::build: 3D candidates must be built with all_types = true"); + } + + clear(); + + if (&_candidates != &m_candidates) { + m_candidates = _candidates; + } + m_candidates.convert_candidates_to_sets(); + const Candidates& candidates = m_candidates; + + // Record the quadrature so that evaluating the set with different + // parameters fails loudly in check_quadrature() instead of misreading the + // per-point collision sets. + m_dim = mesh.dim(); + m_quad_order = params.quad_order; + m_face_quad_rule = params.quad_rule(); + + if (mesh.dim() == 2) { + tbb::enumerable_thread_specific> storage { + ESPCollisionsBuilder<2>() + }; + // Built once (not per thread) since it computes the mesh's + // inclusion-exclusion weights. + const PointPotential point_potential(mesh, candidates, params); + + // Standard mode: loop over all edges with per-QP collision dicts. + tbb::parallel_for( + tbb::blocked_range(0, mesh.num_edges()), + [&](const tbb::blocked_range& r) { + ESPCollisionsBuilder<2>& local_storage = storage.local(); + local_storage.build_edge_collisions( + point_potential, vertices, r.begin(), r.end()); + }); + ESPCollisionsBuilder<2>::merge(storage, *this); + } else { + // A vertex sample needs a collision dict only if something is near + // it, so we mark every vertex of a vertex candidate. + std::vector vertex_mask(mesh.num_vertices(), false); + for (const auto& candidate : candidates.fv_candidates) { + vertex_mask[candidate.vertex_id] = true; + } + for (const auto& candidate : candidates.ev_candidates) { + vertex_mask[candidate.vertex_id] = true; + } + for (const auto& candidate : candidates.vv_candidates) { + vertex_mask[candidate.vertex0_id] = true; + vertex_mask[candidate.vertex1_id] = true; + } + + // Face quadrature includes the face vertices, so a face's vertices + // are only integrated separately when there is no face quadrature + // rule. Edges in no face and isolated vertices have no face + // quadrature, so ESPPotential always integrates over their vertices. + const bool use_face_quadrature = !params.quad_rule().empty(); + std::vector is_vertex_sample( + mesh.num_vertices(), !use_face_quadrature); + if (use_face_quadrature) { + for (const index_t ei : mesh.codim_edges()) { + is_vertex_sample[mesh.edges()(ei, 0)] = true; + is_vertex_sample[mesh.edges()(ei, 1)] = true; + } + for (const index_t vi : mesh.codim_vertices()) { + is_vertex_sample[vi] = true; + } + } + + std::vector vertices_to_process; + vertices_to_process.reserve(mesh.num_vertices()); + for (int i = 0; i < mesh.num_vertices(); ++i) { + if (vertex_mask[i] && is_vertex_sample[i]) { + vertices_to_process.push_back(i); + } + } + + std::vector faces_to_process; + if (use_face_quadrature) { + faces_to_process.resize(mesh.num_faces()); + std::iota(faces_to_process.begin(), faces_to_process.end(), 0); + } + + // create builder and parallel loops + tbb::enumerable_thread_specific storage( + QuadratureCollisionsBuilder(mesh, candidates, params)); + + tbb::parallel_for( + tbb::blocked_range(0, vertices_to_process.size()), + [&](const tbb::blocked_range& r) { + QuadratureCollisionsBuilder& local_storage = storage.local(); + local_storage.build_vertex_collisions( + vertices, vertices_to_process, r.begin(), r.end()); + }); + + if (use_face_quadrature) { + tbb::parallel_for( + tbb::blocked_range(0, faces_to_process.size()), + [&](const tbb::blocked_range& r) { + QuadratureCollisionsBuilder& local_storage = + storage.local(); + local_storage.build_face_collisions( + vertices, faces_to_process, r.begin(), r.end()); + }); + } + + tbb::parallel_for( + tbb::blocked_range(0, candidates.ee_candidates.size()), + [&](const tbb::blocked_range& r) { + QuadratureCollisionsBuilder& local_storage = storage.local(); + local_storage.build_edge_edge_collisions( + vertices, candidates.ee_candidates, r.begin(), r.end()); + }); + + QuadratureCollisionsBuilder::merge(storage, *this); + } +} + +void ESPCollisions::build( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const ESPParameters& params, + BroadPhase* broad_phase) +{ + assert(static_cast(vertices.rows()) == mesh.num_vertices()); + + double inflation_radius = + params.dhat / 2; // TODO use dbar for EE collisions broad phase + + m_candidates.build(mesh, vertices, inflation_radius, broad_phase, true); + + this->build(m_candidates, mesh, vertices, params); +} + +// ============================================================================ +size_t ESPCollisions::size() const +{ + size_t size = 0; + for (const auto& cc : m_maps->vertex_collisions) { + size += cc.second->size(); + } + for (const auto& cc : m_maps->edge_edge_collisions) { + size += cc.second->size(); + } + for (const auto& cc : m_maps->face_collisions) { + for (const auto& dict_ptr : cc.second) { + size += dict_ptr->size(); + } + } + for (const auto& cc : m_maps->edge_collisions_2d) { + for (const auto& dict_ptr : cc.second) { + size += dict_ptr->size(); + } + } + return size; +} +bool ESPCollisions::empty() const +{ + return m_maps->vertex_collisions.empty() + && m_maps->edge_edge_collisions.empty() + && m_maps->face_collisions.empty() + && m_maps->edge_collisions_2d.empty(); +} +void ESPCollisions::clear() +{ + m_maps->vertex_collisions.clear(); + m_maps->edge_edge_collisions.clear(); + m_maps->face_collisions.clear(); + m_maps->edge_collisions_2d.clear(); + num_quadrature_collision_pairs = 0; + m_dim = 0; + m_quad_order = 0; + m_face_quad_rule.clear(); +} + +void ESPCollisions::check_quadrature(const ESPParameters& params) const +{ + if (m_dim == 2 && params.quad_order != m_quad_order) { + log_and_throw_error( + "ESPCollisions were built with quad_order {}, but the " + "parameters use quad_order {}. Use the same ESPParameters to " + "build the collisions and to evaluate them.", + m_quad_order, params.quad_order); + } + + if (m_dim == 3) { + const FaceQuadRule& rule = params.quad_rule(); + const bool same_rule = rule.size() == m_face_quad_rule.size() + && std::equal(rule.begin(), rule.end(), m_face_quad_rule.begin(), + [](const FaceQuadPoint& a, const FaceQuadPoint& b) { + return a.lambda == b.lambda + && a.weight == b.weight; + }); + if (!same_rule) { + log_and_throw_error( + "ESPCollisions were built with a {}-point face quadrature " + "rule, but the parameters use a different {}-point rule. " + "Use the same ESPParameters to build the collisions and " + "to evaluate them, and call set_quad_rule() before " + "constructing ESPPotential, which copies the parameters.", + m_face_quad_rule.size(), rule.size()); + } + } +} + +std::string ESPCollisions::to_string( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const ESPParameters& params) const +{ + std::stringstream ss; + + for (const auto& ccs : m_maps->vertex_collisions) { + for (int i = 0; i < (*ccs.second).size(); i++) { + const auto& cc = (*ccs.second)[i]; + ss << "\n"; + { + ss << fmt::format( + "vert [{}]: ({} {}) weight {} dist sqr {} potential {} grad {}", + cc.name(), cc[0], cc[1], cc.weight, + cc.compute_distance(vertices), cc(cc.dof(vertices), params), + cc.gradient(cc.dof(vertices), params).norm()); + } + } + } + for (const auto& ccs : m_maps->edge_edge_collisions) { + for (int i = 0; i < (*ccs.second).size(); i++) { + const auto& cc = (*ccs.second)[i]; + ss << "\n"; + { + ss << fmt::format( + "edge [{}]: ({} {}) ({} {}) weight {}", cc.name(), + ccs.first.first, ccs.first.second, cc[0], cc[1], cc.weight); + } + } + } + // Face collisions involve the quadrature point, a virtual vertex that is + // not a row of vertices, so only their ids and weights are printed. + for (const auto& ccs : m_maps->face_collisions) { + for (size_t qi = 0; qi < ccs.second.size(); qi++) { + const auto& dict = *ccs.second[qi]; + for (int i = 0; i < dict.size(); i++) { + const auto& cc = dict[i]; + ss << "\n"; + ss << fmt::format( + "face [{}]: {} point {} ({} {}) weight {}", cc.name(), + ccs.first, qi, cc[0], cc[1], cc.weight); + } + } + } + // Same for the 2D edge quadrature points. + for (const auto& ccs : m_maps->edge_collisions_2d) { + for (size_t qi = 0; qi < ccs.second.size(); qi++) { + const auto& dict = *ccs.second[qi]; + for (int i = 0; i < dict.size(); i++) { + const auto& cc = dict[i]; + ss << "\n"; + ss << fmt::format( + "edge [{}]: {} point {} ({} {}) weight {}", cc.name(), + ccs.first, qi, cc[0], cc[1], cc.weight); + } + } + } + + return ss.str(); +} + +// NOTE: Actually distance squared +double ESPCollisions::compute_minimum_distance( + const CollisionMesh& mesh, Eigen::ConstRef vertices) const +{ + assert(static_cast(vertices.rows()) == mesh.num_vertices()); + + if (m_candidates.empty()) { + return std::numeric_limits::infinity(); + } + + const Eigen::MatrixXi& edges = mesh.edges(); + const Eigen::MatrixXi& faces = mesh.faces(); + + tbb::enumerable_thread_specific storage( + std::numeric_limits::infinity()); + + tbb::parallel_for( + tbb::blocked_range(0, m_candidates.size()), + [&](tbb::blocked_range r) { + double& local_min_dist = storage.local(); + + for (size_t i = r.begin(); i < r.end(); i++) { + const double dist = m_candidates[i].compute_distance( + m_candidates[i].dof(vertices, edges, faces)); + + local_min_dist = std::min(dist, local_min_dist); + } + }); + + return storage.combine([](double a, double b) { return std::min(a, b); }); +} + +} // namespace ipc diff --git a/src/ipc/esp/esp_collisions.hpp b/src/ipc/esp/esp_collisions.hpp new file mode 100644 index 000000000..79ca8855a --- /dev/null +++ b/src/ipc/esp/esp_collisions.hpp @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace ipc { +class ESPCollisions { +public: + /// @brief Create an empty collision set; build() fills it. + ESPCollisions(); + virtual ~ESPCollisions(); + + /// @brief Initialize the set of collisions used to compute the barrier potential. + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @param params ESP parameters. + /// @param broad_phase Broad-phase method to use (default if nullptr). + /// @throws std::invalid_argument if params.quad_order < 1 in 2D. + void build( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const ESPParameters& params, + BroadPhase* broad_phase = nullptr); + + /// @brief Initialize the set of collisions used to compute the barrier potential. + /// @param _candidates Distance candidates from which the collision set is + /// built. In 3D, build them with all_types = true. + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @param params ESP parameters. + /// @throws std::invalid_argument if params.quad_order < 1 in 2D. + /// @throws std::invalid_argument In 3D, if the candidates were not built + /// with all_types = true (see Candidates::has_all_types). + void build( + const Candidates& _candidates, + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const ESPParameters& params); + + // ------------------------------------------------------------------------ + + /// @brief Get the number of collisions. + size_t size() const; + + /// @brief Get if the collision set are empty. + bool empty() const; + + /// @brief Clear the collision set. + void clear(); + + /// @brief Compute minimum distance between all contact candidates + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @return Squared minimum distance + double compute_minimum_distance( + const CollisionMesh& mesh, + Eigen::ConstRef vertices) const; + + /// @brief Convert contact pairs to string. Vertex pairs also list their + /// squared distance, potential and gradient norm at vertices. + std::string to_string( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const ESPParameters& params) const; + + /// @brief Number of contact candidates + int n_candidates() const { return m_candidates.size(); } + + /// @brief Check that params use the quadrature this set was built with. + /// + /// The quadrature collision sets hold one entry per quadrature point of + /// the rule used in build(): the Gauss-Lobatto edge rule of order + /// params.quad_order in 2D, or params.quad_rule() in 3D. ESPPotential and + /// the ESP friction build evaluate those entries with their own + /// parameters, so a different rule would pair the wrong points with the + /// wrong weights or read past the stored entries. A set that was never + /// built (or was cleared) holds no entries and accepts any parameters. + /// @param params Parameters about to be used with this set. + /// @throws std::invalid_argument if params' quadrature differs from the + /// one used in build(). + void check_quadrature(const ESPParameters& params) const; + +public: + /// @brief Collision candidates + Candidates m_candidates; + + /// @brief Total number of collision pairs counted across all quadrature build functions + size_t num_quadrature_collision_pairs = 0; + + /// @brief Collision sets of the quadrature points, keyed by primitive. + /// @note Defined in the internal header esp_collision_maps.hpp, so that + /// this header does not pull in Abseil, a private dependency. + struct Maps; + /// @brief Get the collision sets of the quadrature points. + Maps& maps() { return *m_maps; } + /// @brief Get the collision sets of the quadrature points. + const Maps& maps() const { return *m_maps; } + +private: + std::unique_ptr m_maps; + + /// @brief Mesh dimension of the last build(), or 0 if never built. + int m_dim = 0; + /// @brief Gauss-Lobatto edge quadrature order of the last 2D build(). + int m_quad_order = 0; + /// @brief Face quadrature rule of the last 3D build(). + FaceQuadRule m_face_quad_rule; +}; +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/esp/esp_collisions_builder.cpp b/src/ipc/esp/esp_collisions_builder.cpp new file mode 100644 index 000000000..2b307fc38 --- /dev/null +++ b/src/ipc/esp/esp_collisions_builder.cpp @@ -0,0 +1,465 @@ +#include "esp_collisions_builder.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace ipc { + +using IntegrationType = ESPParameters::IntegrationType; + +void ESPCollisionsBuilder<2>::build_edge_collisions( + const PointPotential& point_potential, + Eigen::ConstRef V, + size_t start, + size_t end) +{ + const CollisionMesh& mesh = point_potential.mesh; + const Candidates& candidates = point_potential.candidates; + const ESPParameters& params = point_potential.params; + const GaussLobatto::Rule& rule = GaussLobatto::get_rule(params.quad_order); + + for (size_t edge_idx = start; edge_idx < end; ++edge_idx) { + const index_t ei = static_cast(edge_idx); + + if (params.integration_type == IntegrationType::NO_OBST + && mesh.is_obstacle_edge(ei)) { + continue; + } + if (params.integration_type != IntegrationType::BRUTE_FORCE + && mesh.is_obstacle_edge(ei)) { + // We consult the same sets build_collisions_at_edge_qp() reads. + // Checking ev_set alone misses dynamic edges near this edge's + // endpoints, which only appear in ee_set (through the endpoints' + // ve_set), and would drop real contact with the obstacle. + const auto v_set = candidates.ev_set(mesh, ei); + const auto e_set = candidates.ee_set(mesh, ei); + const bool has_non_obstacle = + std::any_of( + v_set.begin(), v_set.end(), + [&](index_t v) { return !mesh.is_obstacle_vertex(v); }) + || std::any_of(e_set.begin(), e_set.end(), [&](index_t e) { + return !mesh.is_obstacle_edge(e); + }); + if (!has_non_obstacle) { + continue; + } + } + + const double dhat = params.dhat; + std::vector>> + qp_dicts; + qp_dicts.reserve(rule.size()); + bool has_any = false; + + for (const auto& qp : rule) { + const std::array lambda = { { 1.0 - qp.xi, qp.xi } }; + size_t n = 0; + auto dict = point_potential.build_collisions_at_edge_qp( + V, ei, lambda, dhat, n); + if (dict && dict->size() > 0) { + has_any = true; + } + qp_dicts.push_back(std::move(dict)); + num_collision_pairs += n; + } + + if (has_any) { + edge_collisions_2d.emplace_back(ei, std::move(qp_dicts)); + } + } +} + +void ESPCollisionsBuilder<2>::merge( + tbb::enumerable_thread_specific>& local_storage, + ESPCollisions& merged_collisions) +{ + size_t total_pairs = 0; + + // Move per-edge dicts into merged_collisions. No edge is processed by + // more than one thread, so there are no duplicate edge keys. + for (auto& builder : local_storage) { + for (auto& [ei, dicts] : builder.edge_collisions_2d) { + for (const auto& dict : dicts) { + total_pairs += dict->size(); + } + // Use insert with a value_type pair to force the move path. + merged_collisions.maps().edge_collisions_2d.insert( + std::make_pair(ei, std::move(dicts))); + } + merged_collisions.num_quadrature_collision_pairs += + builder.num_collision_pairs; + } + + logger().trace("2D edge QP collision pairs: {}.", total_pairs); +} + +// ============================================================================ + +std::shared_ptr +ESPCollisionsBuilder<3>::reduce_point_triangle_collision( + const FaceVertexCandidate& candidate, + const ESPParameters& params, + const CollisionMesh& mesh, + const VertexMatrixView<3>& vertices, + PointTriangleDistanceType dtype) +{ + const index_t vi = candidate.vertex_id; + const index_t fi = candidate.face_id; + + const index_t t0 = mesh.faces()(fi, 0); + const index_t t1 = mesh.faces()(fi, 1); + const index_t t2 = mesh.faces()(fi, 2); + + const index_t e0 = mesh.faces_to_edges()(fi, 0); + const index_t e1 = mesh.faces_to_edges()(fi, 1); + const index_t e2 = mesh.faces_to_edges()(fi, 2); + + assert(vi != t0 && vi != t1 && vi != t2); + + if (dtype == PointTriangleDistanceType::AUTO) { + dtype = point_triangle_distance_type_exact( + vertices(vi), vertices(t0), vertices(t1), vertices(t2)); + } + + const double dist_sqr = point_triangle_distance( + vertices(vi), vertices(t0), vertices(t1), vertices(t2), dtype); + if (dist_sqr >= params.dhat * params.dhat) { + return nullptr; + } + + switch (dtype) { + case PointTriangleDistanceType::P_T0: + return std::make_shared>( + t0, vi, mesh); + + case PointTriangleDistanceType::P_T1: + return std::make_shared>( + t1, vi, mesh); + + case PointTriangleDistanceType::P_T2: + return std::make_shared>( + t2, vi, mesh); + + case PointTriangleDistanceType::P_E0: + return std::make_shared>( + e0, vi, mesh); + + case PointTriangleDistanceType::P_E1: + return std::make_shared>( + e1, vi, mesh); + + case PointTriangleDistanceType::P_E2: + return std::make_shared>( + e2, vi, mesh); + + default: // AUTO was resolved above + assert(dtype == PointTriangleDistanceType::P_T); + return std::make_shared>( + fi, vi, mesh); + } +} + +std::shared_ptr +ESPCollisionsBuilder<3>::reduce_point_edge_collision( + const EdgeVertexCandidate& candidate, + const ESPParameters& params, + const CollisionMesh& mesh, + const VertexMatrixView<3>& vertices, + PointEdgeDistanceType dtype) +{ + const index_t vi = candidate.vertex_id; + const index_t ei = candidate.edge_id; + + const index_t t0 = mesh.edges()(ei, 0); + const index_t t1 = mesh.edges()(ei, 1); + + if (dtype == PointEdgeDistanceType::AUTO) { + dtype = point_edge_distance_type_exact( + vertices(vi), vertices(t0), vertices(t1)); + } + + const double dist_sqr = + point_edge_distance(vertices(vi), vertices(t0), vertices(t1), dtype); + if (dist_sqr >= params.dhat * params.dhat) { + return nullptr; + } + + switch (dtype) { + case PointEdgeDistanceType::P_E0: + return std::make_shared>( + t0, vi, mesh); + case PointEdgeDistanceType::P_E1: + return std::make_shared>( + t1, vi, mesh); + default: // AUTO was resolved above + assert(dtype == PointEdgeDistanceType::P_E); + return std::make_shared>( + ei, vi, mesh); + } +} + +// ============================================================================ +// QuadratureCollisionsBuilder + +QuadratureCollisionsBuilder::QuadratureCollisionsBuilder( + const CollisionMesh& mesh, + const Candidates& candidates, + const ESPParameters& params) + : point_potential( + std::make_shared(mesh, candidates, params)) +{ +} + +QuadratureCollisionsBuilder::~QuadratureCollisionsBuilder() = default; + +// TBB copies only the empty exemplar into each thread's builder. +QuadratureCollisionsBuilder::QuadratureCollisionsBuilder( + const QuadratureCollisionsBuilder& other) + : point_potential(other.point_potential) +{ + assert(other.vertex_collisions.empty()); + assert(other.edge_edge_collisions.empty()); + assert(other.face_collisions.empty()); +} + +void QuadratureCollisionsBuilder::build_vertex_collisions( + Eigen::ConstRef vertices, + const std::vector& vertex_indices, + const size_t start_i, + const size_t end_i) +{ + const CollisionMesh& mesh = point_potential->mesh; + const ESPParameters& params = point_potential->params; + for (size_t i = start_i; i < end_i; i++) { + const index_t vi = vertex_indices[i]; + if (params.integration_type == IntegrationType::NO_OBST + && mesh.is_obstacle_vertex(vi)) { + continue; + } + if (params.integration_type != IntegrationType::BRUTE_FORCE + && mesh.is_obstacle_vertex(vi)) { + const auto v_set = point_potential->candidates.vv_set(mesh, vi); + const auto e_set = point_potential->candidates.ve_set(vi); + const auto f_set = point_potential->candidates.vf_set(vi); + const bool has_non_obstacle = + std::any_of( + v_set.begin(), v_set.end(), + [&](index_t v) { return !mesh.is_obstacle_vertex(v); }) + || std::any_of( + e_set.begin(), e_set.end(), + [&](index_t e) { return !mesh.is_obstacle_edge(e); }) + || std::any_of(f_set.begin(), f_set.end(), [&](index_t f) { + return !mesh.is_obstacle_face(f); + }); + if (!has_non_obstacle) { + continue; + } + } + size_t n = 0; + auto dict = + point_potential->build_collisions_at_vertex(vertices, vi, n); + if (dict && dict->size() > 0) { + vertex_collisions.push_back(std::move(dict)); + } + num_collision_pairs += n; + } +} + +void QuadratureCollisionsBuilder::build_face_collisions( + Eigen::ConstRef vertices, + const std::vector& face_indices, + const size_t start_i, + const size_t end_i) +{ + const CollisionMesh& mesh = point_potential->mesh; + const auto& face_quad_rule = point_potential->params.quad_rule(); + if (face_quad_rule.empty()) { + return; + } + + const ESPParameters& params = point_potential->params; + for (size_t i = start_i; i < end_i; i++) { + const index_t fi = face_indices[i]; + if (params.integration_type == IntegrationType::NO_OBST + && mesh.is_obstacle_face(fi)) { + continue; + } + if (params.integration_type != IntegrationType::BRUTE_FORCE + && mesh.is_obstacle_face(fi)) { + const auto v_set = point_potential->candidates.fv_set(mesh, fi); + const auto e_set = point_potential->candidates.fe_set(mesh, fi); + const auto f_set = point_potential->candidates.ff_set(mesh, fi); + const bool has_non_obstacle = + std::any_of( + v_set.begin(), v_set.end(), + [&](index_t v) { return !mesh.is_obstacle_vertex(v); }) + || std::any_of( + e_set.begin(), e_set.end(), + [&](index_t e) { return !mesh.is_obstacle_edge(e); }) + || std::any_of(f_set.begin(), f_set.end(), [&](index_t f) { + return !mesh.is_obstacle_face(f); + }); + if (!has_non_obstacle) { + continue; + } + } + + std::vector>> + per_qp_dicts; + per_qp_dicts.reserve(face_quad_rule.size()); + bool any_nonempty = false; + for (const auto& qp : face_quad_rule) { + size_t n = 0; + auto dict = + point_potential->build_collisions_at_face_interior_point( + vertices, fi, qp.lambda, n); + if (dict && dict->size() > 0) { + any_nonempty = true; + } + // Keep the qi indexing aligned with face_quad_rule, even if the + // dict is empty for this quadrature point. + per_qp_dicts.push_back(std::move(dict)); + num_collision_pairs += n; + } + if (any_nonempty) { + face_collisions.push_back({ fi, std::move(per_qp_dicts) }); + } + } +} + +void QuadratureCollisionsBuilder::build_edge_edge_collisions( + Eigen::ConstRef vertices, + const std::vector& ee_candidates, + const size_t start_i, + const size_t end_i) +{ + const ESPParameters& params = point_potential->params; + const CollisionMesh& mesh = point_potential->mesh; + + for (size_t i = start_i; i < end_i; i++) { + const auto& candidate = ee_candidates[i]; + const index_t ei = candidate.edge0_id; + const index_t ej = candidate.edge1_id; + + const index_t ea = mesh.edges()(ei, 0); + const index_t eb = mesh.edges()(ei, 1); + const index_t ec = mesh.edges()(ej, 0); + const index_t ed = mesh.edges()(ej, 1); + + if (ea == ec || ea == ed || eb == ec || eb == ed) { + continue; + } + + if (params.integration_type != IntegrationType::BRUTE_FORCE + && mesh.is_obstacle_edge(ei) && mesh.is_obstacle_edge(ej)) { + continue; + } + + if (is_parallel_edge_edge( + vertices.row(ea), vertices.row(eb), vertices.row(ec), + vertices.row(ed))) { + continue; + } + + const auto dtype = edge_edge_distance_type_exact( + vertices.row(ea), vertices.row(eb), vertices.row(ec), + vertices.row(ed)); + + const double dist_sq = edge_edge_distance_parallel_safe( + vertices.row(ea), vertices.row(eb), vertices.row(ec), + vertices.row(ed), dtype); + + if (dist_sq >= params.ee_support() * params.ee_support()) { + continue; + } + + // Only EA_EB pairs get an edge-edge dict; ESPPotential and the ESP + // friction build assert this. All other edge-edge distance types are + // captured through vertex_collisions at the relevant endpoint. + if (dtype != EdgeEdgeDistanceType::EA_EB) { + continue; + } + + // Outside BRUTE_FORCE, at most one of the two edges is an obstacle + // (see above), so an obstacle edge always has a dynamic candidate and + // only NO_OBST skips it. + const bool skip_obstacles = + params.integration_type == IntegrationType::NO_OBST; + + if (!skip_obstacles || !mesh.is_obstacle_edge(ei)) { + size_t n = 0; + auto dict = + point_potential->build_collisions_at_edge_edge_closest_point( + vertices, ei, ej, dtype, n); + // Keep the dict even if its pairs cancel to empty: its mollifier + // weight still counts in the face's weighted average, so dropping + // it would make the potential jump when the dict changes between + // empty and non-empty. + if (dict) { + edge_edge_collisions.push_back(std::move(dict)); + } + num_collision_pairs += n; + } + + if (!skip_obstacles || !mesh.is_obstacle_edge(ej)) { + size_t n = 0; + auto dict = + point_potential->build_collisions_at_edge_edge_closest_point( + vertices, ej, ei, dtype, n); + if (dict) { // kept even if empty, see above + edge_edge_collisions.push_back(std::move(dict)); + } + num_collision_pairs += n; + } + } +} + +void QuadratureCollisionsBuilder::merge( + tbb::enumerable_thread_specific& local_storage, + ESPCollisions& merged_collisions) +{ + // Reserve space + size_t total_v = 0, total_ee = 0, total_f = 0; + for (const auto& storage : local_storage) { + total_v += storage.vertex_collisions.size(); + total_ee += storage.edge_edge_collisions.size(); + total_f += storage.face_collisions.size(); + } + merged_collisions.maps().vertex_collisions.reserve(total_v); + merged_collisions.maps().edge_edge_collisions.reserve(total_ee); + merged_collisions.maps().face_collisions.reserve(total_f); + + for (auto& storage : local_storage) { + for (auto& cc : storage.vertex_collisions) { + merged_collisions.maps().vertex_collisions.insert( + std::make_pair< + index_t, + std::unique_ptr>>( + cc->primitive_id(), std::move(cc))); + } + for (auto& cc : storage.edge_edge_collisions) { + const auto id = cc->primitive_ids(); + merged_collisions.maps().edge_edge_collisions.insert( + std::make_pair(std::make_pair(id[0], id[1]), std::move(cc))); + } + for (auto& [fi, dicts] : storage.face_collisions) { + merged_collisions.maps().face_collisions.emplace( + fi, std::move(dicts)); + } + merged_collisions.num_quadrature_collision_pairs += + storage.num_collision_pairs; + } +} + +} // namespace ipc diff --git a/src/ipc/esp/esp_collisions_builder.hpp b/src/ipc/esp/esp_collisions_builder.hpp new file mode 100644 index 000000000..0db75f4ee --- /dev/null +++ b/src/ipc/esp/esp_collisions_builder.hpp @@ -0,0 +1,203 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include +#include + +namespace ipc { + +template class ESPCollisionsBuilder; +class PointPotential; +class QuadratureCollisionsBuilder; + +template <> class ESPCollisionsBuilder<2> { +public: + ESPCollisionsBuilder() = default; + // Copy creates an empty builder (used by tbb::enumerable_thread_specific). + ESPCollisionsBuilder(const ESPCollisionsBuilder& /*other*/) + : ESPCollisionsBuilder() + { + } + + /// @brief Build per-edge, per-QP collision dicts for the 2D quadrature path. + /// For each edge ei in [start, end), places Gauss-Lobatto QPs on ei and + /// finds nearby vertices/edges from candidates.ev_set(mesh, ei) and + /// candidates.ee_set(mesh, ei). Results are stored in edge_collisions_2d. + /// @param point_potential Holds the mesh, candidates, parameters, and the + /// mesh's inclusion-exclusion weights, shared by every thread. + void build_edge_collisions( + const PointPotential& point_potential, + Eigen::ConstRef vertices, + size_t start, + size_t end); + + // ------------------------------------------------------------------------- + + static void merge( + tbb::enumerable_thread_specific>& local_storage, + ESPCollisions& merged_collisions); + + // Per-edge QP collision dicts: each entry is {edge_id, [dict_qp0, ...]}. + // Stored as a vector of pairs (not a map) so structured-binding iteration + // gives mutable references, enabling std::move in merge(). + std::vector>>>> + edge_collisions_2d; + + size_t num_collision_pairs = 0; +}; + +template <> class ESPCollisionsBuilder<3> { +public: + ESPCollisionsBuilder() { } + + static std::shared_ptr reduce_point_triangle_collision( + const FaceVertexCandidate& candidate, + const ESPParameters& params, + const CollisionMesh& mesh, + const VertexMatrixView<3>& vertices, + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); + + static std::shared_ptr reduce_point_edge_collision( + const EdgeVertexCandidate& candidate, + const ESPParameters& params, + const CollisionMesh& mesh, + const VertexMatrixView<3>& vertices, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); + + void add_face_vertex_collisions( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const std::vector& candidates, + const ESPParameters& params, + const size_t start_i, + const size_t end_i); + + void add_face_vertex_negative_edge_vertex_collisions( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const std::vector& candidates, + const ESPParameters& params, + const size_t start_i, + const size_t end_i); + + void add_face_vertex_positive_vertex_vertex_collisions( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const std::vector& candidates, + const ESPParameters& params, + const size_t start_i, + const size_t end_i); + + /*/ + ------------------------------------------------------------------------- + + void add_negative_edge_edge_edge_collisions( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const std::vector>& candidates, + const ESPParameters& params, + const double dhat, + const size_t start_i, + const size_t end_i); + + void add_edge_edge_face_collisions( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const std::vector>& candidates, + const ESPParameters& params, + const double dhat, + const size_t start_i, + const size_t end_i); + + void add_edge_edge_vertex_collisions( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const std::vector>& candidates, + const ESPParameters& params, + const double dhat, + const size_t start_i, + const size_t end_i); + + /*/// ------------------------------------------------------------------------- + + static void merge( + const tbb::enumerable_thread_specific>& + local_storage, + ESPCollisions& merged_collisions); + + // Constructed collisions + std::vector> collisions; + + // ------------------------------------------------------------------------- + + // Store the indices to pairs to avoid duplicates. + unordered_map, index_t> vert_vert_3_to_id; + unordered_map, index_t> vert_edge_3_to_id; + unordered_map, index_t> vert_face_3_to_id; + + unordered_map, index_t> eev_3_to_id; + unordered_map, index_t> eef_3_to_id; + unordered_map, index_t> eee_3_to_id; +}; + +class QuadratureCollisionsBuilder { +public: + QuadratureCollisionsBuilder( + const CollisionMesh& mesh, + const Candidates& candidates, + const ESPParameters& params); + QuadratureCollisionsBuilder(QuadratureCollisionsBuilder&&) = default; + QuadratureCollisionsBuilder& + operator=(QuadratureCollisionsBuilder&&) = default; + QuadratureCollisionsBuilder(const QuadratureCollisionsBuilder& other); + QuadratureCollisionsBuilder& + operator=(const QuadratureCollisionsBuilder& other) = delete; + ~QuadratureCollisionsBuilder(); + + void build_vertex_collisions( + Eigen::ConstRef vertices, + const std::vector& vertex_indices, + size_t start, + size_t end); + + void build_face_collisions( + Eigen::ConstRef vertices, + const std::vector& face_indices, + size_t start, + size_t end); + + void build_edge_edge_collisions( + Eigen::ConstRef vertices, + const std::vector& ee_candidates, + const size_t start_i, + const size_t end_i); + + static void merge( + tbb::enumerable_thread_specific& + local_storage, + ESPCollisions& merged_collisions); + + // Local storage + std::vector>> + vertex_collisions; + std::vector>> + edge_edge_collisions; + // face_collisions[i] = {fid, [dict_for_qp0, dict_for_qp1, ...]} + std::vector>>>> + face_collisions; + + size_t num_collision_pairs = 0; + + std::shared_ptr point_potential; +}; +} // namespace ipc diff --git a/src/ipc/esp/esp_distance.hpp b/src/ipc/esp/esp_distance.hpp new file mode 100644 index 000000000..c453fc3e1 --- /dev/null +++ b/src/ipc/esp/esp_distance.hpp @@ -0,0 +1,104 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace ipc { + +/// @brief Edge-edge squared distance for an EdgeEdgeDistanceType that may +/// come from edge_edge_distance_type_exact. +/// @note The exact classifier has no parallel threshold, so it can return +/// EA_EB for parallel edges, where the line-line distance divides by zero. +/// Those cases fall back to the minimum point-line distance. +inline double edge_edge_distance_parallel_safe( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + const EdgeEdgeDistanceType dtype) +{ + if (dtype == EdgeEdgeDistanceType::EA_EB) { + const Eigen::Vector3d normal = (ea1 - ea0).cross(eb1 - eb0); + if (normal.squaredNorm() > 1e-20) { + return line_line_distance(ea0, ea1, eb0, eb1); + } + return std::min( + { point_line_distance(eb0, ea0, ea1), + point_line_distance(eb1, ea0, ea1), + point_line_distance(ea0, eb0, eb1), + point_line_distance(ea1, eb0, eb1) }); + } + return edge_edge_distance(ea0, ea1, eb0, eb1, dtype); +} + +template +T line_line_sqr_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) +{ + const Eigen::Vector3 normal = (ea1 - ea0).cross(eb1 - eb0); + const T line_to_line = (eb0 - ea0).dot(normal); + return line_to_line * line_to_line / normal.squaredNorm(); +} + +template +Eigen::Vector line_line_closest_point_pairs_uv( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) +{ + const Eigen::Vector3 u = ea1 - ea0; + const Eigen::Vector3 v = eb1 - eb0; + const Eigen::Vector3 w = ea0 - eb0; + + // We write this solve with cross products instead of the textbook dot + // products, whose denominator is (u·u)(v·v) − (u·v)². Lagrange's identity + // makes the two forms equal, but for nearly parallel edges (u·u)(v·v) and + // (u·v)² agree to almost every digit, so their difference can round to + // zero or below and the numerators lose all their digits too. That put + // closest points edge lengths off the edge. The cross product u × v stays + // accurate down to the builder's parallel threshold, which tests this same + // ‖u × v‖², so every pair the builder keeps has a positive denominator + // here. + const Eigen::Vector3 n = u.cross(v); + const T fac = n.squaredNorm(); + const T sN = n.dot(v.cross(w)); + const T tN = n.dot(u.cross(w)); + assert(fac > 0); + + return Eigen::Vector(sN, tN) / fac; +} + +/// @brief Local coordinate on edge (ea0, ea1), clamped to [0, 1], of its point +/// closest to the line through (eb0, eb1). ESP edge-edge terms exist only for +/// EA_EB pairs, where this is the closest point between the edges. +template +T closest_point_uv( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) +{ + T uv = line_line_closest_point_pairs_uv(ea0, ea1, eb0, eb1)(0); + + if (uv < 0.) { + uv = 0.; + } else if (uv > 1.) { + uv = 1.; + } + + return uv; +} + +} // namespace ipc diff --git a/src/ipc/esp/esp_parameters.hpp b/src/ipc/esp/esp_parameters.hpp new file mode 100644 index 000000000..d19e9a448 --- /dev/null +++ b/src/ipc/esp/esp_parameters.hpp @@ -0,0 +1,163 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ipc { + +/// A single face quadrature point in barycentric coordinates with its weight. +struct FaceQuadPoint { + std::array lambda; ///< Barycentric coordinates (sum = 1) + double weight; +}; +using FaceQuadRule = std::vector; + +struct ESPParameters { + enum class IntegrationType : std::uint8_t { + BRUTE_FORCE, ///< Integrate all pairs with no obstacle filtering + NORMAL, ///< Filter obstacle-obstacle pairs; skip primitives with only + ///< obstacle candidates + NO_OBST ///< Skip obstacle sources entirely, may miss collisions! + }; + + /// @param _dhat Activation distance of the barrier; must be positive and + /// finite. + /// @param _dbar_factor Near/far split parameter dbar / dhat, in + /// [0, 2]. The far part of the barrier vanishes identically at 2, + /// so the split tends continuously to the unsplit potential; the + /// edge-edge weights use the support min(dbar, dhat) (ee_support). + /// 0 disables edge-edge contact. + /// @param _quad_order Gauss-Lobatto edge quadrature order (2D); see + /// quad_order. + /// @throws std::invalid_argument if _dhat is not positive and finite, if + /// _dbar_factor is not in [0, 2], or if _quad_order is negative. + ESPParameters( + const double _dhat, + const double _dbar_factor = 0.2, + const int _quad_order = 1, + bool _area_weights = true, + const IntegrationType _integration_type = IntegrationType::NORMAL) + : dhat(_dhat) + , dbar(_dbar_factor * dhat) + , quad_order(_quad_order) + , area_weights(_area_weights) + , integration_type(_integration_type) + , m_dbar_factor(_dbar_factor) + { + // A zero dhat silently disables contact, and a negative or non-finite + // one produces negative or NaN broad-phase inflation radii. + if (!std::isfinite(_dhat) || _dhat <= 0) { + log_and_throw_error( + "dhat {} must be positive and finite.", _dhat); + } + if (_quad_order < 0) { + log_and_throw_error( + "quad_order {} must be non-negative.", _quad_order); + } + if (std::isnan(_dbar_factor) || _dbar_factor < 0 || _dbar_factor > 2) { + log_and_throw_error( + "dbar_factor {} is not in [0, 2].", _dbar_factor); + } else if (_dbar_factor == 0) { + logger().warn("dbar_factor = 0 disables edge-edge contact."); + } + } + + const double dhat; + const double dbar; + + /// Barrier evaluated on each pair's unsquared distance d with activation + /// distance dhat, in 2D and 3D. With an InversePowerBarrier of power p, the + /// 3D edge-edge mollifier weights are raised to the power max(1, round(p) + + /// 1). + std::shared_ptr barrier = + std::make_shared>(); + /// Gauss-Lobatto edge quadrature order (2D): order n uses n + 1 points and + /// must be at least 1 in 2D (ESPCollisions::build() checks this). Unused + /// in 3D, where any non-negative value is accepted. + const int quad_order; + /// Weight each face by its area / 9 (3D), or each edge by its area + /// (CollisionMesh::edge_area(), 2D). In 3D, an edge in no face is weighted + /// by its edge area / 4 and an isolated vertex by its vertex area. If + /// false, every weight is 1. + bool area_weights; + const IntegrationType integration_type; + + /// @brief Near/far split parameter dbar / dhat, in [0, 2]. + double dbar_factor() const { return m_dbar_factor; } + + /// @brief Support of the edge-edge (edge point) weights: min(dbar, dhat). + /// Edge-edge pairs farther apart than this get no quadrature point. + double ee_support() const { return std::min(dbar, dhat); } + + /// Face quadrature rule (3D). Empty (default) integrates over the face + /// vertices instead; quad_order does not affect 3D. + const FaceQuadRule& quad_rule() const { return m_face_quad_rule; } + + /// @brief Set the face quadrature rule (3D). + /// @param rule Quadrature points in barycentric coordinates with their + /// weights. Empty integrates over the face vertices instead. + /// @throws std::invalid_argument if a weight is negative or not finite, if + /// the weights sum to zero, or if a barycentric coordinate is + /// negative (point outside the face) or not finite. A negative + /// weight makes the barrier energy unbounded below as its point + /// approaches contact. + /// @note Logs a warning for each point whose barycentric coordinates do + /// not sum to 1 (within 1e-10). + void set_quad_rule(FaceQuadRule rule) + { + double weight_sum = 0; + std::vector not_affine; // points whose coordinates sum != 1 + for (size_t i = 0; i < rule.size(); i++) { + const double weight = rule[i].weight; + if (!std::isfinite(weight) || weight < 0) { + log_and_throw_error( + "Face quadrature point {} has negative or non-finite " + "weight {}.", + i, weight); + } + weight_sum += weight; + + double lambda_sum = 0; + for (const double lambda : rule[i].lambda) { + if (!std::isfinite(lambda) || lambda < 0) { + log_and_throw_error( + "Face quadrature point {} has negative or " + "non-finite barycentric coordinate {}.", + i, lambda); + } + lambda_sum += lambda; + } + if (std::abs(lambda_sum - 1) > 1e-10) { + not_affine.push_back(i); + } + } + if (!rule.empty() && weight_sum == 0) { + log_and_throw_error( + "Face quadrature weights sum to zero."); + } + for (const size_t i : not_affine) { + const auto& l = rule[i].lambda; + logger().warn( + "Barycentric coordinates of face quadrature point {} sum to " + "{}, not 1.", + i, l[0] + l[1] + l[2]); + } + m_face_quad_rule = std::move(rule); + } + +private: + const double m_dbar_factor; + FaceQuadRule m_face_quad_rule; +}; + +} // namespace ipc diff --git a/src/ipc/esp/esp_potential.cpp b/src/ipc/esp/esp_potential.cpp new file mode 100644 index 000000000..d91a2d59a --- /dev/null +++ b/src/ipc/esp/esp_potential.cpp @@ -0,0 +1,1662 @@ +#include "esp_potential.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace ipc { + +constexpr double FACE_QUADRATURE_WEIGHT_SCALE = 1.0; + +namespace { + // Adapt mollifier order to the barrier singularity. + // - Log / default barriers: order 1 (no extra power). + // - InversePowerBarrier(p): order = round(p) + 1, so p=1 -> 2, p=2 -> 3. + int mollifier_order_for_barrier(const std::shared_ptr& barrier) + { + if (const auto* ip = + dynamic_cast*>(barrier.get())) { + const int p = static_cast(std::lround(ip->power())); + return std::max(1, p + 1); + } + return 1; + } + + template inline T pow_int(T x, int n) + { + T r = T(1); + for (int i = 0; i < n; ++i) { + r = r * x; + } + return r; + } + + // An element we integrate the 3D potential over: a face, an edge in no + // face (e.g., a rod), or a vertex in no edge (e.g., a particle). Each + // element's potential is the weighted average of the pointwise potential + // at its samples: its vertices (a face's only without a face quadrature + // rule, since the rule includes them), its face quadrature points (faces + // only), and the closest points of its edges' edge-edge pairs. + // + // A rod or particle has no face, so without elements of its own it would + // get exactly zero potential, however close it comes to anything. + struct IntegrationElement { + index_t face = -1; ///< Face id, or -1 for an edge or vertex element + std::array edges {}; ///< Edges whose edge-edge pairs count + int num_edges = 0; + std::array vertices {}; ///< Vertex samples + int num_vertices = 0; + double weight = 1; ///< Outer weight of the element's average + }; + + const FaceQuadRule NO_FACE_QUADRATURE; + + // We number the elements faces first, then edges in no face, then + // vertices in no edge. + size_t num_integration_elements(const CollisionMesh& mesh) + { + return mesh.num_faces() + mesh.num_codim_edges() + + mesh.num_codim_vertices(); + } + + IntegrationElement integration_element( + const CollisionMesh& mesh, const ESPParameters& params, size_t k) + { + // With area weights, a face weighs its area over the square of its + // vertex count, area / 9. We give an edge in no face the same rule, + // length / 4, and an isolated vertex its vertex area (one by + // default). These have different units, so on a mesh that mixes + // faces with rods or particles the barrier stiffness has to be chosen + // with that in mind. + IntegrationElement elem; + if (k < mesh.num_faces()) { + const auto f = static_cast(k); + elem.face = f; + elem.num_edges = 3; + for (int j = 0; j < 3; j++) { + elem.edges[j] = mesh.faces_to_edges()(f, j); + } + // The face quadrature rule includes the face vertices, so we + // only sample them separately when there is no rule. + if (params.quad_rule().empty()) { + elem.num_vertices = 3; + for (int j = 0; j < 3; j++) { + elem.vertices[j] = mesh.faces()(f, j); + } + } + elem.weight = params.area_weights ? mesh.face_areas()(f) / 9. : 1.; + return elem; + } + k -= mesh.num_faces(); + + if (k < mesh.num_codim_edges()) { + const index_t e = mesh.codim_edges()[k]; + elem.num_edges = 1; + elem.edges[0] = e; + elem.num_vertices = 2; + elem.vertices[0] = mesh.edges()(e, 0); + elem.vertices[1] = mesh.edges()(e, 1); + elem.weight = params.area_weights ? mesh.edge_area(e) / 4. : 1.; + return elem; + } + k -= mesh.num_codim_edges(); + + assert(k < mesh.num_codim_vertices()); + const index_t v = mesh.codim_vertices()[k]; + elem.num_vertices = 1; + elem.vertices[0] = v; + elem.weight = params.area_weights ? mesh.vertex_area(v) : 1.; + return elem; + } +} // namespace + +double ESPPotential::operator()( + const ESPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const +{ + assert(static_cast(X.rows()) == mesh.num_vertices()); + collisions.check_quadrature(m_params); + + if (collisions.empty()) { + return 0; + } + + double result = 0; + + if (mesh.dim() == 2) { + tbb::enumerable_thread_specific potential_storage(0.0); + const GaussLobatto::Rule& rule = + GaussLobatto::get_rule(m_params.quad_order); + + // Collect active edge ids into a flat vector for parallel indexing. + std::vector active_edges; + active_edges.reserve(collisions.maps().edge_collisions_2d.size()); + for (const auto& [ei, _] : collisions.maps().edge_collisions_2d) { + active_edges.push_back(ei); + } + + tbb::parallel_for( + tbb::blocked_range(0, active_edges.size()), + [&](const tbb::blocked_range& r) { + double& total = potential_storage.local(); + for (size_t k = r.begin(); k < r.end(); ++k) { + const index_t ei = active_edges[k]; + const auto& qp_dicts = + collisions.maps().edge_collisions_2d.at(ei); + const double L = mesh.edge_area(ei); + const double w_edge = m_params.area_weights ? L : 1.; + const index_t e0 = mesh.edges()(ei, 0); + const index_t e1 = mesh.edges()(ei, 1); + + for (size_t qi = 0; qi < qp_dicts.size(); ++qi) { + const auto& dict = *qp_dicts[qi]; + if (dict.size() == 0) { + continue; + } + const auto& qp = rule[qi]; + const std::array lambda = { { 1.0 - qp.xi, + qp.xi } }; + const Eigen::RowVector2d q_pos = + lambda[0] * X.row(e0) + lambda[1] * X.row(e1); + VertexMatrixView<2> X_ext(X, q_pos); + total += w_edge * qp.weight + * PointPotentialHelper:: + evaluate_potential_at_edge_qp( + X_ext, dict, m_params); + } + } + }); + + for (const double v : potential_storage) { + result += v; + } + } else if (mesh.dim() == 3) { + { + tbb::enumerable_thread_specific potential_storage(0.0); + + // Split near and far for every dbar_factor in (0, 2]. At 0 the + // edge-edge terms are skipped, so the weights are constant. + const double dbar_factor = m_params.dbar_factor(); + const bool use_nf = m_use_near_far && dbar_factor > 0; + const bool skip_ee = + (dbar_factor == 0); // Skip EE pairs when dbar_factor == 0 + + std::unique_ptr> nf_barrier; + if (use_nf) { + nf_barrier = std::make_unique>( + m_params.barrier, dbar_factor); + } + + auto loop_body = [&](const tbb::blocked_range& r) { + double& total = potential_storage.local(); + for (size_t k = r.begin(); k < r.end(); k++) { + const IntegrationElement elem = + integration_element(mesh, m_params, k); + const double w = elem.weight; + + double total_w = 0, total_w_near = 0, total_w_far = 0; + double total_p = 0, total_p_near = 0, total_p_far = 0; + + for (int le = 0; le < elem.num_edges; le++) { + const index_t edge_id = elem.edges[le]; + const index_t ea = mesh.edges()(edge_id, 0); + const index_t eb = mesh.edges()(edge_id, 1); + + for (index_t other_edge_id : + collisions.m_candidates.ee_set(mesh, edge_id)) { + const index_t ec = mesh.edges()(other_edge_id, 0); + const index_t ed = mesh.edges()(other_edge_id, 1); + + // Skip adjacent edges + if (ea == ec || ea == ed || eb == ec || eb == ed) { + continue; + } + + // Skip EE pairs entirely when dbar_factor == 0 + if (skip_ee) { + continue; + } + + if (auto iter = + collisions.maps().edge_edge_collisions.find( + std::make_pair(edge_id, other_edge_id)); + iter + != collisions.maps() + .edge_edge_collisions.end()) { + + const auto dtype = iter->second->ee_dtype(); + // The builder keeps only EA_EB pairs. + assert(dtype == EdgeEdgeDistanceType::EA_EB); + + const double dist = + sqrt(edge_edge_distance_parallel_safe( + X.row(ea), X.row(eb), X.row(ec), + X.row(ed), dtype)); + + const double uv = closest_point_uv( + X.row(ea), X.row(eb), X.row(ec), X.row(ed)); + + const Eigen::RowVector3d ee_closest_point = + uv * (X.row(eb) - X.row(ea)) + X.row(ea); + + const double dist_sqr = + edge_edge_distance_parallel_safe( + X.row(ea), X.row(eb), X.row(ec), + X.row(ed), dtype); + const auto mtypes = edge_edge_mollifier_type( + X.row(ea).transpose(), + X.row(eb).transpose(), + X.row(ec).transpose(), + X.row(ed).transpose(), dist_sqr); + + double mollifier = + Math::cubic_spline( + dist / m_params.ee_support()) + * 1.5; + mollifier *= edge_edge_mollifier( + X.row(ea).transpose(), + X.row(eb).transpose(), + X.row(ec).transpose(), + X.row(ed).transpose(), mtypes, dist_sqr); + mollifier = pow_int( + mollifier, + mollifier_order_for_barrier( + m_params.barrier)); + + if (use_nf) { + const double p_near = PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions_near( + VertexMatrixView<3>( + X, ee_closest_point), + *(iter->second), m_params, dtype, + *nf_barrier); + total_w_near += mollifier; + total_p_near += mollifier * p_near; + } else { + const double P_val = PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions( + VertexMatrixView<3>( + X, ee_closest_point), + *(iter->second), m_params, dtype); + total_w += mollifier; + total_p += mollifier * P_val; + } + } + } + } + + // Face-interior quadrature points from + // params.quad_rule() (faces only). + const FaceQuadRule& face_quad_rule = elem.face >= 0 + ? m_params.quad_rule() + : NO_FACE_QUADRATURE; + { + auto iter = + collisions.maps().face_collisions.find(elem.face); + for (size_t qi = 0; qi < face_quad_rule.size(); qi++) { + const auto& qp = face_quad_rule[qi]; + if (use_nf) { + total_w_near += + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight; + total_w_far += + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight; + } else { + total_w += + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight; + } + if (iter + != collisions.maps().face_collisions.end()) { + const Eigen::RowVector3d q_pos = qp.lambda[0] + * X.row(mesh.faces()(elem.face, 0)) + + qp.lambda[1] + * X.row(mesh.faces()(elem.face, 1)) + + qp.lambda[2] + * X.row(mesh.faces()(elem.face, 2)); + if (use_nf) { + auto [fq_near, fq_far] = PointPotentialHelper:: + evaluate_potential_at_face_interior_point_with_cached_collisions_nearfar( + VertexMatrixView<3>(X, q_pos), + *iter->second[qi], m_params, + *nf_barrier); + total_p_near += FACE_QUADRATURE_WEIGHT_SCALE + * qp.weight * fq_near; + total_p_far += FACE_QUADRATURE_WEIGHT_SCALE + * qp.weight * fq_far; + } else { + const double fq_val = PointPotentialHelper:: + evaluate_potential_at_face_interior_point_with_cached_collisions( + VertexMatrixView<3>(X, q_pos), + *iter->second[qi], m_params); + total_p += FACE_QUADRATURE_WEIGHT_SCALE + * qp.weight * fq_val; + } + } + } + } + + // Vertex samples (see integration_element()) + for (int lv = 0; lv < elem.num_vertices; lv++) { + const index_t v = elem.vertices[lv]; + if (use_nf) { + total_w_near += 1.; + total_w_far += 1.; + } else { + total_w += 1.; + } + if (auto iter = + collisions.maps().vertex_collisions.find(v); + iter != collisions.maps().vertex_collisions.end()) { + if (use_nf) { + auto [vt_near, vt_far] = PointPotentialHelper:: + evaluate_potential_at_vertex_with_cached_collisions_nearfar( + X, *(iter->second), m_params, + *nf_barrier); + total_p_near += vt_near; + total_p_far += vt_far; + } else { + const double vt_val = PointPotentialHelper:: + evaluate_potential_at_vertex_with_cached_collisions( + X, *(iter->second), m_params); + total_p += vt_val; + } + } + } + + if (use_nf) { + // The element's own vertices or quadrature points + // always add a positive weight to both totals. + assert(total_w_near >= total_w_far - 1e-14); + assert(total_w_far > 0); + total += w + * (total_p_near / total_w_near + + total_p_far / total_w_far); + } else if (m_use_near_far) { + assert(total_w > 0); + total += w * (total_p / total_w); + } else { + total += w * total_p; + } + } + }; + + tbb::parallel_for( + tbb::blocked_range(0, num_integration_elements(mesh)), + loop_body); + + for (const auto& local_potential : potential_storage) { + result += local_potential; + } + } + } + return result; +} + +Eigen::VectorXd ESPPotential::gradient( + const ESPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const +{ + assert(static_cast(X.rows()) == mesh.num_vertices()); + collisions.check_quadrature(m_params); + + if (collisions.empty()) { + return Eigen::VectorXd::Zero(X.size()); + } + + const int dim = X.cols(); + + tbb::enumerable_thread_specific storage( + Eigen::VectorXd::Zero(X.size())); + + if (mesh.dim() == 2) { + const GaussLobatto::Rule& rule = + GaussLobatto::get_rule(m_params.quad_order); + + std::vector active_edges; + active_edges.reserve(collisions.maps().edge_collisions_2d.size()); + for (const auto& [ei, _] : collisions.maps().edge_collisions_2d) { + active_edges.push_back(ei); + } + + tbb::parallel_for( + tbb::blocked_range(0, active_edges.size()), + [&](const tbb::blocked_range& r) { + Eigen::VectorXd& global_grad = storage.local(); + + for (size_t k = r.begin(); k < r.end(); ++k) { + const index_t ei = active_edges[k]; + const auto& qp_dicts = + collisions.maps().edge_collisions_2d.at(ei); + const double L = mesh.edge_area(ei); + const double w_edge = m_params.area_weights ? L : 1.; + const index_t e0 = mesh.edges()(ei, 0); + const index_t e1 = mesh.edges()(ei, 1); + + for (size_t qi = 0; qi < qp_dicts.size(); ++qi) { + const auto& dict = *qp_dicts[qi]; + if (dict.size() == 0) { + continue; + } + const auto& qp = rule[qi]; + const std::array lambda = { { 1.0 - qp.xi, + qp.xi } }; + const Eigen::RowVector2d q_pos = + lambda[0] * X.row(e0) + lambda[1] * X.row(e1); + VertexMatrixView<2> X_ext(X, q_pos); + + const Eigen::VectorXd local_grad = + w_edge * qp.weight + * PointPotentialHelper:: + evaluate_potential_gradient_at_edge_qp( + X_ext, dict, m_params, lambda); + + local_gradient_to_global_gradient( + local_grad, dict.vertex_ids(), dim, global_grad); + } + } + }); + } else if (mesh.dim() == 3) { + { + using T = ADGrad<12>; + + const double dbar_factor = m_params.dbar_factor(); + const bool use_nf_grad = m_use_near_far && dbar_factor > 0; + const bool skip_ee_grad = (dbar_factor == 0); + + auto loop_body = [&](const tbb::blocked_range& r) { + Eigen::VectorXd& grad = storage.local(); + for (size_t k = r.begin(); k < r.end(); k++) { + const IntegrationElement elem = + integration_element(mesh, m_params, k); + const double w = elem.weight; + // Pass 1: collect all quadrature contributions for this + // face + struct EEGradEntry { + const ESPCollisionDict* dict; + double mol_val; + Eigen::Vector mol_grad; + double P; + Eigen::VectorXd grad_p; + }; + struct ConstGradEntry { + const std::vector* dofs; + Eigen::VectorXd grad_p_near, + grad_p_far; // near/far or single (for non-nf) + double p_near, p_far; // near/far or single (for non-nf) + }; + std::vector ee_cache; + std::vector const_cache; + double total_w = 0; + double total_w_near = 0, total_p_near = 0; + double total_w_far = 0; + + std::unique_ptr> nf_barrier; + if (use_nf_grad) { + nf_barrier = std::make_unique>( + m_params.barrier, m_params.dbar_factor()); + } + + for (int le = 0; le < elem.num_edges; le++) { + const index_t edge_id = elem.edges[le]; + const index_t ea = mesh.edges()(edge_id, 0); + const index_t eb = mesh.edges()(edge_id, 1); + + for (index_t other_edge_id : + collisions.m_candidates.ee_set(mesh, edge_id)) { + const index_t ec = mesh.edges()(other_edge_id, 0); + const index_t ed = mesh.edges()(other_edge_id, 1); + + // Skip adjacent edges + if (ea == ec || ea == ed || eb == ec || eb == ed) { + continue; + } + + // Skip EE pairs entirely when dbar_factor == 0 + if (skip_ee_grad) { + continue; + } + + if (auto iter = + collisions.maps().edge_edge_collisions.find( + std::make_pair(edge_id, other_edge_id)); + iter + != collisions.maps() + .edge_edge_collisions.end()) { + + const auto dtype = iter->second->ee_dtype(); + // The builder keeps only EA_EB pairs. + assert(dtype == EdgeEdgeDistanceType::EA_EB); + + Eigen::Vector positions; + positions << X.row(ea).transpose(), + X.row(eb).transpose(), + X.row(ec).transpose(), + X.row(ed).transpose(); + + Eigen::Matrix positionsT = + slice_positions(positions); + + const T dist = sqrt( + line_line_sqr_distance( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3))); + + const T uv = closest_point_uv( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3)); + + const Eigen::RowVector3 ee_closest_point_T = + uv * (positionsT.row(1) - positionsT.row(0)) + + positionsT.row(0); + const Eigen::RowVector3 + ee_closest_point( + ee_closest_point_T(0).val, + ee_closest_point_T(1).val, + ee_closest_point_T(2).val); + + const T dist_sqr = line_line_sqr_distance( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3)); + const auto mtypes = edge_edge_mollifier_type( + X.row(ea).transpose(), + X.row(eb).transpose(), + X.row(ec).transpose(), + X.row(ed).transpose(), dist_sqr.val); + + T mollifier = Math::cubic_spline( + dist / m_params.ee_support()) + * 1.5; + mollifier *= edge_edge_mollifier( + positionsT.row(0).transpose(), + positionsT.row(1).transpose(), + positionsT.row(2).transpose(), + positionsT.row(3).transpose(), mtypes, + dist_sqr); + mollifier = pow_int( + mollifier, + mollifier_order_for_barrier( + m_params.barrier)); + + const ESPCollisionDict& dict = + *(iter->second); + + VertexMatrixView<3> X_extended( + X, ee_closest_point); + assert(X_extended.rows() == X.rows() + 1); + assert( + X_extended.m_a == X.data() + && "VertexMatrixView has made a deepcopy!"); + + double P; + if (use_nf_grad) { + P = PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions_near( + X_extended, dict, m_params, dtype, + *nf_barrier); + } else { + P = PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions( + X_extended, dict, m_params, dtype); + } + Eigen::VectorXd grad_p; + if (use_nf_grad) { + grad_p = PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions_near< + T>( + X_extended, dict, m_params, + ee_closest_point_T, *nf_barrier); + } else { + grad_p = PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions< + T>( + X_extended, dict, m_params, + ee_closest_point_T); + } + + ee_cache.push_back( + { &dict, mollifier.val, mollifier.grad, P, + grad_p }); + total_w += mollifier.val; + if (use_nf_grad) { + total_w_near += mollifier.val; + total_p_near += mollifier.val * P; + } + } + } + } + + // Face-interior quadrature points (faces only) + const FaceQuadRule& face_quad_rule = elem.face >= 0 + ? m_params.quad_rule() + : NO_FACE_QUADRATURE; + { + auto iter = + collisions.maps().face_collisions.find(elem.face); + for (size_t qi = 0; qi < face_quad_rule.size(); qi++) { + const auto& qp = face_quad_rule[qi]; + const double qp_weight_scale = + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight; + total_w += qp_weight_scale; + if (use_nf_grad) { + total_w_near += qp_weight_scale; + total_w_far += qp_weight_scale; + } + if (iter != collisions.maps().face_collisions.end() + && qi < iter->second.size()) { + const auto& dict = *iter->second[qi]; + const Eigen::RowVector3d q_pos = qp.lambda[0] + * X.row(mesh.faces()(elem.face, 0)) + + qp.lambda[1] + * X.row(mesh.faces()(elem.face, 1)) + + qp.lambda[2] + * X.row(mesh.faces()(elem.face, 2)); + VertexMatrixView<3> X_qp(X, q_pos); + + if (use_nf_grad) { + auto [P_n, P_f] = PointPotentialHelper:: + evaluate_potential_at_face_interior_point_with_cached_collisions_nearfar( + X_qp, dict, m_params, *nf_barrier); + auto [grad_n, grad_f] = PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions_nearfar( + X_qp, dict, m_params, qp.lambda, + *nf_barrier); + const_cache.push_back( + ConstGradEntry { + &dict.dofs(), + qp_weight_scale * grad_n, + qp_weight_scale * grad_f, + qp_weight_scale * P_n, + qp_weight_scale * P_f }); + total_p_near += qp_weight_scale * P_n; + } else { + const double P = PointPotentialHelper:: + evaluate_potential_at_face_interior_point_with_cached_collisions( + X_qp, dict, m_params); + const Eigen::VectorXd grad_p = + PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions( + X_qp, dict, m_params, + qp.lambda); + const_cache.push_back( + ConstGradEntry { + &dict.dofs(), + qp_weight_scale * grad_p, + Eigen::VectorXd::Zero(0), + qp_weight_scale * P, 0 }); + } + } + } + } + + // Vertex samples (see integration_element()) + for (int lv = 0; lv < elem.num_vertices; lv++) { + const index_t v = elem.vertices[lv]; + total_w += 1.; + if (use_nf_grad) { + total_w_near += 1.0; + total_w_far += 1.0; + } + if (auto iter = + collisions.maps().vertex_collisions.find(v); + iter != collisions.maps().vertex_collisions.end()) { + if (use_nf_grad) { + auto [P_n, P_f] = PointPotentialHelper:: + evaluate_potential_at_vertex_with_cached_collisions_nearfar( + X, (*iter->second), m_params, + *nf_barrier); + auto [grad_n, grad_f] = PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions_nearfar( + X, (*iter->second), m_params, + *nf_barrier); + const_cache.push_back( + ConstGradEntry { &(*iter->second).dofs(), + grad_n, grad_f, P_n, + P_f }); + total_p_near += P_n; + } else { + const double P = PointPotentialHelper:: + evaluate_potential_at_vertex_with_cached_collisions( + X, (*iter->second), m_params); + const Eigen::VectorXd grad_p = PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions( + X, (*iter->second), m_params); + const_cache.push_back( + ConstGradEntry { + &(*iter->second).dofs(), grad_p, + Eigen::VectorXd::Zero(0), P, 0 }); + } + } + } + + // Pass 2: apply gradient + if (use_nf_grad) { + assert(total_w_near > 0); + assert(total_w_far > 0); + const double avg_P_near = total_p_near / total_w_near; + for (const auto& e : ee_cache) { + grad(e.dict->dofs()) += + (w / total_w_near * e.mol_val) * e.grad_p; + grad(e.dict->primary_dofs()) += + (w / total_w_near * (e.P - avg_P_near)) + * e.mol_grad; + } + for (const auto& e : const_cache) { + grad(*e.dofs) += (w / total_w_near) * e.grad_p_near + + (w / total_w_far) * e.grad_p_far; + } + } else if (m_use_near_far) { + // Normalized but without NearFarBarrier splitting + // (only reached at dbar_factor = 0, which skips the + // edge-edge terms, so the weights are constant) + assert(total_w > 0); + assert(ee_cache.empty()); + for (const auto& e : const_cache) { + grad(*e.dofs) += (w / total_w) * e.grad_p_near; + } + } else { + // Unnormalized + for (const auto& e : ee_cache) { + grad(e.dict->dofs()) += w * e.mol_val * e.grad_p; + grad(e.dict->primary_dofs()) += + w * e.P * e.mol_grad; + } + for (const auto& e : const_cache) { + grad(*e.dofs) += w * e.grad_p_near; + } + } + } + }; + + tbb::parallel_for( + tbb::blocked_range(0, num_integration_elements(mesh)), + loop_body); + } + } + + Eigen::VectorXd grad; + grad.setZero(X.size()); + for (const auto& local_storage : storage) { + grad += local_storage; + } + + return grad; +} + +Eigen::SparseMatrix ESPPotential::hessian( + const ESPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X, + const PSDProjectionMethod project_hessian_to_psd) const +{ + assert(static_cast(X.rows()) == mesh.num_vertices()); + collisions.check_quadrature(m_params); + + if (collisions.empty()) { + return Eigen::SparseMatrix(X.size(), X.size()); + } + + const int dim = X.cols(); + const int n_verts = X.rows(); + const int ndof = X.size(); + + const int max_triplets_size = int(1e7); + const int buffer_size = std::min(max_triplets_size, ndof); + tbb::enumerable_thread_specific storage( + LocalThreadMatStorage(buffer_size, ndof, ndof)); + + if (mesh.dim() == 2) { + const GaussLobatto::Rule& rule = + GaussLobatto::get_rule(m_params.quad_order); + + std::vector active_edges; + active_edges.reserve(collisions.maps().edge_collisions_2d.size()); + for (const auto& [ei, _] : collisions.maps().edge_collisions_2d) { + active_edges.push_back(ei); + } + + tbb::parallel_for( + tbb::blocked_range(0, active_edges.size()), + [&](const tbb::blocked_range& r) { + auto& hess_triplets = storage.local(); + + for (size_t k = r.begin(); k < r.end(); ++k) { + const index_t ei = active_edges[k]; + const auto& qp_dicts = + collisions.maps().edge_collisions_2d.at(ei); + const double L = mesh.edge_area(ei); + const double w_edge = m_params.area_weights ? L : 1.; + const index_t e0 = mesh.edges()(ei, 0); + const index_t e1 = mesh.edges()(ei, 1); + + for (size_t qi = 0; qi < qp_dicts.size(); ++qi) { + const auto& dict = *qp_dicts[qi]; + if (dict.size() == 0) { + continue; + } + const auto& qp = rule[qi]; + const std::array lambda = { { 1.0 - qp.xi, + qp.xi } }; + const Eigen::RowVector2d q_pos = + lambda[0] * X.row(e0) + lambda[1] * X.row(e1); + VertexMatrixView<2> X_ext(X, q_pos); + + const Eigen::MatrixXd local_hess = + w_edge * qp.weight + * PointPotentialHelper:: + evaluate_potential_hessian_at_edge_qp( + X_ext, dict, m_params, lambda, + project_hessian_to_psd); + + local_hessian_to_global_triplets( + local_hess, dict.vertex_ids(), dim, + *(hess_triplets.cache), n_verts); + } + } + }); + } else if (mesh.dim() == 3) { + // With normalized weights (use_near_far) and 0 < dbar_factor <= 2, the + // per-face hessian is assembled as + // Term A (sum of per-stencil H(p_i)) + // + Term B (negative weighted sum of H(mol_i)) + // + Term C (sign-indefinite cross terms ~ sym(G⊗∇Z)). + // Terms B and C are indefinite, so projecting the per-stencil blocks + // is not enough. To still guarantee a PSD per-face contribution (and + // thus a PSD global hessian), defer all per-stencil projections and + // project the assembled per-face block once, over the union of + // involved DOFs. At dbar_factor = 0 the edge-edge terms are skipped, + // so B = C = 0 and the per-stencil projections suffice; with + // use_near_far = false the local-projection path is used. + const double dbar_factor_hess = m_params.dbar_factor(); + const bool use_nf_hess = m_use_near_far && dbar_factor_hess > 0; + const bool skip_ee_hess = (dbar_factor_hess == 0); + const bool combined_psd_projection = + use_nf_hess && project_hessian_to_psd != PSDProjectionMethod::NONE; + const PSDProjectionMethod inner_psd_method = combined_psd_projection + ? PSDProjectionMethod::NONE + : project_hessian_to_psd; + { + using T = ADHessian<12>; + + auto loop_body = [&](const tbb::blocked_range& r) { + auto& hess_triplets = storage.local(); + for (size_t k = r.begin(); k < r.end(); k++) { + const IntegrationElement elem = + integration_element(mesh, m_params, k); + const double w = elem.weight; + + // Pass 1: collect all quadrature contributions for this + // face + struct EEHessEntry { + const ESPCollisionDict* dict; + double mol_val; + Eigen::Vector mol_grad; // on primary_dofs + Eigen::Matrix + mol_hess; // H(mol) on primary_dofs + double P; // only near component for use_nf_hess + Eigen::VectorXd grad_p; // indexed by dict->dofs() + Eigen::MatrixXd local_hess; // H(mol*P), PSD-projected + }; + struct ConstHessEntry { + const std::vector* vertex_ids; + const std::vector* dofs; + double p_near, p_far; + Eigen::VectorXd grad_p_near, + grad_p_far; // indexed by dofs + Eigen::MatrixXd local_hess_near, + local_hess_far; // H(p_near), H(p_far) + }; + std::vector ee_cache; + std::vector const_cache; + double total_w = 0; + double total_w_near = 0, total_p_near = 0; + double total_w_far = 0; + [[maybe_unused]] double total_p_far = 0; // asserts only + + // Construct NearFarBarrier if needed + std::unique_ptr> nf_barrier; + if (use_nf_hess) { + nf_barrier = std::make_unique>( + m_params.barrier, m_params.dbar_factor()); + } + + for (int le = 0; le < elem.num_edges; le++) { + const index_t edge_id = elem.edges[le]; + const index_t ea = mesh.edges()(edge_id, 0); + const index_t eb = mesh.edges()(edge_id, 1); + + for (index_t other_edge_id : + collisions.m_candidates.ee_set(mesh, edge_id)) { + const index_t ec = mesh.edges()(other_edge_id, 0); + const index_t ed = mesh.edges()(other_edge_id, 1); + + // Skip adjacent edges + if (ea == ec || ea == ed || eb == ec || eb == ed) { + continue; + } + + // Skip EE pairs entirely when dbar_factor == 0 + if (skip_ee_hess) { + continue; + } + + if (auto iter = + collisions.maps().edge_edge_collisions.find( + std::make_pair(edge_id, other_edge_id)); + iter + != collisions.maps() + .edge_edge_collisions.end()) { + + const auto dtype = iter->second->ee_dtype(); + // The builder keeps only EA_EB pairs. + assert(dtype == EdgeEdgeDistanceType::EA_EB); + + Eigen::Vector positions; + positions << X.row(ea).transpose(), + X.row(eb).transpose(), + X.row(ec).transpose(), + X.row(ed).transpose(); + + Eigen::Matrix positionsT = + slice_positions(positions); + + const T dist = sqrt( + line_line_sqr_distance( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3))); + + const T uv = closest_point_uv( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3)); + + const Eigen::RowVector3 ee_closest_point_T = + uv * (positionsT.row(1) - positionsT.row(0)) + + positionsT.row(0); + const Eigen::RowVector3 + ee_closest_point( + ee_closest_point_T(0).val, + ee_closest_point_T(1).val, + ee_closest_point_T(2).val); + + const T dist_sqr = line_line_sqr_distance( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3)); + const auto mtypes = edge_edge_mollifier_type( + X.row(ea).transpose(), + X.row(eb).transpose(), + X.row(ec).transpose(), + X.row(ed).transpose(), dist_sqr.val); + + T mollifier = Math::cubic_spline( + dist / m_params.ee_support()) + * 1.5; + mollifier *= edge_edge_mollifier( + positionsT.row(0).transpose(), + positionsT.row(1).transpose(), + positionsT.row(2).transpose(), + positionsT.row(3).transpose(), mtypes, + dist_sqr); + mollifier = pow_int( + mollifier, + mollifier_order_for_barrier( + m_params.barrier)); + + const ESPCollisionDict& dict = + *(iter->second); + + VertexMatrixView<3> X_extended( + X, ee_closest_point); + assert( + X_extended.m_a == X.data() + && "VertexMatrixView has made a deepcopy!"); + + double P; + Eigen::VectorXd grad_p; + Eigen::MatrixXd base_hess; + if (use_nf_hess) { + P = PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions_near( + X_extended, dict, m_params, dtype, + *nf_barrier); + grad_p = PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions_near< + T>( + X_extended, dict, m_params, + ee_closest_point_T, *nf_barrier); + base_hess = PointPotentialHelper:: + evaluate_potential_hessian_at_edge_edge_closest_point_with_cached_collisions_near( + X_extended, dict, m_params, + ee_closest_point_T, *nf_barrier); + } else { + P = PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions( + X_extended, dict, m_params, dtype); + grad_p = PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions< + T>( + X_extended, dict, m_params, + ee_closest_point_T); + base_hess = PointPotentialHelper:: + evaluate_potential_hessian_at_edge_edge_closest_point_with_cached_collisions( + X_extended, dict, m_params, + ee_closest_point_T); + } + Eigen::MatrixXd local_hess = + base_hess * mollifier.val; + + for (index_t i = 0; i < 4; i++) { + for (index_t j = 0; j < 4; j++) { + local_hess.block<3, 3>( + dict.primary_local_ids()[i] * 3, + dict.primary_local_ids()[j] * 3) += + P + * mollifier.Hess.block<3, 3>( + i * 3, j * 3); + } + } + + // GCC false-positive: mollifier.grad is a + // fixed-size member, not a pointer. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wnull-dereference" + for (index_t i = 0; i < 4; i++) { + const Eigen::MatrixXd tmp = + mollifier.grad.segment<3>(i * 3) + * grad_p.transpose(); + local_hess.middleRows( + dict.primary_local_ids()[i] * 3, 3) += + tmp; + local_hess.middleCols( + dict.primary_local_ids()[i] * 3, 3) += + tmp.transpose(); + } +#pragma GCC diagnostic pop + + if (project_hessian_to_psd + != PSDProjectionMethod::NONE + && !combined_psd_projection) { + local_hess = project_to_psd( + local_hess, project_hessian_to_psd); + } + + ee_cache.push_back( + { &dict, mollifier.val, mollifier.grad, + mollifier.Hess, P, grad_p, + std::move(local_hess) }); + total_w += mollifier.val; + if (use_nf_hess) { + total_w_near += mollifier.val; + total_p_near += mollifier.val * P; + } + } + } + } + + // Face-interior quadrature points (faces only) + const FaceQuadRule& face_quad_rule = elem.face >= 0 + ? m_params.quad_rule() + : NO_FACE_QUADRATURE; + { + auto iter = + collisions.maps().face_collisions.find(elem.face); + for (size_t qi = 0; qi < face_quad_rule.size(); qi++) { + const auto& qp = face_quad_rule[qi]; + total_w += FACE_QUADRATURE_WEIGHT_SCALE * qp.weight; + if (use_nf_hess) { + total_w_near += + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight; + total_w_far += + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight; + } + if (iter != collisions.maps().face_collisions.end() + && qi < iter->second.size()) { + const auto& dict = *iter->second[qi]; + const Eigen::RowVector3d q_pos = qp.lambda[0] + * X.row(mesh.faces()(elem.face, 0)) + + qp.lambda[1] + * X.row(mesh.faces()(elem.face, 1)) + + qp.lambda[2] + * X.row(mesh.faces()(elem.face, 2)); + VertexMatrixView<3> X_qp(X, q_pos); + ConstHessEntry entry; + entry.vertex_ids = &dict.vertex_ids(); + entry.dofs = &dict.dofs(); + + if (use_nf_hess) { + auto [P_n, P_f] = PointPotentialHelper:: + evaluate_potential_at_face_interior_point_with_cached_collisions_nearfar( + X_qp, dict, m_params, *nf_barrier); + auto [grad_n, grad_f] = PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions_nearfar( + X_qp, dict, m_params, qp.lambda, + *nf_barrier); + auto [hess_n, hess_f] = PointPotentialHelper:: + evaluate_potential_hessian_at_face_interior_point_with_cached_collisions_nearfar( + X_qp, dict, m_params, qp.lambda, + inner_psd_method, *nf_barrier); + entry.p_near = FACE_QUADRATURE_WEIGHT_SCALE + * qp.weight * P_n; + entry.p_far = FACE_QUADRATURE_WEIGHT_SCALE + * qp.weight * P_f; + entry.grad_p_near = + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight + * grad_n; + entry.grad_p_far = + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight + * grad_f; + entry.local_hess_near = + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight + * hess_n; + entry.local_hess_far = + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight + * hess_f; + total_p_near += entry.p_near; + total_p_far += entry.p_far; + } else { + entry.p_near = FACE_QUADRATURE_WEIGHT_SCALE + * qp.weight + * PointPotentialHelper:: + evaluate_potential_at_face_interior_point_with_cached_collisions( + X_qp, dict, m_params); + entry.grad_p_near = + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight + * PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions( + X_qp, dict, m_params, + qp.lambda); + entry.local_hess_near = + FACE_QUADRATURE_WEIGHT_SCALE * qp.weight + * PointPotentialHelper:: + evaluate_potential_hessian_at_face_interior_point_with_cached_collisions( + X_qp, dict, m_params, qp.lambda, + inner_psd_method); + entry.p_far = 0; + entry.grad_p_far = Eigen::VectorXd::Zero(0); + entry.local_hess_far = + Eigen::MatrixXd::Zero(0, 0); + } + const_cache.push_back(std::move(entry)); + } + } + } + + // Vertex samples (see integration_element()) + for (int lv = 0; lv < elem.num_vertices; lv++) { + const index_t v = elem.vertices[lv]; + total_w += 1.; + if (use_nf_hess) { + total_w_near += 1.0; + total_w_far += 1.0; + } + if (auto iter = + collisions.maps().vertex_collisions.find(v); + iter != collisions.maps().vertex_collisions.end()) { + const auto& dict = *iter->second; + ConstHessEntry entry; + entry.vertex_ids = &dict.vertex_ids(); + entry.dofs = &dict.dofs(); + + if (use_nf_hess) { + auto [P_n, P_f] = PointPotentialHelper:: + evaluate_potential_at_vertex_with_cached_collisions_nearfar( + X, dict, m_params, *nf_barrier); + auto [grad_n, grad_f] = PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions_nearfar( + X, dict, m_params, *nf_barrier); + auto [hess_n, hess_f] = PointPotentialHelper:: + evaluate_potential_hessian_at_vertex_with_cached_collisions_nearfar( + X, dict, m_params, inner_psd_method, + *nf_barrier); + entry.p_near = P_n; + entry.p_far = P_f; + entry.grad_p_near = grad_n; + entry.grad_p_far = grad_f; + entry.local_hess_near = hess_n; + entry.local_hess_far = hess_f; + total_p_near += entry.p_near; + total_p_far += entry.p_far; + } else { + entry.p_near = PointPotentialHelper:: + evaluate_potential_at_vertex_with_cached_collisions( + X, dict, m_params); + entry.grad_p_near = PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions( + X, dict, m_params); + entry.local_hess_near = PointPotentialHelper:: + evaluate_potential_hessian_at_vertex_with_cached_collisions( + X, dict, m_params, inner_psd_method); + entry.p_far = 0; + entry.grad_p_far = Eigen::VectorXd::Zero(0); + entry.local_hess_far = + Eigen::MatrixXd::Zero(0, 0); + } + const_cache.push_back(std::move(entry)); + } + } + + // Pass 2: apply hessian + if (use_nf_hess) { + assert(total_w_near > 0); + if (total_w_far == 0) { + assert(total_p_far == 0); + } + // Apply quotient rule separately for near and far + // components + const double avg_P_near = total_p_near / total_w_near; + const double scale_C_near = + -(w / (total_w_near * total_w_near)); + + if (combined_psd_projection) { + // Nothing contributes from this face: skip combined + // block. + if (ee_cache.empty() && const_cache.empty()) { + continue; + } + // Build the union of vertex IDs across every + // contributing stencil so the entire face block can + // be assembled into one dense matrix and projected + // once. This is the only way Term C + // (sym(G⊗∇Z), sign-indefinite) can be made PSD. + std::vector union_vids; + union_vids.reserve( + 4 * ee_cache.size() + 3 * const_cache.size()); + for (const auto& e : ee_cache) { + for (index_t vid : e.dict->vertex_ids()) { + union_vids.push_back(vid); + } + for (index_t vid : + e.dict->primary_vertex_ids()) { + if (vid >= 0) { + union_vids.push_back(vid); + } + } + } + for (const auto& e : const_cache) { + for (index_t vid : *e.vertex_ids) { + union_vids.push_back(vid); + } + } + std::sort(union_vids.begin(), union_vids.end()); + union_vids.erase( + std::unique( + union_vids.begin(), union_vids.end()), + union_vids.end()); + + std::unordered_map vid_to_local; + vid_to_local.reserve(union_vids.size()); + for (int i = 0; + i < static_cast(union_vids.size()); i++) { + vid_to_local[union_vids[i]] = i; + } + + const int n_union_dofs = + static_cast(union_vids.size()) * dim; + Eigen::MatrixXd H_face = Eigen::MatrixXd::Zero( + n_union_dofs, n_union_dofs); + + // Scatter a (n*dim)x(n*dim) block keyed by vertex + // ids into H_face using the union vid mapping. + auto add_block = [&](const Eigen::MatrixXd& block, + auto&& vids, double scale) { + const int n = + static_cast(block.rows()) / dim; + std::vector lvi(n); + for (int i = 0; i < n; i++) { + lvi[i] = vid_to_local.at(vids[i]); + } + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + H_face.block( + lvi[i] * dim, lvi[j] * dim, dim, + dim) += scale + * block.block( + i * dim, j * dim, dim, dim); + } + } + }; + + // Maps a global DOF index (vid * dim + d in + // RowMajor, d * n_verts + vid in ColMajor) to its + // local index inside H_face, which is always + // vertex-interleaved. + auto global_dof_to_local = [&](index_t gd) { + if constexpr ( + VERTEX_DERIVATIVE_LAYOUT + == Eigen::RowMajor) { + return vid_to_local.at(gd / dim) * dim + + static_cast(gd % dim); + } else { + return vid_to_local.at(gd % n_verts) * dim + + static_cast(gd / n_verts); + } + }; + + // Adds scale * sym(outer(g_vec, gradz_vec)) to + // H_face. + auto add_sym_correction_dense = + [&](const std::vector& g_dofs, + const Eigen::Ref& + g_vec, + const std::vector& gradz_dofs, + const Eigen::Ref& + gradz_vec, + double scale) { + for (int a = 0; + a < static_cast(g_dofs.size()); + a++) { + const int la = + global_dof_to_local(g_dofs[a]); + for (int b = 0; b < static_cast( + gradz_dofs.size()); + b++) { + const int lb = global_dof_to_local( + gradz_dofs[b]); + const double v = + scale * g_vec[a] * gradz_vec[b]; + H_face(la, lb) += v; + H_face(lb, la) += v; + } + } + }; + + // Term A: (w/total_w_near) * H(p_near) + + // (w/total_w_far) * H(p_far) + for (const auto& e : ee_cache) { + add_block( + e.local_hess, e.dict->vertex_ids(), + w / total_w_near); + } + for (const auto& e : const_cache) { + add_block( + e.local_hess_near, *e.vertex_ids, + w / total_w_near); + if (total_w_far > 0) { + add_block( + e.local_hess_far, *e.vertex_ids, + w / total_w_far); + } + } + + // Term B: -(w*avg_P_near/total_w_near) * Σ_i + // H(mol_i) (Z_far has no V-dependence, so no far + // Term B.) + const double scale_B_near = + -(w * avg_P_near / total_w_near); + for (const auto& e : ee_cache) { + add_block( + e.mol_hess, e.dict->primary_vertex_ids(), + scale_B_near); + } + + // Term C: -(w/total_w_near²) * sym(G_near ⊗ + // ∇Z_near) (Z_far has no V-dependence, so no far + // Term C.) + for (const auto& ei : ee_cache) { + const auto& prim_dofs_i = + ei.dict->primary_dofs(); + const Eigen::Vector& mol_grad_i = + ei.mol_grad; + // EE-EE near interactions + for (const auto& ek : ee_cache) { + add_sym_correction_dense( + ek.dict->primary_dofs(), + (ek.P - avg_P_near) * ek.mol_grad, + prim_dofs_i, mol_grad_i, scale_C_near); + add_sym_correction_dense( + ek.dict->dofs(), ek.mol_val * ek.grad_p, + prim_dofs_i, mol_grad_i, scale_C_near); + } + // EE-const near interactions + for (const auto& ej : const_cache) { + add_sym_correction_dense( + *ej.dofs, ej.grad_p_near, prim_dofs_i, + mol_grad_i, scale_C_near); + } + } + + H_face = + project_to_psd(H_face, project_hessian_to_psd); + + local_hessian_to_global_triplets( + H_face, union_vids, dim, *(hess_triplets.cache), + n_verts); + } else { + // Adds scale * sym(outer(g_vec, gradz_vec)) to + // triplets. + auto add_sym_correction = + [&](const std::vector& g_dofs, + const Eigen::Ref& + g_vec, + const std::vector& gradz_dofs, + const Eigen::Ref& + gradz_vec, + double scale) { + for (int a = 0; + a < static_cast(g_dofs.size()); + a++) { + for (int b = 0; b < static_cast( + gradz_dofs.size()); + b++) { + const double v = + scale * g_vec[a] * gradz_vec[b]; + hess_triplets.cache->add_value( + 0, g_dofs[a], gradz_dofs[b], v); + hess_triplets.cache->add_value( + 0, gradz_dofs[b], g_dofs[a], v); + } + } + }; + + // Term A: (w/total_w_near) * H(p_near) + + // (w/total_w_far) * H(p_far) + for (const auto& e : ee_cache) { + local_hessian_to_global_triplets( + (w / total_w_near) * e.local_hess, + e.dict->vertex_ids(), dim, + *(hess_triplets.cache), n_verts); + } + for (const auto& e : const_cache) { + local_hessian_to_global_triplets( + (w / total_w_near) * e.local_hess_near, + *e.vertex_ids, dim, *(hess_triplets.cache), + n_verts); + if (total_w_far > 0) { + local_hessian_to_global_triplets( + (w / total_w_far) * e.local_hess_far, + *e.vertex_ids, dim, + *(hess_triplets.cache), n_verts); + } + } + + // Term B: -(w*avg_P_near/total_w_near) * Σ_i + // H(mol_i) (Z_far has no V-dependence, so no far + // Term B.) + for (const auto& e : ee_cache) { + local_hessian_to_global_triplets( + -(w * avg_P_near / total_w_near) + * e.mol_hess, + e.dict->primary_vertex_ids(), dim, + *(hess_triplets.cache), n_verts); + } + + // Term C: -(w/total_w_near²) * sym(G_near ⊗ + // ∇Z_near) (Z_far has no V-dependence, so no far + // Term C.) + for (const auto& ei : ee_cache) { + const auto& prim_dofs_i = + ei.dict->primary_dofs(); + const Eigen::Vector& mol_grad_i = + ei.mol_grad; + // EE-EE near interactions + for (const auto& ek : ee_cache) { + add_sym_correction( + ek.dict->primary_dofs(), + (ek.P - avg_P_near) * ek.mol_grad, + prim_dofs_i, mol_grad_i, scale_C_near); + add_sym_correction( + ek.dict->dofs(), ek.mol_val * ek.grad_p, + prim_dofs_i, mol_grad_i, scale_C_near); + } + // EE-const near interactions + for (const auto& ej : const_cache) { + add_sym_correction( + *ej.dofs, ej.grad_p_near, prim_dofs_i, + mol_grad_i, scale_C_near); + } + } + } + } else if (m_use_near_far) { + // Normalized but without NearFarBarrier splitting + // (only reached at dbar_factor = 0, which skips the + // edge-edge terms, so the weights are constant) + assert(total_w > 0); + assert(ee_cache.empty()); + for (const auto& e : const_cache) { + local_hessian_to_global_triplets( + (w / total_w) * e.local_hess_near, + *e.vertex_ids, dim, *(hess_triplets.cache), + n_verts); + } + } else { + // Unnormalized: H(w*p_sum) = w * H(p_sum) + for (const auto& e : ee_cache) { + local_hessian_to_global_triplets( + w * e.local_hess, e.dict->vertex_ids(), dim, + *(hess_triplets.cache), n_verts); + } + for (const auto& e : const_cache) { + local_hessian_to_global_triplets( + w * e.local_hess_near, *e.vertex_ids, dim, + *(hess_triplets.cache), n_verts); + } + } + } + }; + + tbb::parallel_for( + tbb::blocked_range(0, num_integration_elements(mesh)), + loop_body); + } + } + + Eigen::SparseMatrix hess(ndof, ndof); + + // Assemble the stiffness matrix by concatenating the tuples in each local + // storage + + // Collect thread storages + std::vector storages(storage.size()); + int index = 0; + for (auto& local_storage : storage) { + storages[index++] = &local_storage; + } + + tbb::parallel_for(size_t(0), storages.size(), [&](size_t i) { + storages[i]->cache->prune(); + }); + + if (storage.empty()) { + return hess; + } + + // Prepares for parallel concatenation + std::vector offsets(storage.size()); + + index = 0; + int triplet_count = 0; + for (auto& local_storage : storage) { + offsets[index++] = triplet_count; + triplet_count += local_storage.cache->triplet_count(); + } + + std::vector> triplets; + + assert(!storages.empty()); + if (triplet_count >= triplets.max_size()) { + // Serial fallback version in case the vector of triplets cannot be + // allocated + + logger().warn( + "Cannot allocate space for triplets, switching to serial assembly."); + + // Serially merge local storages + for (LocalThreadMatStorage& local_storage : storage) { + hess += local_storage.cache->get_matrix(false); // will also prune + } + hess.makeCompressed(); + } else { + triplets.resize(triplet_count); + + // Parallel copy into triplets + tbb::parallel_for(size_t(0), storages.size(), [&](size_t i) { + const SparseMatrixCache& cache = + dynamic_cast(*storages[i]->cache); + int offset = offsets[i]; + + std::copy( + cache.entries().begin(), cache.entries().end(), + triplets.begin() + offset); + offset += cache.entries().size(); + + if (cache.mat().nonZeros() > 0) { + int count = 0; + for (int k = 0; k < cache.mat().outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it( + cache.mat(), k); + it; ++it) { + assert(count < cache.mat().nonZeros()); + triplets[offset + count++] = Eigen::Triplet( + it.row(), it.col(), it.value()); + } + } + } + }); + + // Sort and assemble + hess.setFromTriplets(triplets.begin(), triplets.end()); + } + + return hess; +} + +double ESPPotential::operator()( + const ESPCollision& collision, + Eigen::ConstRef positions) const +{ + return collision.weight * collision(positions, m_params); +} + +Eigen::VectorXd ESPPotential::gradient( + const ESPCollision& collision, + Eigen::ConstRef positions) const +{ + return collision.weight * collision.gradient(positions, m_params); +} + +Eigen::MatrixXd ESPPotential::hessian( + const ESPCollision& collision, + Eigen::ConstRef positions, + const PSDProjectionMethod project_hessian_to_psd) const +{ + Eigen::MatrixXd hess = + collision.weight * collision.hessian(positions, m_params); + return project_to_psd(hess, project_hessian_to_psd); +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/esp/esp_potential.hpp b/src/ipc/esp/esp_potential.hpp new file mode 100644 index 000000000..085b93c69 --- /dev/null +++ b/src/ipc/esp/esp_potential.hpp @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +class ESPPotential { +public: + /// @brief Construct an ESP potential. + /// @param _params ESP parameters. + /// @param _use_near_far Normalize the quadrature weights (3D only); see + /// use_near_far(). + ESPPotential(const ESPParameters& _params, const bool _use_near_far = true) + : m_params(_params) + , m_use_near_far(_use_near_far) + { + } + + virtual ~ESPPotential() = default; + + // -- Cumulative methods --------------------------------------------------- + + /// @brief Compute the potential for a set of collisions. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @returns The potential for a set of collisions. + /// @throws std::invalid_argument if the potential's quadrature differs + /// from the one collisions were built with (see + /// ESPCollisions::check_quadrature()). + double operator()( + const ESPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const; + + /// @brief Compute the gradient of the potential. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @returns The gradient of the potential w.r.t. X. This will have a size of |X|. + /// @throws std::invalid_argument if the potential's quadrature differs + /// from the one collisions were built with (see + /// ESPCollisions::check_quadrature()). + Eigen::VectorXd gradient( + const ESPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const; + + /// @brief Compute the hessian of the potential. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @param project_hessian_to_psd Make sure the hessian is positive semi-definite. + /// With normalized weights and edge-edge terms, each face's block is + /// projected as a whole, since its weight-derivative terms are + /// indefinite; otherwise each stencil's block is projected. + /// @returns The Hessian of the potential w.r.t. X. This will have a size of |X|×|X|. + /// @throws std::invalid_argument if the potential's quadrature differs + /// from the one collisions were built with (see + /// ESPCollisions::check_quadrature()). + virtual Eigen::SparseMatrix hessian( + const ESPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const; + + // -- Single collision methods --------------------------------------------- + + /// @brief Compute the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @return The potential. + double operator()( + const ESPCollision& collision, + Eigen::ConstRef positions) const; + + /// @brief Compute the gradient of the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @return The gradient of the potential. + Eigen::VectorXd gradient( + const ESPCollision& collision, + Eigen::ConstRef positions) const; + + /// @brief Compute the hessian of the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @param project_hessian_to_psd Make sure the hessian is positive semi-definite. + /// @return The hessian of the potential. + Eigen::MatrixXd hessian( + const ESPCollision& collision, + Eigen::ConstRef positions, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const; + + /// @brief Whether to normalize the quadrature weights so they sum to 1, + /// splitting near and far contributions when 0 < dbar_factor <= 2. + /// The far part is nonzero for dbar_factor < 2 (e.g., at 1) and + /// vanishes identically at 2, where the split equals the unsplit + /// normalized potential. 3D only: the 2D potential is never + /// normalized. + bool use_near_far() const { return m_use_near_far; } + +protected: + /// @brief ESP parameters for collision potential + ESPParameters m_params; + /// @brief See use_near_far(). + const bool m_use_near_far; +}; + +} // namespace ipc diff --git a/src/ipc/esp/esp_tangential_collisions.cpp b/src/ipc/esp/esp_tangential_collisions.cpp new file mode 100644 index 000000000..c50d812df --- /dev/null +++ b/src/ipc/esp/esp_tangential_collisions.cpp @@ -0,0 +1,792 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ipc { + +void TangentialCollisions::build( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const ESPCollisions& collisions, + const ESPParameters& params, + const double normal_stiffness, + Eigen::ConstRef mu_s, + Eigen::ConstRef mu_k, + const std::function& blend_mu, + const bool normalize_weights) +{ + assert(mu_s.size() == vertices.rows()); + assert(mu_k.size() == vertices.rows()); + // The quadrature loops below index params' rule by the collision sets' + // per-point entries, so the two must come from the same rule. + collisions.check_quadrature(params); + + const Eigen::MatrixXi& edges = mesh.edges(); + const Eigen::MatrixXi& faces = mesh.faces(); + const int dim = mesh.dim(); + + clear(); + + auto& FC_vv = vv_collisions; + auto& FC_ev = ev_collisions; + auto& FC_ee = ee_collisions; + auto& FC_fv = fv_collisions; + + auto assign_ev_mu = [&](EdgeVertexTangentialCollision& tc) { + const auto& [vi, e0i, e1i, _] = tc.vertex_ids(edges, faces); + const double edge_mu_s = + (mu_s(e1i) - mu_s(e0i)) * tc.closest_point[0] + mu_s(e0i); + tc.mu_s = blend_mu(edge_mu_s, mu_s(vi)); + const double edge_mu_k = + (mu_k(e1i) - mu_k(e0i)) * tc.closest_point[0] + mu_k(e0i); + tc.mu_k = blend_mu(edge_mu_k, mu_k(vi)); + }; + + if (dim == 2) { + const GaussLobatto::Rule& rule = + GaussLobatto::get_rule(params.quad_order); + const index_t n_verts = vertices.rows(); + + auto compute_contact_force_2d = [&](const ESPCollision& cc, + const VertexMatrixView<2>& V_ext, + const double outer_w) -> double { + const Eigen::VectorXd positions = cc.dof(V_ext); + double d2 = 0; + switch (cc.type()) { + case ESPCollisionType::VERTEX_VERTEX: + d2 = point_point_distance( + Eigen::Vector2d(positions.segment<2>(0)), + Eigen::Vector2d(positions.segment<2>(2))); + break; + case ESPCollisionType::EDGE_VERTEX: + d2 = point_edge_distance( + Eigen::Vector2d(positions.segment<2>(0)), + Eigen::Vector2d(positions.segment<2>(2)), + Eigen::Vector2d(positions.segment<2>(4))); + break; + default: + return 0; + } + const double dist = std::sqrt(d2); + const double dhat_val = params.dhat; + return (dist > 0 && dist < dhat_val) ? outer_w * normal_stiffness + * std::abs(params.barrier->first_derivative(dist, dhat_val)) + : 0.0; + }; + + for (const auto& edge_collisions : + collisions.maps().edge_collisions_2d) { + const index_t ei = edge_collisions.first; + const auto& qp_dicts = edge_collisions.second; + const index_t e0 = edges(ei, 0); + const index_t e1 = edges(ei, 1); + const double L = mesh.edge_length(ei); + + for (size_t qi = 0; qi < qp_dicts.size(); ++qi) { + const auto& dict_ptr = qp_dicts[qi]; + if (!dict_ptr || dict_ptr->size() == 0) { + continue; + } + const auto& qp = rule[qi]; + const std::array lambda = { { 1.0 - qp.xi, qp.xi } }; + const Eigen::RowVector2d virtual_pos = + lambda[0] * vertices.row(e0) + lambda[1] * vertices.row(e1); + VertexMatrixView<2> V_ext(vertices, virtual_pos); + const double outer_w = L * qp.weight; + + for (int j = 0; j < dict_ptr->size(); ++j) { + const auto& cc = (*dict_ptr)[j]; + const double contact_force = + compute_contact_force_2d(cc, V_ext, outer_w); + if (contact_force == 0) { + continue; + } + + switch (cc.type()) { + case ESPCollisionType::VERTEX_VERTEX: { + // One vertex is virtual (n_verts), other is real. + // Elevate to EdgeVertex: edge ei vs the real vertex. + const index_t v0 = cc.vertex_id(0); + const index_t v1 = cc.vertex_id(1); + const index_t v_real = (v0 == n_verts) ? v1 : v0; + + Eigen::Matrix cp; + cp.segment<2>(0) = vertices.row(v_real).transpose(); + cp.segment<2>(2) = vertices.row(e0).transpose(); + cp.segment<2>(4) = vertices.row(e1).transpose(); + + FC_ev.emplace_back( + EdgeVertexNormalCollision( + ei, v_real, cc.weight, + Eigen::SparseVector()), + cp, contact_force); + FC_ev.back().weight = cc.weight; + assign_ev_mu(FC_ev.back()); + break; + } + case ESPCollisionType::EDGE_VERTEX: { + // Virtual vertex on edge ei vs real edge ej. + // Distribute to the two endpoints of ej weighted by + // the projection parameter u (parallel to 3D face + // dict EV elevation). + const index_t ea = cc.vertex_id(1); + const index_t eb = cc.vertex_id(2); + const Eigen::Vector2d ea_pos = + vertices.row(ea).transpose(); + const Eigen::Vector2d eb_pos = + vertices.row(eb).transpose(); + const Eigen::Vector2d vp = virtual_pos.transpose(); + + double u = point_edge_closest_point(vp, ea_pos, eb_pos); + if (!std::isfinite(u)) { + break; + } + u = std::clamp(u, 0.0, 1.0); + + auto emit_ev = [&](index_t v_edge, double w) { + if (w <= 0) { + return; + } + Eigen::Matrix cp; + cp.segment<2>(0) = vertices.row(v_edge).transpose(); + cp.segment<2>(2) = vertices.row(e0).transpose(); + cp.segment<2>(4) = vertices.row(e1).transpose(); + FC_ev.emplace_back( + EdgeVertexNormalCollision( + ei, v_edge, cc.weight, + Eigen::SparseVector()), + cp, w * contact_force); + FC_ev.back().weight = cc.weight; + assign_ev_mu(FC_ev.back()); + }; + emit_ev(ea, 1.0 - u); + emit_ev(eb, u); + break; + } + default: + break; + } + } + } + } + } else { + // 3D: collisions are stored in per-primitive dicts. + // VERTEX dicts have only real vertex IDs. + // EDGE and FACE dicts contain a virtual vertex (ID = n_verts) + // representing an edge-edge closest point or face quadrature point. + // We "elevate" sub-collisions with virtual vertices to higher-type + // tangential collisions that use only real vertex IDs. + const index_t n_verts = vertices.rows(); + + // Helper: compute contact force magnitude for a sub-collision. + // + // Uses the scalar derivative of params.barrier w.r.t. distance, + // scaled by outer quadrature weight and barrier stiffness. + auto compute_contact_force = [&](const ESPCollision& cc, + const VertexMatrixView<3>& V_ext, + const double outer_w) -> double { + const Eigen::VectorXd positions = cc.dof(V_ext); + double d2 = 0; + switch (cc.type()) { + case ESPCollisionType::VERTEX_VERTEX: + d2 = point_point_distance( + Eigen::Vector3d(positions.segment<3>(0)), + Eigen::Vector3d(positions.segment<3>(3))); + break; + case ESPCollisionType::EDGE_VERTEX: + d2 = point_edge_distance( + Eigen::Vector3d(positions.segment<3>(6)), + Eigen::Vector3d(positions.segment<3>(0)), + Eigen::Vector3d(positions.segment<3>(3))); + break; + case ESPCollisionType::FACE_VERTEX: + d2 = point_triangle_distance( + Eigen::Vector3d(positions.segment<3>(9)), + Eigen::Vector3d(positions.segment<3>(0)), + Eigen::Vector3d(positions.segment<3>(3)), + Eigen::Vector3d(positions.segment<3>(6))); + break; + default: + return 0; + } + const double dist = sqrt(d2); + const double dhat_val = params.dhat; + return (dist > 0 && dist < dhat_val) ? outer_w * normal_stiffness + * std::abs(params.barrier->first_derivative(dist, dhat_val)) + : 0.0; + }; + + // Helper: assign EE mu values + auto assign_ee_mu = [&](EdgeEdgeTangentialCollision& tc) { + const auto& [ea0i, ea1i, eb0i, eb1i] = tc.vertex_ids(edges, faces); + double ea_mu_s = + (mu_s(ea1i) - mu_s(ea0i)) * tc.closest_point[0] + mu_s(ea0i); + double eb_mu_s = + (mu_s(eb1i) - mu_s(eb0i)) * tc.closest_point[1] + mu_s(eb0i); + tc.mu_s = blend_mu(ea_mu_s, eb_mu_s); + double ea_mu_k = + (mu_k(ea1i) - mu_k(ea0i)) * tc.closest_point[0] + mu_k(ea0i); + double eb_mu_k = + (mu_k(eb1i) - mu_k(eb0i)) * tc.closest_point[1] + mu_k(eb0i); + tc.mu_k = blend_mu(ea_mu_k, eb_mu_k); + }; + + // Helper: assign FV mu values + auto assign_fv_mu = [&](FaceVertexTangentialCollision& tc) { + const auto& [vi, f0i, f1i, f2i] = tc.vertex_ids(edges, faces); + double face_mu_s = mu_s(f0i) + + tc.closest_point[0] * (mu_s(f1i) - mu_s(f0i)) + + tc.closest_point[1] * (mu_s(f2i) - mu_s(f0i)); + tc.mu_s = blend_mu(face_mu_s, mu_s(vi)); + double face_mu_k = mu_k(f0i) + + tc.closest_point[0] * (mu_k(f1i) - mu_k(f0i)) + + tc.closest_point[1] * (mu_k(f2i) - mu_k(f0i)); + tc.mu_k = blend_mu(face_mu_k, mu_k(vi)); + }; + + // --- Precompute per-face normalized outer scale --- + // ESP outer contribution per face f is: (area_f / 9) [* / total_w_f] + // depending on normalize_weights. total_w_f sums active EE mollifiers + // and either face quadrature weights (when active) or 3 (for the 3 + // face vertices, when face quadrature is not active). + const auto& face_quad_rule = params.quad_rule(); + const bool has_face_quad = !face_quad_rule.empty(); + double sum_face_qp_w = 0.0; + for (const auto& qp : face_quad_rule) { + sum_face_qp_w += qp.weight; + } + + // When face quadrature is active, vertices are already included + // in the quadrature rule, so don't count the 3 vertex contributions. + const double base_w = has_face_quad ? sum_face_qp_w : 3.0; + Eigen::VectorXd total_w_per_face = + Eigen::VectorXd::Constant(faces.rows(), base_w); + if (normalize_weights) { + // Add per-face sum of active EE mollifiers (EA_EB only). + for (const auto& [ei_pair, dict_ptr] : + collisions.maps().edge_edge_collisions) { + // The builder keeps only EA_EB pairs of edges that share no + // vertex. + assert(dict_ptr->ee_dtype() == EdgeEdgeDistanceType::EA_EB); + const index_t e0 = ei_pair.first; + const index_t e1 = ei_pair.second; + const index_t e00 = edges(e0, 0), e01 = edges(e0, 1); + const index_t e10 = edges(e1, 0), e11 = edges(e1, 1); + assert(e00 != e10 && e00 != e11 && e01 != e10 && e01 != e11); + const double dist_sqr = edge_edge_distance_parallel_safe( + vertices.row(e00), vertices.row(e01), vertices.row(e10), + vertices.row(e11), EdgeEdgeDistanceType::EA_EB); + const double dist = std::sqrt(dist_sqr); + const auto mtypes = edge_edge_mollifier_type( + vertices.row(e00).transpose(), + vertices.row(e01).transpose(), + vertices.row(e10).transpose(), + vertices.row(e11).transpose(), dist_sqr); + double mol = + Math::cubic_spline(dist / params.ee_support()) + * 1.5; + mol *= edge_edge_mollifier( + vertices.row(e00).transpose(), + vertices.row(e01).transpose(), + vertices.row(e10).transpose(), + vertices.row(e11).transpose(), mtypes, dist_sqr); + // Attribute to every face containing e0 (ESP loop iterates + // over faces and each face's 3 edges). + for (const index_t f : mesh.edges_to_faces()[e0]) { + total_w_per_face(f) += mol; + } + } + } + + // Per-face normalized scale: (area_f / 9) * [1/total_w_f if normalize] + Eigen::VectorXd face_scale(faces.rows()); + for (index_t f = 0; f < faces.rows(); f++) { + const double w_f = mesh.face_areas()(f) / 9.0; + face_scale(f) = + normalize_weights ? (w_f / total_w_per_face(f)) : w_f; + } + // Precompute per-vertex ESP outer weight = sum_{f ∋ v} face_scale(f). + Eigen::VectorXd v_outer_w = Eigen::VectorXd::Zero(n_verts); + for (index_t f = 0; f < faces.rows(); f++) { + for (int lv = 0; lv < 3; lv++) { + v_outer_w(faces(f, lv)) += face_scale(f); + } + } + + // ---- VERTEX dicts: all vertex IDs are real ---- + // Skip when face quadrature is active (non-empty rule), which + // already includes vertices, matching the normal potential's behavior. + if (!has_face_quad) { + for (const auto& [vi, dict_ptr] : + collisions.maps().vertex_collisions) { + VertexMatrixView<3> V_view(vertices); + const double v_w = v_outer_w(vi); + for (int j = 0; j < dict_ptr->size(); j++) { + const auto& cc = (*dict_ptr)[j]; + const double contact_force = + compute_contact_force(cc, V_view, v_w); + if (contact_force == 0) { + continue; + } + + switch (cc.type()) { + case ESPCollisionType::VERTEX_VERTEX: { + const index_t v0 = cc[0]; + const index_t v1 = cc[1]; + Vector6d cp; + cp.head<3>() = vertices.row(v0); + cp.tail<3>() = vertices.row(v1); + FC_vv.emplace_back( + VertexVertexNormalCollision( + v0, v1, cc.weight, + Eigen::SparseVector()), + cp, contact_force); + FC_vv.back().weight = cc.weight; + const auto& [v0i, v1i, _, __] = + FC_vv.back().vertex_ids(edges, faces); + FC_vv.back().mu_s = blend_mu(mu_s(v0i), mu_s(v1i)); + FC_vv.back().mu_k = blend_mu(mu_k(v0i), mu_k(v1i)); + break; + } + case ESPCollisionType::EDGE_VERTEX: { + const index_t edge_id = cc[0]; + const index_t vert_id = cc[1]; + const index_t ea0 = edges(edge_id, 0); + const index_t ea1 = edges(edge_id, 1); + Vector9d cp; + cp.segment<3>(0) = vertices.row(vert_id); + cp.segment<3>(3) = vertices.row(ea0); + cp.segment<3>(6) = vertices.row(ea1); + FC_ev.emplace_back( + EdgeVertexNormalCollision( + edge_id, vert_id, cc.weight, + Eigen::SparseVector()), + cp, contact_force); + FC_ev.back().weight = cc.weight; + assign_ev_mu(FC_ev.back()); + break; + } + case ESPCollisionType::FACE_VERTEX: { + const index_t face_id = cc[0]; + const index_t vert_id = cc[1]; + const index_t f0 = faces(face_id, 0); + const index_t f1 = faces(face_id, 1); + const index_t f2 = faces(face_id, 2); + Vector12d cp; + cp.segment<3>(0) = vertices.row(vert_id); + cp.segment<3>(3) = vertices.row(f0); + cp.segment<3>(6) = vertices.row(f1); + cp.segment<3>(9) = vertices.row(f2); + FC_fv.emplace_back( + FaceVertexNormalCollision( + face_id, vert_id, cc.weight, + Eigen::SparseVector()), + cp, contact_force); + FC_fv.back().weight = cc.weight; + assign_fv_mu(FC_fv.back()); + break; + } + default: + break; + } + } + } + } + + // Precompute per-edge ESP outer weight = sum_{f ∋ e} face_scale(f). + Eigen::VectorXd e_outer_w = Eigen::VectorXd::Zero(edges.rows()); + for (index_t f = 0; f < faces.rows(); f++) { + for (int le = 0; le < 3; le++) { + e_outer_w(mesh.faces_to_edges()(f, le)) += face_scale(f); + } + } + + // ---- EDGE dicts: virtual vertex at edge-edge closest point ---- + for (const auto& [ei_pair, dict_ptr] : + collisions.maps().edge_edge_collisions) { + const index_t e0 = ei_pair.first; + const index_t e1 = ei_pair.second; + const auto dtype = dict_ptr->ee_dtype(); + const index_t e00 = edges(e0, 0), e01 = edges(e0, 1); + const index_t e10 = edges(e1, 0), e11 = edges(e1, 1); + + // The builder keeps only EA_EB pairs. + assert(dtype == EdgeEdgeDistanceType::EA_EB); + + // Compute virtual vertex position on edge e0 + // (same logic as quadrature_potential.cpp) + const double closest_uv = closest_point_uv( + vertices.row(e00).transpose(), vertices.row(e01).transpose(), + vertices.row(e10).transpose(), vertices.row(e11).transpose()); + if (!std::isfinite(closest_uv)) { + continue; + } + + const Eigen::RowVector3d virtual_pos = + closest_uv * (vertices.row(e01) - vertices.row(e00)) + + vertices.row(e00); + VertexMatrixView<3> V_ext(vertices, virtual_pos); + + // ESP potential outer factor for an edge-edge dict (for each face + // f containing edge e0): area_f/9 * mollifier. Since the mollifier + // depends only on the four edge endpoints (not f), it factors out + // and the per-dict outer weight is mollifier * sum_{f∋e0} area_f/9. + const double dist_sqr_ee = edge_edge_distance_parallel_safe( + vertices.row(e00), vertices.row(e01), vertices.row(e10), + vertices.row(e11), dtype); + const double dist_ee = std::sqrt(dist_sqr_ee); + const auto mtypes = edge_edge_mollifier_type( + vertices.row(e00).transpose(), vertices.row(e01).transpose(), + vertices.row(e10).transpose(), vertices.row(e11).transpose(), + dist_sqr_ee); + double mollifier = + Math::cubic_spline(dist_ee / params.ee_support()) * 1.5; + mollifier *= edge_edge_mollifier( + vertices.row(e00).transpose(), vertices.row(e01).transpose(), + vertices.row(e10).transpose(), vertices.row(e11).transpose(), + mtypes, dist_sqr_ee); + const double edge_outer_w = mollifier * e_outer_w(e0); + if (edge_outer_w == 0) { + continue; + } + for (int j = 0; j < dict_ptr->size(); j++) { + const auto& cc = (*dict_ptr)[j]; + const double contact_force = + compute_contact_force(cc, V_ext, edge_outer_w); + if (contact_force == 0) { + continue; + } + + switch (cc.type()) { + case ESPCollisionType::VERTEX_VERTEX: { + // One vertex is virtual (n_verts), the other is real. + // Elevate to EdgeVertex: edge e0 contains the virtual + // vertex, paired with the real vertex. + const index_t v0 = cc[0]; + const index_t v1 = cc[1]; + const index_t v_real = (v0 == n_verts) ? v1 : v0; + + Vector9d cp; + // Order: [vertex, edge_v0, edge_v1] + cp.segment<3>(0) = vertices.row(v_real); + cp.segment<3>(3) = vertices.row(e00); + cp.segment<3>(6) = vertices.row(e01); + + FC_ev.emplace_back( + EdgeVertexNormalCollision( + e0, v_real, cc.weight, + Eigen::SparseVector()), + cp, contact_force); + FC_ev.back().weight = cc.weight; + assign_ev_mu(FC_ev.back()); + break; + } + case ESPCollisionType::EDGE_VERTEX: { + // Real edge (cc[0]) vs virtual vertex (cc[1]) on e0. + // Elevate to EdgeEdge: edge e0 vs the real edge. + const index_t other_e = cc[0]; + const index_t oe0 = edges(other_e, 0); + const index_t oe1 = edges(other_e, 1); + + const Eigen::Vector3d e00_pos = vertices.row(e00); + const Eigen::Vector3d e01_pos = vertices.row(e01); + const Eigen::Vector3d oe0_pos = vertices.row(oe0); + const Eigen::Vector3d oe1_pos = vertices.row(oe1); + + // Mollifier check: skip near-parallel edges + if (edge_edge_cross_squarednorm( + e00_pos, e01_pos, oe0_pos, oe1_pos) + < edge_edge_mollifier_threshold( + e00_pos, e01_pos, oe0_pos, oe1_pos)) { + continue; + } + + Vector12d cp; + // Order: [ea0, ea1, eb0, eb1] + cp.segment<3>(0) = e00_pos; + cp.segment<3>(3) = e01_pos; + cp.segment<3>(6) = oe0_pos; + cp.segment<3>(9) = oe1_pos; + + FC_ee.emplace_back( + EdgeEdgeNormalCollision( + e0, other_e, 0., EdgeEdgeDistanceType::EA_EB), + cp, contact_force); + FC_ee.back().weight = cc.weight; + assign_ee_mu(FC_ee.back()); + break; + } + case ESPCollisionType::FACE_VERTEX: { + // Real face (cc[0]) vs virtual vertex on edge e0. + // No EdgeFace tangential type exists; resolve via the + // point-triangle distance type and elevate to EV (when + // closest to a face vertex) or EE (when closest to a + // face edge). Interior cases are skipped. + const index_t fi = cc[0]; + const index_t fa = faces(fi, 0); + const index_t fb = faces(fi, 1); + const index_t fc = faces(fi, 2); + const Eigen::Vector3d fa_pos = vertices.row(fa); + const Eigen::Vector3d fb_pos = vertices.row(fb); + const Eigen::Vector3d fc_pos = vertices.row(fc); + const Eigen::Vector3d vp = virtual_pos.transpose(); + + const auto dt = point_triangle_distance_type( + vp, fa_pos, fb_pos, fc_pos); + + auto elevate_to_ev = [&](index_t v_face) { + Vector9d cp; + cp.segment<3>(0) = vertices.row(v_face); + cp.segment<3>(3) = vertices.row(e00); + cp.segment<3>(6) = vertices.row(e01); + FC_ev.emplace_back( + EdgeVertexNormalCollision( + e0, v_face, cc.weight, + Eigen::SparseVector()), + cp, contact_force); + FC_ev.back().weight = cc.weight; + assign_ev_mu(FC_ev.back()); + }; + auto elevate_to_ee = [&](int face_edge_local) { + const index_t face_edge_id = + mesh.faces_to_edges()(fi, face_edge_local); + const index_t fe0 = edges(face_edge_id, 0); + const index_t fe1 = edges(face_edge_id, 1); + const Eigen::Vector3d e00_pos = vertices.row(e00); + const Eigen::Vector3d e01_pos = vertices.row(e01); + const Eigen::Vector3d fe0_pos = vertices.row(fe0); + const Eigen::Vector3d fe1_pos = vertices.row(fe1); + // Mollifier check: skip near-parallel edges + if (edge_edge_cross_squarednorm( + e00_pos, e01_pos, fe0_pos, fe1_pos) + < edge_edge_mollifier_threshold( + e00_pos, e01_pos, fe0_pos, fe1_pos)) { + return; + } + Vector12d cp; + cp.segment<3>(0) = e00_pos; + cp.segment<3>(3) = e01_pos; + cp.segment<3>(6) = fe0_pos; + cp.segment<3>(9) = fe1_pos; + FC_ee.emplace_back( + EdgeEdgeNormalCollision( + e0, face_edge_id, 0., + EdgeEdgeDistanceType::EA_EB), + cp, contact_force); + FC_ee.back().weight = cc.weight; + assign_ee_mu(FC_ee.back()); + }; + + switch (dt) { + case PointTriangleDistanceType::P_T0: + elevate_to_ev(fa); + break; + case PointTriangleDistanceType::P_T1: + elevate_to_ev(fb); + break; + case PointTriangleDistanceType::P_T2: + elevate_to_ev(fc); + break; + case PointTriangleDistanceType::P_E0: + elevate_to_ee(0); + break; + case PointTriangleDistanceType::P_E1: + elevate_to_ee(1); + break; + case PointTriangleDistanceType::P_E2: + elevate_to_ee(2); + break; + default: + // P_T (interior): no elevation possible without an + // EdgeFace tangential type. Skip. + break; + } + break; + } + default: + break; + } + } + } + + // ---- FACE dicts: virtual vertex at face quadrature point ---- + for (const auto& face_collisions : collisions.maps().face_collisions) { + const index_t fi = face_collisions.first; + const auto& dicts = face_collisions.second; + const index_t f0 = faces(fi, 0); + const index_t f1 = faces(fi, 1); + const index_t f2 = faces(fi, 2); + // ESP potential outer face weight (optionally normalized). + const double face_w = face_scale(fi); + + for (size_t qi = 0; qi < dicts.size(); qi++) { + const auto& dict_ptr = dicts[qi]; + const auto& qp = face_quad_rule[qi]; + + // Compute virtual vertex position from barycentric coords + const Eigen::RowVector3d virtual_pos = + qp.lambda[0] * vertices.row(f0) + + qp.lambda[1] * vertices.row(f1) + + qp.lambda[2] * vertices.row(f2); + VertexMatrixView<3> V_ext(vertices, virtual_pos); + + // ESP potential per-quadrature factor: area/9 * qp.weight. + const double fq_outer_w = face_w * qp.weight; + + for (int j = 0; j < dict_ptr->size(); j++) { + const auto& cc = (*dict_ptr)[j]; + const double contact_force = + compute_contact_force(cc, V_ext, fq_outer_w); + if (contact_force == 0) { + continue; + } + + switch (cc.type()) { + case ESPCollisionType::VERTEX_VERTEX: { + // One vertex is virtual (on face fi), other is real. + // Elevate to FaceVertex: face fi paired with the + // real vertex. + const index_t v0 = cc[0]; + const index_t v1 = cc[1]; + const index_t v_real = (v0 == n_verts) ? v1 : v0; + + Vector12d cp; + // Order: [vertex, face_v0, face_v1, face_v2] + cp.segment<3>(0) = vertices.row(v_real); + cp.segment<3>(3) = vertices.row(f0); + cp.segment<3>(6) = vertices.row(f1); + cp.segment<3>(9) = vertices.row(f2); + + FC_fv.emplace_back( + FaceVertexNormalCollision( + fi, v_real, cc.weight, + Eigen::SparseVector()), + cp, contact_force); + FC_fv.back().weight = cc.weight; + assign_fv_mu(FC_fv.back()); + break; + } + case ESPCollisionType::EDGE_VERTEX: { + // Real edge (cc[0]) vs virtual vertex on face fi. + // Distribute the sub-collision's contact force to + // the two edge endpoints using the projection + // parameter u (clamped to [0, 1]). Each endpoint + // gets an FV(tangential) entry with face = fi and + // vertex = endpoint, weighted accordingly. This + // handles interior (P_E) as well as P_E0 / P_E1. + const index_t other_e = cc[0]; + const index_t oe0 = edges(other_e, 0); + const index_t oe1 = edges(other_e, 1); + const Eigen::Vector3d oe0_pos = vertices.row(oe0); + const Eigen::Vector3d oe1_pos = vertices.row(oe1); + const Eigen::Vector3d vp = virtual_pos.transpose(); + + double u = + point_edge_closest_point(vp, oe0_pos, oe1_pos); + if (!std::isfinite(u)) { + break; + } + u = std::clamp(u, 0.0, 1.0); + const double w0 = 1.0 - u; + const double w1 = u; + + auto emit_fv = [&](index_t v_edge, double w) { + if (w <= 0) { + return; + } + Vector12d cp; + cp.segment<3>(0) = vertices.row(v_edge); + cp.segment<3>(3) = vertices.row(f0); + cp.segment<3>(6) = vertices.row(f1); + cp.segment<3>(9) = vertices.row(f2); + FC_fv.emplace_back( + FaceVertexNormalCollision( + fi, v_edge, cc.weight, + Eigen::SparseVector()), + cp, w * contact_force); + FC_fv.back().weight = cc.weight; + assign_fv_mu(FC_fv.back()); + }; + emit_fv(oe0, w0); + emit_fv(oe1, w1); + break; + } + case ESPCollisionType::FACE_VERTEX: { + // Real face (cc[0]) vs virtual vertex on face fi. + // Distribute the sub-collision's contact force to + // the three cube-face vertices using barycentric + // coordinates of the projection of the virtual + // point onto the cube face. Each vertex gets an + // FV(tangential) entry with face = fi (plane face) + // and vertex = cube vertex, weighted by its + // barycentric coefficient. This handles interior + // (P_T) and edge cases as well as vertex cases — + // needed for flat-on-flat sliding. + const index_t fj = cc[0]; + const index_t ja = faces(fj, 0); + const index_t jb = faces(fj, 1); + const index_t jc = faces(fj, 2); + const Eigen::Vector3d ja_pos = vertices.row(ja); + const Eigen::Vector3d jb_pos = vertices.row(jb); + const Eigen::Vector3d jc_pos = vertices.row(jc); + const Eigen::Vector3d vp = virtual_pos.transpose(); + + Eigen::Vector2d bary = point_triangle_closest_point( + vp, ja_pos, jb_pos, jc_pos); + if (!bary.allFinite()) { + break; + } + double beta = std::clamp(bary(0), 0.0, 1.0); + double gamma = std::clamp(bary(1), 0.0, 1.0); + if (beta + gamma > 1.0) { + const double s = beta + gamma; + beta /= s; + gamma /= s; + } + const double alpha = 1.0 - beta - gamma; + + auto emit_fv = [&](index_t v_other, double w) { + if (w <= 0) { + return; + } + Vector12d cp; + cp.segment<3>(0) = vertices.row(v_other); + cp.segment<3>(3) = vertices.row(f0); + cp.segment<3>(6) = vertices.row(f1); + cp.segment<3>(9) = vertices.row(f2); + FC_fv.emplace_back( + FaceVertexNormalCollision( + fi, v_other, cc.weight, + Eigen::SparseVector()), + cp, w * contact_force); + FC_fv.back().weight = cc.weight; + assign_fv_mu(FC_fv.back()); + }; + emit_fv(ja, alpha); + emit_fv(jb, beta); + emit_fv(jc, gamma); + break; + } + default: + break; + } + } + } + } + } + + reset_lagged_anisotropic_friction_coefficients(); +} + +} // namespace ipc diff --git a/src/ipc/esp/inclusion_exclusion_weights.cpp b/src/ipc/esp/inclusion_exclusion_weights.cpp new file mode 100644 index 000000000..5aeeae08c --- /dev/null +++ b/src/ipc/esp/inclusion_exclusion_weights.cpp @@ -0,0 +1,24 @@ +#include "inclusion_exclusion_weights.hpp" + +namespace ipc { + +void compute_inclusion_exclusion_weights( + const CollisionMesh& mesh, + std::vector& edge_weights, + std::vector& vertex_weights) +{ + edge_weights.assign(mesh.num_edges(), 1); + vertex_weights.assign(mesh.num_vertices(), 1); + for (int f = 0; f < mesh.faces().rows(); f++) { + for (int j = 0; j < 3; j++) { + --edge_weights[mesh.faces_to_edges()(f, j)]; + ++vertex_weights[mesh.faces()(f, j)]; + } + } + for (int e = 0; e < mesh.edges().rows(); e++) { + --vertex_weights[mesh.edges()(e, 0)]; + --vertex_weights[mesh.edges()(e, 1)]; + } +} + +} // namespace ipc diff --git a/src/ipc/esp/inclusion_exclusion_weights.hpp b/src/ipc/esp/inclusion_exclusion_weights.hpp new file mode 100644 index 000000000..af77329e1 --- /dev/null +++ b/src/ipc/esp/inclusion_exclusion_weights.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include + +#include + +namespace ipc { + +/// @brief Compute the inclusion-exclusion weight of every edge and vertex of +/// a mesh (every face has weight one). +/// +/// We choose the weights so that the weights of the elements containing any +/// point of the mesh sum to one (supplemental S2). Faces take one, so an edge +/// takes 1 - (faces on it) and a vertex takes 1 - (edges at it) + (faces at +/// it), since each face at a vertex has two of its edges there. A closed mesh +/// gets the alternating signs (3D: faces +1, edges -1, vertices +1; 2D: edges +/// +1, vertices -1). A boundary edge or vertex, and an open polyline's end, +/// gets zero (S4). An edge in no face and an isolated vertex get one. +/// +/// Using the closed-mesh signs everywhere instead is wrong on any other +/// input: beyond a straight boundary edge of an open sheet the face and edge +/// terms cancel, so the potential is exactly zero where it should be b(d), and +/// a free edge or isolated vertex gets a negative weight, so its potential is +/// attractive and unbounded below. +/// +/// @param mesh The collision mesh. +/// @param[out] edge_weights Weight of each edge (size mesh.num_edges()). +/// @param[out] vertex_weights Weight of each vertex (size mesh.num_vertices()). +void compute_inclusion_exclusion_weights( + const CollisionMesh& mesh, + std::vector& edge_weights, + std::vector& vertex_weights); + +} // namespace ipc diff --git a/src/ipc/esp/quadrature_potential.cpp b/src/ipc/esp/quadrature_potential.cpp new file mode 100644 index 000000000..b4ca6c1d2 --- /dev/null +++ b/src/ipc/esp/quadrature_potential.cpp @@ -0,0 +1,1470 @@ +#include "quadrature_potential.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace ipc { +namespace { + template + void + insert_pair(unordered_map& map, ValueType&& collision) + { + if (auto iter = map.find(collision->get_typed_hash()); + iter != map.end()) { + iter->second->weight += collision->weight; + if (iter->second->weight == 0) { + map.erase(iter); + } + } else { + map[collision->get_typed_hash()] = std::move(collision); + } + } + +} // namespace + +std::unique_ptr> +PointPotential::build_collisions_at_vertex( + Eigen::ConstRef V, + const index_t vid, + size_t& num_collision_pairs) const +{ + ESPCollisionPairMap pairs; + num_collision_pairs = 0; + + const auto& v_set = candidates.vv_set(mesh, vid); + const auto& e_set = candidates.ve_set(vid); + const auto& f_set = candidates.vf_set(vid); + + const VertexMatrixView<3> V_view(V); + + // BRUTE_FORCE ignores the obstacle flags, so we test it first and never + // query them. + const bool filter_obstacles = + params.integration_type != ESPParameters::IntegrationType::BRUTE_FORCE + && mesh.is_obstacle_vertex(vid); + + for (const auto& other_f : f_set) { + if (filter_obstacles && mesh.is_obstacle_face(other_f)) { + continue; + } + ++num_collision_pairs; + if (std::shared_ptr pair = + ESPCollisionsBuilder<3>::reduce_point_triangle_collision( + FaceVertexCandidate(other_f, vid), params, mesh, V_view)) { + insert_pair(pairs, std::move(pair)); + } + } + + // Every collision takes the inclusion-exclusion weight of the element it + // came from (faces have weight one), even after being reduced to a + // sub-feature. An element of weight zero (a boundary edge or vertex) + // contributes nothing. + for (const auto& other_e : e_set) { + if (filter_obstacles && mesh.is_obstacle_edge(other_e)) { + continue; + } + if (edge_weights[other_e] == 0) { + continue; + } + ++num_collision_pairs; + if (std::shared_ptr pair = + ESPCollisionsBuilder<3>::reduce_point_edge_collision( + EdgeVertexCandidate(other_e, vid), params, mesh, V_view)) { + pair->weight = edge_weights[other_e]; + insert_pair(pairs, std::move(pair)); + } + } + + for (const auto& other_v : v_set) { + if (filter_obstacles && mesh.is_obstacle_vertex(other_v)) { + continue; + } + if (vertex_weights[other_v] == 0 + || (V.row(vid) - V.row(other_v)).squaredNorm() + >= params.dhat * params.dhat) { + continue; + } + std::shared_ptr pair = + std::make_shared>( + vid, other_v, mesh); + pair->weight = vertex_weights[other_v]; + ++num_collision_pairs; + insert_pair(pairs, std::move(pair)); + } + std::unique_ptr> collisions = + std::make_unique>(); + collisions->initialize( + std::vector { vid }, std::vector { vid }, pairs, + mesh.num_vertices()); + return collisions; +} + +double +PointPotentialHelper::evaluate_potential_at_vertex_with_cached_collisions( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params) +{ + double potential = 0; + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + potential += cc.weight * cc(cc.dof(V), params); + } + + return potential; +} + +Eigen::VectorXd PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params) +{ + Eigen::VectorXd grad = + Eigen::VectorXd::Zero(collisions.vertex_ids().size() * 3); + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + Eigen::VectorXd g = cc.weight * cc.gradient(cc.dof(V), params); + assert(g.size() == cc.num_vertices() * 3); + for (index_t j = 0; j < cc.num_vertices(); j++) { + grad.segment<3>( + 3 * collisions.vertex_ids_inverse(cc.vertex_id(j))) += + g.segment<3>(3 * j); + } + } + + return grad; +} + +Eigen::MatrixXd PointPotentialHelper:: + evaluate_potential_hessian_at_vertex_with_cached_collisions( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + PSDProjectionMethod project_to_psd) +{ + Eigen::MatrixXd H = Eigen::MatrixXd::Zero( + collisions.vertex_ids().size() * 3, collisions.vertex_ids().size() * 3); + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + Eigen::MatrixXd h = cc.hessian(cc.dof(V), params); + // The following code can be used only if all weights are positive + // if (project_to_psd != PSDProjectionMethod::NONE) { + // h = ipc::project_to_psd(h, project_to_psd); + // } + h *= cc.weight; + + assert(h.rows() == cc.num_vertices() * 3); + assert(h.cols() == cc.num_vertices() * 3); + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t li = collisions.vertex_ids_inverse(cc.vertex_id(i)); + for (index_t j = 0; j < cc.num_vertices(); j++) { + const index_t lj = + collisions.vertex_ids_inverse(cc.vertex_id(j)); + H.block<3, 3>(3 * li, 3 * lj) += h.block<3, 3>(3 * i, 3 * j); + } + } + } + + if (project_to_psd != PSDProjectionMethod::NONE) { + H = ipc::project_to_psd(H, project_to_psd); + } + return H; +} + +std::unique_ptr> +PointPotential::build_collisions_at_edge_edge_closest_point( + Eigen::ConstRef V, + const index_t e0, + const index_t e1, + EdgeEdgeDistanceType dtype, + size_t& num_collision_pairs) const +{ + const auto& v_set = candidates.ev_set(mesh, e0); + const auto& e_set = candidates.ee_set(mesh, e0); + const auto& f_set = candidates.ef_set(mesh, e0); + + // Compute closest point + const index_t e00 = mesh.edges()(e0, 0); + const index_t e01 = mesh.edges()(e0, 1); + const index_t e10 = mesh.edges()(e1, 0); + const index_t e11 = mesh.edges()(e1, 1); + +#ifndef NDEBUG + if (dtype != EdgeEdgeDistanceType::EA_EB + && dtype != EdgeEdgeDistanceType::EA_EB0 + && dtype != EdgeEdgeDistanceType::EA_EB1) { + log_and_throw_error("Can only handle EA_EB* distance type!"); + } +#endif + + ESPCollisionPairMap pairs; + num_collision_pairs = 0; + + if (edge_edge_distance_parallel_safe( + V.row(e00), V.row(e01), V.row(e10), V.row(e11), dtype) + < params.dhat * params.dhat) { + double closest_uv = 0; + if (dtype == EdgeEdgeDistanceType::EA_EB) { + // We clamp to the edge, as ESPPotential does when it evaluates + // this pair, because for nearly parallel edges the unclamped + // solve can land a hair past an endpoint. + closest_uv = closest_point_uv( + V.row(e00), V.row(e01), V.row(e10), V.row(e11)); + } else if (dtype == EdgeEdgeDistanceType::EA_EB0) { + Eigen::RowVector3d p = V.row(e10); + Eigen::RowVector3d d = p - V.row(e00); + Eigen::RowVector3d t = V.row(e01) - V.row(e00); + closest_uv = d.dot(t) / t.squaredNorm(); + } else if (dtype == EdgeEdgeDistanceType::EA_EB1) { + Eigen::RowVector3d p = V.row(e11); + Eigen::RowVector3d d = p - V.row(e00); + Eigen::RowVector3d t = V.row(e01) - V.row(e00); + closest_uv = d.dot(t) / t.squaredNorm(); + } else { + log_and_throw_error("Invalid dtype!"); + } + + if (!std::isfinite(closest_uv)) { + log_and_throw_error("Potentially parallel edges!"); + } + + const index_t vid = V.rows(); // virtual vertex + + // Eigen::MatrixXd V_view(V.rows() + 1, 3); + // V_view.topRows(V.rows()) = V; + // V_view.row(vid) = closest_uv * (V.row(e01) - V.row(e00)) + + // V.row(e00); + const Eigen::RowVector3d ee_closest_point = + closest_uv * (V.row(e01) - V.row(e00)) + V.row(e00); + VertexMatrixView<3> V_view(V, ee_closest_point); + + const bool filter_obstacles_e = params.integration_type + != ESPParameters::IntegrationType::BRUTE_FORCE + && mesh.is_obstacle_edge(e0); + + // As in build_collisions_at_vertex(), every collision takes the + // inclusion-exclusion weight of the element it came from. + for (const auto& other_v : v_set) { + if (filter_obstacles_e && mesh.is_obstacle_vertex(other_v)) { + continue; + } + if (vertex_weights[other_v] == 0 + || (V_view(vid) - V_view(other_v)).squaredNorm() + >= params.dhat * params.dhat) { + continue; + } + auto pair = + std::make_shared>( + vid, other_v, mesh); + pair->weight = vertex_weights[other_v]; + ++num_collision_pairs; + insert_pair(pairs, std::shared_ptr(pair)); + } + + // ee_set and ef_set exclude e0 and the faces containing it. + for (const auto& other_e : e_set) { + if (filter_obstacles_e && mesh.is_obstacle_edge(other_e)) { + continue; + } + const int edge_weight = edge_weights[other_e]; + if (edge_weight == 0) { + continue; + } + + auto dtype2 = point_edge_distance_type_exact( + V_view(vid), V_view(mesh.edges()(other_e, 0)), + V_view(mesh.edges()(other_e, 1))); + + const double dist_sqr = point_edge_distance( + V_view(vid), V_view(mesh.edges()(other_e, 0)), + V_view(mesh.edges()(other_e, 1)), dtype2); + + if (dist_sqr >= params.dhat * params.dhat) { + continue; + } + + switch (dtype2) { + case PointEdgeDistanceType::P_E0: { + auto pair = + std::make_shared>( + vid, mesh.edges()(other_e, 0), mesh); + ++num_collision_pairs; + pair->weight = edge_weight; + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + case PointEdgeDistanceType::P_E1: { + auto pair = + std::make_shared>( + vid, mesh.edges()(other_e, 1), mesh); + ++num_collision_pairs; + pair->weight = edge_weight; + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + case PointEdgeDistanceType::P_E: { + auto pair = + std::make_shared>( + other_e, vid, mesh); + ++num_collision_pairs; + pair->weight = edge_weight; + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + default: + assert(false); + break; + } + } + + for (const auto& other_f : f_set) { + if (filter_obstacles_e && mesh.is_obstacle_face(other_f)) { + continue; + } + + auto dtype2 = point_triangle_distance_type_exact( + V_view(vid), V_view(mesh.faces()(other_f, 0)), + V_view(mesh.faces()(other_f, 1)), + V_view(mesh.faces()(other_f, 2))); + + const double dist_sqr = point_triangle_distance( + V_view(vid), V_view(mesh.faces()(other_f, 0)), + V_view(mesh.faces()(other_f, 1)), + V_view(mesh.faces()(other_f, 2)), dtype2); + + if (dist_sqr >= params.dhat * params.dhat) { + continue; + } + + switch (dtype2) { + case PointTriangleDistanceType::P_T0: { + ++num_collision_pairs; + auto pair = + std::make_shared>( + vid, mesh.faces()(other_f, 0), mesh); + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + case PointTriangleDistanceType::P_T1: { + ++num_collision_pairs; + auto pair = + std::make_shared>( + vid, mesh.faces()(other_f, 1), mesh); + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + case PointTriangleDistanceType::P_T2: { + ++num_collision_pairs; + auto pair = + std::make_shared>( + vid, mesh.faces()(other_f, 2), mesh); + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + case PointTriangleDistanceType::P_E0: { + ++num_collision_pairs; + auto pair = + std::make_shared>( + mesh.faces_to_edges()(other_f, 0), vid, mesh); + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + case PointTriangleDistanceType::P_E1: { + ++num_collision_pairs; + auto pair = + std::make_shared>( + mesh.faces_to_edges()(other_f, 1), vid, mesh); + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + case PointTriangleDistanceType::P_E2: { + ++num_collision_pairs; + auto pair = + std::make_shared>( + mesh.faces_to_edges()(other_f, 2), vid, mesh); + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + case PointTriangleDistanceType::P_T: { + ++num_collision_pairs; + auto pair = + std::make_shared>( + other_f, vid, mesh); + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + default: + assert(false); + break; + } + } + } + + std::unique_ptr> collisions = + std::make_unique>(); + collisions->initialize( + std::vector { e0, e1 }, std::vector { e00, e01, e10, e11 }, + pairs, mesh.num_vertices()); + collisions->set_ee_dtype(dtype); + return collisions; +} + +double PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + EdgeEdgeDistanceType dtype) +{ + double potential = 0; + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + double term = cc(cc.dof(V_extended), params); + assert(std::isfinite(term)); + potential += cc.weight * term; + } + + return potential; +} + +template +std::enable_if_t< + IsADGrad::value || IsADHessian::value, + Eigen::VectorXd> +PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef> q) +{ + const index_t n_real_vertices = V_extended.rows() - 1; + Eigen::VectorXd grad = + Eigen::VectorXd::Zero(collisions.vertex_ids().size() * 3); + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + Eigen::VectorXd g = cc.weight * cc.gradient(cc.dof(V_extended), params); + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t global_id = cc.vertex_id(i); + if (global_id == n_real_vertices) { + const Vector12d local_grad = + (q(0) * g(3 * i + 0) + q(1) * g(3 * i + 1) + + q(2) * g(3 * i + 2)) + .grad; + // distribute grad wrt virtual vertex to real edge vertices + for (index_t lv = 0; lv < 4; lv++) { + grad.segment<3>(3 * collisions.primary_local_ids()[lv]) += + local_grad.segment<3>(lv * 3); + } + } else { + assert(global_id < n_real_vertices); + grad.segment<3>(3 * collisions.vertex_ids_inverse(global_id)) += + g.segment<3>(i * 3); + } + } + } + + return grad; +} + +template Eigen::VectorXd PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions< + ADGrad<12>>( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef>> q); + +template Eigen::VectorXd PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions< + ADHessian<12>>( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef>> q); + +Eigen::MatrixXd PointPotentialHelper:: + evaluate_potential_hessian_at_edge_edge_closest_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef>> q) +{ + const index_t n_real_vertices = V_extended.rows() - 1; + Eigen::MatrixXd H = Eigen::MatrixXd::Zero( + collisions.vertex_ids().size() * 3, collisions.vertex_ids().size() * 3); + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + const Eigen::VectorXd cc_dof = cc.dof(V_extended); + Eigen::MatrixXd h = cc.weight * cc.hessian(cc_dof, params); + Eigen::VectorXd g = cc.weight * cc.gradient(cc_dof, params); + + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t gi = cc.vertex_id(i); + for (index_t j = 0; j < cc.num_vertices(); j++) { + const index_t gj = cc.vertex_id(j); + if (gi == n_real_vertices && gj == n_real_vertices) { + assert(i == j); + // distribute derivatives wrt virtual vertex to real edge + // vertices + Matrix12d local_hess; + { + Eigen::Matrix tmp_g; + tmp_g << q(0).grad.transpose(), q(1).grad.transpose(), + q(2).grad.transpose(); + local_hess = tmp_g.transpose() + * h.block<3, 3>(3 * i, 3 * j) * tmp_g; + + for (int d = 0; d < 3; d++) { + local_hess += q(d).Hess * g(3 * i + d); + } + } + + for (index_t li = 0; li < 4; li++) { + for (index_t lj = 0; lj < 4; lj++) { + H.block<3, 3>( + collisions.primary_local_ids()[li] * 3, + collisions.primary_local_ids()[lj] * 3) += + local_hess.block<3, 3>(3 * li, 3 * lj); + } + } + } else if (gi == n_real_vertices) { + Eigen::Matrix local_hess; + { + Eigen::Matrix tmp_g; + tmp_g << q(0).grad.transpose(), q(1).grad.transpose(), + q(2).grad.transpose(); + local_hess = + tmp_g.transpose() * h.block<3, 3>(3 * i, 3 * j); + } + for (index_t li = 0; li < 4; li++) { + const index_t lli = collisions.primary_local_ids()[li]; + H.block<3, 3>( + lli * 3, collisions.vertex_ids_inverse(gj) * 3) += + local_hess.block<3, 3>(3 * li, 0); + H.block<3, 3>( + collisions.vertex_ids_inverse(gj) * 3, lli * 3) += + local_hess.block<3, 3>(3 * li, 0).transpose(); + } + } else if (gj == n_real_vertices) { + // Already handled in (gi == n_real_vertices) case + } else { + assert(gi < n_real_vertices); + assert(gj < n_real_vertices); + H.block<3, 3>( + 3 * collisions.vertex_ids_inverse(gi), + 3 * collisions.vertex_ids_inverse(gj)) += + h.block<3, 3>(3 * i, 3 * j); + } + } + } + } + + return H; +} + +std::unique_ptr> +PointPotential::build_collisions_at_face_interior_point( + Eigen::ConstRef V, + const index_t fid, + const std::array& lambda, + size_t& num_collision_pairs) const +{ + const index_t vid = V.rows(); + + const Eigen::RowVector3d q_pos = lambda[0] * V.row(mesh.faces()(fid, 0)) + + lambda[1] * V.row(mesh.faces()(fid, 1)) + + lambda[2] * V.row(mesh.faces()(fid, 2)); + VertexMatrixView<3> V_view(V, q_pos); + + ESPCollisionPairMap pairs; + num_collision_pairs = 0; + + const auto& v_set = candidates.fv_set(mesh, fid); + const auto& e_set = candidates.fe_set(mesh, fid); + const auto& f_set = candidates.ff_set(mesh, fid); + + // Detect boundary quadrature points (for rules that include boundary + // points). lambda[k] == 0.0 means the point lies on the edge opposite + // vertex k, i.e. edge faces_to_edges(fid, (k+1)%3). + const bool lam_zero[3] = { lambda[0] == 0.0, lambda[1] == 0.0, + lambda[2] == 0.0 }; + const int num_zero = lam_zero[0] + lam_zero[1] + lam_zero[2]; + + index_t skip_edge_id = -1; // edge the quad point lies on (1 null lambda) + index_t corner_vertex = + -1; // vertex the quad point coincides with (2 null lambdas) + + if (num_zero == 1) { + for (int k = 0; k < 3; k++) { + if (lam_zero[k]) { + skip_edge_id = mesh.faces_to_edges()(fid, (k + 1) % 3); + break; + } + } + } else if (num_zero == 2) { + for (int k = 0; k < 3; k++) { + if (!lam_zero[k]) { + corner_vertex = mesh.faces()(fid, k); + break; + } + } + } + + const bool filter_obstacles_f = + params.integration_type != ESPParameters::IntegrationType::BRUTE_FORCE + && mesh.is_obstacle_face(fid); + + for (const auto& other_f : f_set) { + assert(other_f != fid); + if (filter_obstacles_f && mesh.is_obstacle_face(other_f)) { + continue; + } + if (skip_edge_id >= 0) { + bool shares_edge = false; + for (int j = 0; j < 3; j++) { + if (mesh.faces_to_edges()(other_f, j) == skip_edge_id) { + shares_edge = true; + break; + } + } + if (shares_edge) { + continue; + } + } + if (corner_vertex >= 0) { + bool has_vertex = false; + for (int j = 0; j < 3; j++) { + if (mesh.faces()(other_f, j) == corner_vertex) { + has_vertex = true; + break; + } + } + if (has_vertex) { + continue; + } + } + ++num_collision_pairs; + if (auto pair = + ESPCollisionsBuilder<3>::reduce_point_triangle_collision( + FaceVertexCandidate(other_f, vid), params, mesh, V_view)) { + insert_pair(pairs, std::shared_ptr(pair)); + } + } + + for (const auto& other_e : e_set) { + if (filter_obstacles_f && mesh.is_obstacle_edge(other_e)) { + continue; + } + if (other_e == skip_edge_id) { + continue; + } + if (corner_vertex >= 0 + && (mesh.edges()(other_e, 0) == corner_vertex + || mesh.edges()(other_e, 1) == corner_vertex)) { + continue; + } + if (edge_weights[other_e] == 0) { + continue; + } + ++num_collision_pairs; + if (auto pair = ESPCollisionsBuilder<3>::reduce_point_edge_collision( + EdgeVertexCandidate(other_e, vid), params, mesh, V_view)) { + pair->weight = edge_weights[other_e]; + insert_pair(pairs, std::shared_ptr(pair)); + } + } + + for (const auto& other_v : v_set) { + if (filter_obstacles_f && mesh.is_obstacle_vertex(other_v)) { + continue; + } + if (other_v == corner_vertex || vertex_weights[other_v] == 0) { + continue; + } + if ((V_view(vid) - V_view(other_v)).squaredNorm() + >= params.dhat * params.dhat) { + continue; + } + std::shared_ptr pair = + std::make_shared>( + vid, other_v, mesh); + pair->weight = vertex_weights[other_v]; + ++num_collision_pairs; + insert_pair(pairs, std::move(pair)); + } + std::unique_ptr> collisions = + std::make_unique>(); + collisions->initialize( + std::vector { fid }, + std::vector { mesh.faces()(fid, 0), mesh.faces()(fid, 1), + mesh.faces()(fid, 2) }, + pairs, mesh.num_vertices()); + return collisions; +} + +double PointPotentialHelper:: + evaluate_potential_at_face_interior_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params) +{ + double potential = 0; + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + potential += cc.weight * cc(cc.dof(V_extended), params); + } + + return potential; +} + +// ------------------------------------------------------------------------- +// General face-interior point (arbitrary barycentric coordinates λ) +// These replace the hard-coded 1/3 (centroid) chain-rule factor with λk. +// For λ = (1/3, 1/3, 1/3) the results are identical to the face-centre +// variants above. +// ------------------------------------------------------------------------- + +Eigen::VectorXd PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda) +{ + const index_t n_real_vertices = V_extended.rows() - 1; + Eigen::VectorXd grad = + Eigen::VectorXd::Zero(collisions.vertex_ids().size() * 3); + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + Eigen::VectorXd g = cc.weight * cc.gradient(cc.dof(V_extended), params); + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t global_id = cc.vertex_id(i); + if (global_id == n_real_vertices) { + // Chain rule: dq/dv_k = lambda[k] + for (index_t lv = 0; lv < 3; lv++) { + grad.segment<3>(collisions.primary_local_ids()[lv] * 3) += + g.segment<3>(3 * i) * lambda[lv]; + } + } else { + assert(global_id < n_real_vertices); + grad.segment<3>(3 * collisions.vertex_ids_inverse(global_id)) += + g.segment<3>(3 * i); + } + } + } + return grad; +} + +Eigen::MatrixXd PointPotentialHelper:: + evaluate_potential_hessian_at_face_interior_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda, + PSDProjectionMethod project_to_psd) +{ + const index_t n_real_vertices = V_extended.rows() - 1; + Eigen::MatrixXd H = Eigen::MatrixXd::Zero( + collisions.vertex_ids().size() * 3, collisions.vertex_ids().size() * 3); + + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + Eigen::MatrixXd h = cc.hessian(cc.dof(V_extended), params); + h *= cc.weight; + + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t gi = cc.vertex_id(i); + for (index_t j = 0; j < cc.num_vertices(); j++) { + const index_t gj = cc.vertex_id(j); + if (gi == n_real_vertices && gj == n_real_vertices) { + // Both indices refer to the virtual vertex q. + // d²P / (dv_li dv_lj) = λ_li · λ_lj · d²P/dq² + for (index_t li = 0; li < 3; li++) { + for (index_t lj = 0; lj < 3; lj++) { + H.block<3, 3>( + collisions.primary_local_ids()[li] * 3, + collisions.primary_local_ids()[lj] * 3) += + h.block<3, 3>(3 * i, 3 * j) * lambda[li] + * lambda[lj]; + } + } + } else if (gi == n_real_vertices) { + // d²P / (dv_li d(other)) = λ_li · d²P / (dq d(other)) + for (index_t li = 0; li < 3; li++) { + H.block<3, 3>( + collisions.primary_local_ids()[li] * 3, + collisions.vertex_ids_inverse(gj) * 3) += + h.block<3, 3>(3 * i, 3 * j) * lambda[li]; + } + } else if (gj == n_real_vertices) { + // Symmetric to the gi == n_real_vertices case. + for (index_t lj = 0; lj < 3; lj++) { + H.block<3, 3>( + collisions.vertex_ids_inverse(gi) * 3, + collisions.primary_local_ids()[lj] * 3) += + h.block<3, 3>(3 * i, 3 * j) * lambda[lj]; + } + } else { + assert(gi < n_real_vertices); + assert(gj < n_real_vertices); + H.block<3, 3>( + 3 * collisions.vertex_ids_inverse(gi), + 3 * collisions.vertex_ids_inverse(gj)) += + h.block<3, 3>(3 * i, 3 * j); + } + } + } + } + + if (project_to_psd != PSDProjectionMethod::NONE) { + H = ipc::project_to_psd(H, project_to_psd); + } + return H; +} + +// ========================================================================= +// 2D edge quadrature point — collision building +// ========================================================================= + +std::unique_ptr> +PointPotential::build_collisions_at_edge_qp( + Eigen::ConstRef V, + const index_t ei, + const std::array& lambda, + const double dhat, + size_t& num_collision_pairs) const +{ + const index_t e0 = mesh.edges()(ei, 0); + const index_t e1 = mesh.edges()(ei, 1); + const index_t virtual_vid = static_cast(V.rows()); + + const Eigen::RowVector2d q_pos = + lambda[0] * V.row(e0) + lambda[1] * V.row(e1); + VertexMatrixView<2> V_view(V, q_pos); + + // If lambda[k] == 0 the QP coincides with the opposite endpoint. + // Parallel to the 3D corner_vertex exclusion. + index_t corner_vertex = -1; + if (lambda[0] == 0.0) { + corner_vertex = e1; + } else if (lambda[1] == 0.0) { + corner_vertex = e0; + } + + const bool filter_obstacles = + params.integration_type != ESPParameters::IntegrationType::BRUTE_FORCE + && mesh.is_obstacle_edge(ei); + + ESPCollisionPairMap pairs; + num_collision_pairs = 0; + const double dhat2 = dhat * dhat; + + // Every collision takes the inclusion-exclusion weight of the element it + // came from: in 2D, edges have weight one and a vertex 1 - (edges at it), + // so -1 on a closed curve, 0 at an open polyline's end, and +1 for an + // isolated vertex. + + // VV pairs: for each nearby vertex within dhat of the QP. + for (const index_t vj : candidates.ev_set(mesh, ei)) { + if (vj == corner_vertex || vertex_weights[vj] == 0) { + continue; + } + if (filter_obstacles && mesh.is_obstacle_vertex(vj)) { + continue; + } + if (point_point_distance(q_pos, V.row(vj)) >= dhat2) { + continue; + } + ++num_collision_pairs; + + std::shared_ptr vv_pair = + std::make_shared>( + virtual_vid, vj, mesh); + vv_pair->weight = vertex_weights[vj]; + insert_pair(pairs, std::move(vv_pair)); + } + + // EV pairs: iterate ee_set directly. + std::unordered_set processed_edges; + processed_edges.insert(ei); + for (const index_t ej : candidates.ee_set(mesh, ei)) { + // if (processed_edges.count(ej)) continue; + processed_edges.insert(ej); + const index_t ea = mesh.edges()(ej, 0); + const index_t eb = mesh.edges()(ej, 1); + if (corner_vertex >= 0 + && (ea == corner_vertex || eb == corner_vertex)) { + continue; + } + if (filter_obstacles && mesh.is_obstacle_edge(ej)) { + continue; + } + const auto dtype = + point_edge_distance_type_exact(q_pos, V.row(ea), V.row(eb)); + if (dtype == PointEdgeDistanceType::P_E0) { + if (point_point_distance(q_pos, V.row(ea)) >= dhat2) { + continue; + } + ++num_collision_pairs; + insert_pair( + pairs, + std::shared_ptr( + std::make_shared>( + virtual_vid, ea, mesh))); + } else if (dtype == PointEdgeDistanceType::P_E1) { + if (point_point_distance(q_pos, V.row(eb)) >= dhat2) { + continue; + } + ++num_collision_pairs; + insert_pair( + pairs, + std::shared_ptr( + std::make_shared>( + virtual_vid, eb, mesh))); + } else { + if (point_edge_distance(q_pos, V.row(ea), V.row(eb), dtype) + >= dhat2) { + continue; + } + ++num_collision_pairs; + insert_pair( + pairs, + std::shared_ptr( + std::make_shared>( + virtual_vid, ej, mesh))); + } + } + + auto dict = std::make_unique>(); + dict->initialize({ ei }, { e0, e1 }, pairs, mesh.num_vertices()); + return dict; +} + +// ========================================================================= +// 2D edge quadrature point — potential evaluation +// ========================================================================= + +double PointPotentialHelper::evaluate_potential_at_edge_qp( + VertexMatrixView<2> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params) +{ + double potential = 0; + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + potential += cc.weight * cc(cc.dof(V_extended), params); + } + return potential; +} + +Eigen::VectorXd PointPotentialHelper::evaluate_potential_gradient_at_edge_qp( + VertexMatrixView<2> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda) +{ + const index_t n_real_vertices = V_extended.rows() - 1; + Eigen::VectorXd grad = + Eigen::VectorXd::Zero(collisions.vertex_ids().size() * 2); + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + Eigen::VectorXd g = cc.weight * cc.gradient(cc.dof(V_extended), params); + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t global_id = cc.vertex_id(i); + if (global_id == n_real_vertices) { + // Chain rule: dq/dv_k = lambda[k] + for (index_t lv = 0; lv < 2; lv++) { + grad.segment<2>(collisions.primary_local_ids()[lv] * 2) += + g.segment<2>(2 * i) * lambda[lv]; + } + } else { + assert(global_id < n_real_vertices); + grad.segment<2>(2 * collisions.vertex_ids_inverse(global_id)) += + g.segment<2>(2 * i); + } + } + } + return grad; +} + +Eigen::MatrixXd PointPotentialHelper::evaluate_potential_hessian_at_edge_qp( + VertexMatrixView<2> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda, + PSDProjectionMethod project_to_psd) +{ + const index_t n_real_vertices = V_extended.rows() - 1; + Eigen::MatrixXd H = Eigen::MatrixXd::Zero( + collisions.vertex_ids().size() * 2, collisions.vertex_ids().size() * 2); + + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + Eigen::MatrixXd h = cc.hessian(cc.dof(V_extended), params); + h *= cc.weight; + + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t gi = cc.vertex_id(i); + for (index_t j = 0; j < cc.num_vertices(); j++) { + const index_t gj = cc.vertex_id(j); + if (gi == n_real_vertices && gj == n_real_vertices) { + for (index_t li = 0; li < 2; li++) { + for (index_t lj = 0; lj < 2; lj++) { + H.block<2, 2>( + collisions.primary_local_ids()[li] * 2, + collisions.primary_local_ids()[lj] * 2) += + h.block<2, 2>(2 * i, 2 * j) * lambda[li] + * lambda[lj]; + } + } + } else if (gi == n_real_vertices) { + for (index_t li = 0; li < 2; li++) { + H.block<2, 2>( + collisions.primary_local_ids()[li] * 2, + collisions.vertex_ids_inverse(gj) * 2) += + h.block<2, 2>(2 * i, 2 * j) * lambda[li]; + } + } else if (gj == n_real_vertices) { + for (index_t lj = 0; lj < 2; lj++) { + H.block<2, 2>( + collisions.vertex_ids_inverse(gi) * 2, + collisions.primary_local_ids()[lj] * 2) += + h.block<2, 2>(2 * i, 2 * j) * lambda[lj]; + } + } else { + assert(gi < n_real_vertices); + assert(gj < n_real_vertices); + H.block<2, 2>( + 2 * collisions.vertex_ids_inverse(gi), + 2 * collisions.vertex_ids_inverse(gj)) += + h.block<2, 2>(2 * i, 2 * j); + } + } + } + } + + if (project_to_psd != PSDProjectionMethod::NONE) { + H = ipc::project_to_psd(H, project_to_psd); + } + return H; +} + +// ========================================================================= +// NearFarBarrier evaluation functions (3D) +// ========================================================================= + +std::pair PointPotentialHelper:: + evaluate_potential_at_vertex_with_cached_collisions_nearfar( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier<>& nf_barrier) +{ + double near_sum = 0, far_sum = 0; + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + auto [n, f] = cc.operator_nearfar(cc.dof(V), params, &nf_barrier); + near_sum += cc.weight * n; + far_sum += cc.weight * f; + } + return { near_sum, far_sum }; +} + +std::pair PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions_nearfar( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier<>& nf_barrier) +{ + const int n_vertices = collisions.vertex_ids().size(); + const int n_dofs = n_vertices * 3; + Eigen::VectorXd grad_near = Eigen::VectorXd::Zero(n_dofs); + Eigen::VectorXd grad_far = Eigen::VectorXd::Zero(n_dofs); + + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + auto [gn, gf] = cc.gradient_nearfar(cc.dof(V), params, &nf_barrier); + + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t global_id = cc.vertex_id(i); + const index_t local_id = collisions.vertex_ids_inverse(global_id); + const index_t offset = local_id * 3; + grad_near.template segment<3>(offset) += + cc.weight * gn.template segment<3>(i * 3); + grad_far.template segment<3>(offset) += + cc.weight * gf.template segment<3>(i * 3); + } + } + + return { grad_near, grad_far }; +} + +std::pair PointPotentialHelper:: + evaluate_potential_hessian_at_vertex_with_cached_collisions_nearfar( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + PSDProjectionMethod project_to_psd, + const NearFarBarrier<>& nf_barrier) +{ + const int n_vertices = collisions.vertex_ids().size(); + const int n_dofs = n_vertices * 3; + + Eigen::MatrixXd H_near = Eigen::MatrixXd::Zero(n_dofs, n_dofs); + Eigen::MatrixXd H_far = Eigen::MatrixXd::Zero(n_dofs, n_dofs); + + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + auto [hn, hf] = cc.hessian_nearfar(cc.dof(V), params, &nf_barrier); + + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t global_id_i = cc.vertex_id(i); + const index_t local_i = collisions.vertex_ids_inverse(global_id_i); + const index_t offset_i = local_i * 3; + + for (index_t j = 0; j < cc.num_vertices(); j++) { + const index_t global_id_j = cc.vertex_id(j); + const index_t local_j = + collisions.vertex_ids_inverse(global_id_j); + const index_t offset_j = local_j * 3; + + H_near.block<3, 3>(offset_i, offset_j) += + cc.weight * hn.block<3, 3>(i * 3, j * 3); + H_far.block<3, 3>(offset_i, offset_j) += + cc.weight * hf.block<3, 3>(i * 3, j * 3); + } + } + } + + if (project_to_psd != PSDProjectionMethod::NONE) { + H_near = ipc::project_to_psd(H_near, project_to_psd); + H_far = ipc::project_to_psd(H_far, project_to_psd); + } + + return { H_near, H_far }; +} + +double PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions_near( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + EdgeEdgeDistanceType dtype, + const NearFarBarrier<>& nf_barrier) +{ + double near_sum = 0; + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + near_sum += cc.weight + * cc.operator_nearfar(cc.dof(V_extended), params, &nf_barrier) + .first; + } + return near_sum; +} + +template +std::enable_if_t< + IsADGrad::value || IsADHessian::value, + Eigen::VectorXd> +PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions_near( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef> q, + const NearFarBarrier<>& nf_barrier) +{ + const index_t n_real_vertices = V_extended.rows() - 1; + Eigen::VectorXd grad = + Eigen::VectorXd::Zero(collisions.vertex_ids().size() * 3); + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + auto [gn, gf] = + cc.gradient_nearfar(cc.dof(V_extended), params, &nf_barrier); + Eigen::VectorXd g = cc.weight * gn; + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t global_id = cc.vertex_id(i); + if (global_id == n_real_vertices) { + const Vector12d local_grad = + (q(0) * g(3 * i + 0) + q(1) * g(3 * i + 1) + + q(2) * g(3 * i + 2)) + .grad; + // distribute grad wrt virtual vertex to real edge vertices + for (index_t lv = 0; lv < 4; lv++) { + grad.segment<3>(3 * collisions.primary_local_ids()[lv]) += + local_grad.segment<3>(lv * 3); + } + } else { + assert(global_id < n_real_vertices); + grad.segment<3>(3 * collisions.vertex_ids_inverse(global_id)) += + g.segment<3>(i * 3); + } + } + } + + return grad; +} + +template Eigen::VectorXd PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions_near< + ADGrad<12>>( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef>> q, + const NearFarBarrier<>& nf_barrier); + +template Eigen::VectorXd PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions_near< + ADHessian<12>>( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef>> q, + const NearFarBarrier<>& nf_barrier); + +Eigen::MatrixXd PointPotentialHelper:: + evaluate_potential_hessian_at_edge_edge_closest_point_with_cached_collisions_near( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef>> q, + const NearFarBarrier<>& nf_barrier) +{ + const index_t n_real_vertices = V_extended.rows() - 1; + Eigen::MatrixXd H = Eigen::MatrixXd::Zero( + collisions.vertex_ids().size() * 3, collisions.vertex_ids().size() * 3); + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + const Eigen::VectorXd cc_dof = cc.dof(V_extended); + auto [gn, gf] = cc.gradient_nearfar(cc_dof, params, &nf_barrier); + auto [hn, hf] = cc.hessian_nearfar(cc_dof, params, &nf_barrier); + Eigen::VectorXd g = cc.weight * gn; + Eigen::MatrixXd h = cc.weight * hn; + + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t gi = cc.vertex_id(i); + for (index_t j = 0; j < cc.num_vertices(); j++) { + const index_t gj = cc.vertex_id(j); + if (gi == n_real_vertices && gj == n_real_vertices) { + assert(i == j); + // distribute derivatives wrt virtual vertex to real edge + // vertices + Matrix12d local_hess; + { + Eigen::Matrix tmp_g; + tmp_g << q(0).grad.transpose(), q(1).grad.transpose(), + q(2).grad.transpose(); + local_hess = tmp_g.transpose() + * h.block<3, 3>(3 * i, 3 * j) * tmp_g; + + for (int d = 0; d < 3; d++) { + local_hess += q(d).Hess * g(3 * i + d); + } + } + + for (index_t li = 0; li < 4; li++) { + for (index_t lj = 0; lj < 4; lj++) { + H.block<3, 3>( + collisions.primary_local_ids()[li] * 3, + collisions.primary_local_ids()[lj] * 3) += + local_hess.block<3, 3>(3 * li, 3 * lj); + } + } + } else if (gi == n_real_vertices) { + Eigen::Matrix local_hess; + { + Eigen::Matrix tmp_g; + tmp_g << q(0).grad.transpose(), q(1).grad.transpose(), + q(2).grad.transpose(); + local_hess = + tmp_g.transpose() * h.block<3, 3>(3 * i, 3 * j); + } + for (index_t li = 0; li < 4; li++) { + const index_t lli = collisions.primary_local_ids()[li]; + H.block<3, 3>( + lli * 3, collisions.vertex_ids_inverse(gj) * 3) += + local_hess.block<3, 3>(3 * li, 0); + H.block<3, 3>( + collisions.vertex_ids_inverse(gj) * 3, lli * 3) += + local_hess.block<3, 3>(3 * li, 0).transpose(); + } + } else if (gj == n_real_vertices) { + // Already handled in (gi == n_real_vertices) case + } else { + assert(gi < n_real_vertices); + assert(gj < n_real_vertices); + H.block<3, 3>( + 3 * collisions.vertex_ids_inverse(gi), + 3 * collisions.vertex_ids_inverse(gj)) += + h.block<3, 3>(3 * i, 3 * j); + } + } + } + } + + return H; +} + +std::pair PointPotentialHelper:: + evaluate_potential_at_face_interior_point_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier<>& nf_barrier) +{ + double near_sum = 0, far_sum = 0; + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + auto [n, f] = + cc.operator_nearfar(cc.dof(V_extended), params, &nf_barrier); + near_sum += cc.weight * n; + far_sum += cc.weight * f; + } + return { near_sum, far_sum }; +} + +std::pair PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda, + const NearFarBarrier<>& nf_barrier) +{ + const int n_vertices = collisions.vertex_ids().size(); + Eigen::VectorXd grad_near = Eigen::VectorXd::Zero(n_vertices * 3); + Eigen::VectorXd grad_far = Eigen::VectorXd::Zero(n_vertices * 3); + + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + auto [gn, gf] = + cc.gradient_nearfar(cc.dof(V_extended), params, &nf_barrier); + + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t global_id = cc.vertex_id(i); + if (global_id == V_extended.rows() - 1) { + for (index_t li = 0; li < 3; li++) { + const index_t local_id = collisions.primary_local_ids()[li]; + grad_near.template segment<3>(local_id * 3) += + cc.weight * lambda[li] * gn.template segment<3>(i * 3); + grad_far.template segment<3>(local_id * 3) += + cc.weight * lambda[li] * gf.template segment<3>(i * 3); + } + } else { + const index_t local_id = + collisions.vertex_ids_inverse(global_id); + grad_near.template segment<3>(local_id * 3) += + cc.weight * gn.template segment<3>(i * 3); + grad_far.template segment<3>(local_id * 3) += + cc.weight * gf.template segment<3>(i * 3); + } + } + } + + return { grad_near, grad_far }; +} + +std::pair PointPotentialHelper:: + evaluate_potential_hessian_at_face_interior_point_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda, + PSDProjectionMethod project_to_psd, + const NearFarBarrier<>& nf_barrier) +{ + const int n_vertices = collisions.vertex_ids().size(); + const int n_dofs = n_vertices * 3; + + Eigen::MatrixXd H_near = Eigen::MatrixXd::Zero(n_dofs, n_dofs); + Eigen::MatrixXd H_far = Eigen::MatrixXd::Zero(n_dofs, n_dofs); + + for (int ci = 0; ci < collisions.size(); ci++) { + const auto& cc = collisions[ci]; + auto [hn, hf] = + cc.hessian_nearfar(cc.dof(V_extended), params, &nf_barrier); + + for (index_t i = 0; i < cc.num_vertices(); i++) { + const index_t global_id_i = cc.vertex_id(i); + const bool i_virtual = (global_id_i == V_extended.rows() - 1); + + for (index_t j = 0; j < cc.num_vertices(); j++) { + const index_t global_id_j = cc.vertex_id(j); + const bool j_virtual = (global_id_j == V_extended.rows() - 1); + + if (!i_virtual && !j_virtual) { + const index_t local_i = + collisions.vertex_ids_inverse(global_id_i); + const index_t local_j = + collisions.vertex_ids_inverse(global_id_j); + H_near.block<3, 3>(local_i * 3, local_j * 3) += + cc.weight * hn.block<3, 3>(i * 3, j * 3); + H_far.block<3, 3>(local_i * 3, local_j * 3) += + cc.weight * hf.block<3, 3>(i * 3, j * 3); + } else if (i_virtual && j_virtual) { + for (index_t li = 0; li < 3; li++) { + for (index_t lj = 0; lj < 3; lj++) { + const index_t local_li = + collisions.primary_local_ids()[li]; + const index_t local_lj = + collisions.primary_local_ids()[lj]; + H_near.block<3, 3>(local_li * 3, local_lj * 3) += + cc.weight * lambda[li] * lambda[lj] + * hn.block<3, 3>(i * 3, j * 3); + H_far.block<3, 3>(local_li * 3, local_lj * 3) += + cc.weight * lambda[li] * lambda[lj] + * hf.block<3, 3>(i * 3, j * 3); + } + } + } else if (i_virtual) { + const index_t local_j = + collisions.vertex_ids_inverse(global_id_j); + for (index_t li = 0; li < 3; li++) { + const index_t local_li = + collisions.primary_local_ids()[li]; + H_near.block<3, 3>(local_li * 3, local_j * 3) += + cc.weight * lambda[li] + * hn.block<3, 3>(i * 3, j * 3); + H_far.block<3, 3>(local_li * 3, local_j * 3) += + cc.weight * lambda[li] + * hf.block<3, 3>(i * 3, j * 3); + } + } else if (j_virtual) { + const index_t local_i = + collisions.vertex_ids_inverse(global_id_i); + for (index_t lj = 0; lj < 3; lj++) { + const index_t local_lj = + collisions.primary_local_ids()[lj]; + H_near.block<3, 3>(local_i * 3, local_lj * 3) += + cc.weight * lambda[lj] + * hn.block<3, 3>(i * 3, j * 3); + H_far.block<3, 3>(local_i * 3, local_lj * 3) += + cc.weight * lambda[lj] + * hf.block<3, 3>(i * 3, j * 3); + } + } + } + } + } + + if (project_to_psd != PSDProjectionMethod::NONE) { + H_near = ipc::project_to_psd(H_near, project_to_psd); + H_far = ipc::project_to_psd(H_far, project_to_psd); + } + + return { H_near, H_far }; +} +} // namespace ipc diff --git a/src/ipc/esp/quadrature_potential.hpp b/src/ipc/esp/quadrature_potential.hpp new file mode 100644 index 000000000..cdfc68748 --- /dev/null +++ b/src/ipc/esp/quadrature_potential.hpp @@ -0,0 +1,248 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace ipc { +namespace PointPotentialHelper { + double evaluate_potential_at_vertex_with_cached_collisions( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params); + + Eigen::VectorXd + evaluate_potential_gradient_at_vertex_with_cached_collisions( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params); + + Eigen::MatrixXd evaluate_potential_hessian_at_vertex_with_cached_collisions( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + PSDProjectionMethod project_to_psd); + + std::pair + evaluate_potential_at_vertex_with_cached_collisions_nearfar( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier<>& nf_barrier); + + std::pair + evaluate_potential_gradient_at_vertex_with_cached_collisions_nearfar( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier<>& nf_barrier); + + std::pair + evaluate_potential_hessian_at_vertex_with_cached_collisions_nearfar( + Eigen::ConstRef V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + PSDProjectionMethod project_to_psd, + const NearFarBarrier<>& nf_barrier); + + double evaluate_potential_at_edge_edge_closest_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + EdgeEdgeDistanceType dtype); + + double + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions_near( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + EdgeEdgeDistanceType dtype, + const NearFarBarrier<>& nf_barrier); + + /// @brief Compute the gradient of P(q) for a point q + /// @return The gradient vector with respect to collisions.m_vertex_ids + /// @param V_extended Extended vertices matrix, with the point q appended to the last row + /// @param collisions Primitives that are close in distance to point q + /// @param q Closest point between two edges, together with the derivatives of q with respect to vids + template + std::enable_if_t< + IsADGrad::value || IsADHessian::value, + Eigen::VectorXd> + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef> q); + + /// @brief Compute the near component gradient (NearFarBarrier) + template + std::enable_if_t< + IsADGrad::value || IsADHessian::value, + Eigen::VectorXd> + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions_near( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef> q, + const NearFarBarrier<>& nf_barrier); + + Eigen::MatrixXd + evaluate_potential_hessian_at_edge_edge_closest_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef>> q); + + Eigen::MatrixXd + evaluate_potential_hessian_at_edge_edge_closest_point_with_cached_collisions_near( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + Eigen::ConstRef>> q, + const NearFarBarrier<>& nf_barrier); + + double evaluate_potential_at_face_interior_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params); + + std::pair + evaluate_potential_at_face_interior_point_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier<>& nf_barrier); + + /// @brief Gradient of the face-interior potential for an arbitrary + /// interior quadrature point q = λ0·v0 + λ1·v1 + λ2·v2. + /// @param lambda Barycentric coordinates of the interior point. + /// The chain-rule factors λk replace the 1/3 used for the centroid. + Eigen::VectorXd + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda); + + /// @brief Hessian of the face-interior potential for an arbitrary + /// interior quadrature point q = λ0·v0 + λ1·v1 + λ2·v2. + Eigen::MatrixXd + evaluate_potential_hessian_at_face_interior_point_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda, + PSDProjectionMethod project_to_psd); + + std::pair + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda, + const NearFarBarrier<>& nf_barrier); + + std::pair + evaluate_potential_hessian_at_face_interior_point_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda, + PSDProjectionMethod project_to_psd, + const NearFarBarrier<>& nf_barrier); + + // ---- 2D edge quadrature point helpers ---- + + /// @brief Evaluate P(q) = sum of barrier values for all pairs in the dict. + /// @param V_extended Vertices extended with the virtual QP as last row. + /// @param dict Per-QP collision dict for edge quadrature. + /// @param params Contact parameters. + double evaluate_potential_at_edge_qp( + VertexMatrixView<2> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params); + + /// @brief Gradient of P(q) w.r.t. all real vertices, using chain rule + /// dP/de_k += lambda[k] * dP/dq. + /// @param lambda Barycentric coords of QP on the edge: q = lambda[0]*e0 + lambda[1]*e1. + Eigen::VectorXd evaluate_potential_gradient_at_edge_qp( + VertexMatrixView<2> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda); + + /// @brief Hessian of P(q) w.r.t. all real vertices. + /// @param lambda Barycentric coords of QP on the edge: q = lambda[0]*e0 + lambda[1]*e1. + Eigen::MatrixXd evaluate_potential_hessian_at_edge_qp( + VertexMatrixView<2> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const std::array& lambda, + PSDProjectionMethod project_to_psd); +} // namespace PointPotentialHelper + +class PointPotential { +public: + constexpr static int R = 2; + + PointPotential( + const CollisionMesh& _mesh, + const Candidates& _candidates, + const ESPParameters& _params) + : mesh(_mesh) + , candidates(_candidates) + , params(_params) + { + compute_inclusion_exclusion_weights(mesh, edge_weights, vertex_weights); + } + + std::unique_ptr> + build_collisions_at_vertex( + Eigen::ConstRef V, + index_t vid, + size_t& num_collision_pairs) const; + + std::unique_ptr> + build_collisions_at_edge_edge_closest_point( + Eigen::ConstRef V, + index_t e0, + index_t e1, + EdgeEdgeDistanceType dtype, + size_t& num_collision_pairs) const; + + std::unique_ptr> + build_collisions_at_face_interior_point( + Eigen::ConstRef V, + index_t fid, + const std::array& lambda, + size_t& num_collision_pairs) const; + + /// @brief Build a per-QP collision dict for a 2D edge quadrature point. + /// @param V Vertex positions (2D). + /// @param ei Source edge index. + /// @param lambda Barycentric coords of QP: q = lambda[0]*e0 + lambda[1]*e1. + /// @param dhat Distance threshold for this edge. + std::unique_ptr> + build_collisions_at_edge_qp( + Eigen::ConstRef V, + index_t ei, + const std::array& lambda, + double dhat, + size_t& num_collision_pairs) const; + + const CollisionMesh& mesh; + const Candidates& candidates; + const ESPParameters params; + /// Inclusion-exclusion weight of each edge and each vertex (every face + /// has weight one); see compute_inclusion_exclusion_weights(). Every + /// collision a builder emits takes the weight of the mesh element it came + /// from, so open meshes and lower-dimensional pieces get the right signs. + std::vector edge_weights, vertex_weights; +}; +} // namespace ipc diff --git a/src/ipc/smooth_contact/CMakeLists.txt b/src/ipc/gcp/CMakeLists.txt similarity index 70% rename from src/ipc/smooth_contact/CMakeLists.txt rename to src/ipc/gcp/CMakeLists.txt index 93ad869af..7279ac433 100644 --- a/src/ipc/smooth_contact/CMakeLists.txt +++ b/src/ipc/gcp/CMakeLists.txt @@ -1,11 +1,11 @@ set(SOURCES common.hpp - smooth_contact_potential.cpp - smooth_contact_potential.hpp - smooth_collisions.cpp - smooth_collisions.hpp - smooth_collisions_builder.cpp - smooth_collisions_builder.hpp + gcp_potential.cpp + gcp_potential.hpp + gcp_collisions.cpp + gcp_collisions.hpp + gcp_collisions_builder.cpp + gcp_collisions_builder.hpp ) source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) diff --git a/src/ipc/smooth_contact/collisions/CMakeLists.txt b/src/ipc/gcp/collisions/CMakeLists.txt similarity index 76% rename from src/ipc/smooth_contact/collisions/CMakeLists.txt rename to src/ipc/gcp/collisions/CMakeLists.txt index d9b91169e..58f978dd2 100644 --- a/src/ipc/smooth_contact/collisions/CMakeLists.txt +++ b/src/ipc/gcp/collisions/CMakeLists.txt @@ -1,6 +1,6 @@ set(SOURCES - smooth_collision.cpp - smooth_collision.hpp + gcp_collision.cpp + gcp_collision.hpp ) source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) diff --git a/src/ipc/smooth_contact/collisions/smooth_collision.cpp b/src/ipc/gcp/collisions/gcp_collision.cpp similarity index 87% rename from src/ipc/smooth_contact/collisions/smooth_collision.cpp rename to src/ipc/gcp/collisions/gcp_collision.cpp index 5f569f09e..b6e7d0328 100644 --- a/src/ipc/smooth_contact/collisions/smooth_collision.cpp +++ b/src/ipc/gcp/collisions/gcp_collision.cpp @@ -1,28 +1,29 @@ -#include "smooth_collision.hpp" +#include "gcp_collision.hpp" #include +#include namespace ipc { // clang-format off -template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::VERTEX_VERTEX; } -template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::VERTEX_VERTEX; } -template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::EDGE_VERTEX; } -template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::EDGE_VERTEX; } -template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::FACE_VERTEX; } -template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::EDGE_EDGE; } +template <> CollisionType GCPCollisionTemplate::type() const { return CollisionType::VERTEX_VERTEX; } +template <> CollisionType GCPCollisionTemplate::type() const { return CollisionType::VERTEX_VERTEX; } +template <> CollisionType GCPCollisionTemplate::type() const { return CollisionType::EDGE_VERTEX; } +template <> CollisionType GCPCollisionTemplate::type() const { return CollisionType::EDGE_VERTEX; } +template <> CollisionType GCPCollisionTemplate::type() const { return CollisionType::FACE_VERTEX; } +template <> CollisionType GCPCollisionTemplate::type() const { return CollisionType::EDGE_EDGE; } // clang-format on // clang-format off -template <> std::string SmoothCollisionTemplate::name() const { return "vert-vert"; } -template <> std::string SmoothCollisionTemplate::name() const { return "vert-vert"; } -template <> std::string SmoothCollisionTemplate::name() const { return "edge-vert"; } -template <> std::string SmoothCollisionTemplate::name() const { return "edge-vert"; } -template <> std::string SmoothCollisionTemplate::name() const { return "face-vert"; } -template <> std::string SmoothCollisionTemplate::name() const { return "edge-edge"; } +template <> std::string GCPCollisionTemplate::name() const { return "vert-vert"; } +template <> std::string GCPCollisionTemplate::name() const { return "vert-vert"; } +template <> std::string GCPCollisionTemplate::name() const { return "edge-vert"; } +template <> std::string GCPCollisionTemplate::name() const { return "edge-vert"; } +template <> std::string GCPCollisionTemplate::name() const { return "face-vert"; } +template <> std::string GCPCollisionTemplate::name() const { return "edge-edge"; } // clang-format on -Eigen::VectorXd SmoothCollision::dof(Eigen::ConstRef X) const +Eigen::VectorXd GCPCollision::dof(Eigen::ConstRef X) const { const int DIM = X.cols(); Eigen::VectorXd x(num_vertices() * DIM); @@ -35,13 +36,13 @@ Eigen::VectorXd SmoothCollision::dof(Eigen::ConstRef X) const x.segment<3>(i * 3) = X.row(m_vertex_ids[i]); } } else { - throw std::runtime_error("Invalid dimension!"); + log_and_throw_error("Invalid dimension!"); } return x; } template -auto SmoothCollisionTemplate::get_core_indices() const +auto GCPCollisionTemplate::get_core_indices() const -> Eigen::Vector { Eigen::Vector core_indices; @@ -54,15 +55,15 @@ auto SmoothCollisionTemplate::get_core_indices() const } template -SmoothCollisionTemplate::SmoothCollisionTemplate( +GCPCollisionTemplate::GCPCollisionTemplate( index_t _primitive0, index_t _primitive1, - SmoothCollisionTemplate::DTYPE dtype, + GCPCollisionTemplate::DTYPE dtype, const CollisionMesh& mesh, - const SmoothContactParameters& params, + const GCPParameters& params, const double _dhat, Eigen::ConstRef V) - : SmoothCollision(_primitive0, _primitive1, _dhat, mesh) + : GCPCollision(_primitive0, _primitive1, _dhat, mesh) { VectorMax3d d = PrimitiveDistance::compute_closest_direction( @@ -103,9 +104,9 @@ SmoothCollisionTemplate::SmoothCollisionTemplate( } template -double SmoothCollisionTemplate::operator()( +double GCPCollisionTemplate::operator()( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const + const GCPParameters& params) const { Eigen::Vector x; x << positions.head(PrimitiveA::N_CORE_POINTS * DIM), @@ -142,10 +143,9 @@ double SmoothCollisionTemplate::operator()( } template -auto SmoothCollisionTemplate::gradient( +auto GCPCollisionTemplate::gradient( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const - -> VectorMax + const GCPParameters& params) const -> VectorMax { const auto core_indices = get_core_indices(); @@ -256,9 +256,9 @@ auto SmoothCollisionTemplate::gradient( } template -auto SmoothCollisionTemplate::hessian( +auto GCPCollisionTemplate::hessian( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const + const GCPParameters& params) const -> MatrixMax { const auto core_indices = get_core_indices(); @@ -470,7 +470,7 @@ auto SmoothCollisionTemplate::hessian( // ---- distance ---- template -double SmoothCollisionTemplate::compute_distance( +double GCPCollisionTemplate::compute_distance( Eigen::ConstRef vertices) const { VectorMax positions = dof(vertices); @@ -485,7 +485,7 @@ double SmoothCollisionTemplate::compute_distance( } template -auto SmoothCollisionTemplate::core_vertex_ids() const +auto GCPCollisionTemplate::core_vertex_ids() const -> std::array { std::array vids {}; @@ -497,11 +497,11 @@ auto SmoothCollisionTemplate::core_vertex_ids() const } // Note: Primitive pair order cannot change -template class SmoothCollisionTemplate; -template class SmoothCollisionTemplate; +template class GCPCollisionTemplate; +template class GCPCollisionTemplate; -template class SmoothCollisionTemplate; -template class SmoothCollisionTemplate; -template class SmoothCollisionTemplate; -template class SmoothCollisionTemplate; +template class GCPCollisionTemplate; +template class GCPCollisionTemplate; +template class GCPCollisionTemplate; +template class GCPCollisionTemplate; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/collisions/smooth_collision.hpp b/src/ipc/gcp/collisions/gcp_collision.hpp similarity index 83% rename from src/ipc/smooth_contact/collisions/smooth_collision.hpp rename to src/ipc/gcp/collisions/gcp_collision.hpp index 21f15fc14..1df8ee54c 100644 --- a/src/ipc/smooth_contact/collisions/smooth_collision.hpp +++ b/src/ipc/gcp/collisions/gcp_collision.hpp @@ -1,8 +1,9 @@ #pragma once #include -#include -#include +#include +#include +#include namespace ipc { @@ -14,12 +15,12 @@ enum class CollisionType : uint8_t { }; /// @brief Contact pair class for Geometric Contact Potential. -/// @note Unlike NormalCollision, SmoothCollision has to be reconstructed whenever vertices change position -class SmoothCollision { +/// @note Unlike NormalCollision, GCPCollision has to be reconstructed whenever vertices change position +class GCPCollision { public: static constexpr int ELEMENT_SIZE = 3 * MAX_VERT_3D; - SmoothCollision( + GCPCollision( const index_t _primitive0, const index_t _primitive1, const double _dhat, @@ -30,7 +31,7 @@ class SmoothCollision { { } - virtual ~SmoothCollision() = default; + virtual ~GCPCollision() = default; /// @brief Check if this contact pair is active (depending on both orientation and distance) bool is_active() const { return m_is_active; } @@ -82,25 +83,25 @@ class SmoothCollision { /// @brief Compute the value of the GCP potential virtual double operator()( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const = 0; + const GCPParameters& params) const = 0; /// @brief Compute the gradient of the GCP potential wrt. vertices involved virtual VectorMax gradient( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const = 0; + const GCPParameters& params) const = 0; /// @brief Compute the Hessian of the GCP potential wrt. vertices involved virtual MatrixMax hessian( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const = 0; + const GCPParameters& params) const = 0; - bool operator==(const SmoothCollision& other) const + bool operator==(const GCPCollision& other) const { return ( primitive0 == other.primitive0 && primitive1 == other.primitive1); } - bool operator!=(const SmoothCollision& other) const + bool operator!=(const GCPCollision& other) const { return !(*this == other); } @@ -112,7 +113,7 @@ class SmoothCollision { } else if (idx == 1) { return primitive1; } else { - throw std::runtime_error("Invalid index in smooth_collision!"); + log_and_throw_error("Invalid index in gcp_collision!"); } } @@ -130,11 +131,15 @@ class SmoothCollision { std::vector m_vertex_ids; }; +using SmoothCollision + [[deprecated("SmoothCollision is deprecated. Use GCPCollision instead.")]] = + GCPCollision; + /// @brief Templated class for various types of contact pairs template -class SmoothCollisionTemplate : public SmoothCollision { +class GCPCollisionTemplate : public GCPCollision { public: - using Super = SmoothCollision; + using Super = GCPCollision; /// @brief Distance type of the contact pair using DTYPE = typename PrimitiveDistType::type; /// @brief Number of points needed to compute the distance between two primitives @@ -146,16 +151,16 @@ class SmoothCollisionTemplate : public SmoothCollision { static constexpr int N_CORE_DOFS = N_CORE_POINTS * DIM; static constexpr int ELEMENT_SIZE = Super::ELEMENT_SIZE; - SmoothCollisionTemplate( + GCPCollisionTemplate( index_t primitive0, index_t primitive1, DTYPE dtype, const CollisionMesh& mesh, - const SmoothContactParameters& params, + const GCPParameters& params, const double dhat, Eigen::ConstRef V); - virtual ~SmoothCollisionTemplate() = default; + virtual ~GCPCollisionTemplate() = default; std::string name() const override; @@ -187,7 +192,7 @@ class SmoothCollisionTemplate : public SmoothCollision { /// @return GCP potential value double operator()( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const override; + const GCPParameters& params) const override; /// @brief Compute the potential gradient wrt. positions /// @param positions Vertex positions @@ -195,7 +200,7 @@ class SmoothCollisionTemplate : public SmoothCollision { /// @return GCP potential gradient VectorMax gradient( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const override; + const GCPParameters& params) const override; /// @brief Compute the potential Hessian wrt. positions /// @param positions Vertex positions @@ -203,7 +208,7 @@ class SmoothCollisionTemplate : public SmoothCollision { /// @return GCP potential Hessian MatrixMax hessian( Eigen::ConstRef> positions, - const SmoothContactParameters& params) const override; + const GCPParameters& params) const override; // ---- distance ---- @@ -218,4 +223,9 @@ class SmoothCollisionTemplate : public SmoothCollision { std::unique_ptr primitive_b; }; +template +using SmoothCollisionTemplate [[deprecated( + "SmoothCollisionTemplate is deprecated. Use GCPCollisionTemplate instead.")]] = + GCPCollisionTemplate; + } // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/common.hpp b/src/ipc/gcp/common.hpp similarity index 90% rename from src/ipc/smooth_contact/common.hpp rename to src/ipc/gcp/common.hpp index e504caa89..5242f8de8 100644 --- a/src/ipc/smooth_contact/common.hpp +++ b/src/ipc/gcp/common.hpp @@ -24,19 +24,19 @@ template <> class MaxVertices<3> { static constexpr int value = MAX_VERT_3D; // NOLINT }; -struct SmoothContactParameters { - SmoothContactParameters() = default; +struct GCPParameters { + GCPParameters() = default; - SmoothContactParameters( + GCPParameters( const double _dhat, const double _alpha_t, const double _beta_t, const int _r) - : SmoothContactParameters(_dhat, _alpha_t, _beta_t, 0, 0.1, _r) + : GCPParameters(_dhat, _alpha_t, _beta_t, 0, 0.1, _r) { } - SmoothContactParameters( + GCPParameters( const double _dhat, const double _alpha_t, const double _beta_t, @@ -103,4 +103,8 @@ struct SmoothContactParameters { double m_adaptive_dhat_ratio = 0.5; }; +using SmoothContactParameters [[deprecated( + "SmoothContactParameters is deprecated. Use GCPParameters instead.")]] = + GCPParameters; + } // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/distance/CMakeLists.txt b/src/ipc/gcp/distance/CMakeLists.txt similarity index 100% rename from src/ipc/smooth_contact/distance/CMakeLists.txt rename to src/ipc/gcp/distance/CMakeLists.txt diff --git a/src/ipc/smooth_contact/distance/edge_edge.cpp b/src/ipc/gcp/distance/edge_edge.cpp similarity index 99% rename from src/ipc/smooth_contact/distance/edge_edge.cpp rename to src/ipc/gcp/distance/edge_edge.cpp index 134193ddb..c8d64fd08 100644 --- a/src/ipc/smooth_contact/distance/edge_edge.cpp +++ b/src/ipc/gcp/distance/edge_edge.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -305,7 +306,7 @@ Eigen::Vector3 edge_edge_closest_point_direction( return line_line_closest_point_direction(ea0, ea1, eb0, eb1); default: - throw std::invalid_argument( + log_and_throw_error( "Invalid distance type for edge-edge distance!"); } } @@ -381,7 +382,7 @@ Eigen::Matrix edge_edge_closest_point_pairs( break; default: - throw std::invalid_argument( + log_and_throw_error( "Invalid distance type for edge-edge distance!"); } diff --git a/src/ipc/smooth_contact/distance/edge_edge.hpp b/src/ipc/gcp/distance/edge_edge.hpp similarity index 100% rename from src/ipc/smooth_contact/distance/edge_edge.hpp rename to src/ipc/gcp/distance/edge_edge.hpp diff --git a/src/ipc/smooth_contact/distance/mollifier.cpp b/src/ipc/gcp/distance/mollifier.cpp similarity index 99% rename from src/ipc/smooth_contact/distance/mollifier.cpp rename to src/ipc/gcp/distance/mollifier.cpp index d6c62d53b..e7485b23c 100644 --- a/src/ipc/smooth_contact/distance/mollifier.cpp +++ b/src/ipc/gcp/distance/mollifier.cpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include namespace ipc { namespace { diff --git a/src/ipc/smooth_contact/distance/mollifier.hpp b/src/ipc/gcp/distance/mollifier.hpp similarity index 100% rename from src/ipc/smooth_contact/distance/mollifier.hpp rename to src/ipc/gcp/distance/mollifier.hpp diff --git a/src/ipc/smooth_contact/distance/mollifier.tpp b/src/ipc/gcp/distance/mollifier.tpp similarity index 100% rename from src/ipc/smooth_contact/distance/mollifier.tpp rename to src/ipc/gcp/distance/mollifier.tpp diff --git a/src/ipc/smooth_contact/distance/point_edge.cpp b/src/ipc/gcp/distance/point_edge.cpp similarity index 98% rename from src/ipc/smooth_contact/distance/point_edge.cpp rename to src/ipc/gcp/distance/point_edge.cpp index b000fdcec..7db8bc3a3 100644 --- a/src/ipc/smooth_contact/distance/point_edge.cpp +++ b/src/ipc/gcp/distance/point_edge.cpp @@ -2,6 +2,7 @@ #include #include +#include namespace ipc { template @@ -209,7 +210,7 @@ PointEdgeDistanceDerivatives::point_edge_closest_point_direction_grad( grad.middleCols(2 * dim, dim).diagonal().array() = -1.; break; default: - throw std::runtime_error("PointEdgeDistanceType not specified!"); + log_and_throw_error("PointEdgeDistanceType not specified!"); } #endif @@ -267,7 +268,7 @@ PointEdgeDistanceDerivatives::point_edge_closest_point_direction_hessian( grad.middleCols(2 * dim, dim).diagonal().array() = -1.; break; default: - throw std::runtime_error("PointEdgeDistanceType not specified!"); + log_and_throw_error("PointEdgeDistanceType not specified!"); } #endif diff --git a/src/ipc/smooth_contact/distance/point_edge.hpp b/src/ipc/gcp/distance/point_edge.hpp similarity index 98% rename from src/ipc/smooth_contact/distance/point_edge.hpp rename to src/ipc/gcp/distance/point_edge.hpp index f8ae36b79..e0d4dc261 100644 --- a/src/ipc/smooth_contact/distance/point_edge.hpp +++ b/src/ipc/gcp/distance/point_edge.hpp @@ -4,8 +4,8 @@ #include #include #include +#include #include -#include #include namespace ipc { diff --git a/src/ipc/smooth_contact/distance/point_face.cpp b/src/ipc/gcp/distance/point_face.cpp similarity index 98% rename from src/ipc/smooth_contact/distance/point_face.cpp rename to src/ipc/gcp/distance/point_face.cpp index bd57c17b5..2923026d4 100644 --- a/src/ipc/smooth_contact/distance/point_face.cpp +++ b/src/ipc/gcp/distance/point_face.cpp @@ -3,6 +3,7 @@ #include "point_edge.hpp" #include +#include namespace ipc { @@ -139,7 +140,7 @@ point_triangle_closest_point_direction_grad( break; default: - throw std::invalid_argument( + log_and_throw_error( "Invalid distance type for point-triangle distance!"); } @@ -227,7 +228,7 @@ point_triangle_closest_point_direction_hessian( break; default: - throw std::invalid_argument( + log_and_throw_error( "Invalid distance type for point-triangle distance!"); } @@ -291,7 +292,7 @@ Eigen::Vector3 point_triangle_closest_point_direction( } default: { - throw std::invalid_argument( + log_and_throw_error( "Invalid distance type for point-triangle distance!"); } } diff --git a/src/ipc/smooth_contact/distance/point_face.hpp b/src/ipc/gcp/distance/point_face.hpp similarity index 100% rename from src/ipc/smooth_contact/distance/point_face.hpp rename to src/ipc/gcp/distance/point_face.hpp diff --git a/src/ipc/smooth_contact/distance/primitive_distance.cpp b/src/ipc/gcp/distance/primitive_distance.cpp similarity index 98% rename from src/ipc/smooth_contact/distance/primitive_distance.cpp rename to src/ipc/gcp/distance/primitive_distance.cpp index 28d670a99..0288aba07 100644 --- a/src/ipc/smooth_contact/distance/primitive_distance.cpp +++ b/src/ipc/gcp/distance/primitive_distance.cpp @@ -5,8 +5,8 @@ #include #include #include -#include -#include +#include +#include namespace ipc { diff --git a/src/ipc/smooth_contact/distance/primitive_distance.hpp b/src/ipc/gcp/distance/primitive_distance.hpp similarity index 97% rename from src/ipc/smooth_contact/distance/primitive_distance.hpp rename to src/ipc/gcp/distance/primitive_distance.hpp index c34e0875d..ce26fc535 100644 --- a/src/ipc/smooth_contact/distance/primitive_distance.hpp +++ b/src/ipc/gcp/distance/primitive_distance.hpp @@ -1,11 +1,11 @@ #pragma once #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/src/ipc/smooth_contact/distance/primitive_distance.tpp b/src/ipc/gcp/distance/primitive_distance.tpp similarity index 100% rename from src/ipc/smooth_contact/distance/primitive_distance.tpp rename to src/ipc/gcp/distance/primitive_distance.tpp diff --git a/src/ipc/smooth_contact/smooth_collisions.cpp b/src/ipc/gcp/gcp_collisions.cpp similarity index 86% rename from src/ipc/smooth_contact/smooth_collisions.cpp rename to src/ipc/gcp/gcp_collisions.cpp index bafc55027..5c21eaff3 100644 --- a/src/ipc/smooth_contact/smooth_collisions.cpp +++ b/src/ipc/gcp/gcp_collisions.cpp @@ -1,12 +1,13 @@ -#include "smooth_collisions.hpp" +#include "gcp_collisions.hpp" -#include "smooth_collisions_builder.hpp" +#include "gcp_collisions_builder.hpp" #include #include #include #include #include +#include #include #include @@ -17,10 +18,10 @@ namespace ipc { -void SmoothCollisions::compute_adaptive_dhat( +void GCPCollisions::compute_adaptive_dhat( const CollisionMesh& mesh, Eigen::ConstRef vertices, // set to zero for rest pose - const SmoothContactParameters params, + const GCPParameters params, BroadPhase* broad_phase) { assert(vertices.rows() == mesh.num_vertices()); @@ -71,7 +72,7 @@ void SmoothCollisions::compute_adaptive_dhat( break; } default: { - throw std::runtime_error("Invalid collision type!"); + log_and_throw_error("Invalid collision type!"); } } } @@ -112,10 +113,10 @@ void SmoothCollisions::compute_adaptive_dhat( } } -void SmoothCollisions::build( +void GCPCollisions::build( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const SmoothContactParameters params, + const GCPParameters params, const bool use_adaptive_dhat, BroadPhase* broad_phase) { @@ -128,11 +129,11 @@ void SmoothCollisions::build( this->build(m_candidates, mesh, vertices, params, use_adaptive_dhat); } -void SmoothCollisions::build( +void GCPCollisions::build( const Candidates& candidates, const CollisionMesh& mesh, Eigen::ConstRef vertices, - const SmoothContactParameters params, + const GCPParameters params, const bool use_adaptive_dhat) { assert(vertices.rows() == mesh.num_vertices()); @@ -164,8 +165,8 @@ void SmoothCollisions::build( }; if (mesh.dim() == 2) { - tbb::enumerable_thread_specific> storage { - SmoothCollisionsBuilder<2>() + tbb::enumerable_thread_specific> storage { + GCPCollisionsBuilder<2>() }; tbb::parallel_for( @@ -177,10 +178,10 @@ void SmoothCollisions::build( edge_dhat, r.begin(), r.end()); }); - SmoothCollisionsBuilder<2>::merge(storage, *this); + GCPCollisionsBuilder<2>::merge(storage, *this); } else { - tbb::enumerable_thread_specific> storage { - SmoothCollisionsBuilder<3>() + tbb::enumerable_thread_specific> storage { + GCPCollisionsBuilder<3>() }; tbb::parallel_for( @@ -201,36 +202,36 @@ void SmoothCollisions::build( edge_dhat, face_dhat, r.begin(), r.end()); }); - SmoothCollisionsBuilder<3>::merge(storage, *this); + GCPCollisionsBuilder<3>::merge(storage, *this); } m_candidates = candidates; } // ============================================================================ -size_t SmoothCollisions::size() const { return collisions.size(); } -bool SmoothCollisions::empty() const { return collisions.empty(); } -void SmoothCollisions::clear() { collisions.clear(); } +size_t GCPCollisions::size() const { return collisions.size(); } +bool GCPCollisions::empty() const { return collisions.empty(); } +void GCPCollisions::clear() { collisions.clear(); } -SmoothCollision& SmoothCollisions::operator[](size_t i) +GCPCollision& GCPCollisions::operator[](size_t i) { if (i < collisions.size()) { return *collisions[i]; } - throw std::out_of_range("Collision index is out of range!"); + log_and_throw_error("Collision index is out of range!"); } -const SmoothCollision& SmoothCollisions::operator[](size_t i) const +const GCPCollision& GCPCollisions::operator[](size_t i) const { if (i < collisions.size()) { return *collisions[i]; } - throw std::out_of_range("Collision index is out of range!"); + log_and_throw_error("Collision index is out of range!"); } -std::string SmoothCollisions::to_string( +std::string GCPCollisions::to_string( const CollisionMesh& mesh, Eigen::ConstRef vertices, - const SmoothContactParameters& params) const + const GCPParameters& params) const { std::stringstream ss; for (const auto& cc : collisions) { @@ -247,7 +248,7 @@ std::string SmoothCollisions::to_string( } // NOTE: Actually distance squared -double SmoothCollisions::compute_minimum_distance( +double GCPCollisions::compute_minimum_distance( const CollisionMesh& mesh, Eigen::ConstRef vertices) const { assert(vertices.rows() == mesh.num_vertices()); @@ -274,7 +275,7 @@ double SmoothCollisions::compute_minimum_distance( return storage.combine([](double a, double b) { return std::min(a, b); }); } -double SmoothCollisions::compute_active_minimum_distance( +double GCPCollisions::compute_active_minimum_distance( const CollisionMesh& mesh, Eigen::ConstRef vertices) const { assert(vertices.rows() == mesh.num_vertices()); diff --git a/src/ipc/gcp/gcp_collisions.hpp b/src/ipc/gcp/gcp_collisions.hpp new file mode 100644 index 000000000..c1c9b6e23 --- /dev/null +++ b/src/ipc/gcp/gcp_collisions.hpp @@ -0,0 +1,166 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace ipc { +class GCPCollisions { +public: + /// @brief The type of the collisions. + using value_type = GCPCollision; + +public: + GCPCollisions() = default; + virtual ~GCPCollisions() = default; + + void compute_adaptive_dhat( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const GCPParameters params, + BroadPhase* broad_phase = nullptr); + + /// @brief Initialize the set of collisions used to compute the barrier potential. + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @param params The smooth contact parameters. + /// @param use_adaptive_dhat Use the per-element activation distances from a + /// prior call to compute_adaptive_dhat instead of a uniform + /// params.dhat. + /// @param broad_phase Broad-phase method to use. + void build( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const GCPParameters params, + const bool use_adaptive_dhat = false, + BroadPhase* broad_phase = nullptr); + + /// @brief Initialize the set of collisions used to compute the barrier potential. + /// @param candidates Distance candidates from which the collision set is built. + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @param params The smooth contact parameters. + /// @param use_adaptive_dhat Use the per-element activation distances from a + /// prior call to compute_adaptive_dhat instead of a uniform + /// params.dhat. + void build( + const Candidates& candidates, + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const GCPParameters params, + const bool use_adaptive_dhat = false); + + // ------------------------------------------------------------------------ + + /// @brief Get the number of collisions. + size_t size() const; + + /// @brief Get if the collision set are empty. + bool empty() const; + + /// @brief Clear the collision set. + void clear(); + + /// @brief Get a reference to collision at index i. + /// @param i The index of the collision. + /// @return A reference to the collision. + GCPCollision& operator[](size_t i); + + /// @brief Get a const reference to collision at index i. + /// @param i The index of the collision. + /// @return A const reference to the collision. + const GCPCollision& operator[](size_t i) const; + + /// @brief Compute minimum distance between all contact candidates + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @return Squared minimum distance + double compute_minimum_distance( + const CollisionMesh& mesh, + Eigen::ConstRef vertices) const; + + /// @brief Compute minimum distance between contact pairs with non-zero potential + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @return Squared minimum distance + double compute_active_minimum_distance( + const CollisionMesh& mesh, + Eigen::ConstRef vertices) const; + + /// @brief Convert contact pairs to string + std::string to_string( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const GCPParameters& params) const; + + /// @brief Get per-vertex dhat value when dhat is adaptive + double get_vert_dhat(int vert_id) const + { + if (vert_adaptive_dhat.size() > 1) { + return vert_adaptive_dhat(vert_id); + } else { + return vert_adaptive_dhat(0); + } + } + /// @brief Get per-edge dhat value when dhat is adaptive + double get_edge_dhat(int edge_id) const + { + if (edge_adaptive_dhat.size() > 1) { + return edge_adaptive_dhat(edge_id); + } else { + return edge_adaptive_dhat(0); + } + } + /// @brief Get per-face dhat value when dhat is adaptive + double get_face_dhat(int face_id) const + { + if (face_adaptive_dhat.size() > 1) { + return face_adaptive_dhat(face_id); + } else { + return face_adaptive_dhat(0); + } + } + /// @brief Get maximum dhat value when dhat is adaptive + double get_max_dhat() const + { + double out = std::max( + vert_adaptive_dhat.maxCoeff(), edge_adaptive_dhat.maxCoeff()); + if (face_adaptive_dhat.size() > 0) { + return std::max(out, face_adaptive_dhat.maxCoeff()); + } + return out; + } + + /// @brief Number of contact candidates + int n_candidates() const { return m_candidates.size(); } + +public: + /// @brief (active) collision pairs + std::vector> collisions; + + /// @brief per-vertex adaptive dhat + Eigen::VectorXd vert_adaptive_dhat; + /// @brief per-edge adaptive dhat + Eigen::VectorXd edge_adaptive_dhat; + /// @brief per-face adaptive dhat + Eigen::VectorXd face_adaptive_dhat; + + /// @brief Collision candidates + Candidates m_candidates; +}; + +using SmoothCollisions [[deprecated( + "SmoothCollisions is deprecated. Use GCPCollisions instead.")]] = + GCPCollisions; + +} // namespace ipc diff --git a/src/ipc/smooth_contact/smooth_collisions_builder.cpp b/src/ipc/gcp/gcp_collisions_builder.cpp similarity index 82% rename from src/ipc/smooth_contact/smooth_collisions_builder.cpp rename to src/ipc/gcp/gcp_collisions_builder.cpp index 21b94a101..31f6d9447 100644 --- a/src/ipc/smooth_contact/smooth_collisions_builder.cpp +++ b/src/ipc/gcp/gcp_collisions_builder.cpp @@ -1,4 +1,4 @@ -#include "smooth_collisions_builder.hpp" +#include "gcp_collisions_builder.hpp" #include #include @@ -15,7 +15,7 @@ namespace { const std::shared_ptr& pair, unordered_map, std::shared_ptr>& cc_to_id, - std::vector>& collisions) + std::vector>& collisions) { assert(pair != nullptr); if (pair->is_active() @@ -29,7 +29,7 @@ namespace { template void add_collision( const std::shared_ptr& pair, - std::vector>& collisions) + std::vector>& collisions) { assert(pair != nullptr); if (pair->is_active()) { @@ -38,11 +38,11 @@ namespace { } } // namespace -void SmoothCollisionsBuilder<2>::add_edge_vertex_collisions( +void GCPCollisionsBuilder<2>::add_edge_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::vector& candidates, - const SmoothContactParameters& params, + const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, const size_t start_i, @@ -56,8 +56,8 @@ void SmoothCollisionsBuilder<2>::add_edge_vertex_collisions( vertices.row(mesh.edges()(ei, 1))); if (pe_dtype == PointEdgeDistanceType::P_E) { - add_collision<2, SmoothCollisionTemplate>( - std::make_shared>( + add_collision<2, GCPCollisionTemplate>( + std::make_shared>( ei, vi, pe_dtype, mesh, params, std::min(edge_dhat(ei), vert_dhat(vi)), vertices), vert_edge_2_to_id, collisions); @@ -69,8 +69,8 @@ void SmoothCollisionsBuilder<2>::add_edge_vertex_collisions( if ((vertices.row(vi) - vertices.row(vj)).norm() >= dhat) { continue; } - add_collision<2, SmoothCollisionTemplate>( - std::make_shared>( + add_collision<2, GCPCollisionTemplate>( + std::make_shared>( std::min(vi, vj), std::max(vi, vj), PointPointDistanceType::P_P, mesh, params, dhat, vertices), vert_vert_2_to_id, collisions); @@ -80,11 +80,11 @@ void SmoothCollisionsBuilder<2>::add_edge_vertex_collisions( // ============================================================================ -void SmoothCollisionsBuilder<3>::add_edge_edge_collisions( +void GCPCollisionsBuilder<3>::add_edge_edge_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::vector& candidates, - const SmoothContactParameters& params, + const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, const size_t start_i, @@ -107,19 +107,19 @@ void SmoothCollisionsBuilder<3>::add_edge_edge_collisions( continue; } - add_collision<3, SmoothCollisionTemplate>( - std::make_shared>( + add_collision<3, GCPCollisionTemplate>( + std::make_shared>( std::min(eai, ebi), std::max(eai, ebi), actual_dtype, mesh, params, std::min(edge_dhat(eai), edge_dhat(ebi)), vertices), collisions); } } -void SmoothCollisionsBuilder<3>::add_face_vertex_collisions( +void GCPCollisionsBuilder<3>::add_face_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::vector& candidates, - const SmoothContactParameters& params, + const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, const std::function& face_dhat, @@ -143,8 +143,8 @@ void SmoothCollisionsBuilder<3>::add_face_vertex_collisions( } if (pt_dtype == PointTriangleDistanceType::P_T) { - add_collision<3, SmoothCollisionTemplate>( - std::make_shared>( + add_collision<3, GCPCollisionTemplate>( + std::make_shared>( fi, vi, pt_dtype, mesh, params, std::min(face_dhat(fi), vert_dhat(vi)), vertices), collisions); @@ -156,8 +156,8 @@ void SmoothCollisionsBuilder<3>::add_face_vertex_collisions( if ((vertices.row(vi) - vertices.row(vj)).norm() >= dhat) { continue; } - add_collision<3, SmoothCollisionTemplate>( - std::make_shared>( + add_collision<3, GCPCollisionTemplate>( + std::make_shared>( std::min(vi, vj), std::max(vi, vj), PointPointDistanceType::P_P, mesh, params, dhat, vertices), vert_vert_3_to_id, collisions); @@ -179,26 +179,26 @@ void SmoothCollisionsBuilder<3>::add_face_vertex_collisions( continue; } - add_collision<3, SmoothCollisionTemplate>( - std::make_shared>( + add_collision<3, GCPCollisionTemplate>( + std::make_shared>( eid, vi, pe_dtype, mesh, params, dhat, vertices), edge_vert_3_to_id, collisions); } } } -void SmoothCollisionsBuilder<3>::merge( - const tbb::enumerable_thread_specific>& +void GCPCollisionsBuilder<3>::merge( + const tbb::enumerable_thread_specific>& local_storage, - SmoothCollisions& merged_collisions) + GCPCollisions& merged_collisions) { unordered_map< std::pair, - std::shared_ptr>> + std::shared_ptr>> vert_vert_3_to_id; unordered_map< std::pair, - std::shared_ptr>> + std::shared_ptr>> edge_vert_3_to_id; // size up the hash items @@ -248,18 +248,18 @@ void SmoothCollisionsBuilder<3>::merge( edge_edge_count); } -void SmoothCollisionsBuilder<2>::merge( - const tbb::enumerable_thread_specific>& +void GCPCollisionsBuilder<2>::merge( + const tbb::enumerable_thread_specific>& local_storage, - SmoothCollisions& merged_collisions) + GCPCollisions& merged_collisions) { unordered_map< std::pair, - std::shared_ptr>> + std::shared_ptr>> vert_vert_2_to_id; unordered_map< std::pair, - std::shared_ptr>> + std::shared_ptr>> vert_edge_2_to_id; // size up the hash items diff --git a/src/ipc/smooth_contact/smooth_collisions_builder.hpp b/src/ipc/gcp/gcp_collisions_builder.hpp similarity index 69% rename from src/ipc/smooth_contact/smooth_collisions_builder.hpp rename to src/ipc/gcp/gcp_collisions_builder.hpp index 0bd04e69e..64271f666 100644 --- a/src/ipc/smooth_contact/smooth_collisions_builder.hpp +++ b/src/ipc/gcp/gcp_collisions_builder.hpp @@ -6,7 +6,7 @@ #pragma once #include -#include +#include #include #include @@ -14,17 +14,17 @@ namespace ipc { -template class SmoothCollisionsBuilder; +template class GCPCollisionsBuilder; -template <> class SmoothCollisionsBuilder<2> { +template <> class GCPCollisionsBuilder<2> { public: - SmoothCollisionsBuilder() { } + GCPCollisionsBuilder() { } void add_edge_vertex_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::vector& candidates, - const SmoothContactParameters& params, + const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, const size_t start_i, @@ -33,35 +33,35 @@ template <> class SmoothCollisionsBuilder<2> { // ------------------------------------------------------------------------- static void merge( - const tbb::enumerable_thread_specific>& + const tbb::enumerable_thread_specific>& local_storage, - SmoothCollisions& merged_collisions); + GCPCollisions& merged_collisions); // Constructed collisions - std::vector> collisions; + std::vector> collisions; // ------------------------------------------------------------------------- // Store the indices to pairs to avoid duplicates. unordered_map< std::pair, - std::shared_ptr>> + std::shared_ptr>> vert_vert_2_to_id; unordered_map< std::pair, - std::shared_ptr>> + std::shared_ptr>> vert_edge_2_to_id; }; -template <> class SmoothCollisionsBuilder<3> { +template <> class GCPCollisionsBuilder<3> { public: - SmoothCollisionsBuilder() { } + GCPCollisionsBuilder() { } void add_edge_edge_collisions( const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::vector& candidates, - const SmoothContactParameters& params, + const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, const size_t start_i, @@ -71,7 +71,7 @@ template <> class SmoothCollisionsBuilder<3> { const CollisionMesh& mesh, Eigen::ConstRef vertices, const std::vector& candidates, - const SmoothContactParameters& params, + const GCPParameters& params, const std::function& vert_dhat, const std::function& edge_dhat, const std::function& face_dhat, @@ -81,12 +81,12 @@ template <> class SmoothCollisionsBuilder<3> { // ------------------------------------------------------------------------- static void merge( - const tbb::enumerable_thread_specific>& + const tbb::enumerable_thread_specific>& local_storage, - SmoothCollisions& merged_collisions); + GCPCollisions& merged_collisions); // Constructed collisions - std::vector> collisions; + std::vector> collisions; // ------------------------------------------------------------------------- @@ -94,12 +94,17 @@ template <> class SmoothCollisionsBuilder<3> { // and Edge-Edge unordered_map< std::pair, - std::shared_ptr>> + std::shared_ptr>> vert_vert_3_to_id; unordered_map< std::pair, - std::shared_ptr>> + std::shared_ptr>> edge_vert_3_to_id; }; -} // namespace ipc \ No newline at end of file +template +using SmoothCollisionsBuilder [[deprecated( + "SmoothCollisionsBuilder is deprecated. Use GCPCollisionsBuilder instead.")]] = + GCPCollisionsBuilder; + +} // namespace ipc diff --git a/src/ipc/smooth_contact/smooth_contact_potential.cpp b/src/ipc/gcp/gcp_potential.cpp similarity index 88% rename from src/ipc/smooth_contact/smooth_contact_potential.cpp rename to src/ipc/gcp/gcp_potential.cpp index d07673c2a..e6f2fcc17 100644 --- a/src/ipc/smooth_contact/smooth_contact_potential.cpp +++ b/src/ipc/gcp/gcp_potential.cpp @@ -1,4 +1,4 @@ -#include "smooth_contact_potential.hpp" +#include "gcp_potential.hpp" #include #include @@ -8,8 +8,8 @@ namespace ipc { -double SmoothContactPotential::operator()( - const SmoothCollisions& collisions, +double GCPPotential::operator()( + const GCPCollisions& collisions, const CollisionMesh& mesh, Eigen::ConstRef X) const { @@ -29,8 +29,8 @@ double SmoothContactPotential::operator()( return storage.combine([](double a, double b) { return a + b; }); } -Eigen::VectorXd SmoothContactPotential::gradient( - const SmoothCollisions& collisions, +Eigen::VectorXd GCPPotential::gradient( + const GCPCollisions& collisions, const CollisionMesh& mesh, Eigen::ConstRef X) const { @@ -48,14 +48,14 @@ Eigen::VectorXd SmoothContactPotential::gradient( return assemble_gradient( X.size(), dim, collisions.size(), [&](const size_t i) -> Eigen::VectorXd { - const SmoothCollision& collision = collisions[i]; + const GCPCollision& collision = collisions[i]; return this->gradient(collision, collision.dof(X)); }, [&](const size_t i) { return collisions[i].vertex_ids(); }); } -Eigen::SparseMatrix SmoothContactPotential::hessian( - const SmoothCollisions& collisions, +Eigen::SparseMatrix GCPPotential::hessian( + const GCPCollisions& collisions, const CollisionMesh& mesh, Eigen::ConstRef X, const PSDProjectionMethod project_hessian_to_psd) const @@ -74,7 +74,7 @@ Eigen::SparseMatrix SmoothContactPotential::hessian( tbb::enumerable_thread_specific storage( LocalThreadMatStorage(buffer_size, ndof, ndof)); tbb::parallel_for(size_t(0), collisions.size(), [&](size_t i) { - const SmoothCollision& collision = collisions[i]; + const GCPCollision& collision = collisions[i]; const Eigen::MatrixXd local_hess = this->hessian( collisions[i], collisions[i].dof(X), project_hessian_to_psd); @@ -164,22 +164,22 @@ Eigen::SparseMatrix SmoothContactPotential::hessian( return hess; } -double SmoothContactPotential::operator()( - const SmoothCollision& collision, +double GCPPotential::operator()( + const GCPCollision& collision, Eigen::ConstRef positions) const { return collision.weight * collision(positions, params); } -Eigen::VectorXd SmoothContactPotential::gradient( - const SmoothCollision& collision, +Eigen::VectorXd GCPPotential::gradient( + const GCPCollision& collision, Eigen::ConstRef positions) const { return collision.weight * collision.gradient(positions, params); } -Eigen::MatrixXd SmoothContactPotential::hessian( - const SmoothCollision& collision, +Eigen::MatrixXd GCPPotential::hessian( + const GCPCollision& collision, Eigen::ConstRef positions, const PSDProjectionMethod project_hessian_to_psd) const { diff --git a/src/ipc/gcp/gcp_potential.hpp b/src/ipc/gcp/gcp_potential.hpp new file mode 100644 index 000000000..cad235025 --- /dev/null +++ b/src/ipc/gcp/gcp_potential.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +class GCPPotential { +public: + GCPPotential(const GCPParameters& _params) : params(_params) { } + + virtual ~GCPPotential() = default; + + // -- Cumulative methods --------------------------------------------------- + + /// @brief Compute the potential for a set of collisions. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @returns The potential for a set of collisions. + double operator()( + const GCPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const; + + /// @brief Compute the gradient of the potential. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @returns The gradient of the potential w.r.t. X. This will have a size of X.size(). + Eigen::VectorXd gradient( + const GCPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const; + + /// @brief Compute the hessian of the potential. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @param project_hessian_to_psd Make sure the hessian is positive semi-definite. + /// @returns The Hessian of the potential w.r.t. X. This will have a size of X.size() by X.size(). + virtual Eigen::SparseMatrix hessian( + const GCPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const; + + // -- Single collision methods --------------------------------------------- + + /// @brief Compute the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @return The potential. + double operator()( + const GCPCollision& collision, + Eigen::ConstRef positions) const; + + /// @brief Compute the gradient of the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @return The gradient of the potential. + Eigen::VectorXd gradient( + const GCPCollision& collision, + Eigen::ConstRef positions) const; + + /// @brief Compute the hessian of the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @param project_hessian_to_psd Whether to project the hessian to the positive semi-definite cone. + /// @return The hessian of the potential. + Eigen::MatrixXd hessian( + const GCPCollision& collision, + Eigen::ConstRef positions, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const; + +protected: + /// @brief GCP parameters for collision potential + GCPParameters params; +}; + +using SmoothContactPotential [[deprecated( + "SmoothContactPotential is deprecated. Use GCPPotential instead.")]] = + GCPPotential; + +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/CMakeLists.txt b/src/ipc/gcp/primitives/CMakeLists.txt similarity index 100% rename from src/ipc/smooth_contact/primitives/CMakeLists.txt rename to src/ipc/gcp/primitives/CMakeLists.txt diff --git a/src/ipc/smooth_contact/primitives/edge.cpp b/src/ipc/gcp/primitives/edge.cpp similarity index 97% rename from src/ipc/smooth_contact/primitives/edge.cpp rename to src/ipc/gcp/primitives/edge.cpp index 24a0db29a..465ebd488 100644 --- a/src/ipc/smooth_contact/primitives/edge.cpp +++ b/src/ipc/gcp/primitives/edge.cpp @@ -10,7 +10,7 @@ Edge::Edge( const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params) + const GCPParameters& params) : Primitive(id, params) { m_vertex_ids = { { mesh.edges()(id, 0), mesh.edges()(id, 1) } }; diff --git a/src/ipc/smooth_contact/primitives/edge.hpp b/src/ipc/gcp/primitives/edge.hpp similarity index 97% rename from src/ipc/smooth_contact/primitives/edge.hpp rename to src/ipc/gcp/primitives/edge.hpp index d1016a948..70899fb0a 100644 --- a/src/ipc/smooth_contact/primitives/edge.hpp +++ b/src/ipc/gcp/primitives/edge.hpp @@ -21,7 +21,7 @@ template class Edge : public Primitive { const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params); + const GCPParameters& params); int n_vertices() const override; int n_dofs() const override { return n_vertices() * DIM; } diff --git a/src/ipc/smooth_contact/primitives/edge2.cpp b/src/ipc/gcp/primitives/edge2.cpp similarity index 97% rename from src/ipc/smooth_contact/primitives/edge2.cpp rename to src/ipc/gcp/primitives/edge2.cpp index 60e8f52e4..c43f081a5 100644 --- a/src/ipc/smooth_contact/primitives/edge2.cpp +++ b/src/ipc/gcp/primitives/edge2.cpp @@ -8,7 +8,7 @@ Edge2::Edge2( const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params) + const GCPParameters& params) : Primitive(id, params) { m_vertex_ids = { { mesh.edges()(id, 0), mesh.edges()(id, 1) } }; diff --git a/src/ipc/smooth_contact/primitives/edge2.hpp b/src/ipc/gcp/primitives/edge2.hpp similarity index 89% rename from src/ipc/smooth_contact/primitives/edge2.hpp rename to src/ipc/gcp/primitives/edge2.hpp index d9bfe5efa..10ef2fa9d 100644 --- a/src/ipc/smooth_contact/primitives/edge2.hpp +++ b/src/ipc/gcp/primitives/edge2.hpp @@ -2,7 +2,7 @@ #include "primitive.hpp" -#include +#include namespace ipc { class Edge2 : public Primitive { @@ -16,7 +16,7 @@ class Edge2 : public Primitive { const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params); + const GCPParameters& params); int n_vertices() const override; int n_dofs() const override { return n_vertices() * DIM; } diff --git a/src/ipc/smooth_contact/primitives/edge3.cpp b/src/ipc/gcp/primitives/edge3.cpp similarity index 99% rename from src/ipc/smooth_contact/primitives/edge3.cpp rename to src/ipc/gcp/primitives/edge3.cpp index 884c355ff..3f3f2ca9b 100644 --- a/src/ipc/smooth_contact/primitives/edge3.cpp +++ b/src/ipc/gcp/primitives/edge3.cpp @@ -15,7 +15,7 @@ Edge3::Edge3( const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params) + const GCPParameters& params) : Primitive(id, params) { orientable = diff --git a/src/ipc/smooth_contact/primitives/edge3.hpp b/src/ipc/gcp/primitives/edge3.hpp similarity index 99% rename from src/ipc/smooth_contact/primitives/edge3.hpp rename to src/ipc/gcp/primitives/edge3.hpp index a2f266e32..c5ec200b6 100644 --- a/src/ipc/smooth_contact/primitives/edge3.hpp +++ b/src/ipc/gcp/primitives/edge3.hpp @@ -2,7 +2,7 @@ #include "primitive.hpp" -#include +#include namespace ipc { class Edge3 : public Primitive { @@ -22,7 +22,7 @@ class Edge3 : public Primitive { const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params); + const GCPParameters& params); /// @brief Get the number of vertices (edge endpoints + face-opposite vertices) int n_vertices() const override { return m_vertex_ids.size(); } diff --git a/src/ipc/smooth_contact/primitives/face.cpp b/src/ipc/gcp/primitives/face.cpp similarity index 98% rename from src/ipc/smooth_contact/primitives/face.cpp rename to src/ipc/gcp/primitives/face.cpp index 9621b8f7e..3c7c2a07c 100644 --- a/src/ipc/smooth_contact/primitives/face.cpp +++ b/src/ipc/gcp/primitives/face.cpp @@ -25,7 +25,7 @@ Face::Face( const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params) + const GCPParameters& params) : Primitive(id, params) { m_vertex_ids = { { mesh.faces()(id, 0), mesh.faces()(id, 1), diff --git a/src/ipc/smooth_contact/primitives/face.hpp b/src/ipc/gcp/primitives/face.hpp similarity index 92% rename from src/ipc/smooth_contact/primitives/face.hpp rename to src/ipc/gcp/primitives/face.hpp index db061b3f6..1b9453055 100644 --- a/src/ipc/smooth_contact/primitives/face.hpp +++ b/src/ipc/gcp/primitives/face.hpp @@ -2,7 +2,7 @@ #include "primitive.hpp" -#include +#include namespace ipc { class Face : public Primitive { @@ -16,7 +16,7 @@ class Face : public Primitive { const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params); + const GCPParameters& params); int n_vertices() const override; int n_dofs() const override { return n_vertices() * DIM; } diff --git a/src/ipc/smooth_contact/primitives/point2.cpp b/src/ipc/gcp/primitives/point2.cpp similarity index 96% rename from src/ipc/smooth_contact/primitives/point2.cpp rename to src/ipc/gcp/primitives/point2.cpp index 9d7e0ba79..d055de14c 100644 --- a/src/ipc/smooth_contact/primitives/point2.cpp +++ b/src/ipc/gcp/primitives/point2.cpp @@ -1,5 +1,7 @@ #include "point2.hpp" +#include + namespace ipc { namespace { @@ -16,7 +18,7 @@ namespace { } else if (mesh.edges()(i, 1) == v) { neighbors[1] = mesh.edges()(i, 0); } else { - throw std::runtime_error("Invalid edge-vertex adjacency!"); + log_and_throw_error("Invalid edge-vertex adjacency!"); } } @@ -28,7 +30,7 @@ namespace { Eigen::ConstRef direc, Eigen::ConstRef e0, Eigen::ConstRef e1, - const SmoothContactParameters& params, + const GCPParameters& params, const bool orientable) { const Eigen::Vector2d dn = -direc.normalized(); @@ -60,7 +62,7 @@ namespace { Eigen::ConstRef> direc, Eigen::ConstRef> e0, Eigen::ConstRef> e1, - const SmoothContactParameters& params, + const GCPParameters& params, const bool orientable) { const Eigen::Vector2 dn = -direc.normalized(); @@ -91,7 +93,7 @@ namespace { Eigen::ConstRef> v, Eigen::ConstRef> direc, Eigen::ConstRef> e0, - const SmoothContactParameters& params) + const GCPParameters& params) { const Eigen::Vector2 dn = -direc.normalized(); const Eigen::Vector2 t0 = e0 - v; @@ -108,7 +110,7 @@ Point2::Point2( const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params) + const GCPParameters& params) : Primitive(id, params) { orientable = mesh.is_orient_vertex(id); diff --git a/src/ipc/smooth_contact/primitives/point2.hpp b/src/ipc/gcp/primitives/point2.hpp similarity index 93% rename from src/ipc/smooth_contact/primitives/point2.hpp rename to src/ipc/gcp/primitives/point2.hpp index a72ac2ea2..699b40a63 100644 --- a/src/ipc/smooth_contact/primitives/point2.hpp +++ b/src/ipc/gcp/primitives/point2.hpp @@ -2,7 +2,7 @@ #include "primitive.hpp" -#include +#include namespace ipc { class Point2 : public Primitive { @@ -16,7 +16,7 @@ class Point2 : public Primitive { const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params); + const GCPParameters& params); Point2( const index_t id, diff --git a/src/ipc/smooth_contact/primitives/point3.cpp b/src/ipc/gcp/primitives/point3.cpp similarity index 99% rename from src/ipc/smooth_contact/primitives/point3.cpp rename to src/ipc/gcp/primitives/point3.cpp index 2ddb6391f..96dff4234 100644 --- a/src/ipc/smooth_contact/primitives/point3.cpp +++ b/src/ipc/gcp/primitives/point3.cpp @@ -11,7 +11,7 @@ Point3::Point3( const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params) + const GCPParameters& params) : Primitive(id, params) { orientable = @@ -406,7 +406,7 @@ bool Point3::smooth_point3_term_type( GradientType<-1> Point3::smooth_point3_term_gradient( Eigen::ConstRef direc, Eigen::ConstRef X, - const SmoothContactParameters& params) const + const GCPParameters& params) const { const int n_dofs = (X.rows() + 1) * 3; const int n_neighbor_dofs = n_neighbors * 3; @@ -453,7 +453,7 @@ GradientType<-1> Point3::smooth_point3_term_gradient( HessianType<-1> Point3::smooth_point3_term_hessian( Eigen::ConstRef direc, Eigen::ConstRef X, - const SmoothContactParameters& params) const + const GCPParameters& params) const { const int n_dofs = (X.rows() + 1) * 3; const int n_neighbor_dofs = n_neighbors * 3; diff --git a/src/ipc/smooth_contact/primitives/point3.hpp b/src/ipc/gcp/primitives/point3.hpp similarity index 94% rename from src/ipc/smooth_contact/primitives/point3.hpp rename to src/ipc/gcp/primitives/point3.hpp index 6e9a76c36..4003ce6a8 100644 --- a/src/ipc/smooth_contact/primitives/point3.hpp +++ b/src/ipc/gcp/primitives/point3.hpp @@ -2,7 +2,7 @@ #include "primitive.hpp" -#include +#include namespace ipc { class Point3 : public Primitive { @@ -16,7 +16,7 @@ class Point3 : public Primitive { const CollisionMesh& mesh, Eigen::ConstRef vertices, Eigen::ConstRef d, - const SmoothContactParameters& params); + const GCPParameters& params); Point3( const index_t id, @@ -55,12 +55,12 @@ class Point3 : public Primitive { GradientType<-1> smooth_point3_term_gradient( Eigen::ConstRef direc, Eigen::ConstRef X, - const SmoothContactParameters& params) const; + const GCPParameters& params) const; HessianType<-1> smooth_point3_term_hessian( Eigen::ConstRef direc, Eigen::ConstRef X, - const SmoothContactParameters& params) const; + const GCPParameters& params) const; GradientType<-1> smooth_point3_term_tangent_gradient( Eigen::ConstRef direc, diff --git a/src/ipc/smooth_contact/primitives/primitive.hpp b/src/ipc/gcp/primitives/primitive.hpp similarity index 90% rename from src/ipc/smooth_contact/primitives/primitive.hpp rename to src/ipc/gcp/primitives/primitive.hpp index a85943718..c2060bc72 100644 --- a/src/ipc/smooth_contact/primitives/primitive.hpp +++ b/src/ipc/gcp/primitives/primitive.hpp @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include namespace ipc { @@ -16,7 +16,7 @@ namespace ipc { */ class Primitive { public: - Primitive(const index_t id, const SmoothContactParameters& params) + Primitive(const index_t id, const GCPParameters& params) : m_params(params) , m_id(id) { @@ -39,7 +39,7 @@ class Primitive { protected: /// @brief GCP parameters - const SmoothContactParameters m_params; + const GCPParameters m_params; /// @brief Vertex IDs on this primitive std::vector m_vertex_ids; /// @brief Vertex/Edge/Face ID of this primitive diff --git a/src/ipc/ipc.cpp b/src/ipc/ipc.cpp index b838f33db..ee4010f6e 100644 --- a/src/ipc/ipc.cpp +++ b/src/ipc/ipc.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #ifdef IPC_TOOLKIT_WITH_CUDA @@ -63,7 +64,7 @@ double compute_collision_free_stepsize( #ifdef IPC_TOOLKIT_WITH_CUDA if (broad_phase->name() == "SweepAndTiniestQueue") { if (vertices_t0.cols() != 3) { - throw std::runtime_error( + log_and_throw_error( "Sweep and Tiniest Queue is only supported in 3D!"); } // Extract tolerance and max_iterations from narrow_phase_ccd if it's diff --git a/src/ipc/math/math.cpp b/src/ipc/math/math.cpp index 175ba657b..6ab30e342 100644 --- a/src/ipc/math/math.cpp +++ b/src/ipc/math/math.cpp @@ -1,7 +1,7 @@ #include "math.hpp" +#include #include -#include #include namespace ipc { diff --git a/src/ipc/math/scalar_math.hpp b/src/ipc/math/scalar_math.hpp index 47160358f..98f027fbf 100644 --- a/src/ipc/math/scalar_math.hpp +++ b/src/ipc/math/scalar_math.hpp @@ -41,6 +41,7 @@ namespace numext { IPC_TOOLKIT_DEFINE_STD(atan2) IPC_TOOLKIT_DEFINE_STD(fma) IPC_TOOLKIT_DEFINE_STD(log) + IPC_TOOLKIT_DEFINE_STD(pow) IPC_TOOLKIT_DEFINE_STD(sqrt) // This is a public header, so the macro does not outlive its use here. diff --git a/src/ipc/ogc/feasible_region.cpp b/src/ipc/ogc/feasible_region.cpp index 21d126b63..5182929dd 100644 --- a/src/ipc/ogc/feasible_region.cpp +++ b/src/ipc/ogc/feasible_region.cpp @@ -1,6 +1,7 @@ #include "feasible_region.hpp" #include +#include namespace ipc::ogc { @@ -83,9 +84,8 @@ bool is_edge_vertex_feasible( case PointEdgeDistanceType::AUTO: default: - throw std::invalid_argument( - fmt::format( - "Invalid PointEdgeDistanceType: {}", static_cast(dtype))); + log_and_throw_error( + "Invalid PointEdgeDistanceType: {}", static_cast(dtype)); } } @@ -162,9 +162,8 @@ bool is_edge_edge_feasible( case EdgeEdgeDistanceType::AUTO: default: - throw std::invalid_argument( - fmt::format( - "Invalid EdgeEdgeDistanceType: {}", static_cast(dtype))); + log_and_throw_error( + "Invalid EdgeEdgeDistanceType: {}", static_cast(dtype)); } } @@ -215,10 +214,8 @@ bool is_face_vertex_feasible( case PointTriangleDistanceType::AUTO: default: - throw std::invalid_argument( - fmt::format( - "Invalid PointTriangleDistanceType: {}", - static_cast(dtype))); + log_and_throw_error( + "Invalid PointTriangleDistanceType: {}", static_cast(dtype)); } } diff --git a/src/ipc/potentials/barrier_potential.cpp b/src/ipc/potentials/barrier_potential.cpp index 5a0ef8004..b84c75b88 100644 --- a/src/ipc/potentials/barrier_potential.cpp +++ b/src/ipc/potentials/barrier_potential.cpp @@ -2,6 +2,10 @@ #include #include +#include + +#include +#include namespace ipc { @@ -19,11 +23,13 @@ BarrierPotential::BarrierPotential( std::shared_ptr barrier, const double dhat, const double stiffness, - const bool use_physical_barrier) + const bool use_physical_barrier, + const bool use_squared_distance) : m_barrier(std::move(barrier)) , m_dhat(dhat) , m_stiffness(stiffness) , m_use_physical_barrier(use_physical_barrier) + , m_use_squared_distance(use_squared_distance) { assert(dhat > 0); assert(stiffness > 0); @@ -33,6 +39,12 @@ BarrierPotential::BarrierPotential( double BarrierPotential::force_magnitude( const double distance_squared, const double dmin) const { + if (!m_use_squared_distance) { + log_and_throw_error( + "BarrierPotential: force_magnitude not implemented for " + "use_squared_distance=false"); + } + double N = barrier_force_magnitude( distance_squared, barrier(), dhat(), stiffness(), dmin); @@ -48,6 +60,12 @@ VectorMax12d BarrierPotential::force_magnitude_gradient( Eigen::ConstRef distance_squared_gradient, const double dmin) const { + if (!m_use_squared_distance) { + log_and_throw_error( + "BarrierPotential: force_magnitude_gradient not implemented for " + "use_squared_distance=false"); + } + VectorMax12d grad_N = barrier_force_magnitude_gradient( distance_squared, distance_squared_gradient, barrier(), dhat(), stiffness(), dmin); @@ -62,6 +80,15 @@ VectorMax12d BarrierPotential::force_magnitude_gradient( double BarrierPotential::operator()( const double distance_squared, const double dmin) const { + if (!m_use_squared_distance) { + const double d = std::sqrt(distance_squared); + double b = barrier()(d - dmin, dhat()); + if (use_physical_barrier()) { + b *= dhat() / barrier().units(dhat()); + } + return stiffness() * b; + } + double b = barrier()(distance_squared - dmin * dmin, (2 * dmin + dhat()) * dhat()); @@ -75,6 +102,24 @@ double BarrierPotential::operator()( double BarrierPotential::gradient( const double distance_squared, const double dmin) const { + if (!m_use_squared_distance) { + const double d = std::sqrt(distance_squared); + // Chain-ruling b'(d) to d/d(d²) divides by d, so exact contact would + // give 0/0 = NaN (or ±inf, which the zero ∇d² then turns into NaN). + // We return 0 there instead to match the squared mode, where the + // barriers report zero derivatives once the distance reaches 0 (for + // the log barriers the energy is +inf there, so a line search rejects + // the step either way). + if (d == 0) { + return 0; + } + double db = barrier().first_derivative(d - dmin, dhat()) / (2.0 * d); + if (use_physical_barrier()) { + db *= dhat() / barrier().units(dhat()); + } + return stiffness() * db; + } + double db = barrier().first_derivative( distance_squared - dmin * dmin, (2 * dmin + dhat()) * dhat()); @@ -88,6 +133,21 @@ double BarrierPotential::gradient( double BarrierPotential::hessian( const double distance_squared, const double dmin) const { + if (!m_use_squared_distance) { + const double d = std::sqrt(distance_squared); + if (d == 0) { + return 0; // Same reasoning as in gradient(). + } + const double b1 = barrier().first_derivative(d - dmin, dhat()); + const double b2 = barrier().second_derivative(d - dmin, dhat()); + double d2b = + b2 / (4.0 * distance_squared) - b1 / (4.0 * d * distance_squared); + if (use_physical_barrier()) { + d2b *= dhat() / barrier().units(dhat()); + } + return stiffness() * d2b; + } + double d2b = barrier().second_derivative( distance_squared - dmin * dmin, (2 * dmin + dhat()) * dhat()); diff --git a/src/ipc/potentials/barrier_potential.hpp b/src/ipc/potentials/barrier_potential.hpp index 3c780c23b..9a0688b7c 100644 --- a/src/ipc/potentials/barrier_potential.hpp +++ b/src/ipc/potentials/barrier_potential.hpp @@ -31,11 +31,19 @@ class BarrierPotential : public NormalPotential { /// @param dhat The activation distance of the barrier. /// @param stiffness The stiffness of the barrier. /// @param use_physical_barrier Whether to use the physical barrier. + /// @param use_squared_distance If true (default), the barrier receives + /// squared distance d² as input (standard IPC convention). If false, + /// the barrier receives the Euclidean distance d - dmin with + /// activation distance dhat; the value, gradient and Hessian are + /// chain-ruled to d² and scaled by the stiffness as in the squared + /// mode. force_magnitude() and force_magnitude_gradient() throw in + /// this mode, so it cannot build tangential collisions. BarrierPotential( std::shared_ptr barrier, const double dhat, const double stiffness, - const bool use_physical_barrier = false); + const bool use_physical_barrier = false, + const bool use_squared_distance = true); /// @brief Get the activation distance of the barrier. double dhat() const { return m_dhat; } @@ -97,6 +105,9 @@ class BarrierPotential : public NormalPotential { Eigen::ConstRef distance_squared_gradient, const double dmin) const override; + /// @brief Get whether to use squared distance as input to the barrier. + bool use_squared_distance() const { return m_use_squared_distance; } + /// @brief Get whether to use the physical barrier. /// @note When using the convergent formulation we want the barrier to /// have units of Pa⋅m, so κ gets units of Pa and the barrier function @@ -153,6 +164,11 @@ class BarrierPotential : public NormalPotential { /// should have units of m. See notebooks/physical_barrier.ipynb for /// more details. bool m_use_physical_barrier = false; + + /// @brief If true (default), the barrier receives d² as input (standard + /// IPC convention). If false, it receives the distance d (see the + /// constructor). + bool m_use_squared_distance = true; }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/potentials/normal_potential.cpp b/src/ipc/potentials/normal_potential.cpp index 03f7bf197..fcd646e7b 100644 --- a/src/ipc/potentials/normal_potential.cpp +++ b/src/ipc/potentials/normal_potential.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -332,8 +333,7 @@ void NormalPotential::shape_derivative( // First term: if (collision.weight_gradient.size() <= 0) { - throw std::runtime_error( - "Shape derivative is not computed for collisions!"); + log_and_throw_error("Shape derivative is not computed for collisions!"); } if (collision.weight_gradient.nonZeros() > 0) { diff --git a/src/ipc/potentials/tangential_potential.cpp b/src/ipc/potentials/tangential_potential.cpp index ad1e53e59..965113689 100644 --- a/src/ipc/potentials/tangential_potential.cpp +++ b/src/ipc/potentials/tangential_potential.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -120,7 +121,7 @@ Eigen::SparseMatrix TangentialPotential::force_jacobian( const TangentialCollision& collision = collisions[i]; assert(collision.weight_gradient.size() == rest_positions.size()); if (collision.weight_gradient.size() != rest_positions.size()) { - throw std::runtime_error( + log_and_throw_error( "Shape derivative is not computed for friction collision!"); } @@ -583,7 +584,7 @@ MatrixMax12d TangentialPotential::force_jacobian( return J; } -Eigen::VectorXd TangentialPotential::smooth_contact_force( +Eigen::VectorXd TangentialPotential::gcp_force( const TangentialCollisions& collisions, const CollisionMesh& mesh, Eigen::ConstRef rest_positions, @@ -604,7 +605,7 @@ Eigen::VectorXd TangentialPotential::smooth_contact_force( velocities.size(), dim, collisions.size(), [&](const size_t i) -> VectorMaxNd { const auto& collision = collisions[i]; - return smooth_contact_force( + return gcp_force( collision, collision.dof(rest_positions, edges, faces), collision.dof(lagged_displacements, edges, faces), collision.dof(velocities, edges, faces), no_mu); @@ -612,13 +613,13 @@ Eigen::VectorXd TangentialPotential::smooth_contact_force( [&](const size_t i) { return collisions[i].vertex_ids(edges, faces); }); } -Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( +Eigen::SparseMatrix TangentialPotential::gcp_force_jacobian( const TangentialCollisions& collisions, const CollisionMesh& mesh, Eigen::ConstRef rest_positions, Eigen::ConstRef lagged_displacements, Eigen::ConstRef velocities, - const SmoothContactParameters& params, + const GCPParameters& params, const DiffWRT wrt, const double dmin, const bool no_mu) const @@ -647,7 +648,7 @@ Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( // contact force const MatrixMaxNd local_force_jacobian = collision.normal_force_magnitude - * smooth_contact_force_jacobian_unit( + * gcp_force_jacobian_unit( collision, collision.dof(lagged_positions, edges, faces), collision.dof(velocities, edges, faces), wrt, false); @@ -661,7 +662,7 @@ Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( } // The term that includes derivatives of normal contact force - const VectorMaxNd local_force = smooth_contact_force( + const VectorMaxNd local_force = gcp_force( collision, collision.dof(rest_positions, edges, faces), collision.dof(lagged_displacements, edges, faces), collision.dof(velocities, edges, faces), false, true); @@ -670,7 +671,7 @@ Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( Eigen::VectorXd normal_force_grad; std::vector cc_vert_ids; Eigen::MatrixXd Xt = rest_positions + lagged_displacements; - auto cc = collision.smooth_collision; + auto cc = collision.gcp_collision; const Eigen::VectorXd contact_grad = cc->gradient(cc->dof(Xt), params); const Eigen::MatrixXd contact_hess = cc->hessian(cc->dof(Xt), params); normal_force_grad = @@ -694,7 +695,7 @@ Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( return jacobian; } -TangentialPotential::VectorMaxNd TangentialPotential::smooth_contact_force( +TangentialPotential::VectorMaxNd TangentialPotential::gcp_force( const TangentialCollision& collision, Eigen::ConstRef rest_positions, // = x Eigen::ConstRef lagged_displacements, // = u @@ -758,8 +759,27 @@ TangentialPotential::VectorMaxNd TangentialPotential::smooth_contact_force( * mu_f1_over_norm_tau * T * tau_aniso; } -TangentialPotential::MatrixMaxNd -TangentialPotential::smooth_contact_force_jacobian_unit( +Eigen::MatrixXd TangentialPotential::gcp_force_jacobian( + const TangentialCollision& collision, + Eigen::ConstRef rest_positions, // = x + Eigen::ConstRef lagged_displacements, // = u + Eigen::ConstRef velocities, // = v + const DiffWRT wrt, + const bool no_mu) const +{ + assert(rest_positions.size() == lagged_displacements.size()); + assert(rest_positions.size() == velocities.size()); + + // The force depends on x and u only through x + u, so the Jacobian is the + // same for either of them. We scale the unit Jacobian by the cached normal + // force N, which gcp_force() also treats as a constant. + return collision.normal_force_magnitude + * gcp_force_jacobian_unit( + collision, rest_positions + lagged_displacements, velocities, + wrt, no_mu); +} + +TangentialPotential::MatrixMaxNd TangentialPotential::gcp_force_jacobian_unit( const TangentialCollision& collision, Eigen::ConstRef lagged_positions, // = x + u^t Eigen::ConstRef velocities, // = v diff --git a/src/ipc/potentials/tangential_potential.hpp b/src/ipc/potentials/tangential_potential.hpp index f72a43960..6f917583f 100644 --- a/src/ipc/potentials/tangential_potential.hpp +++ b/src/ipc/potentials/tangential_potential.hpp @@ -70,7 +70,7 @@ class TangentialPotential : public Potential { const DiffWRT wrt, const double dmin = 0) const; - Eigen::VectorXd smooth_contact_force( + Eigen::VectorXd gcp_force( const TangentialCollisions& collisions, const CollisionMesh& mesh, Eigen::ConstRef rest_positions, @@ -79,13 +79,13 @@ class TangentialPotential : public Potential { const double dmin = 0, const bool no_mu = false) const; - Eigen::SparseMatrix smooth_contact_force_jacobian( + Eigen::SparseMatrix gcp_force_jacobian( const TangentialCollisions& collisions, const CollisionMesh& mesh, Eigen::ConstRef rest_positions, Eigen::ConstRef lagged_displacements, Eigen::ConstRef velocities, - const SmoothContactParameters& params, + const GCPParameters& params, const DiffWRT wrt, const double dmin = 0, const bool no_mu = false) const; @@ -155,7 +155,7 @@ class TangentialPotential : public Potential { const DiffWRT wrt, const double dmin = 0) const; - VectorMaxNd smooth_contact_force( + VectorMaxNd gcp_force( const TangentialCollision& collision, Eigen::ConstRef rest_positions, // = x Eigen::ConstRef lagged_displacements, // = u @@ -163,13 +163,26 @@ class TangentialPotential : public Potential { const bool no_mu = false, const bool no_contact_force_multiplier = false) const; - Eigen::MatrixXd smooth_contact_force_jacobian( + /// @brief Compute the Jacobian of gcp_force() for a single collision. + /// + /// Like gcp_force(), this holds the collision's cached normal force + /// magnitude fixed. Its derivative involves the vertices of the GCP + /// collision rather than this stencil, so the collision-set overload adds + /// it separately. + /// @param collision The collision + /// @param rest_positions Rest positions of the stencil's vertices. + /// @param lagged_displacements Lagged displacements of the stencil's vertices. + /// @param velocities Current velocities of the stencil's vertices. + /// @param wrt Variable to differentiate the friction force with respect to. + /// @param no_mu Whether to not multiply by mu + /// @return Friction force Jacobian + Eigen::MatrixXd gcp_force_jacobian( const TangentialCollision& collision, Eigen::ConstRef rest_positions, // = x Eigen::ConstRef lagged_displacements, // = u Eigen::ConstRef velocities, // = v const DiffWRT wrt, - const bool no_mu) const; + const bool no_mu = false) const; /// @brief Compute the friction force Jacobian assuming the contact force magnitude being 1. /// @param collision The collision @@ -178,13 +191,93 @@ class TangentialPotential : public Potential { /// @param wrt Variable to differentiate the friction force with respect to. /// @param no_mu Whether to not multiply by mu /// @return Friction force Jacobian - MatrixMaxNd smooth_contact_force_jacobian_unit( + MatrixMaxNd gcp_force_jacobian_unit( const TangentialCollision& collision, Eigen::ConstRef lagged_positions, Eigen::ConstRef velocities, const DiffWRT wrt, const bool no_mu = false) const; + // -- Deprecated names from before the smooth_contact -> gcp rename ------- + + [[deprecated("smooth_contact_force is deprecated. Use gcp_force instead.")]] + Eigen::VectorXd smooth_contact_force( + const TangentialCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef rest_positions, + Eigen::ConstRef lagged_displacements, + Eigen::ConstRef velocities, + const double dmin = 0, + const bool no_mu = false) const + { + return gcp_force( + collisions, mesh, rest_positions, lagged_displacements, velocities, + dmin, no_mu); + } + + [[deprecated( + "smooth_contact_force_jacobian is deprecated. Use " + "gcp_force_jacobian instead.")]] + Eigen::SparseMatrix smooth_contact_force_jacobian( + const TangentialCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef rest_positions, + Eigen::ConstRef lagged_displacements, + Eigen::ConstRef velocities, + const GCPParameters& params, + const DiffWRT wrt, + const double dmin = 0, + const bool no_mu = false) const + { + return gcp_force_jacobian( + collisions, mesh, rest_positions, lagged_displacements, velocities, + params, wrt, dmin, no_mu); + } + + [[deprecated("smooth_contact_force is deprecated. Use gcp_force instead.")]] + VectorMaxNd smooth_contact_force( + const TangentialCollision& collision, + Eigen::ConstRef rest_positions, + Eigen::ConstRef lagged_displacements, + Eigen::ConstRef velocities, + const bool no_mu = false, + const bool no_contact_force_multiplier = false) const + { + return gcp_force( + collision, rest_positions, lagged_displacements, velocities, no_mu, + no_contact_force_multiplier); + } + + [[deprecated( + "smooth_contact_force_jacobian is deprecated. Use " + "gcp_force_jacobian instead.")]] + Eigen::MatrixXd smooth_contact_force_jacobian( + const TangentialCollision& collision, + Eigen::ConstRef rest_positions, + Eigen::ConstRef lagged_displacements, + Eigen::ConstRef velocities, + const DiffWRT wrt, + const bool no_mu = false) const + { + return gcp_force_jacobian( + collision, rest_positions, lagged_displacements, velocities, wrt, + no_mu); + } + + [[deprecated( + "smooth_contact_force_jacobian_unit is deprecated. Use " + "gcp_force_jacobian_unit instead.")]] + MatrixMaxNd smooth_contact_force_jacobian_unit( + const TangentialCollision& collision, + Eigen::ConstRef lagged_positions, + Eigen::ConstRef velocities, + const DiffWRT wrt, + const bool no_mu = false) const + { + return gcp_force_jacobian_unit( + collision, lagged_positions, velocities, wrt, no_mu); + } + protected: /// @brief Compute the value of the ∫ μ(y) f₁(y) dy, where f₁ is the first derivative of the smooth mollifier. /// @param x The tangential relative speed. diff --git a/src/ipc/smooth_contact/smooth_collisions.hpp b/src/ipc/smooth_contact/smooth_collisions.hpp index cd1beaa1f..37e7f95b1 100644 --- a/src/ipc/smooth_contact/smooth_collisions.hpp +++ b/src/ipc/smooth_contact/smooth_collisions.hpp @@ -1,162 +1,15 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -namespace ipc { -class SmoothCollisions { -public: - /// @brief The type of the collisions. - using value_type = SmoothCollision; - -public: - SmoothCollisions() = default; - virtual ~SmoothCollisions() = default; - - void compute_adaptive_dhat( - const CollisionMesh& mesh, - Eigen::ConstRef vertices, - const SmoothContactParameters params, - BroadPhase* broad_phase = nullptr); - - /// @brief Initialize the set of collisions used to compute the barrier potential. - /// @param mesh The collision mesh. - /// @param vertices Vertices of the collision mesh. - /// @param params The smooth contact parameters. - /// @param use_adaptive_dhat Use the per-element activation distances from a - /// prior call to compute_adaptive_dhat instead of a uniform - /// params.dhat. - /// @param broad_phase Broad-phase method to use. - void build( - const CollisionMesh& mesh, - Eigen::ConstRef vertices, - const SmoothContactParameters params, - const bool use_adaptive_dhat = false, - BroadPhase* broad_phase = nullptr); - - /// @brief Initialize the set of collisions used to compute the barrier potential. - /// @param candidates Distance candidates from which the collision set is built. - /// @param mesh The collision mesh. - /// @param vertices Vertices of the collision mesh. - /// @param params The smooth contact parameters. - /// @param use_adaptive_dhat Use the per-element activation distances from a - /// prior call to compute_adaptive_dhat instead of a uniform - /// params.dhat. - void build( - const Candidates& candidates, - const CollisionMesh& mesh, - Eigen::ConstRef vertices, - const SmoothContactParameters params, - const bool use_adaptive_dhat = false); - - // ------------------------------------------------------------------------ - - /// @brief Get the number of collisions. - size_t size() const; - - /// @brief Get if the collision set are empty. - bool empty() const; - - /// @brief Clear the collision set. - void clear(); - - /// @brief Get a reference to collision at index i. - /// @param i The index of the collision. - /// @return A reference to the collision. - SmoothCollision& operator[](size_t i); - - /// @brief Get a const reference to collision at index i. - /// @param i The index of the collision. - /// @return A const reference to the collision. - const SmoothCollision& operator[](size_t i) const; - - /// @brief Compute minimum distance between all contact candidates - /// @param mesh The collision mesh. - /// @param vertices Vertices of the collision mesh. - /// @return Squared minimum distance - double compute_minimum_distance( - const CollisionMesh& mesh, - Eigen::ConstRef vertices) const; - - /// @brief Compute minimum distance between contact pairs with non-zero potential - /// @param mesh The collision mesh. - /// @param vertices Vertices of the collision mesh. - /// @return Squared minimum distance - double compute_active_minimum_distance( - const CollisionMesh& mesh, - Eigen::ConstRef vertices) const; - - /// @brief Convert contact pairs to string - std::string to_string( - const CollisionMesh& mesh, - Eigen::ConstRef vertices, - const SmoothContactParameters& params) const; - - /// @brief Get per-vertex dhat value when dhat is adaptive - double get_vert_dhat(int vert_id) const - { - if (vert_adaptive_dhat.size() > 1) { - return vert_adaptive_dhat(vert_id); - } else { - return vert_adaptive_dhat(0); - } - } - /// @brief Get per-edge dhat value when dhat is adaptive - double get_edge_dhat(int edge_id) const - { - if (edge_adaptive_dhat.size() > 1) { - return edge_adaptive_dhat(edge_id); - } else { - return edge_adaptive_dhat(0); - } - } - /// @brief Get per-face dhat value when dhat is adaptive - double get_face_dhat(int face_id) const - { - if (face_adaptive_dhat.size() > 1) { - return face_adaptive_dhat(face_id); - } else { - return face_adaptive_dhat(0); - } - } - /// @brief Get maximum dhat value when dhat is adaptive - double get_max_dhat() const - { - double out = std::max( - vert_adaptive_dhat.maxCoeff(), edge_adaptive_dhat.maxCoeff()); - if (face_adaptive_dhat.size() > 0) { - return std::max(out, face_adaptive_dhat.maxCoeff()); - } - return out; - } - - /// @brief Number of contact candidates - int n_candidates() const { return m_candidates.size(); } - -public: - /// @brief (active) collision pairs - std::vector> collisions; - - /// @brief per-vertex adaptive dhat - Eigen::VectorXd vert_adaptive_dhat; - /// @brief per-edge adaptive dhat - Eigen::VectorXd edge_adaptive_dhat; - /// @brief per-face adaptive dhat - Eigen::VectorXd face_adaptive_dhat; - - /// @brief Collision candidates - Candidates m_candidates; -}; - -} // namespace ipc +// Deprecated: include instead. We warn at include +// time because the old header path has no symbol for [[deprecated]] to attach +// to. The compiler reports this header's path along with the message. +#if defined(_MSC_VER) +#pragma message( \ + "warning: ipc/smooth_contact/smooth_collisions.hpp is deprecated. " \ + "Include instead.") +#else +#pragma GCC warning \ + "Deprecated header. Include instead." +#endif + +#include diff --git a/src/ipc/smooth_contact/smooth_contact_potential.hpp b/src/ipc/smooth_contact/smooth_contact_potential.hpp index 5e755ecf3..f0aaccf2e 100644 --- a/src/ipc/smooth_contact/smooth_contact_potential.hpp +++ b/src/ipc/smooth_contact/smooth_contact_potential.hpp @@ -1,87 +1,15 @@ #pragma once -#include -#include -#include - -namespace ipc { - -class SmoothContactPotential { -public: - SmoothContactPotential(const SmoothContactParameters& _params) - : params(_params) - { - } - - virtual ~SmoothContactPotential() = default; - - // -- Cumulative methods --------------------------------------------------- - - /// @brief Compute the potential for a set of collisions. - /// @param collisions The set of collisions. - /// @param mesh The collision mesh. - /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). - /// @returns The potential for a set of collisions. - double operator()( - const SmoothCollisions& collisions, - const CollisionMesh& mesh, - Eigen::ConstRef X) const; - - /// @brief Compute the gradient of the potential. - /// @param collisions The set of collisions. - /// @param mesh The collision mesh. - /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). - /// @returns The gradient of the potential w.r.t. X. This will have a size of X.size(). - Eigen::VectorXd gradient( - const SmoothCollisions& collisions, - const CollisionMesh& mesh, - Eigen::ConstRef X) const; - - /// @brief Compute the hessian of the potential. - /// @param collisions The set of collisions. - /// @param mesh The collision mesh. - /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). - /// @param project_hessian_to_psd Make sure the hessian is positive semi-definite. - /// @returns The Hessian of the potential w.r.t. X. This will have a size of X.size() by X.size(). - virtual Eigen::SparseMatrix hessian( - const SmoothCollisions& collisions, - const CollisionMesh& mesh, - Eigen::ConstRef X, - const PSDProjectionMethod project_hessian_to_psd = - PSDProjectionMethod::NONE) const; - - // -- Single collision methods --------------------------------------------- - - /// @brief Compute the potential for a single collision. - /// @param collision The collision. - /// @param positions The collision stencil's positions. - /// @return The potential. - double operator()( - const SmoothCollision& collision, - Eigen::ConstRef positions) const; - - /// @brief Compute the gradient of the potential for a single collision. - /// @param collision The collision. - /// @param positions The collision stencil's positions. - /// @return The gradient of the potential. - Eigen::VectorXd gradient( - const SmoothCollision& collision, - Eigen::ConstRef positions) const; - - /// @brief Compute the hessian of the potential for a single collision. - /// @param collision The collision. - /// @param positions The collision stencil's positions. - /// @param project_hessian_to_psd Whether to project the hessian to the positive semi-definite cone. - /// @return The hessian of the potential. - Eigen::MatrixXd hessian( - const SmoothCollision& collision, - Eigen::ConstRef positions, - const PSDProjectionMethod project_hessian_to_psd = - PSDProjectionMethod::NONE) const; - -protected: - /// @brief GCP parameters for collision potential - SmoothContactParameters params; -}; - -} // namespace ipc +// Deprecated: include instead. We warn at include +// time because the old header path has no symbol for [[deprecated]] to attach +// to. The compiler reports this header's path along with the message. +#if defined(_MSC_VER) +#pragma message( \ + "warning: ipc/smooth_contact/smooth_contact_potential.hpp is deprecated. " \ + "Include instead.") +#else +#pragma GCC warning \ + "Deprecated header. Include instead." +#endif + +#include diff --git a/src/ipc/utils/CMakeLists.txt b/src/ipc/utils/CMakeLists.txt index 0660e56a5..3e28d09bc 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -19,7 +19,6 @@ set(SOURCES simd.hpp save_obj.cpp save_obj.hpp - unordered_map_and_set.cpp unordered_map_and_set.hpp vertex_to_min_edge.cpp vertex_to_min_edge.hpp diff --git a/src/ipc/utils/eigen_ext.tpp b/src/ipc/utils/eigen_ext.tpp index d4f342fab..c648a0e51 100644 --- a/src/ipc/utils/eigen_ext.tpp +++ b/src/ipc/utils/eigen_ext.tpp @@ -96,7 +96,7 @@ project_to_psd( D.diagonal()[i] = std::abs(D.diagonal()[i]); break; default: - throw std::runtime_error("Invalid type of PSD projection!"); + log_and_throw_error("Invalid type of PSD projection!"); } } else { break; diff --git a/src/ipc/utils/logger.cpp b/src/ipc/utils/logger.cpp index 331fa945a..a953f6f84 100644 --- a/src/ipc/utils/logger.cpp +++ b/src/ipc/utils/logger.cpp @@ -38,10 +38,4 @@ void set_logger(std::shared_ptr logger) get_shared_logger() = std::move(logger); } -void log_and_throw_error(const std::string& msg) -{ - logger().error(msg); - throw std::runtime_error(msg); -} - } // namespace ipc diff --git a/src/ipc/utils/logger.hpp b/src/ipc/utils/logger.hpp index 07aea0971..322ae975e 100644 --- a/src/ipc/utils/logger.hpp +++ b/src/ipc/utils/logger.hpp @@ -17,13 +17,26 @@ spdlog::logger& logger(); /// @param logger New logger object to be used. void set_logger(std::shared_ptr logger); -[[noreturn]] void log_and_throw_error(const std::string& msg); +template +[[noreturn]] void log_and_throw_error(const std::string& msg) +{ + logger().error(msg); + throw Exception(msg); +} template [[noreturn]] void log_and_throw_error(const std::string& msg, const Args&... args) { - log_and_throw_error(fmt::format(fmt::runtime(msg), args...)); + log_and_throw_error( + fmt::format(fmt::runtime(msg), args...)); +} + +template +[[noreturn]] void +log_and_throw_error(const std::string& msg, const Args&... args) +{ + log_and_throw_error(fmt::format(fmt::runtime(msg), args...)); } } // namespace ipc diff --git a/src/ipc/utils/unordered_map_and_set.cpp b/src/ipc/utils/unordered_map_and_set.cpp deleted file mode 100644 index 7ecb2be67..000000000 --- a/src/ipc/utils/unordered_map_and_set.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "unordered_map_and_set.hpp" - -#ifndef IPC_TOOLKIT_WITH_ABSEIL - -#include // std::pair - -namespace ipc { - -template <> -Hash> -AbslHashValue(Hash> h, const std::pair p) -{ - return Hash>::combine(std::move(h), p.first, p.second); -} - -template <> -Hash> -AbslHashValue(Hash> h, std::pair p) -{ - return Hash>::combine( - std::move(h), p.first, p.second); -} - -template <> Hash AbslHashValue(Hash h, const int i) -{ - return Hash::combine(std::move(h), i); -} - -template <> -Hash> AbslHashValue( - Hash> h, const std::pair p) -{ - return Hash>::combine( - std::move(h), p.first, p.second); -} - -template <> Hash AbslHashValue(Hash h, const size_t i) -{ - return Hash::combine(std::move(h), i); -} - -} // namespace ipc - -#endif diff --git a/src/ipc/utils/unordered_map_and_set.hpp b/src/ipc/utils/unordered_map_and_set.hpp index 1f48bded0..41e5c51f8 100644 --- a/src/ipc/utils/unordered_map_and_set.hpp +++ b/src/ipc/utils/unordered_map_and_set.hpp @@ -10,12 +10,28 @@ #ifdef IPC_TOOLKIT_WITH_ABSEIL #include #else +#include #include #include +#include +#include namespace ipc { +// Types without a std::hash (e.g., the collision candidates) hash themselves +// by declaring a friend AbslHashValue, exactly as they would for Abseil. We +// only declare the primary template so those friends have something to +// overload. template H AbslHashValue(H h, T t); +// Instead of listing every pair and array key type we use, we hash them +// generically, element by element. This keeps the fallback in step with any +// index_t width. +template +H AbslHashValue(H h, const std::pair& p); + +template +H AbslHashValue(H h, const std::array& a); + template struct Hash { Hash() = default; Hash(size_t h) : hash(h) { } @@ -45,13 +61,34 @@ template struct Hash { size_t operator()(const T& t) const { - return AbslHashValue(*this, t); + // Integers (and anything else with a std::hash) need no + // AbslHashValue, so every index_t width works out of the box. + if constexpr (std::is_default_constructible>::value) { + return Hash::combine(*this, t); + } else { + return AbslHashValue(*this, t); + } } operator size_t() const { return hash; } size_t hash = 0; }; + +template +H AbslHashValue(H h, const std::pair& p) +{ + return H::combine(std::move(h), p.first, p.second); +} + +template +H AbslHashValue(H h, const std::array& a) +{ + for (const T& x : a) { + h = H::combine(std::move(h), x); + } + return h; +} } // namespace ipc #endif diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7cfa3e9ef..0c2ea4e80 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -70,6 +70,10 @@ if(IPC_TOOLKIT_WITH_MESHFEM_SPARSE) target_link_libraries(ipc_toolkit_tests PRIVATE MeshFEM::Sparse) endif() +# The exact distance-type reference in the tests uses geogram's exact +# arithmetic directly. +target_link_libraries(ipc_toolkit_tests PRIVATE geogram::multi_precision) + if(IPC_TOOLKIT_WITH_CUDA) # We need to explicitly state that we need all CUDA files in the particle # library to be built with -dc as the member functions could be called by diff --git a/tests/src/tests/barrier/test_adaptive_stiffness.cpp b/tests/src/tests/barrier/test_adaptive_stiffness.cpp index e6c115c30..02b51a15c 100644 --- a/tests/src/tests/barrier/test_adaptive_stiffness.cpp +++ b/tests/src/tests/barrier/test_adaptive_stiffness.cpp @@ -12,7 +12,7 @@ using namespace ipc; TEST_CASE("Initial barrier stiffness", "[stiffness][adaptive_stiffness]") { const double bbox_diagonal = 1.0; - const ClampedLogBarrier barrier; + const ClampedLogBarrier<> barrier; const double dhat = 1e-3; const double average_mass = 1.0; const Eigen::VectorXd grad_energy = Eigen::VectorXd::Constant(1, 100); diff --git a/tests/src/tests/barrier/test_barrier.cpp b/tests/src/tests/barrier/test_barrier.cpp index d50c67f32..8f0e1feed 100644 --- a/tests/src/tests/barrier/test_barrier.cpp +++ b/tests/src/tests/barrier/test_barrier.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -249,7 +250,7 @@ TEST_CASE("negative_orientation_penalty derivatives", "[deriv]") TEST_CASE("point term derivatives", "[deriv]") { - ipc::SmoothContactParameters params(1, 1, 1, 0.01, 0, 2); + ipc::GCPParameters params(1, 1, 1, 0.01, 0, 2); Eigen::MatrixX3d vectors(9, 3); vectors << -0.696515, -0.173578, -0.696231, 0.50146, -0.0017947, 0.999718, @@ -340,7 +341,7 @@ TEST_CASE("point term derivatives", "[deriv]") TEST_CASE("point term normal derivatives", "[deriv]") { - ipc::SmoothContactParameters params(1, 1, 1, 1, 0, 2); + ipc::GCPParameters params(1, 1, 1, 1, 0, 2); Eigen::MatrixX3d vectors(9, 3); vectors << -0.696515, -0.173578, -0.696231, 0.50146, -0.0017947, 0.999718, @@ -401,6 +402,18 @@ TEST_CASE("point term normal derivatives", "[deriv]") } } +TEST_CASE("Inverse-power barrier requires a positive power", "[barrier]") +{ + CHECK_NOTHROW(InversePowerBarrier<>(0.5)); + + // p = 0 would stay bounded at contact, p < 0 would attract, and a + // non-finite power would poison every evaluation. + const double power = GENERATE( + 0.0, -1.0, std::nan(""), std::numeric_limits::infinity()); + CAPTURE(power); + CHECK_THROWS_AS(InversePowerBarrier<>(power), std::invalid_argument); +} + TEST_CASE("Barrier derivatives", "[barrier]") { bool use_dist_sqr = GENERATE(false, true); @@ -436,6 +449,18 @@ TEST_CASE("Barrier derivatives", "[barrier]") { barrier = std::make_unique>(); } + SECTION("InversePower1") + { + barrier = std::make_unique>(1.0); + } + SECTION("InversePower2") + { + barrier = std::make_unique>(2.0); + } + SECTION("InversePower3") + { + barrier = std::make_unique>(3.0); + } if (use_dist_sqr) { d_vec *= d; @@ -474,7 +499,7 @@ TEST_CASE("Physical barrier", "[barrier]") { const bool use_dist_sqr = GENERATE(false, true); - ipc::ClampedLogBarrier original_barrier; + ipc::ClampedLogBarrier<> original_barrier; PhysicalBarrier new_barrier(use_dist_sqr); const double dhat = diff --git a/tests/src/tests/barrier/test_barrier_force_magnitude.cpp b/tests/src/tests/barrier/test_barrier_force_magnitude.cpp index d09f6bee0..6a247d2a2 100644 --- a/tests/src/tests/barrier/test_barrier_force_magnitude.cpp +++ b/tests/src/tests/barrier/test_barrier_force_magnitude.cpp @@ -14,7 +14,7 @@ TEST_CASE( "Point-triangle normal force magnitude", "[friction][point-triangle][normal_force_magnitude]") { - const ClampedLogBarrier barrier; + const ClampedLogBarrier<> barrier; Eigen::Vector3d p(0, 1e-4, 0), t0(-1, 0, 1), t1(1, 0, 1), t2(0, 0, -1); const double dhat = 1e-3, barrier_stiffness = 1e2; @@ -43,7 +43,7 @@ TEST_CASE( "Edge-edge normal force magnitude", "[friction][point-triangle][normal_force_magnitude]") { - const ClampedLogBarrier barrier; + const ClampedLogBarrier<> barrier; Eigen::Vector3d ea0(-1, -1e-4, 0), ea1(1, -1e-4, 0); Eigen::Vector3d eb0(0, 1e-4, -1), eb1(0, 1e-4, 1); @@ -73,7 +73,7 @@ TEST_CASE( "Point-edge normal force magnitude", "[friction][point-triangle][normal_force_magnitude]") { - const ClampedLogBarrier barrier; + const ClampedLogBarrier<> barrier; Eigen::Vector3d p(0, 1e-4, 0), e0(-1, 0, 0), e1(1, 0, 0); const double dhat = 1e-3, barrier_stiffness = 1e2; @@ -101,7 +101,7 @@ TEST_CASE( "Point-point normal force magnitude", "[friction][point-triangle][normal_force_magnitude]") { - const ClampedLogBarrier barrier; + const ClampedLogBarrier<> barrier; Eigen::Vector3d p0(0, 0, 0), p1(0, 0, 1e-4); const double dhat = 1e-3, barrier_stiffness = 1e2; @@ -128,7 +128,7 @@ TEST_CASE( "Point-edge normal force magnitude (2D)", "[friction][point-triangle][normal_force_magnitude]") { - const ClampedLogBarrier barrier; + const ClampedLogBarrier<> barrier; Eigen::Vector2d p(0, 1e-4), e0(-1, 0), e1(1, 0); const double dhat = 1e-3, barrier_stiffness = 1e2; @@ -155,7 +155,7 @@ TEST_CASE( "Point-point normal force magnitude (2D)", "[friction][point-triangle][normal_force_magnitude][2D]") { - const ClampedLogBarrier barrier; + const ClampedLogBarrier<> barrier; Eigen::Vector2d p0(0, 0), p1(0, 1e-4); const double dhat = 1e-3, barrier_stiffness = 1e2; diff --git a/tests/src/tests/barrier/test_simd_barrier.cpp b/tests/src/tests/barrier/test_simd_barrier.cpp index e6c2d13da..f59da40c9 100644 --- a/tests/src/tests/barrier/test_simd_barrier.cpp +++ b/tests/src/tests/barrier/test_simd_barrier.cpp @@ -1,4 +1,5 @@ #include +#include #include @@ -36,6 +37,21 @@ void check_matches_scalar( [&](Batch d) { return batch_value(d, Batch(dhat)); }, 1e-12); } +/// @brief Compare f(barrier, d, dhat) against f(batch_barrier, d, dhat), where +/// one generic callable serves both the scalar and the batch barrier. +template +void check_matches_scalar( + const std::string& name, + const double dhat, + const ScalarBarrier& barrier, + const BatchBarrier& batch_barrier, + F&& f) +{ + check_matches_scalar( + name, dhat, [&](double d, double dh) { return f(barrier, d, dh); }, + [&](Batch d, Batch dh) { return f(batch_barrier, d, dh); }); +} + } // namespace TEST_CASE( @@ -110,4 +126,69 @@ TEST_CASE( }); } +TEST_CASE( + "SIMD batch inverse-power and near-far barriers match the scalar ones " + "lane-wise", + "[barrier][simd]") +{ + const double dhat = 1e-2; + const double power = GENERATE(1.0, 2.0, 3.0); + // The far weight ramps up over [alpha dhat / 2, alpha dhat], so 0.1 dhat + // is on the ramp for alpha = 0.15 and 0.7 dhat is on it for alpha = 1. + // Between the two, each interior region of regions() is checked both on + // the ramp and off it. + const double alpha = GENERATE(0.15, 1.0); + CAPTURE(power, alpha); + + const auto value = [](const auto& b, auto d, auto dh) { return b(d, dh); }; + const auto first_derivative = [](const auto& b, auto d, auto dh) { + return b.first_derivative(d, dh); + }; + const auto second_derivative = [](const auto& b, auto d, auto dh) { + return b.second_derivative(d, dh); + }; + + const InversePowerBarrier inverse_power(power); + const InversePowerBarrier inverse_power_batch(power); + check_matches_scalar( + "InversePowerBarrier", dhat, inverse_power, inverse_power_batch, value); + check_matches_scalar( + "InversePowerBarrier::first_derivative", dhat, inverse_power, + inverse_power_batch, first_derivative); + check_matches_scalar( + "InversePowerBarrier::second_derivative", dhat, inverse_power, + inverse_power_batch, second_derivative); + + // The inverse-power base is +inf at penetration, which is exactly where a + // batch must not let the blended-away ramp lanes leak a NaN. + const NearFarBarrier near_far(&inverse_power, alpha); + const NearFarBarrier near_far_batch(&inverse_power_batch, alpha); + check_matches_scalar( + "NearFarBarrier::near_value", dhat, near_far, near_far_batch, + [](const auto& b, auto d, auto dh) { return b.near_value(d, dh); }); + check_matches_scalar( + "NearFarBarrier::far_value", dhat, near_far, near_far_batch, + [](const auto& b, auto d, auto dh) { return b.far_value(d, dh); }); + check_matches_scalar( + "NearFarBarrier::first_derivative_near", dhat, near_far, near_far_batch, + [](const auto& b, auto d, auto dh) { + return b.first_derivative_near(d, dh); + }); + check_matches_scalar( + "NearFarBarrier::first_derivative_far", dhat, near_far, near_far_batch, + [](const auto& b, auto d, auto dh) { + return b.first_derivative_far(d, dh); + }); + check_matches_scalar( + "NearFarBarrier::second_derivative_near", dhat, near_far, + near_far_batch, [](const auto& b, auto d, auto dh) { + return b.second_derivative_near(d, dh); + }); + check_matches_scalar( + "NearFarBarrier::second_derivative_far", dhat, near_far, near_far_batch, + [](const auto& b, auto d, auto dh) { + return b.second_derivative_far(d, dh); + }); +} + #endif diff --git a/tests/src/tests/candidates/test_candidates.cpp b/tests/src/tests/candidates/test_candidates.cpp index aa1b5e55b..a15e70737 100644 --- a/tests/src/tests/candidates/test_candidates.cpp +++ b/tests/src/tests/candidates/test_candidates.cpp @@ -1,12 +1,41 @@ #include +#include #include #include #include #include +#include + +#include +#include +#include +#include +#include +#include + using namespace ipc; +namespace { +std::vector ids(const AdjacencyList& list) +{ + return std::vector(list.begin(), list.end()); +} + +std::vector ids(const std::set& set) +{ + return std::vector(set.begin(), set.end()); +} + +// Whether the list is strictly ascending, i.e. sorted and free of duplicates. +bool is_sorted_unique(const AdjacencyList& list) +{ + return std::adjacent_find(list.begin(), list.end(), std::greater_equal<>()) + == list.end(); +} +} // namespace + TEST_CASE("Candidates", "[candidates]") { Candidates candidates; @@ -46,7 +75,11 @@ TEST_CASE("Candidates", "[candidates]") FAIL("Should have thrown an exception"); } catch (const std::out_of_range& e) { SUCCEED("Exception thrown"); - CHECK(e.what() == std::string("Candidate index is out of range!")); + CHECK( + e.what() + == fmt::format( + "Candidate index {} is out of range! Total candidates: {}", + candidates.size(), candidates.size())); } catch (...) { FAIL("Uknown exception thrown"); } @@ -79,7 +112,11 @@ TEST_CASE("Candidates", "[candidates]") FAIL("Should have thrown an exception"); } catch (const std::out_of_range& e) { SUCCEED("Exception thrown"); - CHECK(e.what() == std::string("Candidate index is out of range!")); + CHECK( + e.what() + == fmt::format( + "Candidate index {} is out of range! Total candidates: {}", + candidates.size(), candidates.size())); } catch (...) { FAIL("Uknown exception thrown"); } @@ -158,4 +195,235 @@ TEST_CASE("Plane-Vertex Candidate", "[candidates][plane-vertex]") CHECK(PlaneVertexCandidate(plane, 1) != PlaneVertexCandidate(plane, 2)); CHECK(PlaneVertexCandidate(plane, 1) < PlaneVertexCandidate(plane, 2)); CHECK(!(PlaneVertexCandidate(plane, 2) < PlaneVertexCandidate(plane, 1))); -} \ No newline at end of file +} + +TEST_CASE("Candidates adjacency sets", "[candidates]") +{ + // Vertex 2 is 0.05 above the middle of edge 0. + Eigen::MatrixXd V(4, 2); + V << 0, 0, 1, 0, 0.5, 0.05, 3, 3; + Eigen::MatrixXi E(2, 2); + E << 0, 1, 2, 3; + const CollisionMesh mesh(V, E, Eigen::MatrixXi()); + + // Before convert_candidates_to_sets(), every query is empty. + Candidates candidates; + CHECK(candidates.vv_set(mesh, 0).empty()); + CHECK(candidates.ve_set(0).empty()); + CHECK(candidates.vf_set(0).empty()); + CHECK(candidates.ev_set(mesh, 0).empty()); + CHECK(candidates.ee_set(mesh, 0).empty()); + CHECK(candidates.ef_set(mesh, 0).empty()); + CHECK(candidates.fv_set(mesh, 0).empty()); + CHECK(candidates.fe_set(mesh, 0).empty()); + CHECK(candidates.ff_set(mesh, 0).empty()); + + candidates.build(mesh, V, /*inflation_radius=*/0.1); + REQUIRE(!candidates.ev_candidates.empty()); + candidates.convert_candidates_to_sets(); + // In 2D, vv_set also holds the endpoints of the edges near the vertex. + CHECK(ids(candidates.vv_set(mesh, 2)) == std::vector { 0, 1 }); + CHECK(candidates.ve_set(2).contains(0)); + CHECK(candidates.vf_set(2).empty()); +} + +// Two tetrahedra apex to apex, so every primitive pairing has candidates. +TEST_CASE( + "Candidate adjacency lists join candidate partners with mesh neighbors", + "[candidates]") +{ + const double c = std::cos(2 * M_PI / 3), s = std::sin(2 * M_PI / 3); + const double r = std::cos(M_PI / 3), q = std::sin(M_PI / 3); + Eigen::MatrixXd V(8, 3); + V << 0, 0, 0.05, 1, 0, 1, c, s, 1, c, -s, 1, // + 0, 0, -0.05, r, q, -1, -1, 0, -1, r, -q, -1; + Eigen::MatrixXi F(8, 3); + F << 0, 2, 1, 0, 3, 2, 0, 1, 3, 1, 2, 3, // + 4, 5, 6, 4, 6, 7, 4, 7, 5, 5, 7, 6; + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + Candidates candidates; + candidates.build( + mesh, V, /*inflation_radius=*/1, /*broad_phase=*/nullptr, + /*all_types=*/true); + REQUIRE(!candidates.vv_candidates.empty()); + REQUIRE(!candidates.ev_candidates.empty()); + REQUIRE(!candidates.ee_candidates.empty()); + REQUIRE(!candidates.fv_candidates.empty()); + REQUIRE(!candidates.ef_candidates.empty()); + REQUIRE(!candidates.ff_candidates.empty()); + candidates.convert_candidates_to_sets(); + + // The expected neighbors, computed straight from the candidate lists and + // the mesh's vertex ids without using any of the adjacency structure. + const size_t nv = mesh.num_vertices(), ne = mesh.num_edges(), + nf = mesh.num_faces(); + std::vector> vv(nv), ve(nv), vf(nv), ev(ne), ee(ne), + ef(ne), fv(nf), fe(nf), ff(nf); + for (const auto& [a, b] : candidates.vv_candidates) { + vv[a].insert(b); + vv[b].insert(a); + } + for (const auto& [e, v] : candidates.ev_candidates) { + ev[e].insert(v); + ve[v].insert(e); + } + for (const auto& [a, b] : candidates.ee_candidates) { + ee[a].insert(b); + ee[b].insert(a); + } + for (const auto& [f, v] : candidates.fv_candidates) { + fv[f].insert(v); + vf[v].insert(f); + } + for (const auto& [e, f] : candidates.ef_candidates) { + ef[e].insert(f); + fe[f].insert(e); + } + for (const auto& [a, b] : candidates.ff_candidates) { + ff[a].insert(b); + ff[b].insert(a); + } + + const auto has_vertex = [&](const Eigen::MatrixXi& M, index_t i, + index_t v) { + return (M.row(i).array() == v).any(); + }; + for (index_t e = 0; e < index_t(ne); ++e) { + for (index_t v : { mesh.edges()(e, 0), mesh.edges()(e, 1) }) { + ev[e].insert(v); + for (index_t other = 0; other < index_t(ne); ++other) { + if (has_vertex(mesh.edges(), other, v)) { + ee[e].insert(other); + } + } + for (index_t f = 0; f < index_t(nf); ++f) { + if (has_vertex(mesh.faces(), f, v)) { + ef[e].insert(f); + } + } + } + ee[e].erase(e); + for (index_t f = 0; f < index_t(nf); ++f) { + if (has_vertex(mesh.faces(), f, mesh.edges()(e, 0)) + && has_vertex(mesh.faces(), f, mesh.edges()(e, 1))) { + ef[e].erase(f); // A face containing the edge is not near it. + } + } + } + for (index_t f = 0; f < index_t(nf); ++f) { + for (index_t lv = 0; lv < 3; ++lv) { + const index_t v = mesh.faces()(f, lv); + fv[f].insert(v); + for (index_t e = 0; e < index_t(ne); ++e) { + if (has_vertex(mesh.edges(), e, v)) { + fe[f].insert(e); + } + } + for (index_t other = 0; other < index_t(nf); ++other) { + if (has_vertex(mesh.faces(), other, v)) { + ff[f].insert(other); + } + } + } + ff[f].erase(f); + } + for (index_t v = 0; v < index_t(nv); ++v) { + vv[v].erase(v); + } + + const auto check = [&](const AdjacencyList& list, + const std::set& want) { + CHECK(is_sorted_unique(list)); + CHECK(ids(list) == ids(want)); + }; + for (index_t v = 0; v < index_t(nv); ++v) { + CAPTURE(v); + check(candidates.vv_set(mesh, v), vv[v]); + check(candidates.ve_set(v), ve[v]); + check(candidates.vf_set(v), vf[v]); + } + for (index_t e = 0; e < index_t(ne); ++e) { + CAPTURE(e); + check(candidates.ev_set(mesh, e), ev[e]); + check(candidates.ee_set(mesh, e), ee[e]); + check(candidates.ef_set(mesh, e), ef[e]); + } + for (index_t f = 0; f < index_t(nf); ++f) { + CAPTURE(f); + check(candidates.fv_set(mesh, f), fv[f]); + check(candidates.fe_set(mesh, f), fe[f]); + check(candidates.ff_set(mesh, f), ff[f]); + } + + SECTION("Ids with no candidates have empty vertex lists") + { + CHECK(candidates.ve_set(1000).empty()); + CHECK(candidates.vf_set(-1).empty()); + } + + SECTION("A copy shares the lists") + { + const Candidates copy = candidates; + for (index_t v = 0; v < index_t(nv); ++v) { + CHECK(ids(copy.ve_set(v)) == ids(candidates.ve_set(v))); + CHECK(ids(copy.ee_set(mesh, v)) == ids(candidates.ee_set(mesh, v))); + } + } + + SECTION("Clearing the candidates empties every query") + { + candidates.clear(); + CHECK(candidates.ve_set(0).empty()); + CHECK(candidates.ee_set(mesh, 0).empty()); + } +} + +TEST_CASE( + "All-types candidates keep whole-mesh pairs alongside codim. primitives", + "[candidates]") +{ + // Two triangles meet tip to tip (vertices 0 and 3). Far away, a codim. + // vertex (9) sits next to a dangling edge (7, 8), and vertex 6 is stray. + Eigen::MatrixXd V(10, 3); + V << 0, 0, 0.025, 1, 0, 1, -1, 0, 1, // + 0, 0, -0.025, 0, 1, -1, 0, -1, -1, // + 50, 50, 50, // + -50, -50, -50, -49, -50, -50, -49.5, -50, -49.99; + Eigen::MatrixXi F(2, 3); + F << 0, 1, 2, 3, 4, 5; + Eigen::MatrixXi E(7, 2); + E << 0, 1, 1, 2, 2, 0, 3, 4, 4, 5, 5, 3, 7, 8; + const CollisionMesh mesh(V, E, F); + REQUIRE(mesh.num_codim_vertices() == 2); + REQUIRE(mesh.num_codim_edges() == 1); + + const auto contains = [](const auto& list, const auto& candidate) { + return std::find(list.begin(), list.end(), candidate) != list.end(); + }; + + const bool all_types = GENERATE(false, true); + CAPTURE(all_types); + + Candidates candidates; + candidates.build(mesh, V, /*inflation_radius=*/0.05, nullptr, all_types); + CHECK(candidates.has_all_types == all_types); + + // Both builds find the codim. edge next to the codim. vertex. + CHECK(contains(candidates.ev_candidates, EdgeVertexCandidate(6, 9))); + + if (all_types) { + // The tips are a surface vertex-vertex pair, which only all_types + // detects. The codim. primitives must not replace it. + CHECK(contains(candidates.vv_candidates, VertexVertexCandidate(0, 3))); + } + + candidates.clear(); + CHECK(!candidates.has_all_types); + + // Trajectory candidates never include the ESP-only types. + candidates.build(mesh, V, V, /*inflation_radius=*/0.05); + CHECK(!candidates.has_all_types); +} diff --git a/tests/src/tests/collisions/CMakeLists.txt b/tests/src/tests/collisions/CMakeLists.txt index 8fa263aa4..bf61942ef 100644 --- a/tests/src/tests/collisions/CMakeLists.txt +++ b/tests/src/tests/collisions/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES # Tests test_normal_collisions.cpp + test_obstacles.cpp test_plane_vertex_collisions.cpp # Benchmarks diff --git a/tests/src/tests/collisions/test_normal_collisions.cpp b/tests/src/tests/collisions/test_normal_collisions.cpp index c56dee4f9..ecdbcd701 100644 --- a/tests/src/tests/collisions/test_normal_collisions.cpp +++ b/tests/src/tests/collisions/test_normal_collisions.cpp @@ -9,6 +9,8 @@ #include +#include + using namespace ipc; TEST_CASE("Codim. vertex-vertex collisions", "[collisions][codim]") @@ -307,6 +309,65 @@ TEST_CASE("Plane-Vertex NormalCollision", "[collision][plane-vertex]") == 2 * n * n.transpose()); } +TEST_CASE( + "Normal collisions reject 3D candidates with all types", "[collisions]") +{ + constexpr double dhat = 0.1; + + // Two triangles tip to tip, 0.05 apart. + Eigen::MatrixXd V(6, 3); + V << 0, 0, 0.025, 1, 0, 1, -1, 0, 1, // + 0, 0, -0.025, 0, 1, -1, 0, -1, -1; + Eigen::MatrixXi F(2, 3); + F << 0, 1, 2, 3, 4, 5; + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + NormalCollisions expected; + expected.build(mesh, V, dhat); + REQUIRE(!expected.empty()); + + NormalCollisions collisions; + Candidates candidates; + + candidates.build(mesh, V, dhat / 2); + collisions.build(candidates, mesh, V, dhat); + CHECK(collisions.size() == expected.size()); + + candidates.build(mesh, V, dhat / 2, nullptr, /*all_types=*/true); + CHECK_THROWS_AS( + collisions.build(candidates, mesh, V, dhat), std::invalid_argument); +} + +TEST_CASE( + "Normal collisions accept 2D candidates with all types", "[collisions]") +{ + constexpr double dhat = 0.1; + + // Vertex 2 is 0.05 above the middle of edge 0. + Eigen::MatrixXd V(4, 2); + V << 0, 0, 1, 0, 0.5, 0.05, 0.5, 1; + Eigen::MatrixXi E(2, 2); + E << 0, 1, 2, 3; + const CollisionMesh mesh(V, E); + + NormalCollisions expected; + expected.build(mesh, V, dhat); + REQUIRE(!expected.empty()); + + // In 2D, all_types adds no candidate types, so the result is unchanged. + Candidates candidates; + candidates.build(mesh, V, dhat / 2, nullptr, /*all_types=*/true); + NormalCollisions collisions; + collisions.build(candidates, mesh, V, dhat); + + const BarrierPotential barrier(dhat, 1.0); + CHECK( + barrier(collisions, mesh, V) + == Catch::Approx(barrier(expected, mesh, V)).epsilon(1e-12)); +} + TEST_CASE("NormalCollisions::is_*", "[collisions]") { NormalCollisions collisions; diff --git a/tests/src/tests/collisions/test_obstacles.cpp b/tests/src/tests/collisions/test_obstacles.cpp new file mode 100644 index 000000000..eda83bfff --- /dev/null +++ b/tests/src/tests/collisions/test_obstacles.cpp @@ -0,0 +1,593 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +using namespace ipc; + +namespace { + +// Two tetrahedra whose equilateral bases form a star: a dynamic one with its +// base at z = gap (vertices 0-3) over an obstacle one with its base at z = 0, +// rotated by 60° (vertices 4-7). Each base's tips lie near the other base's +// edges, and the base edges cross. Same-body primitives are >= 1 apart. +CollisionMesh star_mesh(const std::vector& obstacle_vertex) +{ + constexpr double gap = 0.05; + Eigen::MatrixXd V(8, 3); + for (int i = 0; i < 3; i++) { + const double a = 2 * igl::PI * i / 3; + V.row(i) << std::cos(a), std::sin(a), gap; + V.row(4 + i) << std::cos(a + igl::PI / 3), std::sin(a + igl::PI / 3), 0; + } + V.row(3) << 0, 0, gap + 1; + V.row(7) << 0, 0, -1; + Eigen::MatrixXi F(8, 3); + F << 0, 2, 1, 0, 1, 3, 1, 2, 3, 2, 0, 3, // + 4, 5, 6, 5, 4, 7, 6, 5, 7, 4, 6, 7; + Eigen::MatrixXi E; + igl::edges(F, E); + + return CollisionMesh( + std::vector(V.rows(), true), std::vector(V.rows(), false), + obstacle_vertex, V, E, F); +} + +/// The three vertices and the centroid of each face. +FaceQuadRule vertices_and_centroid_rule() +{ + return { + { { { 1., 0., 0. } }, 0.25 }, + { { { 0., 1., 0. } }, 0.25 }, + { { { 0., 0., 1. } }, 0.25 }, + { { { 1. / 3, 1. / 3, 1. / 3 } }, 0.25 }, + }; +} + +const std::vector LOWER_OBSTACLE = { false, false, false, false, + true, true, true, true }; + +constexpr double DHAT = 0.6; + +// An obstacle whose tip, a vertex shared by several obstacle edges, points at a +// deformable surface from height h. In 3D, the tip is the apex of a tetrahedron +// above the interior edge (0, 1) of a two-triangle sheet. In 2D, it is the apex +// of a coarse Λ above the apex (vertex 0) of a fine V. Vertex 0 is deformable +// in both, and the tip is the last vertex before the obstacle's other vertices. +Eigen::MatrixXd apex_vertices(const int dim, const double h) +{ + Eigen::MatrixXd V; + if (dim == 3) { + V.resize(8, 3); + V << -1, 0, 0, 1, 0, 0, 0, -2, 0, 0, 2, 0, // sheet + 0, 0, h, 0.3, 0, 3, -0.15, 0.26, 3, -0.15, -0.26, 3; // tetrahedron + } else { + V.resize(6, 2); + V << 0, 0, -0.1, -0.1, 0.1, -0.1, // V + 0, h, -1, 1 + h, 1, 1 + h; // Λ + } + return V; +} + +index_t apex_obstacle_tip(const int dim) { return dim == 3 ? 4 : 3; } + +CollisionMesh apex_mesh(const int dim) +{ + const Eigen::MatrixXd V = apex_vertices(dim, 0.05); + std::vector obstacle(V.rows(), false); + std::fill(obstacle.begin() + apex_obstacle_tip(dim), obstacle.end(), true); + + Eigen::MatrixXi E, F; + if (dim == 3) { + F.resize(6, 3); + F << 0, 2, 1, 0, 1, 3, // + 4, 5, 6, 4, 6, 7, 4, 7, 5, 5, 7, 6; + igl::edges(F, E); + } else { + E.resize(4, 2); + E << 1, 0, 0, 2, 4, 3, 3, 5; + } + + return CollisionMesh( + std::vector(V.rows(), true), std::vector(V.rows(), false), + obstacle, V, E, F); +} + +} // namespace + +TEST_CASE("Collision mesh obstacle flags", "[collision_mesh][obstacle]") +{ + SECTION("Default constructor has no obstacles") + { + const CollisionMesh mesh = star_mesh(std::vector(8, false)); + const CollisionMesh plain( + mesh.rest_positions(), mesh.edges(), mesh.faces()); + for (int i = 0; i < plain.num_vertices(); i++) { + CHECK(!plain.is_obstacle_vertex(i)); + } + } + + SECTION("Flags follow the vertices") + { + const CollisionMesh mesh = star_mesh(LOWER_OBSTACLE); + for (int i = 0; i < mesh.num_vertices(); i++) { + CHECK(mesh.is_obstacle_vertex(i) == (i >= 4)); + } + for (int e = 0; e < mesh.num_edges(); e++) { + CHECK(mesh.is_obstacle_edge(e) == (mesh.edges()(e, 0) >= 4)); + } + for (int f = 0; f < mesh.num_faces(); f++) { + CHECK(mesh.is_obstacle_face(f) == (f >= 4)); + } + } + + SECTION("Flags are mapped to the included vertices") + { + // Full mesh with an extra, excluded obstacle vertex at index 0. + const CollisionMesh star = star_mesh(LOWER_OBSTACLE); + Eigen::MatrixXd V(9, 3); + V.row(0).setZero(); + V.bottomRows(8) = star.rest_positions(); + const Eigen::MatrixXi E = (star.edges().array() + 1).matrix(); + const Eigen::MatrixXi F = (star.faces().array() + 1).matrix(); + + std::vector include(9, true), obstacle(9, true); + include[0] = false; + for (int i = 1; i <= 4; i++) { + obstacle[i] = false; + } + + const CollisionMesh mesh( + include, std::vector(9, false), obstacle, V, E, F); + REQUIRE(mesh.num_vertices() == 8); + for (int i = 0; i < mesh.num_vertices(); i++) { + CHECK(mesh.is_obstacle_vertex(i) == (i >= 4)); + } + } + + SECTION("Edges and faces touching a dynamic vertex are dynamic") + { + // Vertex 0 of the dynamic body is flagged as an obstacle, like the end + // of a rope anchored to a static object. + std::vector obstacle = LOWER_OBSTACLE; + obstacle[0] = true; + const CollisionMesh mesh = star_mesh(obstacle); + for (int e = 0; e < mesh.num_edges(); e++) { + CHECK(mesh.is_obstacle_edge(e) == (mesh.edges()(e, 0) >= 4)); + } + for (int f = 0; f < mesh.num_faces(); f++) { + CHECK(mesh.is_obstacle_face(f) == (f >= 4)); + } + } + + SECTION("Mask must cover the full mesh") + { + const CollisionMesh star = star_mesh(LOWER_OBSTACLE); + const Eigen::MatrixXd& V = star.rest_positions(); + const std::vector all(V.rows(), true); + for (const size_t size : { V.rows() - 1, V.rows() + 1 }) { + CHECK_THROWS_AS( + CollisionMesh( + all, all, std::vector(size, false), V, star.edges(), + star.faces()), + std::invalid_argument); + } + } +} + +TEST_CASE("Normal collisions skip obstacles", "[collisions][obstacle]") +{ + CollisionMesh mesh = star_mesh(LOWER_OBSTACLE); + const Eigen::MatrixXd& V = mesh.rest_positions(); + // A ground plane just below the obstacle's tip (vertex 7). + mesh.planes.emplace_back( + Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0, 0, -1.1)); + + NormalCollisions all; + all.build(mesh, V, DHAT); + + NormalCollisions skipped; + skipped.set_skip_obstacles(true); + CHECK(skipped.skip_obstacles()); + skipped.build(mesh, V, DHAT); + + // Pairs whose vertex is an obstacle are dropped. + const auto count_obstacle_vertices = [&](const NormalCollisions& c) { + int n = 0; + for (const auto& ev : c.ev_collisions) { + n += mesh.is_obstacle_vertex(ev.vertex_id); + } + for (const auto& fv : c.fv_collisions) { + n += mesh.is_obstacle_vertex(fv.vertex_id); + } + for (const auto& pv : c.pv_collisions) { + n += mesh.is_obstacle_vertex(pv.vertex_id); + } + return n; + }; + REQUIRE(count_obstacle_vertices(all) > 0); + REQUIRE(!all.pv_collisions.empty()); + CHECK(count_obstacle_vertices(skipped) == 0); + CHECK(skipped.size() < all.size()); + + // Edge-edge pairs with one obstacle edge are kept at half weight. + REQUIRE(!all.ee_collisions.empty()); + REQUIRE(skipped.ee_collisions.size() == all.ee_collisions.size()); + for (const auto& ee : skipped.ee_collisions) { + REQUIRE( + mesh.is_obstacle_edge(ee.edge0_id) + != mesh.is_obstacle_edge(ee.edge1_id)); + const auto it = std::find_if( + all.ee_collisions.begin(), all.ee_collisions.end(), + [&](const auto& other) { return other == ee; }); + REQUIRE(it != all.ee_collisions.end()); + CHECK(ee.weight == Catch::Approx(0.5 * it->weight)); + } + + // With every vertex an obstacle, nothing is left. + const CollisionMesh all_obstacle = star_mesh(std::vector(8, true)); + NormalCollisions none; + none.set_skip_obstacles(true); + none.build(all_obstacle, V, DHAT); + CHECK(none.empty()); +} + +TEST_CASE( + "Skipping obstacles keeps the barrier repulsive", "[collisions][obstacle]") +{ + using CollisionSetType = NormalCollisions::CollisionSetType; + + const int dim = GENERATE(2, 3); + const CollisionSetType type = + GENERATE(CollisionSetType::IPC, CollisionSetType::IMPROVED_MAX_APPROX); + const bool use_area_weighting = GENERATE(false, true); + CAPTURE(dim, type, use_area_weighting); + + const CollisionMesh mesh = apex_mesh(dim); + const index_t tip = apex_obstacle_tip(dim); + constexpr double dhat = 0.1; + const BarrierPotential barrier(dhat, 1.0); + + // As the tip approaches the deformable surface, the energy must grow and + // push the two apart. The improved max approximation's corrections used to + // outweigh the one-sided contact here, making the barrier attractive. + double prev_energy = 0; + for (const double h : { 0.05, 0.01, 1e-3, 1e-5 }) { + CAPTURE(h); + const Eigen::MatrixXd V = apex_vertices(dim, h); + + NormalCollisions collisions; + collisions.set_use_area_weighting(use_area_weighting); + collisions.set_collision_set_type(type); + collisions.set_skip_obstacles(true); + collisions.build(mesh, V, dhat); + + const double energy = barrier(collisions, mesh, V); + CHECK(energy > prev_energy); + prev_energy = energy; + + // The last coordinate points from the deformable surface to the tip. + const Eigen::VectorXd grad = barrier.gradient(collisions, mesh, V); + CHECK(grad[dim * tip + dim - 1] < 0); + CHECK(grad[dim - 1] > 0); + } +} + +TEST_CASE( + "Obstacle size does not change edge-edge contact stiffness", + "[collisions][obstacle]") +{ + using CollisionSetType = NormalCollisions::CollisionSetType; + + // A short deformable edge crossing under an obstacle edge of varying + // length. + const double obstacle_length = GENERATE(0.2, 2.0, 20.0); + const CollisionSetType type = + GENERATE(CollisionSetType::IPC, CollisionSetType::IMPROVED_MAX_APPROX); + CAPTURE(obstacle_length, type); + + constexpr double dhat = 0.05; + const auto potential_value = [&](const double length, const bool skip) { + Eigen::MatrixXd V(4, 3); + V << -0.1, 0, 0, 0.1, 0, 0, // + 0, -length / 2, 0.01, 0, length / 2, 0.01; + Eigen::MatrixXi E(2, 2); + E << 0, 1, 2, 3; + const CollisionMesh mesh( + std::vector(4, true), std::vector(4, false), + { false, false, true, true }, V, E, Eigen::MatrixXi()); + + NormalCollisions collisions; + collisions.set_use_area_weighting(true); + collisions.set_collision_set_type(type); + collisions.set_skip_obstacles(skip); + collisions.build(mesh, V, dhat); + return BarrierPotential(dhat, 1.0)(collisions, mesh, V); + }; + + // Two equal edges contribute equally, so integrating only over the + // deformable one gives half the two-sided energy, whatever the obstacle's + // size. + const double two_sided = potential_value(0.2, false); + REQUIRE(two_sided > 0); + CHECK( + potential_value(obstacle_length, true) + == Catch::Approx(two_sided / 2).epsilon(1e-12)); +} + +TEST_CASE("ESP integration types", "[esp_potential][obstacle]") +{ + using IntegrationType = ESPParameters::IntegrationType; + + // Vertex integration or a face quadrature rule. + const bool face_rule = GENERATE(false, true); + CAPTURE(face_rule); + + const auto potential_value = [&](const std::vector& obstacle_vertex, + const IntegrationType type) { + const CollisionMesh mesh = star_mesh(obstacle_vertex); + ESPParameters params(DHAT, 1.0, 1, true, type); + if (face_rule) { + params.set_quad_rule(vertices_and_centroid_rule()); + } + ESPCollisions collisions; + collisions.build(mesh, mesh.rest_positions(), params); + return ESPPotential(params)(collisions, mesh, mesh.rest_positions()); + }; + + const std::vector none(8, false), all(8, true); + + // Without obstacles, every integration type is the same. + const double expected = potential_value(none, IntegrationType::BRUTE_FORCE); + REQUIRE(expected > 0); + CHECK( + potential_value(none, IntegrationType::NORMAL) + == Catch::Approx(expected).epsilon(1e-12)); + CHECK( + potential_value(none, IntegrationType::NO_OBST) + == Catch::Approx(expected).epsilon(1e-12)); + + // BRUTE_FORCE ignores the obstacle flags. + for (const auto& flags : { LOWER_OBSTACLE, all }) { + CHECK( + potential_value(flags, IntegrationType::BRUTE_FORCE) + == Catch::Approx(expected).epsilon(1e-12)); + } + + // The two bodies are mirror images, so each surface integrates half of the + // contact; NO_OBST integrates the dynamic one only. + CHECK( + potential_value(LOWER_OBSTACLE, IntegrationType::NO_OBST) + == Catch::Approx(expected / 2).epsilon(1e-12)); + + // NORMAL keeps the obstacle's sources but drops their obstacle-obstacle + // pairs: the base edges cross within dhat of the obstacle's own vertices. + const double normal = + potential_value(LOWER_OBSTACLE, IntegrationType::NORMAL); + CHECK(normal > expected / 2); + CHECK(normal != Catch::Approx(expected).epsilon(1e-12)); + + // Obstacle-only contact is integrated only by BRUTE_FORCE. + CHECK(potential_value(all, IntegrationType::NORMAL) == 0); + CHECK(potential_value(all, IntegrationType::NO_OBST) == 0); +} + +TEST_CASE("ESP integration types 2D", "[esp_potential][obstacle]") +{ + using IntegrationType = ESPParameters::IntegrationType; + + // Two squares 0.2 apart, mirror images of each other about x = 0. + Eigen::MatrixXd V(8, 2); + V << -1, 1, -1, 0, -.1, 0, -.1, 1, .1, 1, .1, 0, 1, 0, 1, 1; + Eigen::MatrixXi E(8, 2); + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + + const auto potential_value = [&](const std::vector& obstacle_vertex, + const IntegrationType type) { + const CollisionMesh mesh( + std::vector(V.rows(), true), + std::vector(V.rows(), false), obstacle_vertex, V, E, + Eigen::MatrixXi()); + const ESPParameters params(0.6, 1.0, 2, true, type); + ESPCollisions collisions; + collisions.build(mesh, V, params); + return ESPPotential(params)(collisions, mesh, V); + }; + + const std::vector none(8, false), all(8, true); + const std::vector left = { true, true, true, true, + false, false, false, false }; + + const double expected = potential_value(none, IntegrationType::BRUTE_FORCE); + REQUIRE(expected > 0); + for (const auto type : + { IntegrationType::NORMAL, IntegrationType::NO_OBST }) { + CHECK( + potential_value(none, type) + == Catch::Approx(expected).epsilon(1e-12)); + } + for (const auto& flags : { left, all }) { + CHECK( + potential_value(flags, IntegrationType::BRUTE_FORCE) + == Catch::Approx(expected).epsilon(1e-12)); + } + + // NO_OBST integrates the dynamic square only. + CHECK( + potential_value(left, IntegrationType::NO_OBST) + == Catch::Approx(expected / 2).epsilon(1e-12)); + // The obstacle-obstacle terms NORMAL drops sum to zero here. + CHECK( + potential_value(left, IntegrationType::NORMAL) + == Catch::Approx(expected).epsilon(1e-12)); + + CHECK(potential_value(all, IntegrationType::NORMAL) == 0); + CHECK(potential_value(all, IntegrationType::NO_OBST) == 0); +} + +TEST_CASE( + "ESP obstacle vertices next to dynamic vertices", + "[esp_potential][obstacle]") +{ + using IntegrationType = ESPParameters::IntegrationType; + + // Two unit cubes 0.001 apart with aligned vertices; the first is an + // obstacle, so its vertices have dynamic vertex-vertex candidates. + Eigen::MatrixXd V_cube; + Eigen::MatrixXi E_cube, F_cube; + REQUIRE(tests::load_mesh("cube.ply", V_cube, E_cube, F_cube)); + const int n = V_cube.rows(); + Eigen::MatrixXd V(2 * n, 3); + V << V_cube, V_cube.rowwise() + Eigen::RowVector3d(1.001, 0, 0); + Eigen::MatrixXi F(2 * F_cube.rows(), 3); + F << F_cube, (F_cube.array() + n).matrix(); + Eigen::MatrixXi E; + igl::edges(F, E); + std::vector obstacle(2 * n, false); + std::fill_n(obstacle.begin(), n, true); + const CollisionMesh mesh( + std::vector(2 * n, true), std::vector(2 * n, false), + obstacle, V, E, F); + + const bool face_rule = GENERATE(false, true); + CAPTURE(face_rule); + const auto potential_value = [&](const IntegrationType type) { + ESPParameters params(0.5, 1.0, 0, true, type); + if (face_rule) { + params.set_quad_rule(vertices_and_centroid_rule()); + } + ESPCollisions collisions; + collisions.build(mesh, V, params); + return ESPPotential(params)(collisions, mesh, V); + }; + + const double expected = potential_value(IntegrationType::BRUTE_FORCE); + REQUIRE(expected > 0); + // No obstacle-obstacle pair is within dhat, so NORMAL drops nothing. + CHECK( + potential_value(IntegrationType::NORMAL) + == Catch::Approx(expected).epsilon(1e-12)); + CHECK( + potential_value(IntegrationType::NO_OBST) + == Catch::Approx(expected / 2).epsilon(1e-12)); +} + +TEST_CASE( + "ESP integrates edges and faces with mixed obstacle flags", + "[esp_potential][obstacle]") +{ + using IntegrationType = ESPParameters::IntegrationType; + + const int dim = GENERATE(2, 3); + CAPTURE(dim); + + // In 3D, the star with one dynamic vertex flagged as an obstacle. In 2D, a + // rope hanging from the top right corner of a static block, 0.05 from its + // side; the rope's first edge has one obstacle and one dynamic vertex. + CollisionMesh mesh; + double dhat; + if (dim == 3) { + std::vector obstacle = LOWER_OBSTACLE; + obstacle[0] = true; + mesh = star_mesh(obstacle); + dhat = DHAT; + } else { + Eigen::MatrixXd V(6, 2); + V << -0.5, 0, -0.5, -1, 0.5, -1, 0.5, 0, // block + 0.55, -0.3, 0.55, -0.7; // rope + Eigen::MatrixXi E(6, 2); + E << 0, 1, 1, 2, 2, 3, 3, 0, 3, 4, 4, 5; + mesh = CollisionMesh( + std::vector(6, true), std::vector(6, false), + { true, true, true, true, false, false }, V, E, Eigen::MatrixXi()); + dhat = 0.1; + } + const Eigen::MatrixXd& V = mesh.rest_positions(); + + const auto potential_value = [&](const CollisionMesh& m, + const IntegrationType type) { + const ESPParameters params(dhat, 1.0, 1, true, type); + ESPCollisions collisions; + collisions.build(m, V, params); + return ESPPotential(params)(collisions, m, V); + }; + + // BRUTE_FORCE ignores the flags, so it matches the unflagged mesh. + const CollisionMesh unflagged(V, mesh.edges(), mesh.faces()); + const double expected = + potential_value(unflagged, IntegrationType::BRUTE_FORCE); + REQUIRE(expected > 0); + CHECK( + potential_value(mesh, IntegrationType::BRUTE_FORCE) + == Catch::Approx(expected).epsilon(1e-12)); + + // The mixed primitives move, so the filtered types integrate them too. + for (const auto type : + { IntegrationType::NORMAL, IntegrationType::NO_OBST }) { + CAPTURE(type); + double value = 0; + REQUIRE_NOTHROW(value = potential_value(mesh, type)); + CHECK(value > 0); + } +} + +TEST_CASE( + "ESP NORMAL integration sees dynamic edges near obstacle corners in 2D", + "[esp_potential][obstacle]") +{ + using IntegrationType = ESPParameters::IntegrationType; + + // A dynamic box resting on a static obstacle whose top lies under the + // box's bottom edge but has no vertex near it: a spike pointing at the + // edge, or a block narrower than the box. Only the obstacle's corner + // vertices are close to the box, so the box's edge reaches the obstacle's + // edges through those corners. + const bool spike = GENERATE(true, false); + const double gap = GENERATE(0.05, 0.01, 1e-3, 1e-6); + const int quad_order = GENERATE(1, 2); + CAPTURE(spike, gap, quad_order); + + Eigen::MatrixXd V(spike ? 7 : 8, 2); + V.topRows(4) << -1, gap, 1, gap, 1, gap + 1, -1, gap + 1; + Eigen::MatrixXi E(spike ? 7 : 8, 2); + E.topRows(4) << 0, 1, 1, 2, 2, 3, 3, 0; + if (spike) { + V.bottomRows(3) << 0, 0, -0.5, -1, 0.5, -1; + E.bottomRows(3) << 4, 5, 5, 6, 6, 4; + } else { + V.bottomRows(4) << -0.5, 0, -0.5, -1, 0.5, -1, 0.5, 0; + E.bottomRows(4) << 4, 5, 5, 6, 6, 7, 7, 4; + } + std::vector obstacle(V.rows(), true); + std::fill_n(obstacle.begin(), 4, false); + const CollisionMesh mesh( + std::vector(V.rows(), true), std::vector(V.rows(), false), + obstacle, V, E, Eigen::MatrixXi()); + + const auto potential_value = [&](const IntegrationType type) { + const ESPParameters params(0.1, 0.2, quad_order, true, type); + ESPCollisions collisions; + collisions.build(mesh, V, params); + return ESPPotential(params)(collisions, mesh, V); + }; + + // No two obstacle primitives are within dhat of each other, so NORMAL has + // nothing to drop and must match BRUTE_FORCE. + const double expected = potential_value(IntegrationType::BRUTE_FORCE); + REQUIRE(expected > 0); + CHECK( + potential_value(IntegrationType::NORMAL) + == Catch::Approx(expected).epsilon(1e-12)); +} diff --git a/tests/src/tests/distance/distance_type_reference.hpp b/tests/src/tests/distance/distance_type_reference.hpp new file mode 100644 index 000000000..36f164305 --- /dev/null +++ b/tests/src/tests/distance/distance_type_reference.hpp @@ -0,0 +1,248 @@ +#pragma once +#include +#include +#include +#include + +using namespace ipc; +using ExReal = ExactReal; // exact scalar type +using ExVec3 = ExactVector3; // exact vector + +// Mirrors the relative sin² threshold of ipc::edge_edge_distance_type's +// parallel-edge handling (and ipc::PARALLEL_THRESHOLD). +constexpr double REFERENCE_PARALLEL_THRESHOLD = 2.5e-16; + +inline void init_pck() { init_exact_arithmetic(); } + +inline ExVec3 make_exact(Eigen::ConstRef v) +{ + return ExVec3::from(v.data(), int(v.size())); // z = 0 for 2D vectors +} +inline PointEdgeDistanceType point_edge_distance_type_reference( + Eigen::ConstRef p_, + Eigen::ConstRef e0_, + Eigen::ConstRef e1_) +{ + init_pck(); + const ExVec3 p = make_exact(p_); + const ExVec3 e0 = make_exact(e0_); + const ExVec3 e1 = make_exact(e1_); + const ExVec3 e = e1 - e0; + + if (e.dot(p - e0) <= 0) { + return PointEdgeDistanceType::P_E0; // PP (p-e0) + } else if ((-e).dot(p - e1) <= 0) { + return PointEdgeDistanceType::P_E1; // PP (p-e1) + } else { + return PointEdgeDistanceType::P_E; // PE + } +} + +inline PointTriangleDistanceType point_triangle_distance_type_reference( + Eigen::ConstRef p_, + Eigen::ConstRef t0_, + Eigen::ConstRef t1_, + Eigen::ConstRef t2_) +{ + init_pck(); + const ExVec3 p = make_exact(p_); + const ExVec3 t0 = make_exact(t0_); + const ExVec3 t1 = make_exact(t1_); + const ExVec3 t2 = make_exact(t2_); + + const ExVec3 e0 = t1 - t0; + const ExVec3 e1 = t2 - t1; + const ExVec3 e2 = t0 - t2; + + const ExVec3 r0 = p - t0; + const ExVec3 r1 = p - t1; + const ExVec3 r2 = p - t2; + + if (r0.dot(e0) <= 0 && r0.dot(e2) >= 0) { + return PointTriangleDistanceType::P_T0; + } + if (r1.dot(e1) <= 0 && r1.dot(e0) >= 0) { + return PointTriangleDistanceType::P_T1; + } + if (r2.dot(e2) <= 0 && r2.dot(e1) >= 0) { + return PointTriangleDistanceType::P_T2; + } + + const ExVec3 n = e0.cross(-e1); + + if (n.dot(r0.cross(e0)) <= 0 && r0.dot(e0) > 0 && r1.dot(e0) < 0) { + return PointTriangleDistanceType::P_E0; + } + if (n.dot(r1.cross(e1)) <= 0 && r1.dot(e1) > 0 && r2.dot(e1) < 0) { + return PointTriangleDistanceType::P_E1; + } + if (n.dot(r2.cross(e2)) <= 0 && r2.dot(e2) > 0 && r0.dot(e2) < 0) { + return PointTriangleDistanceType::P_E2; + } + + return PointTriangleDistanceType::P_T; +} + +inline bool is_parallel_edge_edge_reference( + Eigen::ConstRef ea0_, + Eigen::ConstRef ea1_, + Eigen::ConstRef eb0_, + Eigen::ConstRef eb1_) +{ + // TODO use a zero filter? + init_pck(); + const ExVec3 ea0 = make_exact(ea0_); + const ExVec3 ea1 = make_exact(ea1_); + const ExVec3 eb0 = make_exact(eb0_); + const ExVec3 eb1 = make_exact(eb1_); + + const ExVec3 u = ea1 - ea0; + const ExVec3 v = eb1 - eb0; + + const ExReal cross_norm_sqr = u.cross(v).squared_norm(); + if constexpr (REFERENCE_PARALLEL_THRESHOLD == 0.0) + return cross_norm_sqr == 0; + const ExReal a = u.squared_norm(); + const ExReal c = v.squared_norm(); + return cross_norm_sqr < a * c * REFERENCE_PARALLEL_THRESHOLD; +} + +inline EdgeEdgeDistanceType edge_edge_parallel_distance_type_reference( + Eigen::ConstRef ea0_, + Eigen::ConstRef ea1_, + Eigen::ConstRef eb0_, + Eigen::ConstRef eb1_) +{ + init_pck(); + const ExVec3 ea0 = make_exact(ea0_); + const ExVec3 ea1 = make_exact(ea1_); + const ExVec3 eb0 = make_exact(eb0_); + const ExVec3 eb1 = make_exact(eb1_); + + const ExVec3 ea = ea1 - ea0; + const ExReal ea_len_sqr = ea.squared_norm(); + const ExReal alpha_N = (eb0 - ea0).dot(ea); + const ExReal beta_N = (eb1 - ea0).dot(ea); + + uint8_t eac; // 0: EA0, 1: EA1, 2: EA + uint8_t ebc; // 0: EB0, 1: EB1, 2: EB + if (alpha_N < 0) { + eac = (beta_N >= 0 && beta_N <= ea_len_sqr) ? 2 : 0; + ebc = (beta_N <= alpha_N) ? 0 : (beta_N <= ea_len_sqr ? 1 : 2); + } else if (alpha_N > ea_len_sqr) { + eac = (beta_N >= 0 && beta_N <= ea_len_sqr) ? 2 : 1; + ebc = (beta_N >= alpha_N) ? 0 : (beta_N >= 0 ? 1 : 2); + } else { + eac = 2; + ebc = 0; + } + + // f(0, 0) = 0000 = 0 -> EA0_EB0 + // f(0, 1) = 0001 = 1 -> EA0_EB1 + // f(1, 0) = 0010 = 2 -> EA1_EB0 + // f(1, 1) = 0011 = 3 -> EA1_EB1 + // f(2, 0) = 0100 = 4 -> EA_EB0 + // f(2, 1) = 0101 = 5 -> EA_EB1 + // f(0, 2) = 0110 = 6 -> EA0_EB + // f(1, 2) = 0111 = 7 -> EA1_EB + // f(2, 2) = 1000 = 8 -> EA_EB + + assert(eac != 2 || ebc != 2); // This case results in a degenerate line-line + return EdgeEdgeDistanceType(ebc < 2 ? (eac << 1 | ebc) : (6 + eac)); +} + +// A more robust implementation of http://geomalgorithms.com/a07-_distance.html +/// @param parallel_threshold Relative sin² tolerance used to decide whether the +/// edges are parallel. Pass 0 for a fully exact reference: only edges +/// that are *exactly* parallel take the parallel branch. Any non-zero +/// value makes this a hybrid (exact arithmetic, approximate parallelism +/// test) which can misclassify near-parallel edges, since +/// edge_edge_parallel_distance_type_reference is only valid for +/// genuinely parallel edges. Defaults to the library's threshold so the +/// reference mirrors the shipped behaviour unless asked otherwise. +inline EdgeEdgeDistanceType edge_edge_distance_type_reference( + Eigen::ConstRef ea0_, + Eigen::ConstRef ea1_, + Eigen::ConstRef eb0_, + Eigen::ConstRef eb1_, + const double parallel_threshold = REFERENCE_PARALLEL_THRESHOLD) +{ + init_pck(); + const ExVec3 ea0 = make_exact(ea0_); + const ExVec3 ea1 = make_exact(ea1_); + const ExVec3 eb0 = make_exact(eb0_); + const ExVec3 eb1 = make_exact(eb1_); + + const ExVec3 u = ea1 - ea0; + const ExVec3 v = eb1 - eb0; + const ExVec3 w = ea0 - eb0; + + const ExReal a = u.squared_norm(); // always ≥ 0 + const ExReal b = u.dot(v); + const ExReal c = v.squared_norm(); // always ≥ 0 + const ExReal d = u.dot(w); + const ExReal e = v.dot(w); + const ExReal D = a * c - b * b; // always ≥ 0 + + // Degenerate cases should not happen in practice, but we handle them + if (a == 0 && c == 0) { + return EdgeEdgeDistanceType::EA0_EB0; + } else if (a == 0) { + return EdgeEdgeDistanceType::EA0_EB; + } else if (c == 0) { + return EdgeEdgeDistanceType::EA_EB0; + } + + // Special handling for parallel edges + const ExReal cross_norm_sqr = u.cross(v).squared_norm(); + bool is_parallel; + if (parallel_threshold == 0.0) { + is_parallel = (cross_norm_sqr == 0); + } else { + is_parallel = cross_norm_sqr < a * c * parallel_threshold; + } + if (is_parallel) { + return edge_edge_parallel_distance_type_reference( + ea0_, ea1_, eb0_, eb1_); + } + + EdgeEdgeDistanceType default_case = EdgeEdgeDistanceType::EA_EB; + + // compute the line parameters of the two closest points + const ExReal sN = (b * e - c * d); + ExReal tN, tD; // tc = tN / tD + if (sN <= 0) { // sc < 0 ⟹ the s=0 edge is visible + tN = e; + tD = c; + default_case = EdgeEdgeDistanceType::EA0_EB; + } else if (sN >= D) { // sc > 1 ⟹ the s=1 edge is visible + tN = e + b; + tD = c; + default_case = EdgeEdgeDistanceType::EA1_EB; + } else { + tN = (a * e - b * d); + tD = D; // default tD = D ≥ 0 + } + + if (tN <= 0) { // tc < 0 ⟹ the t=0 edge is visible + // recompute sc for this edge + if (-d <= 0) { + return EdgeEdgeDistanceType::EA0_EB0; + } else if (-d >= a) { + return EdgeEdgeDistanceType::EA1_EB0; + } else { + return EdgeEdgeDistanceType::EA_EB0; + } + } else if (tN >= tD) { // tc > 1 ⟹ the t=1 edge is visible + // recompute sc for this edge + if ((-d + b) <= 0) { + return EdgeEdgeDistanceType::EA0_EB1; + } else if ((-d + b) >= a) { + return EdgeEdgeDistanceType::EA1_EB1; + } else { + return EdgeEdgeDistanceType::EA_EB1; + } + } + + return default_case; +} \ No newline at end of file diff --git a/tests/src/tests/distance/test_distance_type.cpp b/tests/src/tests/distance/test_distance_type.cpp index 3b5121613..663bc866b 100644 --- a/tests/src/tests/distance/test_distance_type.cpp +++ b/tests/src/tests/distance/test_distance_type.cpp @@ -6,9 +6,15 @@ #include #include +#include #include #include +#include "distance_type_reference.hpp" + +#include +#include + using namespace ipc; TEST_CASE("Point-edge distance type", "[distance][distance-type][point-edge]") @@ -55,6 +61,244 @@ TEST_CASE("Point-edge distance type", "[distance][distance-type][point-edge]") } } +// These compare the shipped classifiers, standard and exact, against an +// exact-arithmetic reference. +TEST_CASE( + "Point-edge distance type random", + "[distance][distance-type][point-edge][exact]") +{ + const int num_random_tests = 10000; + + for (int i = 0; i < num_random_tests; ++i) { + const VectorMax3d p = Eigen::Vector3d::Random() * 10; + const VectorMax3d e0 = Eigen::Vector3d::Random() * 10; + const VectorMax3d e1 = Eigen::Vector3d::Random() * 10; + + const PointEdgeDistanceType dtype_ref = + point_edge_distance_type_reference(p, e0, e1); + + CAPTURE(p.transpose(), e0.transpose(), e1.transpose()); + CHECK(point_edge_distance_type(p, e0, e1) == dtype_ref); + CHECK(point_edge_distance_type_exact(p, e0, e1) == dtype_ref); + } +} + +TEST_CASE( + "Point-triangle distance type random", + "[distance][distance-type][point-triangle][exact]") +{ + const int num_random_tests = 10000; + + for (int i = 0; i < num_random_tests; ++i) { + const VectorMax3d p = Eigen::Vector3d::Random() * 10; + const VectorMax3d t0 = Eigen::Vector3d::Random() * 10; + const VectorMax3d t1 = Eigen::Vector3d::Random() * 10; + const VectorMax3d t2 = Eigen::Vector3d::Random() * 10; + + const PointTriangleDistanceType dtype_ref = + point_triangle_distance_type_reference(p, t0, t1, t2); + + CAPTURE(p.transpose(), t0.transpose(), t1.transpose(), t2.transpose()); + CHECK(point_triangle_distance_type(p, t0, t1, t2) == dtype_ref); + CHECK(point_triangle_distance_type_exact(p, t0, t1, t2) == dtype_ref); + } +} + +TEST_CASE( + "Edge-edge distance type random", + "[distance][distance-type][edge-edge][exact]") +{ + const int num_random_tests = 10000; + + for (int i = 0; i < num_random_tests; ++i) { + const VectorMax3d e0 = Eigen::Vector3d::Random() * 10; + const VectorMax3d e1 = Eigen::Vector3d::Random() * 10; + const VectorMax3d e2 = Eigen::Vector3d::Random() * 10; + const VectorMax3d e3 = Eigen::Vector3d::Random() * 10; + + const EdgeEdgeDistanceType dtype_ref = + edge_edge_distance_type_reference(e0, e1, e2, e3); + + CAPTURE(e0.transpose(), e1.transpose(), e2.transpose(), e3.transpose()); + CHECK(edge_edge_distance_type(e0, e1, e2, e3) == dtype_ref); + CHECK(edge_edge_distance_type_exact(e0, e1, e2, e3) == dtype_ref); + } +} + +// Nearly parallel random edges, with sin²θ between them spread across the +// threshold of ipc::is_parallel_edge_edge (sin²θ < 2.5e-16), so we exercise +// both sides of it. +// +// On the parallel side, ipc::edge_edge_distance_type takes its parallel branch, +// so we compare it against the reference with the *same* threshold. With a +// threshold of 0 (a fully exact reference), the reference would only take its +// parallel branch for exactly parallel edges and the two would disagree. +// +// On the other side, the exact classifier must agree with the fully exact +// reference. This is the regime ESP relies on: the edges are as close to +// parallel as the threshold lets through, which is where the analytic +// classifier's a·c - b² cancels down to rounding noise. +TEST_CASE( + "Edge-edge distance type random parallel", + "[distance][distance-type][edge-edge][exact][parallel]") +{ + const int num_random_tests = 10000; + int num_parallel = 0, num_not_parallel = 0; + + for (int i = 0; i < num_random_tests; ++i) { + const Eigen::Vector3d ea0 = Eigen::Vector3d::Random() * 10; + const Eigen::Vector3d ea1 = Eigen::Vector3d::Random() * 10; + const Eigen::Vector3d u = (ea1 - ea0).normalized(); + + // A unit direction perpendicular to edge a to rotate edge b towards. + Eigen::Vector3d n = Eigen::Vector3d::Random(); + n = (n - n.dot(u) * u).normalized(); + + // We draw log₁₀(sin²θ) uniformly from [-18, -14], so about 60% of the + // pairs fall below the threshold. Edge b is 0.5-1.5x as long as edge a, + // in either orientation, and offset from it. + const Eigen::Vector3d r = Eigen::Vector3d::Random(); + const double sin2_theta = std::pow(10.0, -16.0 + 2.0 * r[0]); + const double length = + (ea1 - ea0).norm() * (1.0 + 0.5 * r[1]) * (i % 2 == 0 ? 1.0 : -1.0); + + const Eigen::Vector3d eb0 = ea0 + Eigen::Vector3d::Random() * 5; + const Eigen::Vector3d eb1 = eb0 + + length + * (std::sqrt(1 - sin2_theta) * u + std::sqrt(sin2_theta) * n); + + CAPTURE( + sin2_theta, ea0.transpose(), ea1.transpose(), eb0.transpose(), + eb1.transpose()); + if (is_parallel_edge_edge(ea0, ea1, eb0, eb1)) { + ++num_parallel; + CHECK( + edge_edge_distance_type(ea0, ea1, eb0, eb1) + == edge_edge_distance_type_reference(ea0, ea1, eb0, eb1)); + } else { + // The exact classifier has no parallel branch; its callers skip + // the pairs is_parallel_edge_edge flags. + ++num_not_parallel; + CHECK( + edge_edge_distance_type_exact(ea0, ea1, eb0, eb1) + == edge_edge_distance_type_reference( + ea0, ea1, eb0, eb1, /*parallel_threshold=*/0.0)); + } + } + + // Both sides of the threshold must actually be exercised. + CHECK(num_parallel > num_random_tests / 4); + CHECK(num_not_parallel > num_random_tests / 4); +} + +// The *_exact classifiers must agree with each other, e.g., a point-triangle +// P_E0 implies a point-edge P_E on that edge. ESP relies on this. The points +// are placed on or near the region boundaries, where rounding makes the +// predicates near-ties. +TEST_CASE( + "Exact distance-type classifiers are mutually consistent", + "[distance][distance-type][exact]") +{ + using PE = PointEdgeDistanceType; + using PT = PointTriangleDistanceType; + using EE = EdgeEdgeDistanceType; + + const auto pe = [](const Eigen::Vector3d& p, const Eigen::Vector3d& e0, + const Eigen::Vector3d& e1) { + return point_edge_distance_type_exact(p, e0, e1); + }; + + const std::array params { { -0.5, 0, 1.0 / 3.0, 0.5, 0.7, 1, + 1.5 } }; + const std::array offsets { { 0, 1e-3, 1 } }; + const int n_random = 50; + + for (int i = 0; i < n_random; i++) { + const Eigen::Vector3d t0 = Eigen::Vector3d::Random(); + const Eigen::Vector3d t1 = Eigen::Vector3d::Random(); + const Eigen::Vector3d t2 = Eigen::Vector3d::Random(); + const Eigen::Vector3d n = (t1 - t0).cross(t2 - t0).normalized(); + + for (const double a : params) { + for (const double b : params) { + for (const double c : offsets) { + const Eigen::Vector3d p = + t0 + a * (t1 - t0) + b * (t2 - t0) + c * n; + CAPTURE(i, a, b, c); + + switch (point_triangle_distance_type_exact(p, t0, t1, t2)) { + case PT::P_T0: + CHECK(pe(p, t0, t1) == PE::P_E0); + CHECK(pe(p, t0, t2) == PE::P_E0); + break; + case PT::P_T1: + CHECK(pe(p, t1, t2) == PE::P_E0); + CHECK(pe(p, t1, t0) == PE::P_E0); + break; + case PT::P_T2: + CHECK(pe(p, t2, t0) == PE::P_E0); + CHECK(pe(p, t2, t1) == PE::P_E0); + break; + case PT::P_E0: + CHECK(pe(p, t0, t1) == PE::P_E); + CHECK(pe(p, t1, t0) == PE::P_E); + break; + case PT::P_E1: + CHECK(pe(p, t1, t2) == PE::P_E); + CHECK(pe(p, t2, t1) == PE::P_E); + break; + case PT::P_E2: + CHECK(pe(p, t2, t0) == PE::P_E); + CHECK(pe(p, t0, t2) == PE::P_E); + break; + default: + break; + } + + // Edge-edge: ea = (t0, t1), eb = (t2, p). + const Eigen::Vector3d &ea0 = t0, &ea1 = t1, &eb0 = t2; + const Eigen::Vector3d& eb1 = p; + if (is_parallel_edge_edge(ea0, ea1, eb0, eb1)) { + continue; + } + switch (edge_edge_distance_type_exact(ea0, ea1, eb0, eb1)) { + case EE::EA0_EB0: + CHECK(pe(ea0, eb0, eb1) == PE::P_E0); + CHECK(pe(eb0, ea0, ea1) == PE::P_E0); + break; + case EE::EA0_EB1: + CHECK(pe(ea0, eb0, eb1) == PE::P_E1); + CHECK(pe(eb1, ea0, ea1) == PE::P_E0); + break; + case EE::EA1_EB0: + CHECK(pe(ea1, eb0, eb1) == PE::P_E0); + CHECK(pe(eb0, ea0, ea1) == PE::P_E1); + break; + case EE::EA1_EB1: + CHECK(pe(ea1, eb0, eb1) == PE::P_E1); + CHECK(pe(eb1, ea0, ea1) == PE::P_E1); + break; + case EE::EA_EB0: + CHECK(pe(eb0, ea0, ea1) == PE::P_E); + break; + case EE::EA_EB1: + CHECK(pe(eb1, ea0, ea1) == PE::P_E); + break; + case EE::EA0_EB: + CHECK(pe(ea0, eb0, eb1) == PE::P_E); + break; + case EE::EA1_EB: + CHECK(pe(ea1, eb0, eb1) == PE::P_E); + break; + default: + break; + } + } + } + } + } +} + struct RandomBarycentricCoordGenerator : Catch::Generators::IGenerator { Eigen::Vector3d bc; @@ -566,3 +810,31 @@ TEST_CASE( point_triangle_distance_type(c.p, c.t0, c.t1, c.t2) == c.expected); } } + +TEST_CASE( + "Exact distance types with the standard classifiers", + "[distance][distance-type]") +{ + // Restore the default even if a check fails. + struct Restore { + ~Restore() { DistanceTypeConfig::instance().set_use_standard(false); } + } restore; + DistanceTypeConfig::instance().set_use_standard(true); + REQUIRE(DistanceTypeConfig::instance().use_standard()); + + for (int i = 0; i < 100; ++i) { + const Eigen::Vector3d p = Eigen::Vector3d::Random(), + a = Eigen::Vector3d::Random(), + b = Eigen::Vector3d::Random(), + c = Eigen::Vector3d::Random(); + CHECK( + point_edge_distance_type_exact(p, a, b) + == point_edge_distance_type(p, a, b)); + CHECK( + point_triangle_distance_type_exact(p, a, b, c) + == point_triangle_distance_type(p, a, b, c)); + CHECK( + edge_edge_distance_type_exact(p, a, b, c) + == edge_edge_distance_type(p, a, b, c)); + } +} diff --git a/tests/src/tests/distance/test_edge_edge.cpp b/tests/src/tests/distance/test_edge_edge.cpp index a3dadcb3e..25bc87b54 100644 --- a/tests/src/tests/distance/test_edge_edge.cpp +++ b/tests/src/tests/distance/test_edge_edge.cpp @@ -8,9 +8,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -257,6 +257,23 @@ TEST_CASE( std::swap(e10, e11); } + // These edges are collinear (or nearly so, for the small |e0y| cases), + // so the thresholded parallel-edge classifier may resolve the shared + // closest endpoints as EA*_EB (edge-interior-vs-vertex) rather than + // vertex-vertex: the "interior" side degenerates to the same endpoint, + // so the returned distance is unaffected either way (checked below). + EdgeEdgeDistanceType dtype = edge_edge_distance_type(e00, e01, e10, e11); + CAPTURE(dtype); + REQUIRE( + ((dtype == EdgeEdgeDistanceType::EA0_EB0) + || (dtype == EdgeEdgeDistanceType::EA0_EB1) + || (dtype == EdgeEdgeDistanceType::EA1_EB0) + || (dtype == EdgeEdgeDistanceType::EA1_EB1) + || (dtype == EdgeEdgeDistanceType::EA0_EB) + || (dtype == EdgeEdgeDistanceType::EA1_EB) + || (dtype == EdgeEdgeDistanceType::EA_EB0) + || (dtype == EdgeEdgeDistanceType::EA_EB1))); + double distance = edge_edge_distance(e00, e01, e10, e11); double expected_distance = point_point_distance( Eigen::Vector3d(gap, e0y, 0), Eigen::Vector3d(-gap, 0, 0)); @@ -445,7 +462,7 @@ struct Edge3TestFixture { const Eigen::Vector3d& e1, const Eigen::MatrixX3d& face_verts, // nf x 3 const Eigen::Vector3d& dn, - const SmoothContactParameters& params, + const GCPParameters& params, bool orient) { const int nf = static_cast(face_verts.rows()); @@ -647,7 +664,7 @@ TEST_CASE("Edge normal term", "[distance][edge-edge][gradient]") SECTION("2 neighbors, VARIANT") { const double alpha_n = 0.85, beta_n = 0.2; - SmoothContactParameters params { 1e-3, 1, 0, alpha_n, beta_n, 2 }; + GCPParameters params { 1e-3, 1, 0, alpha_n, beta_n, 2 }; Eigen::Vector3d f0(0.4, 0.3, GENERATE(take(10, random(-0.2, 0.2)))); Eigen::Vector3d f1(0.6, -0.2, GENERATE(take(10, random(-0.2, 0.2)))); @@ -668,7 +685,7 @@ TEST_CASE("Edge normal term", "[distance][edge-edge][gradient]") SECTION("1 neighbor, VARIANT") { const double alpha_n = 0.85, beta_n = 0.2; - SmoothContactParameters params(1e-3, 1, 0, alpha_n, beta_n, 2); + GCPParameters params(1e-3, 1, 0, alpha_n, beta_n, 2); Eigen::Vector3d f0(0.4, 0.3, GENERATE(take(5, random(-0.2, 0.2)))); @@ -689,7 +706,7 @@ TEST_CASE("Edge normal term", "[distance][edge-edge][gradient]") // The normal-term functions expect direction = -d.normalized(), so we // pass neg_dn = -dn below. const double alpha_n = 0.85, beta_n = 0.2; - SmoothContactParameters params(1e-3, 1, 0, alpha_n, beta_n, 2); + GCPParameters params(1e-3, 1, 0, alpha_n, beta_n, 2); Eigen::Vector3d neg_dn = -dn; // Face normals for the fixture winding are: @@ -726,7 +743,7 @@ TEST_CASE("Edge normal term", "[distance][edge-edge][gradient]") SECTION("non-orientable edge (early-return path)") { const double alpha_n = 0.85, beta_n = 0.2; - SmoothContactParameters params(1e-3, 1, 0, alpha_n, beta_n, 2); + GCPParameters params(1e-3, 1, 0, alpha_n, beta_n, 2); Eigen::Vector3d f0(0.4, 0.3, 0.1), f1(0.6, -0.2, 0.1); @@ -750,7 +767,7 @@ TEST_CASE("Edge normal term", "[distance][edge-edge][gradient]") // and normal_sum >= 1 at construction, forcing all normal types to // ONE. const double alpha_n = 0.5, beta_n = 0.1; - SmoothContactParameters params(1e-3, 1, 0, alpha_n, beta_n, 2); + GCPParameters params(1e-3, 1, 0, alpha_n, beta_n, 2); // Place face vertices far from z=0 in the +y direction, so that the // face normals point nearly purely in +z (aligned with dn). @@ -780,7 +797,7 @@ TEST_CASE("Edge tangent term", "[distance][edge-edge][gradient]") // With dn=(0,0,1), t=(0, y, z), we need z/|t| ∈ (0, 1) ⇒ z > 0. // ONE when dn·t/|t| <= 0 ⇒ z <= 0. const double alpha_t = 1.0, beta_t = 0.0; - SmoothContactParameters params(1e-3, alpha_t, beta_t, 0.85, 0.2, 2); + GCPParameters params(1e-3, alpha_t, beta_t, 0.85, 0.2, 2); const Eigen::Vector3d e0(0, 0, 0), e1(1, 0, 0), dn(0, 0, 1); diff --git a/tests/src/tests/distance/test_line_line.cpp b/tests/src/tests/distance/test_line_line.cpp index 8ac7c4356..1502477b8 100644 --- a/tests/src/tests/distance/test_line_line.cpp +++ b/tests/src/tests/distance/test_line_line.cpp @@ -5,7 +5,7 @@ #include #include -#include +#include #include #include diff --git a/tests/src/tests/distance/test_point_edge.cpp b/tests/src/tests/distance/test_point_edge.cpp index f13d75474..3982c407c 100644 --- a/tests/src/tests/distance/test_point_edge.cpp +++ b/tests/src/tests/distance/test_point_edge.cpp @@ -5,8 +5,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/tests/src/tests/distance/test_point_line.cpp b/tests/src/tests/distance/test_point_line.cpp index 3d2f52a1e..ce356c0f3 100644 --- a/tests/src/tests/distance/test_point_line.cpp +++ b/tests/src/tests/distance/test_point_line.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include diff --git a/tests/src/tests/distance/test_point_point.cpp b/tests/src/tests/distance/test_point_point.cpp index 9c4cc2f19..8728ef884 100644 --- a/tests/src/tests/distance/test_point_point.cpp +++ b/tests/src/tests/distance/test_point_point.cpp @@ -4,7 +4,7 @@ #include #include -#include +#include #include #include diff --git a/tests/src/tests/distance/test_point_triangle.cpp b/tests/src/tests/distance/test_point_triangle.cpp index 7d16f11ad..c062f24fe 100644 --- a/tests/src/tests/distance/test_point_triangle.cpp +++ b/tests/src/tests/distance/test_point_triangle.cpp @@ -8,8 +8,8 @@ #include #include #include -#include -#include +#include +#include #include #include diff --git a/tests/src/tests/dof_layout.hpp b/tests/src/tests/dof_layout.hpp index 689969b51..ad0951838 100644 --- a/tests/src/tests/dof_layout.hpp +++ b/tests/src/tests/dof_layout.hpp @@ -25,6 +25,7 @@ #include #include +#include #include namespace ipc::tests { @@ -48,6 +49,19 @@ inline auto unflatten(const Eigen::VectorXd& x, int dim) return x.reshaped(x.size() / dim, dim); } +/// The DOF indices of a vertex's coordinates in VERTEX_DERIVATIVE_LAYOUT +/// order, e.g. to pick out that vertex's block of a global Hessian. +template inline std::array vertex_dofs(int vid, int n_verts) +{ + std::array dofs; + for (int d = 0; d < dim; ++d) { + dofs[d] = VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor + ? vid * dim + d + : d * n_verts + vid; + } + return dofs; +} + // ─── Reorder FD results from RowMajor to VERTEX_DERIVATIVE_LAYOUT ────────── // // These are used when the FD callback is forced to stay in RowMajor order diff --git a/tests/src/tests/friction/friction_data_generator.cpp b/tests/src/tests/friction/friction_data_generator.cpp index 29d4472c2..ab3a11989 100644 --- a/tests/src/tests/friction/friction_data_generator.cpp +++ b/tests/src/tests/friction/friction_data_generator.cpp @@ -1,12 +1,13 @@ #include "friction_data_generator.hpp" +#include #include #include #include #include -#include +#include Eigen::VectorXd LogSpaced(int num, double start, double stop, double base) { @@ -148,9 +149,9 @@ FrictionData friction_data_generator() using namespace ipc; -SmoothFrictionData smooth_friction_data_generator_3d() +GCPFrictionData smooth_friction_data_generator_3d() { - SmoothFrictionData data; + GCPFrictionData data; auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = data; @@ -168,7 +169,7 @@ SmoothFrictionData smooth_friction_data_generator_3d() barrier_stiffness = 1.; // 100; #endif - params = SmoothContactParameters(dhat, 0.8, 0, 1, 0, 2); + params = GCPParameters(dhat, 0.8, 0, 1, 0, 2); const double max_d = dhat * 0.9; const double min_d = dhat * 0.1; const double d = GENERATE_COPY(range(min_d, max_d, max_d / 10)); @@ -196,7 +197,7 @@ SmoothFrictionData smooth_friction_data_generator_3d() CollisionMesh mesh(V0, E, F); collisions.collisions.push_back( - std::make_shared>( + std::make_shared>( 0, 0, PointTriangleDistanceType::P_T, mesh, params, dhat, V0)); } SECTION("edge-edge") @@ -244,7 +245,7 @@ SmoothFrictionData smooth_friction_data_generator_3d() CollisionMesh mesh(V0, E, F); collisions.collisions.push_back( - std::make_shared>( + std::make_shared>( e0, e1, EdgeEdgeDistanceType::EA_EB, mesh, params, dhat, V0)); } SECTION("point-edge") @@ -281,7 +282,7 @@ SmoothFrictionData smooth_friction_data_generator_3d() CollisionMesh mesh(V0, E, F); collisions.collisions.push_back( - std::make_shared>( + std::make_shared>( e, 0, PointEdgeDistanceType::AUTO, mesh, params, dhat, V0)); } SECTION("point-point") @@ -306,16 +307,16 @@ SmoothFrictionData smooth_friction_data_generator_3d() CollisionMesh mesh(V0, E, F); collisions.collisions.push_back( - std::make_shared>( + std::make_shared>( 0, 1, PointPointDistanceType::AUTO, mesh, params, dhat, V0)); } return data; } -SmoothFrictionData smooth_friction_data_generator_2d() +GCPFrictionData smooth_friction_data_generator_2d() { - SmoothFrictionData data; + GCPFrictionData data; auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = data; @@ -333,7 +334,7 @@ SmoothFrictionData smooth_friction_data_generator_2d() barrier_stiffness = 1.; // 100; #endif - params = SmoothContactParameters(dhat, 0.8, 0, 1, 0, 2); + params = GCPParameters(dhat, 0.8, 0, 1, 0, 2); const double max_d = dhat * 0.9; const double min_d = dhat * 0.1; const double d = GENERATE_COPY(range(min_d, max_d, max_d / 10)); @@ -363,7 +364,7 @@ SmoothFrictionData smooth_friction_data_generator_2d() CollisionMesh mesh(V0, E, F); collisions.collisions.push_back( - std::make_shared>( + std::make_shared>( e, 0, PointEdgeDistanceType::AUTO, mesh, params, dhat, V0)); } SECTION("point-point 2D") @@ -386,9 +387,96 @@ SmoothFrictionData smooth_friction_data_generator_2d() CollisionMesh mesh(V0, E, F); collisions.collisions.push_back( - std::make_shared>( + std::make_shared>( 0, 1, PointPointDistanceType::AUTO, mesh, params, dhat, V0)); } + return data; +} + +ESPFrictionSceneData3D esp_friction_scene_generator_3d(double d) +{ + ESPFrictionSceneData3D data; + auto& [X, E, F, upper_vertices] = data; + + SECTION("point-triangle") + { + // Vertex 0 above the interior of triangle (1,2,3), both in closed + // manifolds. + X.resize(8, 3); + F.resize(8, 3); + X << 0, d, 0, // 0: upper contact point + -1, 0, 1, // 1: lower triangle v0 + 2, 0, 0, // 2: lower triangle v1 + -1, 0, -1, // 3: lower triangle v2 + -1, d + 2, 1, // 4: upper manifold + 2, d + 2, 0, // 5 + -1, d + 2, -1, // 6 + 0, -2, 0; // 7: lower manifold apex + F << 1, 2, 3, 1, 7, 2, 2, 7, 3, 3, 7, 1, 4, 5, 6, 4, 0, 5, 5, 0, 6, 6, + 0, 4; + upper_vertices = { 0, 4, 5, 6 }; + } + + SECTION("point-edge") + { + // Vertex 0 above the interior of edge (1,2) along z, + // vertex 0 in a closed tetrahedron, edge (1,2) in a closed bipyramid. + X.resize(9, 3); + F.resize(10, 3); + X << 0, d, 0, // 0: upper contact point + 0, 0, -1, // 1: lower edge v0 + 0, 0, 1, // 2: lower edge v1 + -1, d + 2, 1, // 3: upper manifold + 2, d + 2, 0, // 4 + -1, d + 2, -1, // 5 + 1, 0, 0, // 6: lower manifold + -1, 0, 0, // 7 + 0, -2, 0; // 8 + F << 3, 4, 5, 3, 0, 4, 4, 0, 5, 5, 0, 3, 1, 7, 2, 1, 2, 6, 2, 7, 8, 7, + 1, 8, 1, 6, 8, 6, 2, 8; + upper_vertices = { 0, 3, 4, 5 }; + } + + SECTION("point-point") + { + // Vertex 0 (upper tet tip) directly above vertex 1 (lower tet tip), + // each at the apex of its own closed tetrahedron. + X.resize(8, 3); + F.resize(8, 3); + X << 0, d, 0, // 0: upper contact point (lower tip of upper tet) + 0, 0, 0, // 1: lower contact point (upper tip of lower tet) + -1, -d, 1, // 2: lower manifold + 2, -d, 0, // 3 + -1, -d, -1, // 4 + -1, d + 2, 1, // 5: upper manifold + 2, d + 2, 0, // 6 + -1, d + 2, -1; // 7 + F << 1, 2, 3, 1, 3, 4, 1, 4, 2, 3, 2, 4, 5, 6, 7, 5, 0, 6, 6, 0, 7, 7, + 0, 5; + upper_vertices = { 0, 5, 6, 7 }; + } + + SECTION("edge-edge") + { + // Edge (0,1) of the upper tetrahedron, along x, crosses above edge + // (4,5) of the lower tetrahedron, along z, at distance d. Every vertex + // is farther than 2d from the other tetrahedron. + X.resize(8, 3); + F.resize(8, 3); + X << -1, d, 0, // 0: upper edge v0 + 1, d, 0, // 1: upper edge v1 + 0, d + 1, 1, // 2: upper manifold + 0, d + 1, -1, // 3 + 0, 0, -1, // 4: lower edge v0 + 0, 0, 1, // 5: lower edge v1 + 1, -1, 0, // 6: lower manifold + -1, -1, 0; // 7 + F << 0, 1, 2, 0, 3, 1, 0, 2, 3, 1, 3, 2, 4, 6, 5, 4, 5, 7, 4, 7, 6, 5, + 6, 7; + upper_vertices = { 0, 1, 2, 3 }; + } + + igl::edges(F, E); return data; } \ No newline at end of file diff --git a/tests/src/tests/friction/friction_data_generator.hpp b/tests/src/tests/friction/friction_data_generator.hpp index fe1299932..e88a7e7b2 100644 --- a/tests/src/tests/friction/friction_data_generator.hpp +++ b/tests/src/tests/friction/friction_data_generator.hpp @@ -3,7 +3,7 @@ #include #include -#include +#include struct FrictionData { Eigen::MatrixXd V0; @@ -22,17 +22,29 @@ Eigen::VectorXd GeomSpaced(int num, double start, double stop); FrictionData friction_data_generator(); -struct SmoothFrictionData { +struct GCPFrictionData { Eigen::MatrixXd V0; Eigen::MatrixXd V1; Eigen::MatrixXi E; Eigen::MatrixXi F; - ipc::SmoothCollisions collisions; + ipc::GCPCollisions collisions; double mu; double epsv_times_h; - ipc::SmoothContactParameters p; + ipc::GCPParameters p; double barrier_stiffness; }; -SmoothFrictionData smooth_friction_data_generator_2d(); -SmoothFrictionData smooth_friction_data_generator_3d(); +GCPFrictionData smooth_friction_data_generator_2d(); +GCPFrictionData smooth_friction_data_generator_3d(); + +/// Scene geometry for "ESP friction force jacobian 3D" tests. +/// Sections: "point-triangle", "point-edge", "point-point", "edge-edge". +struct ESPFrictionSceneData3D { + Eigen::MatrixXd X; + Eigen::MatrixXi E; + Eigen::MatrixXi F; + /// Vertex indices of the "upper object" that slides during the test. + std::vector upper_vertices; +}; + +ESPFrictionSceneData3D esp_friction_scene_generator_3d(double d); diff --git a/tests/src/tests/friction/test_force_jacobian.cpp b/tests/src/tests/friction/test_force_jacobian.cpp index 12675fe28..cef7c429c 100644 --- a/tests/src/tests/friction/test_force_jacobian.cpp +++ b/tests/src/tests/friction/test_force_jacobian.cpp @@ -4,12 +4,15 @@ #include #include +#include #include #include -#include +#include #include #include +#include +#include #include #include @@ -413,10 +416,10 @@ void check_smooth_friction_force_jacobian( const CollisionMesh& mesh, const Eigen::MatrixXd& Ut, const Eigen::MatrixXd& U, - const SmoothCollisions& collisions, + const GCPCollisions& collisions, const double mu, const double epsv_times_h, - const SmoothContactParameters& params, + const GCPParameters& params, const double barrier_stiffness, const bool recompute_collisions) { @@ -454,11 +457,11 @@ void check_smooth_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// - const Eigen::VectorXd force = D.smooth_contact_force( - friction_collisions, mesh, X, Ut_mesh, velocities); + const Eigen::VectorXd force = + D.gcp_force(friction_collisions, mesh, X, Ut_mesh, velocities); const Eigen::VectorXd grad_D = D.gradient(friction_collisions, mesh, velocities); - CHECK((force + grad_D).norm() <= 1e-8 * force.norm()); + CHECK((force + grad_D).norm() <= 1e-8 * std::max(force.norm(), 1e-8)); /////////////////////////////////////////////////////////////////////////// @@ -483,7 +486,7 @@ void check_smooth_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// - Eigen::MatrixXd jac_force = D.smooth_contact_force_jacobian( + Eigen::MatrixXd jac_force = D.gcp_force_jacobian( friction_collisions, mesh, X, Ut_mesh, velocities, params, FrictionPotential::DiffWRT::VELOCITIES); CHECK((hess_D + jac_force).norm() <= 1e-7 * hess_D.norm()); @@ -493,31 +496,29 @@ void check_smooth_friction_force_jacobian( auto create_smooth_collision = [&](const CollisionMesh& fd_mesh, const Eigen::MatrixXd& fd_lagged_positions) { - SmoothCollisions fd_collisions; + GCPCollisions fd_collisions; assert(friction_collisions.size() == 1); - auto cc = friction_collisions[0].smooth_collision; - std::shared_ptr fd_cc; + auto cc = friction_collisions[0].gcp_collision; + std::shared_ptr fd_cc; if (dim == 3) { if (cc->type() == CollisionType::EDGE_EDGE) { - fd_cc = std::make_shared>( + fd_cc = std::make_shared>( (*cc)[0], (*cc)[1], PrimitiveDistType::type::AUTO, fd_mesh, params, dhat, fd_lagged_positions); } else if (cc->type() == CollisionType::EDGE_VERTEX) { - fd_cc = - std::make_shared>( - (*cc)[0], (*cc)[1], - PrimitiveDistType::type::AUTO, fd_mesh, - params, dhat, fd_lagged_positions); + fd_cc = std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); } else if (cc->type() == CollisionType::VERTEX_VERTEX) { - fd_cc = - std::make_shared>( - (*cc)[0], (*cc)[1], - PrimitiveDistType::type::AUTO, fd_mesh, - params, dhat, fd_lagged_positions); + fd_cc = std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); } else if (cc->type() == CollisionType::FACE_VERTEX) { - fd_cc = std::make_shared>( + fd_cc = std::make_shared>( (*cc)[0], (*cc)[1], PrimitiveDistType::type::AUTO, fd_mesh, params, dhat, fd_lagged_positions); @@ -526,17 +527,15 @@ void check_smooth_friction_force_jacobian( fd_collisions.collisions.push_back(fd_cc); } else { if (cc->type() == CollisionType::EDGE_VERTEX) { - fd_cc = - std::make_shared>( - (*cc)[0], (*cc)[1], - PrimitiveDistType::type::AUTO, fd_mesh, - params, dhat, fd_lagged_positions); + fd_cc = std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); } else if (cc->type() == CollisionType::VERTEX_VERTEX) { - fd_cc = - std::make_shared>( - (*cc)[0], (*cc)[1], - PrimitiveDistType::type::AUTO, fd_mesh, - params, dhat, fd_lagged_positions); + fd_cc = std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); } fd_collisions.collisions.push_back(fd_cc); @@ -554,7 +553,7 @@ void check_smooth_friction_force_jacobian( // Eigen::VectorXd::Zero(X.size()); // { // auto cc = create_smooth_collision(mesh, lagged_positions); - // SmoothContactPotential potential(params); + // GCPPotential potential(params); // Eigen::VectorXd g = potential.gradient(cc, mesh, // lagged_positions); Eigen::SparseMatrix h = // potential.hessian(cc, mesh, lagged_positions); @@ -570,7 +569,7 @@ void check_smooth_friction_force_jacobian( // auto fd_cc = create_smooth_collision(fd_mesh, // fd_lagged_positions); - // SmoothContactPotential potential(params); + // GCPPotential potential(params); // return potential.gradient(fd_cc, fd_mesh, // fd_lagged_positions).norm(); // }; @@ -583,7 +582,7 @@ void check_smooth_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// - Eigen::MatrixXd JF_wrt_X = D.smooth_contact_force_jacobian( + Eigen::MatrixXd JF_wrt_X = D.gcp_force_jacobian( friction_collisions, mesh, X, Ut_mesh, velocities, params, FrictionPotential::DiffWRT::REST_POSITIONS); @@ -612,7 +611,7 @@ void check_smooth_friction_force_jacobian( fd_friction_collisions.update_lagged_anisotropic_friction_coefficients( fd_mesh, fd_X, Ut_mesh, velocities); - return D.smooth_contact_force( + return D.gcp_force( fd_friction_collisions, fd_mesh, fd_X, Ut_mesh, velocities); }; Eigen::MatrixXd fd_JF_wrt_X; @@ -629,7 +628,7 @@ void check_smooth_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// - Eigen::MatrixXd JF_wrt_Ut = D.smooth_contact_force_jacobian( + Eigen::MatrixXd JF_wrt_Ut = D.gcp_force_jacobian( friction_collisions, mesh, X, Ut_mesh, velocities, params, FrictionPotential::DiffWRT::LAGGED_DISPLACEMENTS); @@ -647,8 +646,7 @@ void check_smooth_friction_force_jacobian( fd_friction_collisions.update_lagged_anisotropic_friction_coefficients( mesh, X, fd_Ut, velocities); - return D.smooth_contact_force( - fd_friction_collisions, mesh, X, fd_Ut, velocities); + return D.gcp_force(fd_friction_collisions, mesh, X, fd_Ut, velocities); }; Eigen::MatrixXd fd_JF_wrt_Ut; fd::finite_jacobian( @@ -664,12 +662,12 @@ void check_smooth_friction_force_jacobian( /////////////////////////////////////////////////////////////////////////// - Eigen::MatrixXd JF_wrt_V = D.smooth_contact_force_jacobian( + Eigen::MatrixXd JF_wrt_V = D.gcp_force_jacobian( friction_collisions, mesh, X, Ut_mesh, velocities, params, FrictionPotential::DiffWRT::VELOCITIES); auto F_V = [&](const Eigen::VectorXd& v) { - return D.smooth_contact_force( + return D.gcp_force( friction_collisions, mesh, X, Ut_mesh, tests::unflatten(v, velocities.cols())); }; @@ -689,12 +687,12 @@ void check_smooth_friction_force_jacobian( friction_collisions.update_lagged_anisotropic_friction_coefficients( mesh, X, Ut_mesh, velocities); - Eigen::MatrixXd JF_wrt_V_aniso = D.smooth_contact_force_jacobian( + Eigen::MatrixXd JF_wrt_V_aniso = D.gcp_force_jacobian( friction_collisions, mesh, X, Ut_mesh, velocities, params, FrictionPotential::DiffWRT::VELOCITIES, 0.0, false); auto F_V_aniso = [&](const Eigen::VectorXd& v) { - return D.smooth_contact_force( + return D.gcp_force( friction_collisions, mesh, X, Ut_mesh, fd::unflatten(v, velocities.cols()), 0.0, false); }; @@ -714,7 +712,7 @@ void check_smooth_friction_force_jacobian( TEST_CASE( "Smooth friction force jacobian 2D", "[friction-smooth][force-jacobian]") { - SmoothFrictionData data = smooth_friction_data_generator_2d(); + GCPFrictionData data = smooth_friction_data_generator_2d(); const auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = data; @@ -730,6 +728,419 @@ TEST_CASE( false); } +// ============================================================================ + +void check_esp_friction_force_jacobian( + const CollisionMesh& mesh, + const Eigen::MatrixXd& Ut, + const Eigen::MatrixXd& U, + const ESPCollisions& collisions, + const double mu, + const double epsv_times_h, + const ESPParameters& params, + const double normal_stiffness, + const bool normalize_weights = true) +{ + const Eigen::MatrixXd& X = mesh.rest_positions(); + const Eigen::MatrixXd velocities = U - Ut; + + CAPTURE( + mu, epsv_times_h, params.dhat, normal_stiffness, collisions.size(), + normalize_weights, params.quad_rule().size()); + + TangentialCollisions friction_collisions; + friction_collisions.build( + mesh, X + Ut, collisions, params, normal_stiffness, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu, + TangentialCollisions::default_blend_mu, normalize_weights); + CHECK(!friction_collisions.empty()); + + const FrictionPotential D(epsv_times_h); + + // Check: force = -grad + const Eigen::VectorXd force = + D.gcp_force(friction_collisions, mesh, X, Ut, velocities); + REQUIRE(force.norm() > 0); + const Eigen::VectorXd grad_D = + D.gradient(friction_collisions, mesh, velocities); + CHECK((force + grad_D).norm() <= 1e-8 * force.norm()); + + // Check: hessian vs FD of gradient wrt velocities + const Eigen::MatrixXd hess_D = + D.hessian(friction_collisions, mesh, velocities); + + auto grad_func = [&](const Eigen::VectorXd& v) { + return D.gradient( + friction_collisions, mesh, fd::unflatten(v, velocities.cols())); + }; + Eigen::MatrixXd fd_hessian; + fd::finite_jacobian( + fd::flatten(velocities), grad_func, fd_hessian, + fd::AccuracyOrder::FOURTH, 1e-8 * params.dhat); + CHECK( + (hess_D.norm() == 0 + || (hess_D - fd_hessian).norm() <= 1e-7 * hess_D.norm())); + + // NOTE: The direct gcp_force_jacobian() path is intentionally + // not exercised for ESP 3D collisions here because some ESP + // friction stencils include virtual vertices. +} + +TEST_CASE("ESP friction force jacobian 2D", "[friction-esp][force-jacobian]") +{ + constexpr double BA = 1e-7; + const double dhat = 0.6; + const double mu = 1.; + const double epsv_times_h = 1.; + const double normal_stiffness = 1.; + const bool normalize_weights = GENERATE(true, false); + const ESPParameters params(dhat, 1., 2); + + // Two close 2D rectangles (gap ~0.2 < dhat=0.6) + Eigen::MatrixXd V0(8, 2), V1; + Eigen::MatrixXi E(8, 2), F; + V0 << -1., 1., -1., 0., -.1, 0. + BA, -.1, 1. + BA, .1, 1., .1, 0., 1., 0., + 1., 1.; + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + + CollisionMesh mesh( + std::vector(V0.rows(), true), std::vector(V0.rows(), false), + V0, E, F); + + ESPCollisions collisions; + collisions.build(mesh, V0, params); + REQUIRE(!collisions.empty()); + + // Slide left square to the right to create tangential velocity + V1 = V0; + V1.block(0, 0, 4, 1).array() += 0.05; + + const Eigen::MatrixXd Ut = Eigen::MatrixXd::Zero(V0.rows(), V0.cols()); + const Eigen::MatrixXd U = V1 - V0; + + check_esp_friction_force_jacobian( + mesh, Ut, U, collisions, mu, epsv_times_h, params, normal_stiffness, + normalize_weights); +} + +/// Build a simple face quadrature rule with the 3 triangle vertices. +static FaceQuadRule make_vertex_quad_rule() +{ + return { + { { { 1.0, 0.0, 0.0 } }, 1.0 / 3.0 }, + { { { 0.0, 1.0, 0.0 } }, 1.0 / 3.0 }, + { { { 0.0, 0.0, 1.0 } }, 1.0 / 3.0 }, + }; +} + +/// Build a face quadrature rule with 3 vertices + centroid. +static FaceQuadRule make_vertex_plus_centroid_quad_rule() +{ + return { + { { { 1.0, 0.0, 0.0 } }, 0.25 }, + { { { 0.0, 1.0, 0.0 } }, 0.25 }, + { { { 0.0, 0.0, 1.0 } }, 0.25 }, + { { { 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0 } }, 0.25 }, + }; +} + +TEST_CASE("ESP friction force jacobian 3D", "[friction-esp][force-jacobian]") +{ + const double dhat = 0.15; + const double mu = 1.; + const double epsv_times_h = 1.; + const double normal_stiffness = 1.; + const bool normalize_weights = GENERATE(true, false); + // quad_order=0 uses vertex-only collisions (no face quadrature rule). + // quad_order=1 with a face quadrature rule set uses face quadrature. + const int quad_order = GENERATE(0, 1); + ESPParameters params(dhat, 1., quad_order); + if (quad_order > 0) { + params.set_quad_rule(GENERATE_COPY( + make_vertex_quad_rule(), make_vertex_plus_centroid_quad_rule())); + } + + const ESPFrictionSceneData3D scene = + esp_friction_scene_generator_3d(dhat * 0.5); + const Eigen::MatrixXd& X = scene.X; + const std::vector& upper_vertices = scene.upper_vertices; + + const Eigen::MatrixXd Ut = Eigen::MatrixXd::Zero(X.rows(), X.cols()); + CollisionMesh mesh(X, scene.E, scene.F); + ESPCollisions collisions; + collisions.build(mesh, X + Ut, params); + REQUIRE(!collisions.empty()); + + // Test both tangential slide directions for each scene. + auto run_check = [&](const Eigen::RowVector3d& disp) { + Eigen::MatrixXd V1 = X; + for (int v : upper_vertices) + V1.row(v) += disp; + check_esp_friction_force_jacobian( + mesh, Ut, V1 - X, collisions, mu, epsv_times_h, params, + normal_stiffness, normalize_weights); + }; + run_check({ 0.05, 0, 0 }); // slide_x + run_check({ 0, 0, 0.05 }); // slide_z +} + +TEST_CASE( + "ESP friction force jacobian 3D two cubes", + "[friction-esp][force-jacobian]") +{ + Eigen::MatrixXd X; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("two-cubes-close.ply", X, E, F)); + const CollisionMesh mesh(X, E, F); + const ESPParameters params(0.1, 1.0, 0); + const bool normalize_weights = GENERATE(true, false); + + ESPCollisions collisions; + collisions.build(mesh, X, params); + REQUIRE(!collisions.empty()); + + // The edge-edge pairs give vertex-vertex, edge-vertex and face-vertex + // sub-collisions, which become edge-vertex, edge-edge and face-vertex + // friction stencils. + const Eigen::VectorXd mu = Eigen::VectorXd::Ones(X.rows()); + TangentialCollisions friction_collisions; + friction_collisions.build( + mesh, X, collisions, params, 1.0, mu, mu, + TangentialCollisions::default_blend_mu, normalize_weights); + CHECK(!friction_collisions.ev_collisions.empty()); + CHECK(!friction_collisions.ee_collisions.empty()); + CHECK(!friction_collisions.fv_collisions.empty()); + + // Slide the second cube along y. + const Eigen::MatrixXd Ut = Eigen::MatrixXd::Zero(X.rows(), X.cols()); + Eigen::MatrixXd U = Ut; + for (int i = 0; i < X.rows(); i++) { + if (X(i, 0) > 0.96) { + U(i, 1) = 0.05; + } + } + check_esp_friction_force_jacobian( + mesh, Ut, U, collisions, /*mu=*/1., /*epsv_times_h=*/1., params, + /*normal_stiffness=*/1., normalize_weights); +} + +TEST_CASE("ESP friction with zero normal stiffness", "[friction-esp]") +{ + // Every stencil has zero normal force, so none is kept. + SECTION("2D") + { + const ESPParameters params(0.6, 1., 2); + Eigen::MatrixXd V(8, 2); + Eigen::MatrixXi E(8, 2), F; + V << -1., 1., -1., 0., -.1, 0., -.1, 1., .1, 1., .1, 0., 1., 0., 1., 1.; + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + const CollisionMesh mesh( + std::vector(V.rows(), true), + std::vector(V.rows(), false), V, E, F); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + + const Eigen::VectorXd mu = Eigen::VectorXd::Ones(V.rows()); + TangentialCollisions friction_collisions; + friction_collisions.build( + mesh, V, collisions, params, /*normal_stiffness=*/0., mu, mu); + CHECK(friction_collisions.empty()); + } + + SECTION("3D") + { + const double dhat = 0.15; + const ESPFrictionSceneData3D scene = + esp_friction_scene_generator_3d(dhat * 0.5); + const CollisionMesh mesh(scene.X, scene.E, scene.F); + const ESPParameters params(dhat, 1., 0); + + ESPCollisions collisions; + collisions.build(mesh, scene.X, params); + REQUIRE(!collisions.empty()); + + const Eigen::VectorXd mu = Eigen::VectorXd::Ones(scene.X.rows()); + TangentialCollisions friction_collisions; + friction_collisions.build( + mesh, scene.X, collisions, params, /*normal_stiffness=*/0., mu, mu); + CHECK(friction_collisions.empty()); + } +} + +TEST_CASE( + "ESP friction blends coefficients with a positionally passed blend_mu", + "[friction-esp]") +{ + // A blend that ignores its inputs makes every stencil's coefficients + // equal to its result, whatever the per-vertex values are. + const bool normalize_weights = GENERATE(true, false); + const double dhat = 0.15; + const ESPFrictionSceneData3D scene = + esp_friction_scene_generator_3d(dhat * 0.5); + const CollisionMesh mesh(scene.X, scene.E, scene.F); + const ESPParameters params(dhat, 1., 0); + + ESPCollisions collisions; + collisions.build(mesh, scene.X, params); + REQUIRE(!collisions.empty()); + + const Eigen::VectorXd mu = Eigen::VectorXd::Ones(scene.X.rows()); + TangentialCollisions friction_collisions; + friction_collisions.build( + mesh, scene.X, collisions, params, /*normal_stiffness=*/1., mu, mu, + [](double, double) { return 0.25; }, normalize_weights); + REQUIRE(!friction_collisions.empty()); + + for (size_t i = 0; i < friction_collisions.size(); i++) { + CHECK(friction_collisions[i].mu_s == 0.25); + CHECK(friction_collisions[i].mu_k == 0.25); + } +} + +TEST_CASE("ESP friction requires the build's quadrature", "[friction-esp]") +{ + SECTION("2D") + { + const ESPParameters params(0.6, 1., 2); + Eigen::MatrixXd V(8, 2); + Eigen::MatrixXi E(8, 2), F; + V << -1., 1., -1., 0., -.1, 0., -.1, 1., .1, 1., .1, 0., 1., 0., 1., 1.; + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + const CollisionMesh mesh( + std::vector(V.rows(), true), + std::vector(V.rows(), false), V, E, F); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + + const Eigen::VectorXd mu = Eigen::VectorXd::Ones(V.rows()); + TangentialCollisions friction_collisions; + CHECK_NOTHROW( + friction_collisions.build(mesh, V, collisions, params, 1., mu, mu)); + CHECK_THROWS_AS( + friction_collisions.build( + mesh, V, collisions, ESPParameters(0.6, 1., 3), 1., mu, mu), + std::invalid_argument); + } + + SECTION("3D") + { + const double dhat = 0.1; + Eigen::MatrixXd X; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("two-cubes-close.ply", X, E, F)); + const CollisionMesh mesh(X, E, F); + ESPParameters params(dhat, 1.); + params.set_quad_rule( + { + { { { 0., .5, .5 } }, 1. / 3 }, + { { { .5, 0., .5 } }, 1. / 3 }, + { { { .5, .5, 0. } }, 1. / 3 }, + }); + + ESPCollisions collisions; + collisions.build(mesh, X, params); + REQUIRE(!collisions.empty()); + + const Eigen::VectorXd mu = Eigen::VectorXd::Ones(X.rows()); + TangentialCollisions friction_collisions; + CHECK_NOTHROW( + friction_collisions.build(mesh, X, collisions, params, 1., mu, mu)); + // Freshly constructed parameters have no face quadrature rule. + CHECK_THROWS_AS( + friction_collisions.build( + mesh, X, collisions, ESPParameters(dhat, 1.), 1., mu, mu), + std::invalid_argument); + } +} + +// The friction build reads the vertices through views of the vertex matrix, +// so a block of a taller matrix (whose columns are further apart than its row +// count) must give the same stencils as the equivalent contiguous matrix. +TEST_CASE( + "ESP friction reads a strided vertex matrix like a contiguous one", + "[friction-esp]") +{ + const bool normalize_weights = GENERATE(true, false); + + Eigen::MatrixXd V, U; + CollisionMesh mesh; + double dhat = 0.15; + int quad_order = 0; + FaceQuadRule face_quad_rule; + SECTION("2D") + { + V.resize(8, 2); + V << -1., 1., -1., 0., -.1, 0., -.1, 1., .1, 1., .1, 0., 1., 0., 1., 1.; + Eigen::MatrixXi E(8, 2), F; + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + mesh = CollisionMesh( + std::vector(V.rows(), true), + std::vector(V.rows(), false), V, E, F); + U = Eigen::MatrixXd::Zero(V.rows(), V.cols()); + U.topRows(4).col(1).array() = 0.05; // slide the left rectangle + dhat = 0.6; + quad_order = 2; + } + // Slide the upper surface of the 3D scene along x. + const auto load_3d_scene = [&]() { + const ESPFrictionSceneData3D scene = + esp_friction_scene_generator_3d(dhat * 0.5); + V = scene.X; + mesh = CollisionMesh(V, scene.E, scene.F); + U = Eigen::MatrixXd::Zero(V.rows(), V.cols()); + for (int v : scene.upper_vertices) { + U(v, 0) = 0.05; + } + }; + SECTION("3D vertex quadrature") { load_3d_scene(); } + SECTION("3D face quadrature") + { + load_3d_scene(); + quad_order = 1; + face_quad_rule = make_vertex_plus_centroid_quad_rule(); + } + ESPParameters params(dhat, 1., quad_order); + params.set_quad_rule(face_quad_rule); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + + // The rows below V hold values far from the mesh, so reading them instead + // of V changes the stencils. + Eigen::MatrixXd V_big = + Eigen::MatrixXd::Constant(V.rows() + 7, V.cols(), 1e3); + V_big.topRows(V.rows()) = V; + + const Eigen::VectorXd mu = Eigen::VectorXd::Ones(V.rows()); + TangentialCollisions friction_collisions, strided_friction_collisions; + friction_collisions.build( + mesh, V, collisions, params, /*normal_stiffness=*/1., mu, mu, + TangentialCollisions::default_blend_mu, normalize_weights); + strided_friction_collisions.build( + mesh, V_big.topRows(V.rows()), collisions, params, + /*normal_stiffness=*/1., mu, mu, TangentialCollisions::default_blend_mu, + normalize_weights); + REQUIRE(!friction_collisions.empty()); + CHECK(strided_friction_collisions.size() == friction_collisions.size()); + + const FrictionPotential D(/*epsv_times_h=*/1.); + const double value = D(friction_collisions, mesh, U); + const Eigen::VectorXd grad = D.gradient(friction_collisions, mesh, U); + REQUIRE(grad.norm() > 0); + CHECK( + std::abs(D(strided_friction_collisions, mesh, U) - value) + <= 1e-12 * std::abs(value)); + CHECK( + (D.gradient(strided_friction_collisions, mesh, U) - grad).norm() + <= 1e-12 * grad.norm()); +} + TEST_CASE( "Smooth friction force no_mu and no_contact_force_multiplier", "[friction-smooth][force][no-mu]") @@ -740,7 +1151,7 @@ TEST_CASE( "skipped in debug mode"); #endif - SmoothFrictionData data = smooth_friction_data_generator_3d(); + GCPFrictionData data = smooth_friction_data_generator_3d(); const auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = data; @@ -765,11 +1176,11 @@ TEST_CASE( const FrictionPotential D(epsv_times_h); - // Batch smooth_contact_force with no_mu=false then no_mu=true - const Eigen::VectorXd force_default = D.smooth_contact_force( - friction_collisions, mesh, X, Ut, velocities, 0.0, false); - const Eigen::VectorXd force_no_mu = D.smooth_contact_force( - friction_collisions, mesh, X, Ut, velocities, 0.0, true); + // Batch gcp_force with no_mu=false then no_mu=true + const Eigen::VectorXd force_default = + D.gcp_force(friction_collisions, mesh, X, Ut, velocities, 0.0, false); + const Eigen::VectorXd force_no_mu = + D.gcp_force(friction_collisions, mesh, X, Ut, velocities, 0.0, true); CHECK(force_default.array().isFinite().all()); CHECK(force_no_mu.array().isFinite().all()); @@ -786,9 +1197,9 @@ TEST_CASE( const auto vel = collision.dof(velocities, E, F); const Eigen::VectorXd local_force_N = - D.smooth_contact_force(collision, rest, lagged, vel, false, false); + D.gcp_force(collision, rest, lagged, vel, false, false); const Eigen::VectorXd local_force_no_N = - D.smooth_contact_force(collision, rest, lagged, vel, false, true); + D.gcp_force(collision, rest, lagged, vel, false, true); CHECK(local_force_N.array().isFinite().all()); CHECK(local_force_no_N.array().isFinite().all()); @@ -800,14 +1211,80 @@ TEST_CASE( <= 1e-8 * local_force_N.norm()); } - // Cover batch smooth_contact_force_jacobian with no_mu=true - const Eigen::SparseMatrix jac_no_mu = - D.smooth_contact_force_jacobian( - friction_collisions, mesh, X, Ut, velocities, params, - FrictionPotential::DiffWRT::VELOCITIES, 0.0, true); + // Cover batch gcp_force_jacobian with no_mu=true + const Eigen::SparseMatrix jac_no_mu = D.gcp_force_jacobian( + friction_collisions, mesh, X, Ut, velocities, params, + FrictionPotential::DiffWRT::VELOCITIES, 0.0, true); CHECK(jac_no_mu.size() > 0); } +TEST_CASE( + "Smooth friction force jacobian of a single collision", + "[friction-smooth][force-jacobian]") +{ +#if (defined(WIN32) || defined(_WIN32) || defined(__WIN32)) && !defined(NDEBUG) + SKIP( + "'Smooth friction force jacobian of a single collision' test is " + "skipped in debug mode"); +#endif + using DiffWRT = FrictionPotential::DiffWRT; + const DiffWRT wrt = GENERATE( + DiffWRT::REST_POSITIONS, DiffWRT::LAGGED_DISPLACEMENTS, + DiffWRT::VELOCITIES); + + GCPFrictionData data = smooth_friction_data_generator_3d(); + const auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = + data; + + const Eigen::MatrixXd& X = V0; + const Eigen::MatrixXd Ut = Eigen::MatrixXd::Zero(X.rows(), X.cols()); + const Eigen::MatrixXd velocities = V1 - V0; + const CollisionMesh mesh(X, E, F); + + TangentialCollisions friction_collisions; + friction_collisions.build( + mesh, X + Ut, collisions, params, barrier_stiffness, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu); + if (friction_collisions.empty()) { + return; + } + friction_collisions.update_lagged_anisotropic_friction_coefficients( + mesh, X, Ut, velocities); + + const FrictionPotential D(epsv_times_h); + for (size_t i = 0; i < friction_collisions.size(); i++) { + const TangentialCollision& collision = friction_collisions[i]; + const Eigen::VectorXd x = collision.dof(X, E, F); + const Eigen::VectorXd u = collision.dof(Ut, E, F); + const Eigen::VectorXd v = collision.dof(velocities, E, F); + + const Eigen::MatrixXd J = D.gcp_force_jacobian(collision, x, u, v, wrt); + + // The single-collision force, as a function of the variable we + // differentiate with respect to. + const auto force = [&](const Eigen::VectorXd& y) -> Eigen::VectorXd { + switch (wrt) { + case DiffWRT::REST_POSITIONS: + return D.gcp_force(collision, y, u, v); + case DiffWRT::LAGGED_DISPLACEMENTS: + return D.gcp_force(collision, x, y, v); + default: + return D.gcp_force(collision, x, u, y); + } + }; + const Eigen::VectorXd& y0 = wrt == DiffWRT::REST_POSITIONS ? x + : wrt == DiffWRT::LAGGED_DISPLACEMENTS ? u + : v; + Eigen::MatrixXd fd_J; + fd::finite_jacobian( + y0, force, fd_J, fd::AccuracyOrder::FOURTH, 1e-6 * params.dhat); + + CAPTURE(i, static_cast(wrt)); + CHECK((J - fd_J).norm() <= 1e-7 * std::max(J.norm(), 1e-8)); + } +} + TEST_CASE( "Smooth friction force jacobian 3D", "[friction-smooth][force-jacobian]") { @@ -815,7 +1292,7 @@ TEST_CASE( SKIP("'Smooth friction force jacobian 3D' test is skipped in debug mode"); #endif - SmoothFrictionData data = smooth_friction_data_generator_3d(); + GCPFrictionData data = smooth_friction_data_generator_3d(); const auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = data; @@ -829,4 +1306,4 @@ TEST_CASE( check_smooth_friction_force_jacobian( mesh, Ut, U, collisions, mu, epsv_times_h, params, barrier_stiffness, false); -} \ No newline at end of file +} diff --git a/tests/src/tests/potential/CMakeLists.txt b/tests/src/tests/potential/CMakeLists.txt index b7f00b894..7e8419e07 100644 --- a/tests/src/tests/potential/CMakeLists.txt +++ b/tests/src/tests/potential/CMakeLists.txt @@ -1,8 +1,10 @@ set(SOURCES # Tests test_adhesion_potentials.cpp + test_arbitrary_point_esp.cpp test_barrier_potential.cpp - test_smooth_potential.cpp + test_gcp_potential.cpp + test_esp_potential.cpp test_friction_potential.cpp test_distance_vector_methods.cpp test_full_dof_assembly.cpp @@ -11,6 +13,7 @@ set(SOURCES # Benchmarks benchmark_assembly.cpp + benchmark_esp_adjacency.cpp benchmark_gradient_assembly.cpp benchmark_simd_barrier_potential.cpp @@ -25,4 +28,4 @@ target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) ################################################################################ # Subfolders -################################################################################ \ No newline at end of file +################################################################################ diff --git a/tests/src/tests/potential/benchmark_esp_adjacency.cpp b/tests/src/tests/potential/benchmark_esp_adjacency.cpp new file mode 100644 index 000000000..bfa905e18 --- /dev/null +++ b/tests/src/tests/potential/benchmark_esp_adjacency.cpp @@ -0,0 +1,233 @@ +// End-to-end ESP timings that track the cost of the candidate adjacency lists +// (Candidates::*_set), which ESP queries for every primitive it integrates +// over. The first test goes through the public ESP API only, so the very same +// benchmark runs against any implementation of those lists. +// +// Each scene benchmarks +// - ESPCollisions::build (convert_candidates_to_sets + the builder queries), +// - ESPPotential value, gradient, and Hessian, +// and prints the results themselves, so two implementations can be checked +// against each other. They agree to roundoff, not bitwise: the sums run over +// hash containers whose iteration order changes from one process to the next. +// +// Run with (single-threaded is the primary measurement): +// ./ipc_toolkit_tests "[esp_adjacency_benchmark]" --nthreads 1 \ +// --benchmark-samples 7 +// A single build takes hundreds of milliseconds on the large scenes, so lower +// the sample count from Catch2's default of 100. +// +// Environment: +// IPC_TOOLKIT_BENCH_SCENES comma separated scene labels to run + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +using namespace ipc; + +namespace { + +struct SceneSpec { + std::string label; + std::string mesh_name; + double dhat; + int quad_rule; // 0 = vertices, 1 = vertices + centroid +}; + +const std::vector& scene_specs() +{ + static const std::vector specs = { + { "sphere", "wrapped_sphere.obj", 0.15, 0 }, + { "sphere-face", "wrapped_sphere.obj", 0.15, 1 }, + { "two-cubes", "two-cubes-close.ply", 0.1, 0 }, + { "bunny", "bunny.ply", 3e-2, 0 }, + { "cloth-ball", "cloth_ball92.ply", 1e-3, 0 }, + { "cloth-funnel", "cloth-funnel/227.ply", 4.0e-3, 0 }, + }; + return specs; +} + +bool scene_selected(const std::string& label) +{ + const char* v = std::getenv("IPC_TOOLKIT_BENCH_SCENES"); + if (!v) { + return true; + } + std::stringstream ss(v); + for (std::string tok; std::getline(ss, tok, ',');) { + if (tok == label) { + return true; + } + } + return false; +} + +std::vector selected_scenes(const bool vertices_only = false) +{ + std::vector out; + for (const SceneSpec& spec : scene_specs()) { + if (scene_selected(spec.label) + && (!vertices_only || spec.quad_rule == 0)) { + out.push_back(spec); + } + } + return out; +} + +} // namespace + +TEST_CASE( + "ESP adjacency end-to-end timings", "[esp_adjacency_benchmark][!benchmark]") +{ + const auto scenes = selected_scenes(); + if (scenes.empty()) { + SKIP("no scene selected"); + } + const SceneSpec spec = GENERATE_COPY(from_range(scenes)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + const std::string path = (tests::DATA_DIR / spec.mesh_name).string(); + if (!igl::read_triangle_mesh(path, V, F)) { + SKIP("missing " << path); + } + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + ESPParameters params(spec.dhat, 1., 0); + if (spec.quad_rule == 1) { + params.set_quad_rule( + { + { { { 1., 0., 0. } }, 0.25 }, + { { { 0., 1., 0. } }, 0.25 }, + { { { 0., 0., 1. } }, 0.25 }, + { { { 1. / 3, 1. / 3, 1. / 3 } }, 0.25 }, + }); + } + const ESPPotential potential(params); + + ESPCollisions collisions; + double value = 0; + Eigen::VectorXd grad; + Eigen::SparseMatrix hess; + + BENCHMARK(fmt::format("{}: build", spec.label)) + { + collisions.build(mesh, V, params); + return collisions.size(); + }; + + BENCHMARK(fmt::format("{}: value", spec.label)) + { + value = potential(collisions, mesh, V); + return value; + }; + + BENCHMARK(fmt::format("{}: gradient", spec.label)) + { + grad = potential.gradient(collisions, mesh, V); + return grad.size(); + }; + + // The Hessian of a scene with 100k collisions takes minutes and says + // nothing the gradient does not, so we skip it past this size. + constexpr size_t MAX_HESSIAN_COLLISIONS = 20000; + if (collisions.size() <= MAX_HESSIAN_COLLISIONS) { + BENCHMARK(fmt::format("{}: hessian", spec.label)) + { + hess = potential.hessian(collisions, mesh, V); + return hess.nonZeros(); + }; + } + + std::printf( + "%-12s verts=%zu faces=%zu colls=%zu P=%.17g |g|=%.17g sum(g)=%.17g " + "|H|=%.17g\n", + spec.label.c_str(), size_t(V.rows()), size_t(F.rows()), + collisions.size(), value, grad.norm(), grad.sum(), hess.norm()); + std::fflush(stdout); +} + +// The accessors in isolation: build the candidates once, then sweep every +// primitive id through each accessor and walk the result, as the ESP code does. +// This is an upper bound on what a faster container can buy -- it measures the +// queries with nothing else around them -- so the end-to-end numbers above are +// the ones to trust for the speedup. +TEST_CASE( + "ESP adjacency accessors in isolation", + "[esp_adjacency_benchmark][!benchmark]") +{ + const auto scenes = selected_scenes(/*vertices_only=*/true); + if (scenes.empty()) { + SKIP("no scene selected"); + } + const SceneSpec spec = GENERATE_COPY(from_range(scenes)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + const std::string path = (tests::DATA_DIR / spec.mesh_name).string(); + if (!igl::read_triangle_mesh(path, V, F)) { + SKIP("missing " << path); + } + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + Candidates candidates; + candidates.build(mesh, V, spec.dhat, nullptr, /*all_types=*/true); + + const index_t nv = index_t(mesh.num_vertices()); + const index_t ne = index_t(mesh.num_edges()); + const index_t nf = index_t(mesh.num_faces()); + + BENCHMARK(fmt::format("{}: convert", spec.label)) + { + candidates.convert_candidates_to_sets(); + return candidates.size(); + }; + + // The accessors read the lists that convert_candidates_to_sets built. + candidates.convert_candidates_to_sets(); + + const auto sweep = [&](const std::string& name, const index_t n, + auto&& query) { + BENCHMARK(fmt::format("{}: {}", spec.label, name)) + { + size_t sum = 0; + for (index_t id = 0; id < n; ++id) { + for (const index_t x : query(id)) { + sum += size_t(x) + 1; + } + } + return sum; + }; + }; + + sweep("vv", nv, [&](index_t i) { return candidates.vv_set(mesh, i); }); + sweep("ve", nv, [&](index_t i) { return candidates.ve_set(i); }); + sweep("vf", nv, [&](index_t i) { return candidates.vf_set(i); }); + sweep("ev", ne, [&](index_t i) { return candidates.ev_set(mesh, i); }); + sweep("ee", ne, [&](index_t i) { return candidates.ee_set(mesh, i); }); + sweep("ef", ne, [&](index_t i) { return candidates.ef_set(mesh, i); }); + sweep("fv", nf, [&](index_t i) { return candidates.fv_set(mesh, i); }); + sweep("fe", nf, [&](index_t i) { return candidates.fe_set(mesh, i); }); + sweep("ff", nf, [&](index_t i) { return candidates.ff_set(mesh, i); }); +} diff --git a/tests/src/tests/potential/test_arbitrary_point_esp.cpp b/tests/src/tests/potential/test_arbitrary_point_esp.cpp new file mode 100644 index 000000000..39ff3c8e2 --- /dev/null +++ b/tests/src/tests/potential/test_arbitrary_point_esp.cpp @@ -0,0 +1,580 @@ +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace ipc; + +namespace { + +struct Fixture { + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + CollisionMesh mesh; + double dhat; + + Fixture() + { + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + mesh = CollisionMesh(V, E, F); + const double bbox_diag = + (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm(); + dhat = 0.2 * bbox_diag; + } + + // A point just off the surface, offset from face 0's centroid along its + // (unnormalized-triangle) outward normal by a fraction of dhat. + Eigen::RowVector3d near_surface_point(double frac = 0.3) const + { + const Eigen::RowVector3d v0 = V.row(F(0, 0)); + const Eigen::RowVector3d v1 = V.row(F(0, 1)); + const Eigen::RowVector3d v2 = V.row(F(0, 2)); + const Eigen::RowVector3d centroid = (v0 + v1 + v2) / 3.0; + const Eigen::RowVector3d normal = (v1 - v0).cross(v2 - v0).normalized(); + return centroid + frac * dhat * normal; + } +}; + +// The 2D analogue: the unit square as a closed 4-edge polyline, so a query +// point can be placed against an edge interior or against a corner (where two +// edges both reduce to the same vertex and must cancel against the direct +// vertex term). +struct Fixture2D { + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + CollisionMesh mesh; + double dhat = 0.2; + + Fixture2D() + { + V.resize(4, 2); + V << 0, 0, 1, 0, 1, 1, 0, 1; + E.resize(4, 2); + E << 0, 1, 1, 2, 2, 3, 3, 0; + mesh = CollisionMesh(V, E, F); + } + + // Outside the bottom edge's midpoint (closest feature is an edge + // interior), at frac * dhat. + Eigen::RowVector2d near_edge_point(double frac) const + { + return Eigen::RowVector2d(0.5, -frac * dhat); + } + + // Outside the corner at vertex 0, along the diagonal (closest feature is + // a corner shared by two edges). + Eigen::RowVector2d near_corner_point(double frac) const + { + return Eigen::RowVector2d(0, 0) + - frac * dhat * Eigen::RowVector2d(1, 1).normalized(); + } +}; + +} // namespace + +TEST_CASE( + "Arbitrary Point ESP: zero beyond dhat", + "[esp_potential],[arbitrary_point_esp]") +{ + Fixture fx; + ESPParameters params(fx.dhat); + ArbitraryPointESP<3> potential(fx.mesh, params); + potential.update(fx.V); + + const Eigen::RowVector3d far_point = + fx.V.colwise().maxCoeff() + Eigen::RowVector3d::Constant(10 * fx.dhat); + + REQUIRE(potential(fx.V, far_point) == 0.0); + REQUIRE(potential.gradient(fx.V, far_point).isZero()); + REQUIRE(potential.hessian(fx.V, far_point).isZero()); +} + +TEST_CASE( + "Arbitrary Point ESP: FD gradient/hessian at an off-mesh point", + "[esp_potential],[arbitrary_point_esp]") +{ + Fixture fx; + ESPParameters params(fx.dhat); + ArbitraryPointESP<3> potential(fx.mesh, params); + potential.update(fx.V); + + const Eigen::RowVector3d q = fx.near_surface_point(); + + // Sanity: the point should actually be within range of the surface. + REQUIRE(potential(fx.V, q) != 0.0); + + SECTION("gradient") + { + const Eigen::Vector3d g = potential.gradient(fx.V, q); + + Eigen::VectorXd fg; + fd::finite_gradient( + Eigen::VectorXd(q.transpose()), + [&](const Eigen::VectorXd& y) { + return potential(fx.V, Eigen::RowVector3d(y.transpose())); + }, + fg, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE((fg - g).norm() < std::max(1e-8, fg.norm()) * 1e-5); + } + + SECTION("hessian") + { + const Eigen::Matrix3d h = potential.hessian(fx.V, q); + + Eigen::MatrixXd fh; + fd::finite_jacobian( + Eigen::VectorXd(q.transpose()), + [&](const Eigen::VectorXd& y) { + return potential.gradient( + fx.V, Eigen::RowVector3d(y.transpose())); + }, + fh, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE((fh - h).norm() < std::max(1e-8, fh.norm()) * 1e-5); + } +} + +TEST_CASE( + "Arbitrary Point ESP: evaluate() matches operator()/gradient()/hessian()", + "[esp_potential],[arbitrary_point_esp]") +{ + Fixture fx; + ESPParameters params(fx.dhat); + ArbitraryPointESP<3> potential(fx.mesh, params); + potential.update(fx.V); + + // Sweep from just outside dhat down through the surface to well inside + // the mesh, so both the "no collisions" and "several collisions merged + // via symbolic cancellation" code paths are exercised. + for (const double frac : { -0.3, 0.05, 0.3, 0.7, 0.95 }) { + CAPTURE(frac); + const Eigen::RowVector3d q = fx.near_surface_point(frac); + + const double value = potential(fx.V, q); + const Eigen::Vector3d grad = potential.gradient(fx.V, q); + const Eigen::Matrix3d hess = potential.hessian(fx.V, q); + + const auto [value2, grad2, hess2] = potential.evaluate(fx.V, q); + + // evaluate() runs the exact same per-collision computation and + // accumulation order as the three separate calls, just fused into + // one pass -- expect bit-exact agreement, not just Approx. + REQUIRE(value2 == value); + REQUIRE(grad2 == grad); + REQUIRE(hess2 == hess); + } + + // Beyond dhat: all three outputs zero, consistent with the separate + // accessors (see the "zero beyond dhat" test above). + const Eigen::RowVector3d far_point = + fx.V.colwise().maxCoeff() + Eigen::RowVector3d::Constant(10 * fx.dhat); + const auto [value, grad, hess] = potential.evaluate(fx.V, far_point); + REQUIRE(value == 0.0); + REQUIRE(grad.isZero()); + REQUIRE(hess.isZero()); +} + +namespace { + +// Evaluate the potential on V and on the same vertices stored as the top rows +// of a taller matrix, whose columns are further apart than V's row count. Both +// must give the same value, gradient and Hessian. +template +void check_strided_vertices_match( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const double dhat, + const Eigen::RowVector& q) +{ + Eigen::MatrixXd V_big = Eigen::MatrixXd::Constant(V.rows() + 7, dim, 1e3); + V_big.topRows(V.rows()) = V; + const auto V_strided = V_big.topRows(V.rows()); + + const ESPParameters params(dhat); + ArbitraryPointESP potential(mesh, params); + ArbitraryPointESP strided_potential(mesh, params); + potential.update(V); + strided_potential.update(V_strided); + + const auto [value, grad, hess] = potential.evaluate(V, q); + REQUIRE(value != 0.0); + CHECK(strided_potential(V_strided, q) == value); + CHECK(strided_potential.gradient(V_strided, q) == grad); + CHECK(strided_potential.hessian(V_strided, q) == hess); +} + +} // namespace + +TEST_CASE( + "Arbitrary Point ESP reads a strided vertex matrix like a contiguous one", + "[esp_potential],[arbitrary_point_esp]") +{ + SECTION("3D") + { + Fixture fx; + check_strided_vertices_match<3>( + fx.mesh, fx.V, fx.dhat, fx.near_surface_point(0.3)); + } + SECTION("2D") + { + Fixture2D fx; + for (const Eigen::RowVector2d& q : + { fx.near_edge_point(0.3), fx.near_corner_point(0.3) }) { + check_strided_vertices_match<2>(fx.mesh, fx.V, fx.dhat, q); + } + } +} + +TEST_CASE( + "Arbitrary Point ESP 2D: zero beyond dhat", + "[esp_potential],[arbitrary_point_esp]") +{ + Fixture2D fx; + ESPParameters params(fx.dhat); + ArbitraryPointESP<2> potential(fx.mesh, params); + potential.update(fx.V); + + const Eigen::RowVector2d far_point(0.5, -10 * fx.dhat); + + REQUIRE(potential(fx.V, far_point) == 0.0); + REQUIRE(potential.gradient(fx.V, far_point).isZero()); + REQUIRE(potential.hessian(fx.V, far_point).isZero()); +} + +TEST_CASE( + "Arbitrary Point ESP 2D: FD gradient/hessian at an off-mesh point", + "[esp_potential],[arbitrary_point_esp]") +{ + Fixture2D fx; + ESPParameters params(fx.dhat); + ArbitraryPointESP<2> potential(fx.mesh, params); + potential.update(fx.V); + + // Both an edge-interior closest feature and a corner, where the two + // incident edges reduce to the same vertex and their +1s must merge with + // the direct vertex term's -1. + const Eigen::RowVector2d q = GENERATE_COPY( + Eigen::RowVector2d(fx.near_edge_point(0.3)), + Eigen::RowVector2d(fx.near_corner_point(0.3))); + CAPTURE(q); + + REQUIRE(potential(fx.V, q) != 0.0); + + SECTION("gradient") + { + const Eigen::Vector2d g = potential.gradient(fx.V, q); + + Eigen::VectorXd fg; + fd::finite_gradient( + Eigen::VectorXd(q.transpose()), + [&](const Eigen::VectorXd& y) { + return potential(fx.V, Eigen::RowVector2d(y.transpose())); + }, + fg, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE((fg - g).norm() < std::max(1e-8, fg.norm()) * 1e-5); + } + + SECTION("hessian") + { + const Eigen::Matrix2d h = potential.hessian(fx.V, q); + + Eigen::MatrixXd fh; + fd::finite_jacobian( + Eigen::VectorXd(q.transpose()), + [&](const Eigen::VectorXd& y) { + return potential.gradient( + fx.V, Eigen::RowVector2d(y.transpose())); + }, + fh, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE((fh - h).norm() < std::max(1e-8, fh.norm()) * 1e-5); + } +} + +TEST_CASE( + "Arbitrary Point ESP 2D: corner value is a single vertex-vertex term", + "[esp_potential],[arbitrary_point_esp]") +{ + // Outside the convex corner at V.row(0), both incident edges reduce to + // that corner vertex (+1 each) and the direct vertex term contributes -1, + // so the symbolic cancellation must leave exactly one vertex-vertex + // barrier at the corner distance. This is the assertion that fails if the + // 2D sign convention or the endpoint reduction is wrong: getting it + // backwards leaves 3 terms or 0, both of which still pass a finite + // difference check. + Fixture2D fx; + ESPParameters params(fx.dhat); + ArbitraryPointESP<2> potential(fx.mesh, params); + potential.update(fx.V); + + for (const double frac : { 0.2, 0.5, 0.9 }) { + CAPTURE(frac); + const Eigen::RowVector2d q = fx.near_corner_point(frac); + const double d = (q - fx.V.row(0)).norm(); + REQUIRE( + potential(fx.V, q) == Catch::Approx((*params.barrier)(d, fx.dhat))); + } +} + +// The tests below check the inclusion-exclusion weights on inputs that are not +// closed manifolds: an open sheet and an open polyline (boundary edges, +// boundary vertices and polyline ends weigh zero, supplemental S4), a 3D +// polyline in no face (its edges weigh one, its interior vertices minus one), +// isolated vertices (one) and a 2D junction of three edges (minus two). On each +// input the query points lie where the potential is the barrier of the +// distance. With the closed-mesh signs instead, the potential is exactly zero +// beyond a boundary edge, and edges in no face and 2D isolated vertices enter +// as negative barriers. + +TEST_CASE( + "Arbitrary Point ESP: inclusion-exclusion weights on an open 3D sheet", + "[esp_potential],[arbitrary_point_esp]") +{ + const Eigen::RowVector3d q = GENERATE( + Eigen::RowVector3d(1.2, 0.5, 0.0), // beyond a boundary edge + Eigen::RowVector3d(1.1, 0.5, 0.2), // beyond it, above the plane + Eigen::RowVector3d(1.2, 1.1, 0.1), // beyond a boundary corner + Eigen::RowVector3d(0.9, 0.5, 0.1), // inside, near the boundary + Eigen::RowVector3d(1.0, 0.5, 0.2), // above a boundary vertex + Eigen::RowVector3d(0.5, 0.5, 0.2)); // above the interior vertex + CAPTURE(q); + + const double dhat = 0.5; + ESPParameters params(dhat); + + // The unit square at z = 0 as a 3x3 grid of vertices, 8 triangles. + Eigen::MatrixXd V(9, 3); + for (int i = 0; i < 9; i++) { + V.row(i) << (i % 3) / 2.0, (i / 3) / 2.0, 0; + } + Eigen::MatrixXi F(8, 3); + F << 0, 1, 4, 0, 4, 3, 1, 2, 5, 1, 5, 4, 3, 4, 7, 3, 7, 6, 4, 5, 8, 4, 8, 7; + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + ArbitraryPointESP<3> potential(mesh, params); + potential.update(V); + + const double dx = std::max({ 0.0, -q(0), q(0) - 1 }); + const double dy = std::max({ 0.0, -q(1), q(1) - 1 }); + const double d = std::sqrt(dx * dx + dy * dy + q(2) * q(2)); + CHECK(potential(V, q) == Catch::Approx((*params.barrier)(d, dhat))); + + Eigen::VectorXd fg; + fd::finite_gradient( + Eigen::VectorXd(q.transpose()), + [&](const Eigen::VectorXd& y) { + return potential(V, Eigen::RowVector3d(y.transpose())); + }, + fg, fd::AccuracyOrder::SECOND, 1e-8); + const Eigen::Vector3d g = potential.gradient(V, q); + CHECK((fg - g).norm() < std::max(1e-8, fg.norm()) * 1e-5); +} + +TEST_CASE( + "Arbitrary Point ESP: inclusion-exclusion weights on an open 2D polyline", + "[esp_potential],[arbitrary_point_esp]") +{ + const Eigen::RowVector2d q = GENERATE( + Eigen::RowVector2d(-0.2, 0.0), // beyond an end + Eigen::RowVector2d(-0.1, 0.1), // beyond it, off the line + Eigen::RowVector2d(0.1, 0.2), // beside the line, near an end + Eigen::RowVector2d(1 / 3.0, 0.1), // above an interior vertex + Eigen::RowVector2d(0.5, -0.15)); // beside an edge interior + CAPTURE(q); + + const double dhat = 0.5; + ESPParameters params(dhat); + + // The segment [0, 1] x {0} as three edges. + Eigen::MatrixXd V(4, 2); + V << 0, 0, 1 / 3.0, 0, 2 / 3.0, 0, 1, 0; + Eigen::MatrixXi E(3, 2); + E << 0, 1, 1, 2, 2, 3; + const CollisionMesh mesh(V, E, Eigen::MatrixXi()); + ArbitraryPointESP<2> potential(mesh, params); + potential.update(V); + + const double dx = std::max({ 0.0, -q(0), q(0) - 1 }); + const double d = std::sqrt(dx * dx + q(1) * q(1)); + CHECK(potential(V, q) == Catch::Approx((*params.barrier)(d, dhat))); +} + +TEST_CASE( + "Arbitrary Point ESP: inclusion-exclusion weights on a 3D polyline in no " + "face", + "[esp_potential],[arbitrary_point_esp]") +{ + const Eigen::RowVector3d q = GENERATE( + Eigen::RowVector3d(-0.2, 0.0, 0.0), // beyond an end + Eigen::RowVector3d(0.1, 0.1, 0.1), // beside it, near an end + Eigen::RowVector3d(1 / 3.0, 0.0, 0.2), // beside an interior vertex + Eigen::RowVector3d(0.5, -0.1, 0.1)); // beside an edge interior + CAPTURE(q); + + const double dhat = 0.5; + ESPParameters params(dhat); + + // The segment [0, 1] x {0} x {0} as three edges, no faces. + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, 1 / 3.0, 0, 0, 2 / 3.0, 0, 0, 1, 0, 0; + Eigen::MatrixXi E(3, 2); + E << 0, 1, 1, 2, 2, 3; + const CollisionMesh mesh(V, E, Eigen::MatrixXi()); + ArbitraryPointESP<3> potential(mesh, params); + potential.update(V); + + const double dx = std::max({ 0.0, -q(0), q(0) - 1 }); + const double d = std::sqrt(dx * dx + q(1) * q(1) + q(2) * q(2)); + CHECK(potential(V, q) == Catch::Approx((*params.barrier)(d, dhat))); +} + +TEST_CASE( + "Arbitrary Point ESP: inclusion-exclusion weights on isolated vertices", + "[esp_potential],[arbitrary_point_esp]") +{ + const double dhat = 0.5; + ESPParameters params(dhat); + + const Eigen::MatrixXd V3 = Eigen::MatrixXd::Zero(1, 3); + const CollisionMesh mesh3(V3, Eigen::MatrixXi(), Eigen::MatrixXi()); + ArbitraryPointESP<3> potential3(mesh3, params); + potential3.update(V3); + const Eigen::RowVector3d q3(0.1, -0.2, 0.15); + CHECK( + potential3(V3, q3) + == Catch::Approx((*params.barrier)(q3.norm(), dhat))); + + const Eigen::MatrixXd V2 = Eigen::MatrixXd::Zero(1, 2); + const CollisionMesh mesh2(V2, Eigen::MatrixXi(), Eigen::MatrixXi()); + ArbitraryPointESP<2> potential2(mesh2, params); + potential2.update(V2); + const Eigen::RowVector2d q2(0.1, -0.2); + CHECK( + potential2(V2, q2) + == Catch::Approx((*params.barrier)(q2.norm(), dhat))); +} + +TEST_CASE( + "Arbitrary Point ESP: inclusion-exclusion weights at a 2D junction", + "[esp_potential],[arbitrary_point_esp]") +{ + const Eigen::RowVector2d q = GENERATE( + Eigen::RowVector2d(0.0, -0.2), // below the junction + Eigen::RowVector2d(0.1, -0.1), // below, beside the junction + Eigen::RowVector2d(-0.3, -0.25)); // below the bar + CAPTURE(q); + + const double dhat = 0.5; + ESPParameters params(dhat); + + // A bar (-1, 0)-(0, 0)-(1, 0) and a stem (0, 0)-(0, 1). Below the bar the + // ball around a query point meets the input in one connected piece, so + // the potential is the barrier of the distance. + Eigen::MatrixXd V(4, 2); + V << -1, 0, 0, 0, 1, 0, 0, 1; + Eigen::MatrixXi E(3, 2); + E << 0, 1, 1, 2, 1, 3; + const CollisionMesh mesh(V, E, Eigen::MatrixXi()); + ArbitraryPointESP<2> potential(mesh, params); + potential.update(V); + + CHECK(potential(V, q) == Catch::Approx((*params.barrier)(-q(1), dhat))); +} + +TEST_CASE( + "Arbitrary Point ESP: mesh dimension mismatch", + "[esp_potential][arbitrary_point_esp]") +{ + const Fixture2D fx2; + CHECK_THROWS_AS( + ArbitraryPointESP<3>(fx2.mesh, ESPParameters(fx2.dhat)), + std::runtime_error); + const Fixture fx3; + CHECK_THROWS_AS( + ArbitraryPointESP<2>(fx3.mesh, ESPParameters(fx3.dhat)), + std::runtime_error); +} + +TEST_CASE( + "Arbitrary point BVH finds every edge of a deep, graded fan", + "[esp_potential][arbitrary_point_esp]") +{ + // A fan of edges from a hub at the origin, with their centers graded + // toward the hub one Morton bit at a time, so the edge tree is about as + // deep as a Morton-code build allows. Every edge's box contains the hub, + // so a tiny query box there overlaps the whole tree, and the traversal + // has to postpone a subtree at nearly every level on the way down. + constexpr double CELLS = 1 << 21; // Morton cells per axis in 3D + const auto cell = [&](const uint64_t c) { + return (static_cast(c) + 0.5) / CELLS; + }; + + std::vector points { Eigen::RowVector3d::Zero() }; + std::vector> fan; + const auto add_edge = [&](const Eigen::RowVector3d& endpoint) { + points.push_back(endpoint); + fan.push_back({ { 0, static_cast(points.size()) - 1 } }); + }; + // Each edge's center (half its endpoint) lies in Morton cell (x, y, z). + const auto add_edge_in_cell = [&](uint64_t x, uint64_t y, uint64_t z) { + add_edge(2 * Eigen::RowVector3d(cell(x), cell(y), cell(z))); + }; + for (int i = 0; i < 20; ++i) { + const uint64_t c = uint64_t(1) << i; + add_edge_in_cell(c, 0, 0); + add_edge_in_cell(c, 0, 1); + add_edge_in_cell(0, c, 0); + add_edge_in_cell(0, c, 1); + add_edge_in_cell(0, 0, i == 0 ? 1 : c); + add_edge_in_cell(0, 0, i == 0 ? 1 : c + 1); + } + // Short edges whose centers share the hub's cell, so ties in the Morton + // code add further levels. + for (int k = 0; k < 64; ++k) { + add_edge( + Eigen::RowVector3d(cell(0) * (1 + 0.01 * k), cell(0), cell(0))); + } + // An isolated vertex that fixes the Morton domain to the unit cube. + points.emplace_back(1, 1, 1); + + Eigen::MatrixXd V(points.size(), 3); + for (int i = 0; i < V.rows(); ++i) { + V.row(i) = points[i]; + } + Eigen::MatrixXi E(fan.size(), 2); + for (int i = 0; i < E.rows(); ++i) { + E.row(i) << fan[i][0], fan[i][1]; + } + const CollisionMesh mesh(V, E, Eigen::MatrixXi(0, 3)); + + LBVH bvh; + bvh.build(V, mesh.edges(), mesh.faces(), /*inflation_radius=*/0.0); + + std::vector vertex_ids, edge_ids, face_ids; + bvh.query_point( + Eigen::RowVector3d::Zero(), 1e-9, vertex_ids, edge_ids, face_ids); + + std::vector all_edges(E.rows()); + std::iota(all_edges.begin(), all_edges.end(), 0); + std::sort(edge_ids.begin(), edge_ids.end()); + CHECK(edge_ids == all_edges); + CHECK(face_ids.empty()); +} diff --git a/tests/src/tests/potential/test_barrier_potential.cpp b/tests/src/tests/potential/test_barrier_potential.cpp index 5a82f0403..b43be9316 100644 --- a/tests/src/tests/potential/test_barrier_potential.cpp +++ b/tests/src/tests/potential/test_barrier_potential.cpp @@ -533,6 +533,89 @@ TEST_CASE( CHECK(min_eig >= -1e-10 * std::max(scale, 1.0)); } +TEST_CASE( + "Barrier potential with unsquared distance", + "[potential][barrier_potential][gradient][hessian]") +{ + const double dhat = 0.1; + const double dmin = GENERATE(0.0, 0.01); + const double d = GENERATE(0.02, 0.05, 0.09); + const double stiffness = GENERATE(1.0, 3.5); + const bool use_physical_barrier = GENERATE(false, true); + + const auto barrier = std::make_shared>(2.0); + const BarrierPotential potential( + barrier, dhat, stiffness, use_physical_barrier, + /*use_squared_distance=*/false); + CHECK(!potential.use_squared_distance()); + + VertexVertexNormalCollision collision(0, 1); + collision.dmin = dmin; + + Vector6d x; + x.head<3>() << 0.1, -0.2, 0.3; + x.tail<3>() = + x.head<3>() + (d + dmin) * Eigen::Vector3d(1, 2, 3).normalized(); + + // The barrier sees the unsquared distance, offset by dmin, and is scaled + // by the stiffness (and the physical units) as in the squared mode. + const double units = use_physical_barrier ? dhat / barrier->units(dhat) : 1; + CHECK( + potential(collision, x) + == Catch::Approx(stiffness * units * (*barrier)(d, dhat))); + + const VectorMax12d grad = potential.gradient(collision, x); + Eigen::VectorXd fgrad; + fd::finite_gradient( + x, [&](const Eigen::VectorXd& y) { return potential(collision, y); }, + fgrad); + CHECK(fd::compare_gradient(grad, fgrad)); + + const MatrixMax12d hess = potential.hessian(collision, x); + Eigen::MatrixXd fhess; + fd::finite_jacobian( + x, + [&](const Eigen::VectorXd& y) -> Eigen::VectorXd { + return potential.gradient(collision, y); + }, + fhess); + CHECK(fd::compare_hessian(hess, fhess, 1e-3)); + + CHECK_THROWS(potential.force_magnitude(d * d, dmin)); + CHECK_THROWS( + potential.force_magnitude_gradient(d * d, VectorMax12d::Zero(6), dmin)); +} + +// At exact contact the energy is +inf, but the gradient and Hessian must stay +// finite in both distance modes so assembling them does not spread NaNs. +TEST_CASE( + "Barrier potential derivatives are finite at exact contact", + "[potential][barrier_potential][gradient][hessian]") +{ + const double dhat = 0.1; + const double dmin = GENERATE(0.0, 0.01); + const std::shared_ptr barrier = GENERATE( + as>(), std::make_shared>(), + std::make_shared>(2.0)); + const bool use_squared_distance = GENERATE(true, false); + CAPTURE(dmin, use_squared_distance); + + const BarrierPotential potential( + barrier, dhat, /*stiffness=*/1.0, /*use_physical_barrier=*/false, + use_squared_distance); + + VertexVertexNormalCollision collision(0, 1); + collision.dmin = dmin; + + Vector6d x; + x.head<3>() << 0.1, -0.2, 0.3; + x.tail<3>() = x.head<3>(); + + CHECK(potential(collision, x) == std::numeric_limits::infinity()); + CHECK(potential.gradient(collision, x).allFinite()); + CHECK(potential.hessian(collision, x).allFinite()); +} + // -- Benchmarking ------------------------------------------------------------ TEST_CASE( diff --git a/tests/src/tests/potential/test_esp_potential.cpp b/tests/src/tests/potential/test_esp_potential.cpp new file mode 100644 index 000000000..9f3f05798 --- /dev/null +++ b/tests/src/tests/potential/test_esp_potential.cpp @@ -0,0 +1,2491 @@ +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "igl/read_triangle_mesh.h" +#include "ipc/distance/edge_edge.hpp" + +#include "ipc/esp/quadrature_potential.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef _WIN32 +#include +#endif + +using namespace ipc; + +namespace { + +struct TriMeshData { + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + CollisionMesh mesh; +}; + +TriMeshData load_triangle_mesh(const std::string& path) +{ + TriMeshData data; + // Fail loudly if the mesh is missing (e.g., an outdated test-data + // checkout) instead of running the tests on an empty mesh. + REQUIRE(igl::read_triangle_mesh(path, data.V, data.F)); + igl::edges(data.F, data.E); + data.mesh = CollisionMesh(data.V, data.E, data.F); + return data; +} + +TriMeshData load_wrapped_sphere() +{ + return load_triangle_mesh( + (tests::DATA_DIR / "wrapped_sphere.obj").string()); +} + +/// Smallest and largest eigenvalues of a symmetric sparse matrix. Only the +/// rows and columns with a nonzero entry enter the dense solve: every other +/// one contributes an exact zero eigenvalue, so the result is unchanged and +/// the solve is much smaller when few DOFs are in contact. +std::pair eigenvalue_range(const Eigen::SparseMatrix& H) +{ + std::vector local(H.rows(), -1); + int n_active = 0; + for (int k = 0; k < H.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it(H, k); it; ++it) { + if (it.value() != 0) { + for (const auto i : { it.row(), it.col() }) { + if (local[i] < 0) { + local[i] = n_active++; + } + } + } + } + } + if (n_active == 0) { + return { 0.0, 0.0 }; + } + + Eigen::MatrixXd Hd = Eigen::MatrixXd::Zero(n_active, n_active); + for (int k = 0; k < H.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it(H, k); it; ++it) { + if (it.value() != 0) { + Hd(local[it.row()], local[it.col()]) += it.value(); + } + } + } + // Symmetrize numerically to remove tiny asymmetry from triplet ordering. + Hd = 0.5 * (Hd + Hd.transpose()).eval(); + + Eigen::SelfAdjointEigenSolver es( + Hd, Eigen::EigenvaluesOnly); + REQUIRE(es.info() == Eigen::Success); + double lambda_min = es.eigenvalues().minCoeff(); + double lambda_max = es.eigenvalues().maxCoeff(); + if (n_active < H.rows()) { // the inactive DOFs' zero eigenvalues + lambda_min = std::min(lambda_min, 0.0); + lambda_max = std::max(lambda_max, 0.0); + } + return { lambda_min, lambda_max }; +} + +/// Positive-weight face quadrature rules for the 3D face-quadrature tests: +/// "vertices+centroid" (the three vertices and the centroid) or +/// "edge-midpoints" (one point on each edge of the face). +FaceQuadRule make_face_quad_rule(const std::string& name) +{ + if (name == "vertices+centroid") { + return { + { { { 1., 0., 0. } }, 0.25 }, + { { { 0., 1., 0. } }, 0.25 }, + { { { 0., 0., 1. } }, 0.25 }, + { { { 1. / 3, 1. / 3, 1. / 3 } }, 0.25 }, + }; + } + REQUIRE(name == "edge-midpoints"); + return { + { { { 0., .5, .5 } }, 1. / 3 }, + { { { .5, 0., .5 } }, 1. / 3 }, + { { { .5, .5, 0. } }, 1. / 3 }, + }; +} + +CollisionMesh +make_2d_collision_mesh(const Eigen::MatrixXd& V, const Eigen::MatrixXi& E) +{ + Eigen::MatrixXi F; + return CollisionMesh( + std::vector(V.rows(), true), std::vector(V.rows(), false), + V, E, F); +} + +inline std::shared_ptr make_inverse_quadratic_barrier() +{ + return std::make_shared>(2.0); +} +inline std::shared_ptr make_linear_inverse_barrier() +{ + return std::make_shared>(1.0); +} + +struct EeLimitSweepStats { + double max_abs_P = 0; + double max_abs_g = 0; + double max_dP = 0; // max |P(eps_{i+1}) - P(eps_i)| + double max_dg = 0; // max ||g|(eps_{i+1}) - |g|(eps_i)| + double max_fd_slope_P = 0; // max |dP / d eps| + double max_fd_slope_g = 0; // max |d|g| / d eps| + double max_shift_P = 0; // max |P(eps) - P(eps_min)| + double max_shift_g = 0; // max ||g|(eps) - |g|(eps_min)| + bool all_finite = true; +}; + +// Build the EA_EB tet-tet geometry used by the three edge-edge limit tests, +// parametrised by the horizontal offset epsilon. +inline void build_ee_limit_geometry( + const double epsilon, + Eigen::MatrixXd& V, + Eigen::MatrixXi& E, + Eigen::MatrixXi& F) +{ + V.resize(8, 3); + V << 0, 0, 0, 1, 0, 0, 0.5, -0.5, 1, 0.5, 0.5, 1, epsilon, 0.5, -0.01, + epsilon, -0.5, -0.01, epsilon + 0.5, 0, -1.01, epsilon - 0.5, 0, -1.01; + F.resize(8, 3); + F << 0, 1, 2, 0, 1, 3, 0, 2, 3, 1, 2, 3, 4, 5, 6, 4, 6, 7, 4, 5, 7, 5, 6, 7; + E.resize(12, 2); + E << 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3, 4, 5, 4, 6, 4, 7, 5, 6, 5, 7, 6, 7; +} + +// Sweep the EA_EB limit over logspaced epsilons in (eps_min, eps_max], compute +// the potential and gradient norm at each sample, and return max absolute +// finite-difference between consecutive samples (both raw ΔP, Δ|g|, and the +// slope ΔP/Δeps, Δ|g|/Δeps). +inline EeLimitSweepStats ee_limit_fd_sweep( + std::shared_ptr barrier, + int n_samples = 25, + double eps_min = 1e-16, + double eps_max = 1e-5) +{ + EeLimitSweepStats stats; + std::vector eps_vec, P_vec, g_vec; + eps_vec.reserve(n_samples); + P_vec.reserve(n_samples); + g_vec.reserve(n_samples); + + const double log_lo = std::log10(eps_min); + const double log_hi = std::log10(eps_max); + + for (int i = 0; i < n_samples; ++i) { + const double t = double(i) / double(n_samples - 1); + const double eps = std::pow(10.0, log_lo + t * (log_hi - log_lo)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + build_ee_limit_geometry(eps, V, E, F); + + CollisionMesh mesh(V, E, F); + const double dhat = 0.1; + ESPParameters params(dhat, 1., 0); + params.barrier = barrier; + + ESPCollisions collisions; + collisions.build(mesh, V, params); + ESPPotential potential(params); + + const double x = potential(collisions, mesh, V); + const double gn = potential.gradient(collisions, mesh, V).norm(); + + if (!std::isfinite(x) || !std::isfinite(gn)) + stats.all_finite = false; + + stats.max_abs_P = std::max(stats.max_abs_P, std::abs(x)); + stats.max_abs_g = std::max(stats.max_abs_g, std::abs(gn)); + + eps_vec.push_back(eps); + P_vec.push_back(x); + g_vec.push_back(gn); + } + + for (size_t i = 1; i < eps_vec.size(); ++i) { + const double dP = std::abs(P_vec[i] - P_vec[i - 1]); + const double dg = std::abs(g_vec[i] - g_vec[i - 1]); + const double deps = std::abs(eps_vec[i] - eps_vec[i - 1]); + stats.max_dP = std::max(stats.max_dP, dP); + stats.max_dg = std::max(stats.max_dg, dg); + if (deps > 0) { + stats.max_fd_slope_P = std::max(stats.max_fd_slope_P, dP / deps); + stats.max_fd_slope_g = std::max(stats.max_fd_slope_g, dg / deps); + } + } + + // Shift relative to the sample at the smallest eps (first sample). + if (!eps_vec.empty()) { + const double P0 = P_vec.front(); + const double g0 = g_vec.front(); + for (size_t i = 0; i < eps_vec.size(); ++i) { + stats.max_shift_P = + std::max(stats.max_shift_P, std::abs(P_vec[i] - P0)); + stats.max_shift_g = + std::max(stats.max_shift_g, std::abs(g_vec[i] - g0)); + } + } + return stats; +} + +// Two closed tetrahedra, apex to apex along z and separated by a gap. Each apex +// has valence 3, so ESP has to resolve the vertex-vertex contact through the +// vertex-vertex and edge-vertex candidates. +inline void build_apex_to_apex_tets( + const double gap, + Eigen::MatrixXd& V, + Eigen::MatrixXi& E, + Eigen::MatrixXi& F) +{ + const double c = std::cos(2 * M_PI / 3), s = std::sin(2 * M_PI / 3); + const double r = std::cos(M_PI / 3), q = std::sin(M_PI / 3); + V.resize(8, 3); + V << 0, 0, gap / 2, 1, 0, 1, c, s, 1, c, -s, 1, // + 0, 0, -gap / 2, r, q, -1, -1, 0, -1, r, -q, -1; + F.resize(8, 3); + F << 0, 2, 1, 0, 3, 2, 0, 1, 3, 1, 2, 3, // + 4, 5, 6, 4, 6, 7, 4, 7, 5, 5, 7, 6; + igl::edges(F, E); +} + +} // anonymous namespace + +// When the edge-edge closest point approaches the end points of the edge, the +// potential should converge to a finite number +TEST_CASE( + "Convergent Quadrature Edge Edge Limit", + "[esp_potential], [esp_potential_3d]") +{ + auto stats = + ee_limit_fd_sweep(std::make_shared>()); + CHECK(stats.all_finite); + REQUIRE(stats.max_abs_P < 2); + REQUIRE(stats.max_abs_g < 200); + // 10x current measured shift from the smallest-eps sample + // (current: max|ΔP|≈2.4e-5, max|Δ|g||≈1.64). + CHECK(stats.max_shift_P < 2.4e-4); + CHECK(stats.max_shift_g < 16.5); +} + +// Same configuration as above, but uses an inverse-quadratic barrier to probe +// whether the ESP potential stays finite under a stronger barrier. +TEST_CASE( + "Convergent Quadrature Edge Edge Limit (Inverse Quadratic Barrier)", + "[esp_potential], [esp_potential_3d]") +{ + auto stats = ee_limit_fd_sweep(make_inverse_quadratic_barrier()); + CHECK(stats.all_finite); + CHECK(stats.max_abs_P < 1e8); + CHECK(stats.max_abs_g < 1e10); + // 10x current measured shift from the smallest-eps sample + // (current: max|ΔP|≈1.4e-3, max|Δ|g||≈1.55). + CHECK(stats.max_shift_P < 1.4e-2); + CHECK(stats.max_shift_g < 15.5); +} + +// Same configuration but with a linear-inverse barrier (1/d divergence). +TEST_CASE( + "Convergent Quadrature Edge Edge Limit (Linear Inverse Barrier)", + "[esp_potential], [esp_potential_3d]") +{ + auto stats = ee_limit_fd_sweep(make_linear_inverse_barrier()); + CHECK(stats.all_finite); + CHECK(stats.max_abs_P < 1e8); + CHECK(stats.max_abs_g < 1e10); + // 10x current measured shift from the smallest-eps sample + // (current: max|ΔP|≈1.85e-5, max|Δ|g||≈0.044). + CHECK(stats.max_shift_P < 1.85e-4); + CHECK(stats.max_shift_g < 0.44); +} + +TEST_CASE( + "Convergent Quadrature Gradient and Hessian", + "[esp_potential], [esp_potential_3d]") +{ + TriMeshData data = load_wrapped_sphere(); + Eigen::MatrixXd& V = data.V; + CollisionMesh& mesh = data.mesh; + + const double dbar_factor = GENERATE(0.3, 1.0, 2.0); + // Keep the edge-edge support min(dbar, dhat) at 0.15 so the active contact + // set is comparable across dbar_factor values. + const double dhat = 0.15 / std::min(dbar_factor, 1.0); + CAPTURE(dbar_factor); + ESPParameters params(dhat, dbar_factor, 0); + + const bool use_near_far = GENERATE(true, false); + CAPTURE(use_near_far, dbar_factor); + ESPPotential potential(params, use_near_far); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + + // full finite difference is too expensive, verify directional derivative + // only + Eigen::VectorXd test_dir(V.size(), 1); + for (int i = 0; i < test_dir.size(); i++) { + test_dir(i) = i; + } + test_dir.normalize(); + + SECTION("gradient") + { + Eigen::VectorXd g = potential.gradient(collisions, mesh, V); + + Eigen::VectorXd fg; + fd::finite_gradient( + Eigen::VectorXd::Zero(1), + [&](const Eigen::VectorXd& y) { + Eigen::MatrixXd V_ = V + tests::unflatten(test_dir, 3) * y(0); + ESPCollisions collisions_; + collisions_.build(mesh, V_, params); + return potential(collisions_, mesh, V_); + }, + fg, fd::AccuracyOrder::FOURTH, 1e-5); + + REQUIRE( + abs(fg(0) - g.dot(test_dir)) < std::max(fg.norm() * 1e-5, 1e-9)); + } + + SECTION("hessian") + { + Eigen::MatrixXd h = potential.hessian(collisions, mesh, V); + + Eigen::MatrixXd fh; + fd::finite_jacobian( + Eigen::VectorXd::Zero(1), + [&](const Eigen::VectorXd& y) { + Eigen::MatrixXd V_ = V + tests::unflatten(test_dir, 3) * y(0); + ESPCollisions collisions_; + collisions_.build(mesh, V_, params); + return potential.gradient(collisions_, mesh, V_); + }, + fh, fd::AccuracyOrder::FOURTH, 1e-5); + + REQUIRE( + (fh.col(0) - h * test_dir).norm() + < std::max(fh.norm() * 1e-6, 1e-9)); + } +} + +TEST_CASE( + "Convergent Quadrature Gradient and Hessian Expensive", + "[.][esp_potential][esp_potential_3d]") +{ + TriMeshData data = load_wrapped_sphere(); + Eigen::MatrixXd& V = data.V; + CollisionMesh& mesh = data.mesh; + + const double dhat = 0.1; + const double dbar_factor = GENERATE(1.0, 0.7, 0.4, 0.1); + ESPParameters params(dhat, dbar_factor, 0); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + + const bool normalize_weights = GENERATE(true, false); + ESPPotential potential(params, normalize_weights); + + SECTION("gradient") + { + Eigen::VectorXd g = potential.gradient(collisions, mesh, V); + + Eigen::VectorXd fg; + fd::finite_gradient( + tests::flatten(V), + [&](const Eigen::VectorXd& y) { + Eigen::MatrixXd V_ = tests::unflatten(y, 3); + ESPCollisions collisions_; + collisions_.build(mesh, V_, params); + return potential(collisions_, mesh, V_); + }, + fg, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE((fg - g).norm() < std::max(1e-8, fg.norm()) * 1e-6); + } + + SECTION("hessian") + { + Eigen::MatrixXd h = potential.hessian(collisions, mesh, V); + + Eigen::MatrixXd fh; + fd::finite_jacobian( + tests::flatten(V), + [&](const Eigen::VectorXd& y) { + Eigen::MatrixXd V_ = tests::unflatten(y, 3); + ESPCollisions collisions_; + collisions_.build(mesh, V_, params); + return potential.gradient(collisions_, mesh, V_); + }, + fh, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE((fh - h).norm() < std::max(1e-8, fh.norm()) * 1e-6); + } +} + +TEST_CASE( + "Convergent Quadrature Zero on Sphere", + "[esp_potential], [esp_potential_3d]") +{ + auto [V, E, F, mesh] = + load_triangle_mesh((tests::DATA_DIR / "sphere.obj").string()); + + const double dhat = 0.2; + const double dbar_factor = GENERATE(1.0, 0.9); + ESPParameters params(dhat, dbar_factor, 0); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + + ESPPotential potential(params); + double val = potential(collisions, mesh, V); + REQUIRE(val == 0); + + auto g = potential.gradient(collisions, mesh, V); + REQUIRE(g.norm() == 0); + + auto H = potential.hessian(collisions, mesh, V); + REQUIRE(H.norm() == 0); +} + +TEST_CASE( + "Convergent Quadrature Vertex Hessian", + "[esp_potential], [esp_potential_3d]") +{ + const auto method = make_default_broad_phase(); + TriMeshData data = load_wrapped_sphere(); + Eigen::MatrixXd& V = data.V; + CollisionMesh& mesh = data.mesh; + + const double dhat = 0.15; + const double dbar_factor = GENERATE(1.0, 0.7, 0.4, 0.1); + ESPParameters params(dhat, dbar_factor, 0); + + Candidates candidates; + candidates.build(mesh, V, dhat / 2, method.get(), true); + candidates.convert_candidates_to_sets(); + PointPotential point_potential(mesh, candidates, params); + + for (int vid = 0; vid < V.rows(); ++vid) { + size_t num_collision_pairs = 0; + const auto collisions = point_potential.build_collisions_at_vertex( + V, vid, num_collision_pairs); + + if (collisions->size() == 0) { + continue; + } + + std::vector indices; + { + Eigen::VectorXd local_grad = PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions( + V, *collisions, params); + indices = collisions->dofs(); + + if (local_grad.norm() < 1e-10) { + continue; + } + } + + Eigen::MatrixXd h = PointPotentialHelper:: + evaluate_potential_hessian_at_vertex_with_cached_collisions( + V, *collisions, params, PSDProjectionMethod::NONE); + + Eigen::MatrixXd fh; + fd::finite_jacobian( + Eigen::VectorXd(tests::flatten(V))(indices), + [&](const Eigen::VectorXd& y) { + Eigen::VectorXd y_ = tests::flatten(V); + y_(indices) = y; + Eigen::MatrixXd V_fd = tests::unflatten(y_, 3); + + return PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions( + V_fd, *collisions, params); + }, + fh, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE( + (h - fh).norm() < 1e-6 * std::max({ h.norm(), fh.norm(), 1e-8 })); + } +} + +// Test FV-3D mollification: two aligned cubes with vertices approaching face +// edges This configuration makes the mollification issue critical: vertices of +// one cube approach the faces of another cube, with closest points near +// triangle edges +TEST_CASE( + "ESP potential 3D finite differences (FV mollification)", + "[esp_potential], [esp_potential_3d]") +{ + const auto method = make_default_broad_phase(); + + // Load cube mesh + Eigen::MatrixXd V_single; + Eigen::MatrixXi E_single, F_single; + REQUIRE(tests::load_mesh("cube.ply", V_single, E_single, F_single)); + + // Create two cubes: one fixed, one translated slightly + Eigen::MatrixXd V(V_single.rows() * 2, 3); + V.topRows(V_single.rows()) = V_single; + // Second cube: translate along x-axis to create face-vertex collisions + // with aligned vertices approaching the faces of the first cube + V.bottomRows(V_single.rows()) = + V_single.rowwise() + Eigen::RowVector3d(1.001, 0, 0); + + Eigen::MatrixXi F(F_single.rows() * 2, 3); + F.topRows(F_single.rows()) = F_single; + F.bottomRows(F_single.rows()) = + F_single.array() + static_cast(V_single.rows()); + + Eigen::MatrixXi E; + igl::edges(F, E); + + CollisionMesh mesh(V, E, F); + + const double dhat = .5; + const double dbar_factor = GENERATE(1.0, 0.7, 0.4, 0.1); + ESPParameters params(dhat, dbar_factor, 0); + + Candidates candidates; + candidates.build(mesh, V, dhat / 2, method.get(), true); + candidates.convert_candidates_to_sets(); + + ESPCollisions collisions; + collisions.build(candidates, mesh, V, params); + + REQUIRE(!collisions.empty()); + REQUIRE(!has_intersections(mesh, V)); + + ESPPotential potential(params); + double energy = potential(collisions, mesh, V); + CAPTURE(energy); + CHECK(energy > 0); + CHECK(std::isfinite(energy)); + + // Test gradient accuracy + Eigen::VectorXd grad = potential.gradient(collisions, mesh, V); + Eigen::VectorXd fgrad; + fd::finite_gradient( + tests::flatten(V), + [&](const Eigen::VectorXd& x) { + return potential(collisions, mesh, tests::unflatten(x, V.cols())); + }, + fgrad, fd::AccuracyOrder::SECOND, 1e-8); + + CAPTURE(grad.norm()); + CAPTURE(fgrad.norm()); + const double error = (grad - fgrad).norm(); + const double threshold = + 1e-3 * std::max({ grad.norm(), fgrad.norm(), 1e-8 }); + CAPTURE(error); + CAPTURE(threshold); + // Without mollification: FD will mismatch analytical gradient near face + // edges With mollification: both should agree + CHECK(error < threshold); +} + +// 2D TESTS // + +TEST_CASE( + "ESP vertex quadrature ignores quad_order in 3D", + "[esp_potential], [esp_potential_3d]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("two-cubes-close.ply", V, E, F)); + const CollisionMesh mesh(V, E, F); + const double dhat = 0.1; + + // Without a face quadrature rule, 3D integrates over the face vertices; + // quad_order only sets the 2D edge quadrature. + const auto potential_value = [&](const int quad_order) { + const ESPParameters params(dhat, 1.0, quad_order); + ESPCollisions collisions; + collisions.build(mesh, V, params); + return ESPPotential(params)(collisions, mesh, V); + }; + const double expected = potential_value(0); + REQUIRE(expected > 0); + CHECK(potential_value(1) == Catch::Approx(expected).epsilon(1e-12)); + CHECK(potential_value(2) == Catch::Approx(expected).epsilon(1e-12)); +} + +TEST_CASE( + "ESP potential without edge-edge terms", + "[esp_potential], [esp_potential_3d]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("two-cubes-close.ply", V, E, F)); + const CollisionMesh mesh(V, E, F); + + // dbar_factor = 0 drops the edge-edge terms, so every face's total weight + // is that of its three vertices, 3, and the normalized potential is the + // unnormalized one divided by 3. + const ESPParameters params(0.15, 0.0); + ESPCollisions collisions; + collisions.build(mesh, V, params); + + const ESPPotential normalized(params, /*use_near_far=*/true); + const ESPPotential unnormalized(params, /*use_near_far=*/false); + + const double energy = unnormalized(collisions, mesh, V); + REQUIRE(energy > 0); + CHECK( + normalized(collisions, mesh, V) + == Catch::Approx(energy / 3).epsilon(1e-12)); + + const Eigen::VectorXd grad = unnormalized.gradient(collisions, mesh, V); + CHECK( + (normalized.gradient(collisions, mesh, V) - grad / 3).norm() + <= 1e-12 * grad.norm()); + + const Eigen::SparseMatrix hess = + unnormalized.hessian(collisions, mesh, V); + CHECK( + (normalized.hessian(collisions, mesh, V) - hess / 3).norm() + <= 1e-12 * hess.norm()); +} + +TEST_CASE( + "ESP potential on a 3D mesh without faces", + "[esp_potential], [esp_potential_3d]") +{ + // Two crossing segments 0.05 apart: an edge-edge pair but no faces. The + // segments are rods, so they must repel each other even though no face + // carries the integral. + Eigen::MatrixXd V(4, 3); + V << -1, 0, 0, 1, 0, 0, 0, -1, 0.05, 0, 1, 0.05; + Eigen::MatrixXi E(2, 2); + E << 0, 1, 2, 3; + const CollisionMesh mesh(V, E, Eigen::MatrixXi(0, 3)); + + const ESPParameters params(0.1, 1.0); + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + + const bool use_near_far = GENERATE(true, false); + CAPTURE(use_near_far); + const ESPPotential potential(params, use_near_far); + CHECK(potential(collisions, mesh, V) > 0); + + // Pulling the rods apart lowers the energy, so the force pushes them + // apart. + const Eigen::VectorXd grad = potential.gradient(collisions, mesh, V); + REQUIRE(grad.size() == V.size()); + const Eigen::MatrixXd grad_V = tests::unflatten(grad, 3); + CHECK(grad_V(0, 2) + grad_V(1, 2) > 0); + CHECK(grad_V(2, 2) + grad_V(3, 2) < 0); + + // The collision set changes with the positions here, so the finite + // differences rebuild it. + const auto energy = [&](const Eigen::VectorXd& x) { + const Eigen::MatrixXd V_ = tests::unflatten(x, 3); + ESPCollisions collisions_; + collisions_.build(mesh, V_, params); + return potential(collisions_, mesh, V_); + }; + const auto gradient = [&](const Eigen::VectorXd& x) { + const Eigen::MatrixXd V_ = tests::unflatten(x, 3); + ESPCollisions collisions_; + collisions_.build(mesh, V_, params); + return potential.gradient(collisions_, mesh, V_); + }; + + Eigen::VectorXd fgrad; + fd::finite_gradient(tests::flatten(V), energy, fgrad); + CHECK(fd::compare_gradient(grad, fgrad)); + + const Eigen::MatrixXd hess = potential.hessian(collisions, mesh, V); + REQUIRE(hess.rows() == V.size()); + REQUIRE(hess.cols() == V.size()); + Eigen::MatrixXd fhess; + fd::finite_jacobian(tests::flatten(V), gradient, fhess); + CHECK(fd::compare_hessian(hess, fhess, 1e-3)); +} + +// A vertex beside the boundary edge of an open sheet sees the sheet's face +// reduced to that edge, and nothing else: a boundary edge has weight zero. +// With the closed-mesh signs, the edge's -1 cancels the face's +1 and the +// vertex feels nothing at any distance. +TEST_CASE( + "ESP potential beside an open sheet's boundary", + "[esp_potential], [esp_potential_3d]") +{ + // A unit-square sheet in z = 0 and a tet whose apex approaches the + // sheet's boundary edge x = 0 from outside, in the sheet's plane. + Eigen::MatrixXd V(8, 3); + V << 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, // sheet + 0, 0.5, 0, -1, 0.2, -0.3, -1, 0.8, -0.3, -1, 0.5, 0.4; // tet + Eigen::MatrixXi F(6, 3); + F << 0, 1, 2, 0, 2, 3, 4, 5, 6, 4, 6, 7, 4, 7, 5, 5, 7, 6; + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + const ESPParameters params(0.1); + const ESPPotential potential(params); + const auto energy_at = [&](const double h) { + Eigen::MatrixXd V_ = V; + V_(4, 0) = -h; + ESPCollisions collisions; + collisions.build(mesh, V_, params); + return potential(collisions, mesh, V_); + }; + + const double h = GENERATE(0.05, 0.01, 0.001); + CAPTURE(h); + const double e = energy_at(h); + CHECK(e > 0); + CHECK(energy_at(h / 2) > e); // repulsive: rises as the apex approaches +} + +// A vertex resting exactly on another face's interior is an exact contact, so +// the energy must be +inf (which a line search rejects), never NaN (which +// slips past an E_new > E_old check). +TEST_CASE( + "ESP potential is infinite at an exact contact", + "[esp_potential], [esp_potential_3d]") +{ + // A unit-square sheet in z = 0 and a tet whose apex touches the interior + // of the sheet's first face. + Eigen::MatrixXd V(8, 3); + V << 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, // sheet + 0.6, 0.3, 0, 0.3, 0, 1, 0.9, 0, 1, 0.6, 0.6, 1; // tet + Eigen::MatrixXi F(6, 3); + F << 0, 1, 2, 0, 2, 3, 4, 5, 6, 4, 6, 7, 4, 7, 5, 5, 7, 6; + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + const ESPParameters params(0.1); + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + + const bool use_near_far = GENERATE(true, false); + CAPTURE(use_near_far); + const double e = ESPPotential(params, use_near_far)(collisions, mesh, V); + CHECK(std::isinf(e)); + CHECK(e > 0); +} + +// An isolated vertex is its own integration element with a single sample, so +// with unit weights its ESP is exactly the pointwise potential of the rest of +// the mesh at its position. We cross-check that against ArbitraryPointESP on a +// mesh of the rest alone. +TEST_CASE( + "ESP potential of an isolated vertex matches ArbitraryPointESP", + "[esp_potential], [esp_potential_3d]") +{ + const std::string target_name = + GENERATE("triangle interior", "open sheet boundary", "rod"); + const std::string quad_rule = GENERATE("none", "vertices+centroid"); + const bool use_near_far = GENERATE(true, false); + CAPTURE(target_name, quad_rule, use_near_far); + + // Each target keeps its vertices and, under a face quadrature rule, its + // face quadrature points more than dhat from q, so only q's own sample + // sees contact. + Eigen::MatrixXd V_target; + Eigen::MatrixXi E_target, F_target(0, 3); + Eigen::RowVector3d q; + if (target_name == "triangle interior") { + V_target.resize(3, 3); + V_target << 0, 0, 0, 1, 0, 0, 0, 1, 0; + F_target.resize(1, 3); + F_target << 0, 1, 2; + q << 0.15, 0.6, 0.05; + } else if (target_name == "open sheet boundary") { + V_target.resize(4, 3); + V_target << 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0; + F_target.resize(2, 3); + F_target << 0, 1, 2, 0, 2, 3; + q << -0.05, 0.5, 0; + } else { // a bent rod: two edges in no face + V_target.resize(3, 3); + V_target << 0, 0, 0, 1, 0, 0, 2, 0.2, 0; + E_target.resize(2, 2); + E_target << 0, 1, 1, 2; + q << 0.5, 0.05, 0; + } + if (F_target.rows() > 0) { + igl::edges(F_target, E_target); + } + + ESPParameters params(0.1, 0.2, 1, /*area_weights=*/false); + if (quad_rule != "none") { + params.set_quad_rule(make_face_quad_rule(quad_rule)); + } + + // The full mesh: the target plus the isolated vertex q. + const index_t qi = V_target.rows(); + Eigen::MatrixXd V(qi + 1, 3); + V << V_target, q; + const CollisionMesh mesh(V, E_target, F_target); + REQUIRE(mesh.is_codim_vertex(qi)); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + const ESPPotential potential(params, use_near_far); + + const CollisionMesh target(V_target, E_target, F_target); + ArbitraryPointESP<3> pointwise(target, params); + pointwise.update(V_target); + const auto [expected, expected_grad, expected_hess] = + pointwise.evaluate(V_target, q); + REQUIRE(expected > 0); + + CHECK( + potential(collisions, mesh, V) + == Catch::Approx(expected).epsilon(1e-12)); + + const Eigen::VectorXd grad = potential.gradient(collisions, mesh, V); + CHECK( + (tests::unflatten(grad, 3).row(qi).transpose() - expected_grad).norm() + <= 1e-10 * expected_grad.norm()); + + const Eigen::MatrixXd hess = potential.hessian(collisions, mesh, V); + const std::array q_dofs = tests::vertex_dofs<3>(qi, V.rows()); + CHECK( + (hess(q_dofs, q_dofs) - expected_hess).norm() + <= 1e-10 * expected_hess.norm()); +} + +TEST_CASE("ESP collisions accessors", "[esp_potential], [esp_potential_3d]") +{ + SECTION("edge-edge") + { + // Edges (0,1) and (4,5) cross 0.01 apart. + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + build_ee_limit_geometry(0.25, V, E, F); + const CollisionMesh mesh(V, E, F); + const ESPParameters params(0.1, 1.0, 0); + + ESPCollisions collisions; + CHECK(collisions.empty()); + CHECK(collisions.size() == 0); + CHECK( + collisions.compute_minimum_distance(mesh, V) + == std::numeric_limits::infinity()); + CHECK(collisions.to_string(mesh, V, params).empty()); + + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + CHECK(collisions.size() > 0); + CHECK(collisions.n_candidates() > 0); + // compute_minimum_distance returns the squared distance. + CHECK( + collisions.compute_minimum_distance(mesh, V) + == Catch::Approx(1e-4)); + CHECK( + collisions.to_string(mesh, V, params).find("edge [") + != std::string::npos); + + collisions.clear(); + CHECK(collisions.empty()); + CHECK(collisions.size() == 0); + } + + SECTION("vertex and face") + { + // Two unit cubes 0.001 apart with aligned vertices. + Eigen::MatrixXd V_cube; + Eigen::MatrixXi E_cube, F_cube; + REQUIRE(tests::load_mesh("cube.ply", V_cube, E_cube, F_cube)); + const int n = V_cube.rows(); + Eigen::MatrixXd V(2 * n, 3); + V << V_cube, V_cube.rowwise() + Eigen::RowVector3d(1.001, 0, 0); + Eigen::MatrixXi F(2 * F_cube.rows(), 3); + F << F_cube, (F_cube.array() + n).matrix(); + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + ESPParameters params(0.5, 1.0, 0); + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + CHECK( + collisions.to_string(mesh, V, params).find("vert [") + != std::string::npos); + CHECK( + collisions.compute_minimum_distance(mesh, V) + == Catch::Approx(1e-6)); + + params.set_quad_rule(make_face_quad_rule("vertices+centroid")); + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + CHECK( + collisions.to_string(mesh, V, params).find("face [") + != std::string::npos); + } +} + +TEST_CASE( + "ESP collisions skip adjacent edge-edge candidates", + "[esp_potential], [esp_potential_3d]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + build_ee_limit_geometry(0.25, V, E, F); + const CollisionMesh mesh(V, E, F); + const ESPParameters params(0.1, 1.0, 0); + + // Edges 0 and 1 share vertex 0; broad phases never emit such a pair. + REQUIRE(E(0, 0) == E(1, 0)); + Candidates candidates; + candidates.ee_candidates.emplace_back(0, 1); + candidates.has_all_types = true; // We fill every type we need by hand. + ESPCollisions collisions; + collisions.build(candidates, mesh, V, params); + CHECK(collisions.empty()); + + // Edges (0,1) and (4,5) cross, so their pair is kept. + REQUIRE(E(6, 0) == 4); + REQUIRE(E(6, 1) == 5); + candidates.ee_candidates = { EdgeEdgeCandidate(0, 6) }; + collisions.build(candidates, mesh, V, params); + CHECK(!collisions.empty()); +} + +TEST_CASE( + "ESP contact is unaffected by distant codim. primitives", + "[esp_potential], [esp_potential_3d]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + build_apex_to_apex_tets(0.05, V, E, F); + const ESPParameters params(0.1); + const ESPPotential potential(params); + + const CollisionMesh clean_mesh(V, E, F); + ESPCollisions collisions; + collisions.build(clean_mesh, V, params); + const double expected_energy = potential(collisions, clean_mesh, V); + const Eigen::VectorXd expected_grad = + potential.gradient(collisions, clean_mesh, V); + REQUIRE(expected_energy > 0); + + // Three stray vertices far from the tets, optionally with two of them + // joined into a dangling edge. + const bool add_codim_edge = GENERATE(false, true); + CAPTURE(add_codim_edge); + + Eigen::MatrixXd V_codim(V.rows() + 3, 3); + V_codim << V, 50, 50, 50, -50, -50, -50, -49, -50, -50; + Eigen::MatrixXi E_codim = E; + if (add_codim_edge) { + E_codim.conservativeResize(E.rows() + 1, Eigen::NoChange); + E_codim.bottomRows<1>() << 9, 10; + } + const CollisionMesh mesh(V_codim, E_codim, F); + REQUIRE(mesh.num_codim_vertices() > 0); + REQUIRE(mesh.num_codim_edges() == (add_codim_edge ? 1 : 0)); + + collisions.build(mesh, V_codim, params); + CHECK( + potential(collisions, mesh, V_codim) + == Catch::Approx(expected_energy).epsilon(1e-12)); + + // We compare per-vertex rows so the check holds in either DOF layout. + const Eigen::MatrixXd grad = + tests::unflatten(potential.gradient(collisions, mesh, V_codim), 3); + CHECK( + (grad.topRows(V.rows()) - tests::unflatten(expected_grad, 3)).norm() + <= 1e-12 * expected_grad.norm()); + CHECK(grad.bottomRows(3).isZero()); +} + +TEST_CASE( + "ESP collisions reject 3D candidates without all types", + "[esp_potential], [esp_potential_3d]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + build_apex_to_apex_tets(0.05, V, E, F); + const CollisionMesh mesh(V, E, F); + const ESPParameters params(0.1); + const double inflation_radius = params.dhat / 2; + + ESPCollisions collisions; + + SECTION("IPC candidates") + { + Candidates candidates; + candidates.build(mesh, V, inflation_radius); + CHECK_THROWS_AS( + collisions.build(candidates, mesh, V, params), + std::invalid_argument); + } + + SECTION("Trajectory candidates") + { + Candidates candidates; + candidates.build(mesh, V, V, inflation_radius); + CHECK_THROWS_AS( + collisions.build(candidates, mesh, V, params), + std::invalid_argument); + } + + SECTION("All-types candidates") + { + Candidates candidates; + candidates.build(mesh, V, inflation_radius, nullptr, true); + collisions.build(candidates, mesh, V, params); + + ESPCollisions expected; + expected.build(mesh, V, params); + const ESPPotential potential(params); + CHECK( + potential(collisions, mesh, V) + == Catch::Approx(potential(expected, mesh, V)).epsilon(1e-12)); + } +} + +TEST_CASE( + "ESP collisions describe and count their 2D edge quadrature contacts", + "[esp_potential][esp_potential_2d]") +{ + // Two unit squares 0.2 apart, well within dhat of each other. + Eigen::MatrixXd V(8, 2); + V << -1., 1., -1., 0., -.1, 0., -.1, 1., .1, 1., .1, 0., 1., 0., 1., 1.; + Eigen::MatrixXi E(8, 2), F; + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + const CollisionMesh mesh(V, E, F); + const ESPParameters params(0.6, 1., 2); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + + const std::string description = collisions.to_string(mesh, V, params); + // One line per collision pair, each starting on a new line. + CHECK( + static_cast( + std::count(description.begin(), description.end(), '\n')) + == collisions.size()); + CHECK(description.find("edge [") != std::string::npos); + + // Every stored pair was counted when it was built. + CHECK(collisions.num_quadrature_collision_pairs >= collisions.size()); +} + +TEST_CASE("ESP collision dict index out of range", "[esp_potential]") +{ + ESPCollisionDict dict; + CHECK(dict.size() == 0); + CHECK_THROWS_AS(dict[0], std::runtime_error); + const auto& const_dict = dict; + CHECK_THROWS_AS(const_dict[0], std::runtime_error); +} + +TEST_CASE("ESP collision stencils", "[esp_potential]") +{ + constexpr double NONE = std::numeric_limits::max(); + + SECTION("3D") + { + Eigen::MatrixXd V(4, 3); + V << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.2, 0.2, 0.5; + Eigen::MatrixXi F(1, 3); + F << 0, 1, 2; + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + const ESPCollisionTemplate vv(3, 0, mesh); + const ESPCollisionTemplate ev(0, 3, mesh); + const ESPCollisionTemplate fv(0, 3, mesh); + CHECK(vv.name() == "vv_3d"); + CHECK(ev.name() == "ev_3d"); + CHECK(fv.name() == "fv_3d"); + CHECK(ev.n_vertices_a() == 2); + + CHECK( + vv.compute_distance(V) + == Catch::Approx((V.row(3) - V.row(0)).squaredNorm())); + CHECK( + ev.compute_distance(V) + == Catch::Approx(point_edge_distance( + V.row(3).transpose(), V.row(E(0, 0)).transpose(), + V.row(E(0, 1)).transpose()))); + CHECK(fv.compute_distance(V) == Catch::Approx(0.25)); + // Without vertex 3 there is no distance to report. + for (const ESPCollision* c : + std::initializer_list { &vv, &ev, &fv }) { + CHECK(c->compute_distance(V.topRows(3)) == NONE); + } + + CHECK_THROWS_AS(vv[2], std::runtime_error); + CHECK_THROWS_AS( + vv.dof(Eigen::MatrixXd::Zero(4, 4)), std::runtime_error); + const Eigen::MatrixXd Z22 = Eigen::MatrixXd::Zero(2, 2); + const Eigen::MatrixXd Z23 = Eigen::MatrixXd::Zero(2, 3); + CHECK_THROWS_AS(VertexMatrixView<3>(Z22), std::runtime_error); + CHECK_THROWS_AS( + VertexMatrixView<3>(Z23, Z22.topRows(1)), std::runtime_error); + } + + SECTION("2D") + { + Eigen::MatrixXd V(3, 2); + V << 0, 0, 1, 0, 0.5, 0.3; + Eigen::MatrixXi E(1, 2); + E << 0, 1; + const CollisionMesh mesh(V, E, Eigen::MatrixXi()); + + const ESPCollisionTemplate vv(2, 0, mesh); + const ESPCollisionTemplate ve(2, 0, mesh); + CHECK(vv.name() == "vv_2d_pt"); + CHECK(ve.name() == "ev_2d_pt"); + CHECK(vv.compute_distance(V) == Catch::Approx(0.34)); + CHECK(ve.compute_distance(V) == Catch::Approx(0.09)); + CHECK(vv.compute_distance(V.topRows(2)) == NONE); + CHECK(ve.compute_distance(V.topRows(2)) == NONE); + + Eigen::VectorXd expected(6); + expected << 0.5, 0.3, 0, 0, 1, 0; + CHECK(ve.dof(V) == expected); + } +} + +TEST_CASE("ESP edge-edge distance helpers", "[esp_potential]") +{ + const Eigen::Vector3d ea0(0, 0, 0), ea1(1, 0, 0); + + // Parallel edges 0.1 apart, where the line-line distance divides by zero. + const Eigen::Vector3d eb0(0.25, 0.1, 0), eb1(0.75, 0.1, 0); + CHECK( + edge_edge_distance_parallel_safe( + ea0, ea1, eb0, eb1, EdgeEdgeDistanceType::EA_EB) + == Catch::Approx(0.01)); + + // Nearly parallel edges that the exact classifier calls EA_EB. + const Eigen::Vector3d fb0(0.25, -1e-12, 0.1), fb1(0.75, 1e-12, 0.1); + const EdgeEdgeDistanceType dtype = + edge_edge_distance_type_exact(ea0, ea1, fb0, fb1); + CHECK(dtype == EdgeEdgeDistanceType::EA_EB); + CHECK( + edge_edge_distance_parallel_safe(ea0, ea1, fb0, fb1, dtype) + == Catch::Approx(0.01)); + + // The closest point on edge A is clamped to the edge. + const Eigen::Vector3d gb0(2, -1, 1), gb1(2, 1, 1); + const Eigen::Vector3d hb0(-1, -1, 1), hb1(-1, 1, 1); + CHECK(closest_point_uv(ea0, ea1, gb0, gb1) == 1.0); + CHECK(closest_point_uv(ea0, ea1, hb0, hb1) == 0.0); +} + +// Edges just above the parallel threshold (sin²θ ≈ 2.5e-16) are still EA_EB +// pairs, so the builder places a virtual vertex at their closest point. These +// two pairs once put it 2.2 edge lengths off the edge (with FMA) or made +// ESPCollisions::build throw (without FMA). The exact closest-point +// parameters come from rational arithmetic on the same doubles. +TEST_CASE( + "ESP closest point of nearly parallel edges stays on both edges", + "[esp_potential], [esp_potential_3d]") +{ + struct NearParallelPair { + Eigen::Vector3d ea0, ea1, eb0, eb1; + double s, t; // exact closest-point parameters on edges a and b + }; + const NearParallelPair p = GENERATE( + values({ + { Eigen::Vector3d( + -0.2270162012615336, -0.037097538696173923, + 0.24897999838085089), + Eigen::Vector3d( + -0.95435862566408902, -0.54571710280362784, + 1.0400682050760843), + Eigen::Vector3d( + -0.35558625789861842, -0.12718485650007386, + 0.38892518786460573), + Eigen::Vector3d( + -0.94859663947219408, -0.54186805484382239, + 1.033908195533616), + 0.42851486912107406, 0.3086223761078891 }, + { Eigen::Vector3d( + -0.97676019036967865, 0.37790242165616783, + -3.4459492857995926), + Eigen::Vector3d( + -1.3277219282265249, 0.58419418412471269, + -2.4629010958870214), + Eigen::Vector3d( + -1.0428448425578856, 0.41555483262155285, + -3.2623999701070976), + Eigen::Vector3d( + -1.3887808876763006, 0.61889254451729769, + -2.2934288586893583), + 0.37398578721715675, 0.1899773688708917 }, + })); + + REQUIRE(!is_parallel_edge_edge(p.ea0, p.ea1, p.eb0, p.eb1)); + REQUIRE( + edge_edge_distance_type_exact(p.ea0, p.ea1, p.eb0, p.eb1) + == EdgeEdgeDistanceType::EA_EB); + + const Eigen::Vector2d st = + line_line_closest_point_pairs_uv(p.ea0, p.ea1, p.eb0, p.eb1); + CHECK(st(0) == Catch::Approx(p.s).margin(1e-3)); + CHECK(st(1) == Catch::Approx(p.t).margin(1e-3)); + CHECK( + closest_point_uv(p.ea0, p.ea1, p.eb0, p.eb1) + == Catch::Approx(p.s).margin(1e-3)); + + // Both pairs are within the edge-edge support, so the builder places a + // virtual vertex at their closest point. + Eigen::MatrixXd V(4, 3); + V << p.ea0.transpose(), p.ea1.transpose(), p.eb0.transpose(), + p.eb1.transpose(); + Eigen::MatrixXi E(2, 2); + E << 0, 1, 2, 3; + const CollisionMesh mesh(V, E, Eigen::MatrixXi()); + + const ESPParameters params(0.01); + ESPCollisions collisions; + REQUIRE_NOTHROW(collisions.build(mesh, V, params)); + CHECK(!collisions.empty()); + + const ESPPotential potential(params); + const double energy = potential(collisions, mesh, V); + CHECK(std::isfinite(energy)); + CHECK(energy > 0); + CHECK(potential.gradient(collisions, mesh, V).allFinite()); +} + +TEST_CASE( + "ESP closest point of random nearly parallel edges is accurate", + "[esp_potential], [esp_potential_3d]") +{ + const double scale = GENERATE(0.01, 1.0, 100.0); + CAPTURE(scale); + + // We build each pair around a known closest pair: P on edge a at s and + // Q on edge b at t, separated along the common normal by less than the + // edge-edge support. + std::mt19937_64 rng(12345); + std::uniform_real_distribution uniform(0, 1); + std::normal_distribution gaussian(0, 1); + const auto random_vector = [&] { + return Eigen::Vector3d(gaussian(rng), gaussian(rng), gaussian(rng)); + }; + const double ee_support = ESPParameters(0.01 * scale).ee_support(); + + int num_tested = 0; + for (int i = 0; i < 2000; i++) { + const Eigen::Vector3d dir_a = random_vector().normalized(); + Eigen::Vector3d perp = random_vector(); + perp = (perp - perp.dot(dir_a) * dir_a).normalized(); + const double sin2 = 2.5e-16 + 5.5e-16 * uniform(rng); + const double theta = std::asin(std::sqrt(sin2)); + const Eigen::Vector3d dir_b = + std::cos(theta) * dir_a + std::sin(theta) * perp; + + const Eigen::Vector3d u = scale * (0.5 + uniform(rng)) * dir_a; + const Eigen::Vector3d v = scale * (0.5 + uniform(rng)) * dir_b; + const double s = 0.1 + 0.8 * uniform(rng); + const double t = 0.1 + 0.8 * uniform(rng); + const double gap = ee_support * (0.05 + 0.9 * uniform(rng)); + + const Eigen::Vector3d ea0 = scale * random_vector(); + const Eigen::Vector3d ea1 = ea0 + u; + const Eigen::Vector3d eb0 = + ea0 + s * u + gap * dir_a.cross(dir_b).normalized() - t * v; + const Eigen::Vector3d eb1 = eb0 + v; + + // Only pairs the builder keeps reach the closest-point solve. + if (is_parallel_edge_edge(ea0, ea1, eb0, eb1) + || edge_edge_distance_type_exact(ea0, ea1, eb0, eb1) + != EdgeEdgeDistanceType::EA_EB) { + continue; + } + num_tested++; + + CAPTURE(i, sin2, s, t); + const Eigen::Vector2d st = + line_line_closest_point_pairs_uv(ea0, ea1, eb0, eb1); + REQUIRE(st.allFinite()); + CHECK(st(0) == Catch::Approx(s).margin(0.05)); + CHECK(st(1) == Catch::Approx(t).margin(0.05)); + } + CHECK(num_tested > 1000); +} + +TEST_CASE("ESP parameters dbar_factor range", "[esp_potential]") +{ + CHECK_NOTHROW(ESPParameters(0.1, 0.0)); + CHECK_NOTHROW(ESPParameters(0.1, 0.5)); + CHECK_NOTHROW(ESPParameters(0.1, 1.0)); + CHECK_NOTHROW(ESPParameters(0.1, 2.0)); + CHECK_THROWS_AS(ESPParameters(0.1, -0.1), std::invalid_argument); + CHECK_THROWS_AS(ESPParameters(0.1, 2.5), std::invalid_argument); + CHECK_THROWS_AS(ESPParameters(0.1, std::nan("")), std::invalid_argument); +} + +TEST_CASE("ESP parameters reject an invalid dhat", "[esp_potential]") +{ + // Zero dhat would silently disable contact, and a negative or non-finite + // one would give the broad phase a negative or NaN inflation radius. + const double dhat = GENERATE( + 0.0, -0.1, std::nan(""), std::numeric_limits::infinity()); + CAPTURE(dhat); + CHECK_THROWS_AS(ESPParameters(dhat), std::invalid_argument); +} + +TEST_CASE("ESP parameters reject a negative quad_order", "[esp_potential]") +{ + // 3D ignores quad_order, so zero is accepted here; 2D builds require at + // least 1 (see the 2D quadrature mismatch test). + CHECK_NOTHROW(ESPParameters(0.1, 0.2, 0)); + CHECK_THROWS_AS(ESPParameters(0.1, 0.2, -1), std::invalid_argument); +} + +TEST_CASE( + "ESP 2D evaluation requires the build's quadrature order", + "[esp_potential], [esp_potential_2d]") +{ + // Two parallel segments 0.05 apart. + Eigen::MatrixXd V(4, 2); + V << -1, 0, 1, 0, -1, 0.05, 1, 0.05; + Eigen::MatrixXi E(2, 2); + E << 0, 1, 2, 3; + const CollisionMesh mesh = make_2d_collision_mesh(V, E); + + const ESPParameters params(0.1, 1.0, /*quad_order=*/2); + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + CHECK(ESPPotential(params)(collisions, mesh, V) > 0); + + // A different order has a different number of points per edge than the + // collision sets store. + const int other_order = GENERATE(1, 3); + CAPTURE(other_order); + const ESPPotential other(ESPParameters(0.1, 1.0, other_order)); + CHECK_THROWS_AS(other(collisions, mesh, V), std::invalid_argument); + CHECK_THROWS_AS(other.gradient(collisions, mesh, V), std::invalid_argument); + CHECK_THROWS_AS(other.hessian(collisions, mesh, V), std::invalid_argument); + + // 2D edge quadrature needs at least one point per edge. + CHECK_THROWS_AS( + collisions.build(mesh, V, ESPParameters(0.1, 1.0, 0)), + std::invalid_argument); +} + +TEST_CASE( + "ESP 3D evaluation requires the build's face quadrature rule", + "[esp_potential], [esp_potential_3d]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("two-cubes-close.ply", V, E, F)); + const CollisionMesh mesh(V, E, F); + + ESPParameters params(0.1, 1.0); + params.set_quad_rule(make_face_quad_rule("edge-midpoints")); + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + CHECK(ESPPotential(params)(collisions, mesh, V) > 0); + + // An empty rule is what an ESPPotential constructed before + // set_quad_rule() holds, since it copies the parameters. The other rules + // have fewer points, more points, or the same points with other weights. + const std::string other_rule = + GENERATE("empty", "centroid", "vertices+centroid", "reweighted"); + CAPTURE(other_rule); + ESPParameters other_params(0.1, 1.0); + if (other_rule == "centroid") { + other_params.set_quad_rule({ { { { 1. / 3, 1. / 3, 1. / 3 } }, 1. } }); + } else if (other_rule == "vertices+centroid") { + other_params.set_quad_rule(make_face_quad_rule(other_rule)); + } else if (other_rule == "reweighted") { + FaceQuadRule rule = make_face_quad_rule("edge-midpoints"); + rule[0].weight *= 2; + other_params.set_quad_rule(rule); + } + + const ESPPotential other(other_params); + CHECK_THROWS_AS(other(collisions, mesh, V), std::invalid_argument); + CHECK_THROWS_AS(other.gradient(collisions, mesh, V), std::invalid_argument); + CHECK_THROWS_AS(other.hessian(collisions, mesh, V), std::invalid_argument); + + // Clearing the set forgets the build's rule along with its collisions. + collisions.clear(); + CHECK(other(collisions, mesh, V) == 0); +} + +TEST_CASE("ESP face quadrature rule validation", "[esp_potential]") +{ + ESPParameters params(0.1); + CHECK(params.quad_rule().empty()); + + const FaceQuadRule centroid = { { { { 1. / 3, 1. / 3, 1. / 3 } }, 1.0 } }; + CHECK_NOTHROW(params.set_quad_rule(centroid)); + REQUIRE(params.quad_rule().size() == 1); + + // A zero weight is allowed as long as the weights sum to a positive value. + CHECK_NOTHROW(params.set_quad_rule( + { + { { { 1., 0., 0. } }, 0.5 }, + { { { 0., 1., 0. } }, 0.5 }, + { { { 0., 0., 1. } }, 0.0 }, + })); + CHECK(params.quad_rule().size() == 3); + + // Degree-3 rule with a negative centroid weight (listed last, so the check + // must reach it): its barrier term would be unbounded below. + const FaceQuadRule negative = { + { { { 0.6, 0.2, 0.2 } }, 25. / 48 }, + { { { 0.2, 0.6, 0.2 } }, 25. / 48 }, + { { { 0.2, 0.2, 0.6 } }, 25. / 48 }, + { { { 1. / 3, 1. / 3, 1. / 3 } }, -27. / 48 }, + }; + CHECK_THROWS_AS(params.set_quad_rule(negative), std::invalid_argument); + CHECK(params.quad_rule().size() == 3); // unchanged on failure + + CHECK_THROWS_AS( + params.set_quad_rule({ { { { 1. / 3, 1. / 3, 1. / 3 } }, 0.0 } }), + std::invalid_argument); + for (const double bad : + { std::nan(""), std::numeric_limits::infinity() }) { + CAPTURE(bad); + CHECK_THROWS_AS( + params.set_quad_rule( + { + { { { 1., 0., 0. } }, 0.5 }, + { { { 0., 1., 0. } }, bad }, + }), + std::invalid_argument); + } + CHECK(params.quad_rule().size() == 3); // unchanged on failure + + // A point outside the face (negative barycentric coordinate) or with a + // non-finite coordinate is rejected. + for (const double bad : { -0.1, std::nan("") }) { + CAPTURE(bad); + CHECK_THROWS_AS( + params.set_quad_rule( + { + { { { 1., 0., 0. } }, 0.5 }, + { { { 0.5, 0.5, bad } }, 0.5 }, // the only bad coordinate + }), + std::invalid_argument); + } + CHECK(params.quad_rule().size() == 3); // unchanged on failure + + CHECK_NOTHROW(params.set_quad_rule({})); // back to vertex integration + CHECK(params.quad_rule().empty()); +} + +TEST_CASE("ESP 2D Gauss-Lobatto weights are positive", "[esp_potential]") +{ + // quad_order n selects the (n + 1)-point Gauss-Lobatto rule, tabulated up + // to 20 points and computed beyond, so ESPParameters accepts any order. + for (int order = 1; order <= 25; order++) { + CAPTURE(order); + CHECK_NOTHROW(ESPParameters(0.1, 0.2, order)); + const GaussLobatto::Rule& rule = GaussLobatto::get_rule(order); + REQUIRE(rule.size() == static_cast(order + 1)); + double sum = 0; + for (const EdgeQuadPoint& qp : rule) { + CHECK(qp.weight > 0); + CHECK(qp.xi >= 0); + CHECK(qp.xi <= 1); + sum += qp.weight; + } + CHECK(sum == Catch::Approx(1.0).epsilon(1e-12)); + } + + // A rule needs at least two points. + CHECK_THROWS_AS(GaussLobatto::get_rule(0), std::runtime_error); + std::vector x, w; + CHECK_THROWS_AS(lobatto_compute(0, x, w), std::runtime_error); + CHECK_THROWS_AS(lobatto_set(1, x, w), std::domain_error); +} + +TEST_CASE( + "NearFarBarrier far part vanishes at alpha = 2", "[esp_potential][barrier]") +{ + const double dhat = 0.1; + const NormalizedClampedLogBarrier<> base; + const NearFarBarrier<> nf1(&base, 1.0), nf2(&base, 2.0); + for (int i = 1; i < 100; i++) { + const double d = dhat * i / 100.0; + CAPTURE(d); + CHECK(nf2.far_value(d, dhat) == 0.0); + CHECK(nf2.near_value(d, dhat) == base(d, dhat)); + if (d > dhat / 2) { + CHECK(nf1.far_value(d, dhat) > 0.0); // a real split at alpha = 1 + } + } +} + +// The near/far split is used for every dbar_factor in (0, 2] and its far part +// vanishes at 2, so the potential and its derivatives are continuous in +// dbar_factor, including at 1, where the split used to switch off. +TEST_CASE( + "ESP potential is continuous in dbar_factor", + "[esp_potential], [esp_potential_3d]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("two-cubes-close.ply", V, E, F)); + const CollisionMesh mesh(V, E, F); + const double dhat = 0.15; + const double alpha = GENERATE(1.0, 2.0); + const double eps = 1e-7; + CAPTURE(alpha); + + struct Eval { + double e; + Eigen::VectorXd g; + Eigen::MatrixXd h; + }; + const auto eval = [&](const double dbar_factor) { + const ESPParameters params(dhat, dbar_factor, 0); + ESPCollisions collisions; + collisions.build(mesh, V, params); + const ESPPotential potential(params); + return Eval { potential(collisions, mesh, V), + potential.gradient(collisions, mesh, V), + Eigen::MatrixXd(potential.hessian(collisions, mesh, V)) }; + }; + const auto check_close = [](const Eval& a, const Eval& b) { + CHECK(a.e == Catch::Approx(b.e).epsilon(1e-5)); + CHECK((a.g - b.g).norm() <= 1e-5 * b.g.norm()); + CHECK((a.h - b.h).norm() <= 1e-5 * b.h.norm()); + }; + + const Eval at = eval(alpha); + REQUIRE(at.e > 0); + check_close(eval(alpha - eps), at); + if (alpha < 2) { + check_close(eval(alpha + eps), at); + } +} + +// An edge-edge quadrature point whose collision set cancels to empty still has +// a positive weight in its faces' weighted average. Here edges e0 (on T0) and +// e1 (on T1) form an EA_EB pair whose edge-point sets cancel to empty; vertex u +// gives T0 a nonzero vertex potential. Moving vertex v across dhat of the +// closest point on e0 makes that set non-empty; the potential must not jump. +TEST_CASE( + "ESP edge-edge weights count when their collisions cancel", + "[esp_potential], [esp_potential_3d]") +{ + const double dhat = 0.5, d0 = 0.2; + Eigen::MatrixXd V(8, 3); + V << -1, 0, 0, 1, 0, 0, 0, -1, 0, // T0, in z = 0 + 0, -1, d0, 0, 1, d0, 0, 0, d0 + 1, // T1, in x = 0 + 1, 0, -0.2, // u + 0, 0, -dhat; // v + Eigen::MatrixXi F(2, 3); + F << 0, 1, 2, 3, 4, 5; + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + const ESPParameters params(dhat, 1.0, 0); + const ESPPotential potential(params); + const auto energy_at = [&](const double z) { + V(7, 2) = -z; + ESPCollisions collisions; + collisions.build(mesh, V, params); + return potential(collisions, mesh, V); + }; + + const double eta = 1e-6; + const double e_out = energy_at(dhat + eta); + const double e_in = energy_at(dhat - eta); + CAPTURE(e_out, e_in); + REQUIRE(e_out != 0); + CHECK(e_in == Catch::Approx(e_out).epsilon(1e-6)); +} + +TEST_CASE("ESP potential codim", "[esp_potential], [esp_potential_2d]") +{ + const auto method = make_default_broad_phase(); + double dhat = 2; + const int quadrature_order = 2; + ESPParameters params(dhat, 1., quadrature_order); + + Eigen::MatrixXd vertices(4, 2); + Eigen::MatrixXi edges(2, 2); + + vertices << -1, 0, 0, 0, 1, 0, 1.5, 0.2; + edges << 0, 1, 1, 2; + + CollisionMesh mesh = make_2d_collision_mesh(vertices, edges); + + ESPCollisions collisions; + collisions.build(mesh, vertices, params, method.get()); + CAPTURE(dhat, method); + CHECK(!collisions.empty()); + CHECK(!has_intersections(mesh, vertices)); + + ESPPotential potential(params); + double energy = potential(collisions, mesh, vertices); + CHECK(energy > 0); + + // Gradient + const Eigen::VectorXd grad = potential.gradient(collisions, mesh, vertices); + + Eigen::VectorXd fgrad; + fd::finite_gradient( + tests::flatten(vertices), + [&](const Eigen::VectorXd& x) { + return potential( + collisions, mesh, tests::unflatten(x, vertices.cols())); + }, + fgrad, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE(grad.squaredNorm() > 1e-8); + CHECK((grad - fgrad).norm() / grad.norm() < 1e-4); + + // Hessian + Eigen::MatrixXd hess = potential.hessian(collisions, mesh, vertices); + + Eigen::MatrixXd fhess; + fd::finite_jacobian( + tests::flatten(vertices), + [&](const Eigen::VectorXd& x) { + return potential.gradient( + collisions, mesh, tests::unflatten(x, vertices.cols())); + }, + fhess, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE(hess.squaredNorm() > 1e-8); + CHECK((hess - fhess).norm() / hess.norm() < 1e-3); +} + +// The ends of an open polyline have weight zero and an isolated vertex has +// weight one, so a free edge alone feels nothing and an isolated vertex is +// repelled by an edge. With the closed-curve signs (every vertex -1) the free +// edge pulls itself shorter and the vertex is attracted to the edge, with an +// energy unbounded below. +// Even quadrature orders put a quadrature point at the edge's midpoint, right +// under the isolated vertex in the second test below. +TEST_CASE( + "ESP potential 2D on open and codimensional pieces: free edge", + "[esp_potential], [esp_potential_2d]") +{ + const int quadrature_order = GENERATE(2, 4); + CAPTURE(quadrature_order); + + Eigen::MatrixXd V(2, 2); + V << 0, 0, 1, 0; + Eigen::MatrixXi E(1, 2); + E << 0, 1; + const CollisionMesh mesh = make_2d_collision_mesh(V, E); + + const ESPParameters params(2, 1., quadrature_order); + ESPCollisions collisions; + collisions.build(mesh, V, params); + const ESPPotential potential(params); + CHECK(potential(collisions, mesh, V) == 0); + CHECK(potential.gradient(collisions, mesh, V).squaredNorm() == 0); +} + +TEST_CASE( + "ESP potential 2D on open and codimensional pieces: isolated vertex above " + "an edge", + "[esp_potential], [esp_potential_2d]") +{ + const int quadrature_order = GENERATE(2, 4); + const double h = GENERATE(0.1, 1e-2, 1e-5); + CAPTURE(quadrature_order, h); + + Eigen::MatrixXd V(3, 2); + V << 0, 0, 1, 0, 0.5, h; + Eigen::MatrixXi E(1, 2); + E << 0, 1; + const CollisionMesh mesh = make_2d_collision_mesh(V, E); + + const ESPParameters params(0.5, 1., quadrature_order); + const ESPPotential potential(params); + const auto energy_at = [&](const double y) { + Eigen::MatrixXd V_ = V; + V_(2, 1) = y; + ESPCollisions collisions; + collisions.build(mesh, V_, params); + return potential(collisions, mesh, V_); + }; + + const double e = energy_at(h); + CHECK(e > 0); + CHECK(energy_at(h / 2) > e); + + // The force on the vertex points away from the edge. + ESPCollisions collisions; + collisions.build(mesh, V, params); + CHECK(potential.gradient(collisions, mesh, V)(5) < 0); +} + +namespace { + +/// Check that a closed 2D polygon whose edges are all farther than dhat apart +/// gets no energy, forces, or stiffness. +void check_esp_2d_no_forces( + const Eigen::MatrixXd& V, + const Eigen::MatrixXi& E, + const int quadrature_order) +{ + CAPTURE(quadrature_order); + const auto method = make_default_broad_phase(); + const ESPParameters params(/*dhat=*/1., 1., quadrature_order); + + CollisionMesh mesh = make_2d_collision_mesh(V, E); + + ESPCollisions collisions; + collisions.build(mesh, V, params, method.get()); + + REQUIRE(!has_intersections(mesh, V)); + + ESPPotential potential(params); + double energy = potential(collisions, mesh, V); + CHECK(energy == 0); + + Eigen::VectorXd grad = potential.gradient(collisions, mesh, V); + CHECK(grad.squaredNorm() == 0); + + Eigen::MatrixXd hess = potential.hessian(collisions, mesh, V); + CHECK(hess.squaredNorm() == 0); +} + +} // namespace + +TEST_CASE("ESP potential 2D no forces", "[esp_potential], [esp_potential_2d]") +{ + const int quadrature_order = GENERATE(1, 14); + + Eigen::MatrixXd V; + Eigen::MatrixXi E; + SECTION("square_1") + { + V.resize(4, 2); + E.resize(4, 2); + V << -1., -1., 1., -1., 1., 1., -1., 1.; + E << 0, 1, 1, 2, 2, 3, 3, 0; + } + SECTION("square_2") + { + V.resize(8, 2); + E.resize(8, 2); + V << -1., -1., 0., -1., 1., -1., 1., 0., 1., 1., 0., 1., -1., 1., -1., + 0.; + E << 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 0; + } + + check_esp_2d_no_forces(V, E, quadrature_order); +} + +TEST_CASE( + "ESP potential 2D no forces on a circle", + "[esp_potential], [esp_potential_2d]") +{ + const int quadrature_order = GENERATE(1, 14); + const int n = GENERATE(5, 6, 7, 8, 9, 10, 50, 100, 200); + CAPTURE(n); + + // A regular n-gon inscribed in the unit circle. + Eigen::MatrixXd V(n, 2); + Eigen::MatrixXi E(n, 2); + for (int i = 0; i < n; i++) { + V(i, 0) = std::cos(2 * M_PI * i / n); + V(i, 1) = std::sin(2 * M_PI * i / n); + E(i, 0) = i; + E(i, 1) = (i + 1) % n; + } + + check_esp_2d_no_forces(V, E, quadrature_order); +} + +namespace { + +constexpr double BA = 0; // a small constant to break perfect alignments + +/// Check the 2D ESP gradient (and optionally the Hessian) against finite +/// differences. +/// +/// We check the Hessian against an FD of the analytic gradient on the set +/// built at V: the potential is only C1 across a distance-type switch +/// (point-line vs. point-point distance), so rebuilding at each step is not +/// an option. The fixed set is valid as long as no stored Vertex2-Edge2P1 +/// pair (P_E) is within the FD step of a switch, since that collision +/// asserts P_E. Exact ties are classified to endpoint types and are +/// harmless, but rounding can leave P_E pairs ~1e-17 from a switch; as in +/// the GCP tests, such configurations get the gradient check only. +void check_esp_2d_finite_differences( + const Eigen::MatrixXd& V, + const Eigen::MatrixXi& E, + const int quadrature_order, + const bool check_hessian) +{ + CAPTURE(quadrature_order); + const auto method = make_default_broad_phase(); + const ESPParameters params(/*dhat=*/0.6, 1., quadrature_order); + + CollisionMesh mesh = make_2d_collision_mesh(V, E); + + ESPCollisions collisions; + collisions.build(mesh, V, params, method.get()); + + REQUIRE(!collisions.empty()); + REQUIRE(!has_intersections(mesh, V)); + + ESPPotential potential(params); + double energy = potential(collisions, mesh, V); + CHECK(energy > 0); + + Eigen::VectorXd grad = potential.gradient(collisions, mesh, V); + REQUIRE(grad.squaredNorm() > 1e-8); + + // Rebuild the collisions at each perturbed configuration: a + // collision's type encodes its distance type (e.g., Vertex2-Edge2P1 + // only at P_E) and asserts it, so the set built at V cannot be + // evaluated across a switch, which the energy FD may cross. + const auto energy_at = [&](const Eigen::VectorXd& x) { + const Eigen::MatrixXd V_ = tests::unflatten(x, V.cols()); + ESPCollisions collisions_; + collisions_.build(mesh, V_, params, method.get()); + return potential(collisions_, mesh, V_); + }; + + CAPTURE(grad.norm()); + if (V.size() <= 32) { + Eigen::VectorXd fgrad; + fd::finite_gradient( + tests::flatten(V), energy_at, fgrad, fd::AccuracyOrder::SECOND, + 1e-8); + CAPTURE(fgrad.norm()); + CHECK( + (grad - fgrad).norm() < std::max( + 1e-4 * std::max({ grad.norm(), fgrad.norm(), 1e-8 }), 1e-9)); + } else { + // A full finite difference with a rebuild per evaluation is too + // expensive on mesh_1 (880 DOFs), verify directional derivative + // only + Eigen::VectorXd test_dir(V.size()); + for (int i = 0; i < test_dir.size(); i++) { + test_dir(i) = i; + } + test_dir.normalize(); + + Eigen::VectorXd fg; + fd::finite_gradient( + Eigen::VectorXd::Zero(1), + [&](const Eigen::VectorXd& y) { + return energy_at(tests::flatten(V) + test_dir * y(0)); + }, + fg, fd::AccuracyOrder::SECOND, 1e-8); + const double g_dir = grad.dot(test_dir); + CAPTURE(fg(0), g_dir); + CHECK( + std::abs(fg(0) - g_dir) < std::max( + 1e-4 * std::max({ std::abs(fg(0)), std::abs(g_dir), 1e-8 }), + 1e-9)); + } + + if (!check_hessian) { + return; + } + + Eigen::MatrixXd hess = potential.hessian(collisions, mesh, V); + REQUIRE(hess.squaredNorm() > 1e-3); + Eigen::MatrixXd fhess; + fd::finite_jacobian( + tests::flatten(V), + [&](const Eigen::VectorXd& x) { + return potential.gradient( + collisions, mesh, tests::unflatten(x, V.cols())); + }, + fhess, fd::AccuracyOrder::SECOND, 1e-12); + CAPTURE(hess.norm()); + CAPTURE(fhess.norm()); + CHECK( + (hess - fhess).norm() + < std::max(3e-3 * std::max({ hess.norm(), fhess.norm(), 1e-8 }), 1e-9)); +} + +} // namespace + +TEST_CASE( + "ESP potential 2D finite differences", + "[esp_potential], [esp_potential_2d]") +{ + const int quadrature_order = GENERATE(1, 14); + + Eigen::MatrixXd V; + Eigen::MatrixXi E; + + SECTION("squares") + { + V.resize(8, 2); + E.resize(8, 2); + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + SECTION("horizontal_squares") + { + INFO("horizontal_squares"); + V << -1., 1. + BA, -1., 0. + BA, -.1, 0. + BA, -.1, 1. + BA, .1, 1., + .1, 0., 1., 0., 1., 1.; + // Rounding leaves P_E pairs ~1e-17 from a switch at order 14. + check_esp_2d_finite_differences(V, E, quadrature_order, false); + } + SECTION("vertical_squares") + { + INFO("vertical_squares"); + V << 0. + BA, -1., 1. + BA, -1., 1. + BA, -.1, 0. + BA, -.1, 0., .1, + 1., .1, 1., 1., 0., 1.; + // Rounding leaves P_E pairs ~1e-17 from a switch at order 14. + check_esp_2d_finite_differences(V, E, quadrature_order, false); + } + } + + SECTION("mesh_1") + { + INFO("mesh 1"); + std::string mesh_name = + (tests::DATA_DIR / "gcp" / "nonlinear_solve_iter020.obj").string(); + bool success = igl::readCSV(mesh_name + "-v.csv", V); + success = success && igl::readCSV(mesh_name + "-e.csv", E); + REQUIRE(success); + V.col(0) += Eigen::VectorXd::Random(V.rows()) * BA; + // The closest P_E pair is ~1e-11 from a switch, above h = 1e-12. + check_esp_2d_finite_differences(V, E, quadrature_order, true); + } + + SECTION("mesh_2") + { + INFO("mesh 2"); + std::string mesh_name = + (tests::DATA_DIR / "gcp" / "simple_2d.obj").string(); + bool success = igl::readCSV(mesh_name + "-v.csv", V); + success = success && igl::readCSV(mesh_name + "-e.csv", E); + REQUIRE(success); + V.col(0) += Eigen::VectorXd::Random(V.rows()) * BA; + // Rounding leaves P_E pairs ~1e-17 from a switch at order 14. + check_esp_2d_finite_differences(V, E, quadrature_order, false); + } +} + +TEST_CASE( + "ESP potential 2D finite differences at corners", + "[esp_potential], [esp_potential_2d]") +{ + const int quadrature_order = GENERATE(1, 14); + const double P0x = GENERATE(.01, -.01, 0.0); + const double P1y = GENERATE(.49, .5, .51); + CAPTURE(P0x, P1y); + + Eigen::MatrixXd V(8, 2); + Eigen::MatrixXi E(8, 2); + V << -1., 1., -1., 0., 0., 0., P0x, .5 + BA, 0., 1., 1., 0., 1., 1., .02, + P1y; + E << 0, 1, 1, 2, 2, 3, 3, 4, 4, 0, 5, 6, 6, 7, 7, 5; + check_esp_2d_finite_differences(V, E, quadrature_order, true); +} + +// Each 2D quadrature point's Hessian is projected, so their sum is PSD. +TEST_CASE("ESP potential 2D Hessian PSD", "[esp_potential], [esp_potential_2d]") +{ + const int quadrature_order = GENERATE(1, 2, 7); + const PSDProjectionMethod psd_method = + GENERATE(PSDProjectionMethod::CLAMP, PSDProjectionMethod::ABS); + const ESPParameters params(0.6, 1., quadrature_order); + + Eigen::MatrixXd V(8, 2); + Eigen::MatrixXi E(8, 2); + SECTION("corners") + { + V << -1., 1., -1., 0., 0., 0., .01, .5, 0., 1., 1., 0., 1., 1., .02, + .49; + E << 0, 1, 1, 2, 2, 3, 3, 4, 4, 0, 5, 6, 6, 7, 7, 5; + } + SECTION("squares") + { + V << -1., 1., -1., 0., -.1, 0., -.1, 1., .1, 1., .1, 0., 1., 0., 1., 1.; + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + } + const CollisionMesh mesh = make_2d_collision_mesh(V, E); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + + const auto [lambda_min, lambda_max] = eigenvalue_range( + ESPPotential(params).hessian(collisions, mesh, V, psd_method)); + CHECK(lambda_min >= -std::max(1e-10, 1e-10 * std::abs(lambda_max))); +} + +// 3D FACE QUADRATURE TESTS // + +// Verify that face quadrature gives gradient/hessian consistent with finite +// differences on the wrapped-sphere geometry, for two positive-weight face +// quadrature rules (params.set_quad_rule()). The edge-midpoint rule puts each +// point on an edge, which skips the face across that edge. +TEST_CASE( + "Face Quadrature Gradient and Hessian", + "[esp_potential], [esp_potential_3d]") +{ + TriMeshData data = load_wrapped_sphere(); + Eigen::MatrixXd& V = data.V; + CollisionMesh& mesh = data.mesh; + + const double dhat = 0.15; + const std::string rule_name = + GENERATE(as(), "vertices+centroid", "edge-midpoints"); + CAPTURE(rule_name); + ESPParameters params(dhat, 1.); + params.set_quad_rule(make_face_quad_rule(rule_name)); + + const bool normalize_weights = GENERATE(true, false); + ESPPotential potential(params, normalize_weights); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + + REQUIRE(potential(collisions, mesh, V) != 0); + + // Directional finite-difference to keep the test inexpensive + Eigen::VectorXd test_dir(V.size()); + for (int i = 0; i < test_dir.size(); i++) { + test_dir(i) = i; + } + test_dir.normalize(); + + SECTION("gradient") + { + Eigen::VectorXd g = potential.gradient(collisions, mesh, V); + + Eigen::VectorXd fg; + fd::finite_gradient( + Eigen::VectorXd::Zero(1), + [&](const Eigen::VectorXd& y) { + Eigen::MatrixXd V_ = V + tests::unflatten(test_dir, 3) * y(0); + ESPCollisions c; + c.build(mesh, V_, params); + return potential(c, mesh, V_); + }, + fg, fd::AccuracyOrder::SECOND, 1e-7); + + REQUIRE(abs(fg(0) - g.dot(test_dir)) < fg.norm() * 1e-5); + } + + SECTION("hessian") + { + Eigen::MatrixXd h = potential.hessian(collisions, mesh, V); + + Eigen::MatrixXd fh; + fd::finite_jacobian( + Eigen::VectorXd::Zero(1), + [&](const Eigen::VectorXd& y) { + Eigen::MatrixXd V_ = V + tests::unflatten(test_dir, 3) * y(0); + ESPCollisions c; + c.build(mesh, V_, params); + return potential.gradient(c, mesh, V_); + }, + fh, fd::AccuracyOrder::SECOND, 1e-6); + + REQUIRE((fh.col(0) - h * test_dir).norm() < fh.norm() * 1e-4); + } +} + +// Verify that the global hessian is PSD whenever project_hessian_to_psd is set. +// With normalized weights (the second ESPPotential argument, use_near_far) and +// 0 < dbar_factor <= 2, the edge-edge weights add indefinite terms, so each +// face's block is projected as a whole; every dbar_factor in that range takes +// this path. The non-normalized branch projects each stencil's block. two- +// cubes-close has many edge-edge pairs and vertex potentials; the wrapped +// sphere has few. +TEST_CASE( + "Convergent Quadrature Hessian PSD", "[esp_potential], [esp_potential_3d]") +{ + const std::string mesh_name = + GENERATE(as(), "wrapped_sphere", "two-cubes-close"); + auto [V, E, F, mesh] = mesh_name == "wrapped_sphere" + ? load_wrapped_sphere() + : load_triangle_mesh( + (tests::DATA_DIR / "two-cubes-close.ply").string()); + + const double dhat = 0.15; + const double dbar_factor = 0.4; + ESPParameters params(dhat, dbar_factor, 0); + + const bool normalize_weights = GENERATE(true, false); + const PSDProjectionMethod psd_method = + GENERATE(PSDProjectionMethod::CLAMP, PSDProjectionMethod::ABS); + + ESPPotential potential(params, normalize_weights); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + + const auto [lambda_min, lambda_max] = + eigenvalue_range(potential.hessian(collisions, mesh, V, psd_method)); + const double tol = std::max(1e-10, 1e-10 * std::abs(lambda_max)); + + INFO( + "mesh=" << mesh_name << " dbar_factor=" << dbar_factor + << " normalize_weights=" << normalize_weights << " method=" + << static_cast(psd_method) << " lambda_min=" << lambda_min + << " lambda_max=" << lambda_max); + REQUIRE(lambda_min >= -tol); +} + +TEST_CASE("NearFarBarrier decomposition", "[esp_potential][barrier]") +{ + const double dhat = 0.1; + const double alpha = GENERATE(0.01, 0.25, 0.5, 0.75, 0.99); + + enum class BarrierType { + ClampedLog, + ClampedLogSq, + Cubic, + TwoStage, + InversePower1, + InversePower2 + }; + + const BarrierType type = GENERATE( + BarrierType::ClampedLog, BarrierType::ClampedLogSq, BarrierType::Cubic, + BarrierType::TwoStage, BarrierType::InversePower1, + BarrierType::InversePower2); + + auto run_test = [&](const auto& base) { + NearFarBarrier<> nf(&base, alpha); + + constexpr int N = 200; + for (int i = 0; i < N; ++i) { + const double d = dhat * (i + 1.0) / N; + + const double b = base(d, dhat); + CHECK( + nf.near_value(d, dhat) + nf.far_value(d, dhat) + == Catch::Approx(b)); + + const double db = base.first_derivative(d, dhat); + CHECK( + nf.first_derivative_near(d, dhat) + + nf.first_derivative_far(d, dhat) + == Catch::Approx(db)); + + const double ddb = base.second_derivative(d, dhat); + CHECK( + nf.second_derivative_near(d, dhat) + + nf.second_derivative_far(d, dhat) + == Catch::Approx(ddb)); + + // The unsplit evaluation forwards to the base barrier. + CHECK(nf(d, dhat) == b); + CHECK(nf.first_derivative(d, dhat) == db); + CHECK(nf.second_derivative(d, dhat) == ddb); + + CAPTURE(d); + CAPTURE(alpha); + CAPTURE(dhat); + CAPTURE(alpha * dhat); + CAPTURE(alpha * dhat / 2); + // Check that near barrier is 0 above alpha*dhat and non-zero below + constexpr double eps_tol = 1e-9; + if (d >= alpha * dhat) { + CHECK(nf.near_value(d, dhat) == 0.0); + } else if (d < alpha * dhat - eps_tol) { + CHECK(nf.near_value(d, dhat) > 0.0); + } + + // Check that far barrier is 0 below dhat*alpha/2 and non-zero above + if (d <= dhat * alpha / 2 || d >= dhat) { + CHECK(nf.far_value(d, dhat) == 0.0); + } else if (d > dhat * alpha / 2 + eps_tol) { + CHECK(nf.far_value(d, dhat) > 0.0); + } + } + CHECK(nf.units(dhat) == base.units(dhat)); + }; + + switch (type) { + case BarrierType::ClampedLog: + run_test(ClampedLogBarrier<>()); + break; + case BarrierType::ClampedLogSq: + run_test(ClampedLogSqBarrier<>()); + break; + case BarrierType::Cubic: + run_test(CubicBarrier<>()); + break; + case BarrierType::TwoStage: + run_test(TwoStageBarrier<>()); + break; + case BarrierType::InversePower1: + run_test(InversePowerBarrier<>(1.0)); + break; + case BarrierType::InversePower2: + run_test(InversePowerBarrier<>(2.0)); + break; + default: + FAIL("Unknown barrier type"); + } +} + +// At and below alpha * dhat / 2 the far weight vanishes, so the near part is +// the whole barrier and the far part is exactly zero. This must hold even +// where the base barrier is +inf (contact, penetration, or InversePowerBarrier +// overflowing for tiny d), where multiplying by a zero weight would give NaN. +TEST_CASE( + "NearFarBarrier keeps the whole barrier near at contact", + "[esp_potential][barrier]") +{ + const double dhat = 1e-3; + const double alpha = GENERATE(0.2, 2.0); + const double d = GENERATE(0.0, -1e-4, 1e-200); + + const std::vector> bases { + std::make_shared>(), + std::make_shared>(), + std::make_shared>(2.0), + std::make_shared>(), + }; + const size_t i = GENERATE(range(0, 4)); + const Barrier& base = *bases[i]; + CAPTURE(alpha, d, i); + + const NearFarBarrier<> nf(bases[i], alpha); + + CHECK(nf.near_value(d, dhat) == base(d, dhat)); + CHECK(nf.far_value(d, dhat) == 0.0); + CHECK(nf.first_derivative_near(d, dhat) == base.first_derivative(d, dhat)); + CHECK(nf.first_derivative_far(d, dhat) == 0.0); + CHECK( + nf.second_derivative_near(d, dhat) == base.second_derivative(d, dhat)); + CHECK(nf.second_derivative_far(d, dhat) == 0.0); + + // The near + far = b identity holds at contact too, including b = +inf. + CHECK(nf.near_value(d, dhat) + nf.far_value(d, dhat) == base(d, dhat)); +} + +TEST_CASE( + "NearFarBarrier requires a positive split parameter", + "[esp_potential][barrier]") +{ + const auto base = std::make_shared>(); + CHECK_NOTHROW(NearFarBarrier<>(base, 0.2)); + + // At alpha = 0 the step collapses to a jump with NaN derivatives. + const double alpha = GENERATE( + 0.0, -0.5, std::nan(""), std::numeric_limits::infinity()); + CAPTURE(alpha); + CHECK_THROWS_AS(NearFarBarrier<>(base, alpha), std::invalid_argument); + CHECK_THROWS_AS(NearFarBarrier<>(base.get(), alpha), std::invalid_argument); +} + +// Same check with a face quadrature rule (params.set_quad_rule()), which adds +// face-point stencils to each face's block. dbar_factor = 0 covers the +// per-stencil projection branch of the normalized potential. +TEST_CASE("Face Quadrature Hessian PSD", "[esp_potential], [esp_potential_3d]") +{ + const std::string mesh_name = + GENERATE(as(), "wrapped_sphere", "two-cubes-close"); + auto [V, E, F, mesh] = mesh_name == "wrapped_sphere" + ? load_wrapped_sphere() + : load_triangle_mesh( + (tests::DATA_DIR / "two-cubes-close.ply").string()); + + const double dhat = 0.15; + const std::string rule_name = "vertices+centroid"; + const double dbar_factor = GENERATE(0.4, 0.0); + ESPParameters params(dhat, dbar_factor); + params.set_quad_rule(make_face_quad_rule(rule_name)); + + const bool normalize_weights = GENERATE(true, false); + const PSDProjectionMethod psd_method = + GENERATE(PSDProjectionMethod::CLAMP, PSDProjectionMethod::ABS); + + ESPPotential potential(params, normalize_weights); + + ESPCollisions collisions; + collisions.build(mesh, V, params); + + const auto [lambda_min, lambda_max] = + eigenvalue_range(potential.hessian(collisions, mesh, V, psd_method)); + const double tol = std::max(1e-10, 1e-10 * std::abs(lambda_max)); + + INFO( + "mesh=" << mesh_name << " dbar_factor=" << dbar_factor + << " normalize_weights=" << normalize_weights << " rule=" + << rule_name << " method=" << static_cast(psd_method) + << " lambda_min=" << lambda_min + << " lambda_max=" << lambda_max); + REQUIRE(lambda_min >= -tol); +} + +// A block of a taller matrix, such as `V_big.topRows(n)`, binds to the +// potential's vertex argument without a copy, so its columns are further apart +// than its row count. The potential must read it exactly like the equivalent +// contiguous matrix, both when building the collisions and when evaluating. +TEST_CASE( + "ESP potential reads a strided vertex matrix like a contiguous one", + "[esp_potential]") +{ + const bool use_near_far = GENERATE(true, false); + + Eigen::MatrixXd V; + CollisionMesh mesh; + double dhat = 0.15; + int quad_order = 0; + FaceQuadRule face_quad_rule; + SECTION("3D vertex quadrature") + { + TriMeshData data = load_wrapped_sphere(); + V = data.V; + mesh = data.mesh; + } + SECTION("3D face quadrature") + { + TriMeshData data = load_wrapped_sphere(); + V = data.V; + mesh = data.mesh; + face_quad_rule = make_face_quad_rule("vertices+centroid"); + } + SECTION("2D") + { + // Two rectangles 0.2 apart. + V.resize(8, 2); + V << -1, 1, -1, 0, -.1, 0, -.1, 1, .1, 1, .1, 0, 1, 0, 1, 1; + Eigen::MatrixXi E(8, 2); + E << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4; + mesh = make_2d_collision_mesh(V, E); + dhat = 0.6; + quad_order = 2; + } + ESPParameters params(dhat, 1., quad_order); + params.set_quad_rule(face_quad_rule); + + // The rows below V hold values far from the mesh, so reading them instead + // of V changes the result. + Eigen::MatrixXd V_big = + Eigen::MatrixXd::Constant(V.rows() + 7, V.cols(), 1e3); + V_big.topRows(V.rows()) = V; + const auto V_strided = V_big.topRows(V.rows()); + + ESPCollisions collisions, strided_collisions; + collisions.build(mesh, V, params); + strided_collisions.build(mesh, V_strided, params); + REQUIRE(!collisions.empty()); + CHECK(strided_collisions.size() == collisions.size()); + + const ESPPotential potential(params, use_near_far); + const double P = potential(collisions, mesh, V); + REQUIRE(P != 0); + const Eigen::VectorXd g = potential.gradient(collisions, mesh, V); + const Eigen::SparseMatrix H = + potential.hessian(collisions, mesh, V); + + for (const ESPCollisions* c : { &collisions, &strided_collisions }) { + CHECK( + potential(*c, mesh, V_strided) == Catch::Approx(P).epsilon(1e-12)); + CHECK( + (potential.gradient(*c, mesh, V_strided) - g).norm() + <= 1e-12 * g.norm()); + CHECK( + (potential.hessian(*c, mesh, V_strided) - H).norm() + <= 1e-12 * H.norm()); + } +} + +// The ESP Hessian keeps its per-collision blocks on the stack, so their size +// decides whether a caller's thread survives. macOS gives secondary threads +// (std::thread included) a 512 KiB stack, and tbb::parallel_for also runs work +// on the calling thread, so the Hessian has to fit there. We pin TBB to one +// thread to make the calling thread do all of the work, then run the Hessian +// on a pthread with exactly that stack size. Overflowing it crashes the test. +TEST_CASE( + "ESP Hessian fits in a 512 KiB thread stack", + "[esp_potential], [esp_potential_3d]") +{ +#ifdef _WIN32 + SKIP("Uses pthreads to choose the thread's stack size"); +#else + // Two triangles 0.01 apart: vertex 3 sits over the interior of edge (0,1) + // and edge (3,4) crosses it, so we get vertex and edge-edge stencils. + Eigen::MatrixXd V(6, 3); + V << 0, 0, 0, 1, 0, 0, 0, 1, 0, // + 0.5, -0.05, 0.01, 0.6, 0.6, 0.01, 0.2, 0.5, 0.01; + Eigen::MatrixXi F(2, 3); + F << 0, 1, 2, 3, 4, 5; + Eigen::MatrixXi E; + igl::edges(F, E); + const CollisionMesh mesh(V, E, F); + + const ESPParameters params(0.1); // default 3D parameters + ESPCollisions collisions; + collisions.build(mesh, V, params); + REQUIRE(!collisions.empty()); + const ESPPotential potential(params); + + struct Job { + const ESPPotential& potential; + const ESPCollisions& collisions; + const CollisionMesh& mesh; + const Eigen::MatrixXd& V; + Eigen::SparseMatrix hess; + } job { potential, collisions, mesh, V, {} }; + + const tbb::global_control single_thread( + tbb::global_control::max_allowed_parallelism, 1); + + pthread_attr_t attr; + REQUIRE(pthread_attr_init(&attr) == 0); + REQUIRE(pthread_attr_setstacksize(&attr, 512 * 1024) == 0); + pthread_t thread; + REQUIRE( + pthread_create( + &thread, &attr, + [](void* arg) -> void* { + auto& j = *static_cast(arg); + j.hess = j.potential.hessian(j.collisions, j.mesh, j.V); + return nullptr; + }, + &job) + == 0); + REQUIRE(pthread_join(thread, nullptr) == 0); + pthread_attr_destroy(&attr); + + CHECK(job.hess.nonZeros() > 0); +#endif +} diff --git a/tests/src/tests/potential/test_smooth_potential.cpp b/tests/src/tests/potential/test_gcp_potential.cpp similarity index 93% rename from tests/src/tests/potential/test_smooth_potential.cpp rename to tests/src/tests/potential/test_gcp_potential.cpp index 2e9af48e3..a1b0e090e 100644 --- a/tests/src/tests/potential/test_smooth_potential.cpp +++ b/tests/src/tests/potential/test_gcp_potential.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include #include @@ -17,7 +17,7 @@ using namespace ipc; -TEST_CASE("Smooth barrier potential codim", "[smooth_potential]") +TEST_CASE("Smooth barrier potential codim", "[gcp_potential]") { const auto method = make_default_broad_phase(); double dhat = 2; @@ -31,18 +31,18 @@ TEST_CASE("Smooth barrier potential codim", "[smooth_potential]") CollisionMesh mesh; - SmoothCollisions collisions; + GCPCollisions collisions; mesh = CollisionMesh( std::vector(vertices.rows(), true), std::vector(vertices.rows(), false), vertices, edges, faces); - SmoothContactParameters params(dhat, 0.85, 0.5, 0.95, 0.6, 2); + GCPParameters params(dhat, 0.85, 0.5, 0.95, 0.6, 2); collisions.build(mesh, vertices, params, false, method.get()); CAPTURE(dhat, method); CHECK(!collisions.empty()); CHECK(!has_intersections(mesh, vertices)); - SmoothContactPotential potential(params); + GCPPotential potential(params); std::cout << "energy: " << potential(collisions, mesh, vertices) << "\n"; // ------------------------------------------------------------------------- @@ -105,9 +105,9 @@ TEST_CASE("Smooth barrier potential codim", "[smooth_potential]") } #if defined(NDEBUG) && !defined(WIN32) -std::string tagsopt = "[smooth_potential]"; +static std::string tagsopt = "[gcp_potential]"; #else -std::string tagsopt = "[.][smooth_potential]"; +static std::string tagsopt = "[.][gcp_potential]"; #endif TEST_CASE("Smooth barrier potential full gradient and hessian 3D", tagsopt) @@ -143,7 +143,7 @@ TEST_CASE("Smooth barrier potential full gradient and hessian 3D", tagsopt) CollisionMesh mesh; - SmoothCollisions collisions; + GCPCollisions collisions; if (all_vertices_on_surface) { mesh = CollisionMesh( std::vector(vertices.rows(), true), @@ -158,7 +158,7 @@ TEST_CASE("Smooth barrier potential full gradient and hessian 3D", tagsopt) vertices = mesh.vertices(vertices); } - SmoothContactParameters params(dhat, 0.85, 0.5, 0.95, 0.6, 2); + GCPParameters params(dhat, 0.85, 0.5, 0.95, 0.6, 2); params.set_adaptive_dhat_ratio(min_dist_ratio); collisions.compute_adaptive_dhat(mesh, vertices, params, method.get()); collisions.build(mesh, vertices, params, adaptive_dhat, method.get()); @@ -166,7 +166,7 @@ TEST_CASE("Smooth barrier potential full gradient and hessian 3D", tagsopt) CHECK(!collisions.empty()); CHECK(!has_intersections(mesh, vertices)); - SmoothContactPotential potential(params); + GCPPotential potential(params); std::cout << "energy: " << potential(collisions, mesh, vertices) << "\n"; // ------------------------------------------------------------------------- @@ -228,7 +228,7 @@ TEST_CASE("Smooth barrier potential full gradient and hessian 3D", tagsopt) CHECK((hess_b - fhess_b).norm() / hess_b.norm() < 1e-5); } -TEST_CASE("Smooth barrier potential real sim 2D C^2", "[smooth_potential]") +TEST_CASE("Smooth barrier potential real sim 2D C^2", "[gcp_potential]") { const auto method = make_default_broad_phase(); const bool adaptive_dhat = GENERATE(true, false); @@ -254,9 +254,9 @@ TEST_CASE("Smooth barrier potential real sim 2D C^2", "[smooth_potential]") // std::cout << "\n" << vertices << "\n" << edges << "\n"; CollisionMesh mesh; - SmoothContactParameters params(dhat, 0.9, -0.05, 0.95, 0.05, 1); + GCPParameters params(dhat, 0.9, -0.05, 0.95, 0.05, 1); params.set_adaptive_dhat_ratio(min_dist_ratio); - SmoothCollisions collisions; + GCPCollisions collisions; mesh = CollisionMesh( std::vector(vertices.rows(), true), std::vector(vertices.rows(), orientable), vertices, edges, faces); @@ -269,7 +269,7 @@ TEST_CASE("Smooth barrier potential real sim 2D C^2", "[smooth_potential]") CHECK(!has_intersections(mesh, vertices)); - SmoothContactPotential potential(params); + GCPPotential potential(params); std::cout << "energy: " << potential(collisions, mesh, vertices) << "\n"; // ------------------------------------------------------------------------- @@ -322,7 +322,7 @@ TEST_CASE("Smooth barrier potential real sim 2D C^2", "[smooth_potential]") // CHECK(fd::compare_hessian(hess_b, fhess_b, 1e-3)); } -TEST_CASE("Smooth barrier potential real sim 2D C^1", "[smooth_potential]") +TEST_CASE("Smooth barrier potential real sim 2D C^1", "[gcp_potential]") { const auto method = make_default_broad_phase(); const bool adaptive_dhat = GENERATE(true, false); @@ -346,9 +346,9 @@ TEST_CASE("Smooth barrier potential real sim 2D C^1", "[smooth_potential]") // std::cout << "\n" << vertices << "\n" << edges << "\n"; CollisionMesh mesh; - SmoothContactParameters params(dhat, 0.9, -0.05, 0.95, 0.05, 1); + GCPParameters params(dhat, 0.9, -0.05, 0.95, 0.05, 1); params.set_adaptive_dhat_ratio(min_dist_ratio); - SmoothCollisions collisions; + GCPCollisions collisions; mesh = CollisionMesh(vertices, edges, faces); collisions.compute_adaptive_dhat(mesh, vertices, params, method.get()); collisions.build(mesh, vertices, params, adaptive_dhat, method.get()); @@ -360,7 +360,7 @@ TEST_CASE("Smooth barrier potential real sim 2D C^1", "[smooth_potential]") CHECK(!has_intersections(mesh, vertices)); - SmoothContactPotential potential(params); + GCPPotential potential(params); std::cout << "energy: " << potential(collisions, mesh, vertices) << "\n"; // ------------------------------------------------------------------------- diff --git a/tests/src/tests/utils/CMakeLists.txt b/tests/src/tests/utils/CMakeLists.txt index c4386badf..972a0d5c3 100644 --- a/tests/src/tests/utils/CMakeLists.txt +++ b/tests/src/tests/utils/CMakeLists.txt @@ -3,6 +3,7 @@ set(SOURCES test_cuda_is_available.cpp test_local_to_global.cpp test_matrixcache.cpp + test_vertex_matrix_view.cpp test_profiler.cpp test_simd_utils.cpp test_utils.cpp diff --git a/tests/src/tests/utils/test_vertex_matrix_view.cpp b/tests/src/tests/utils/test_vertex_matrix_view.cpp new file mode 100644 index 000000000..5e2c2f703 --- /dev/null +++ b/tests/src/tests/utils/test_vertex_matrix_view.cpp @@ -0,0 +1,193 @@ +#include +#include + +#include + +using namespace ipc; +using Catch::Approx; + +TEST_CASE("VertexMatrixView single matrix", "[vertex_matrix_view]") +{ + Eigen::MatrixXd A(3, 3); + // clang-format off + A << 1, 2, 3, + 4, 5, 6, + 7, 8, 9; + // clang-format on + + VertexMatrixView<3> view(A); + + REQUIRE(view.rows() == 3); + REQUIRE(view.cols() == 3); + REQUIRE(view.m_b == nullptr); + + for (index_t i = 0; i < 3; i++) { + auto row = view(i); + for (int j = 0; j < 3; j++) { + CHECK(row(j) == Approx(A(i, j))); + } + } +} + +TEST_CASE("VertexMatrixView two-matrix concatenation", "[vertex_matrix_view]") +{ + Eigen::MatrixXd A(2, 3); + Eigen::MatrixXd B(3, 3); + // clang-format off + A << 1, 2, 3, + 4, 5, 6; + B << 7, 8, 9, + 10, 11, 12, + 13, 14, 15; + // clang-format on + + VertexMatrixView<3> view(A, B); + + REQUIRE(view.rows() == 5); + REQUIRE(view.cols() == 3); + REQUIRE(view.m_n_a_rows == 2); + REQUIRE(view.m_n_b_rows == 3); + + // Check rows from A + for (index_t i = 0; i < 2; i++) { + auto row = view(i); + for (int j = 0; j < 3; j++) { + CHECK(row(j) == Approx(A(i, j))); + } + } + + // Check rows from B + for (index_t i = 0; i < 3; i++) { + auto row = view(2 + i); + for (int j = 0; j < 3; j++) { + CHECK(row(j) == Approx(B(i, j))); + } + } +} + +TEST_CASE( + "VertexMatrixView concatenation matches naive vstack", + "[vertex_matrix_view]") +{ + // Build random matrices and verify the view matches manual stacking + const int nA = 4, nB = 5; + Eigen::MatrixXd A = Eigen::MatrixXd::Random(nA, 3); + Eigen::MatrixXd B = Eigen::MatrixXd::Random(nB, 3); + + Eigen::MatrixXd AB(nA + nB, 3); + AB.topRows(nA) = A; + AB.bottomRows(nB) = B; + + VertexMatrixView<3> view(A, B); + + REQUIRE(view.rows() == nA + nB); + + for (index_t i = 0; i < view.rows(); i++) { + auto row = view(i); + for (int j = 0; j < 3; j++) { + CHECK(row(j) == Approx(AB(i, j))); + } + } +} + +TEST_CASE("VertexMatrixView non-owning semantics", "[vertex_matrix_view]") +{ + Eigen::MatrixXd A(2, 3); + Eigen::MatrixXd B(1, 3); + A << 1, 2, 3, 4, 5, 6; + B << 7, 8, 9; + + VertexMatrixView<3> view(A, B); + + // The view should point to the original data + CHECK(view.m_a == A.data()); + CHECK(view.m_b == B.data()); + + // Mutate A and verify the view reflects the change + A(0, 0) = 99; + auto row = view(0); + CHECK(row(0) == Approx(99)); +} + +TEST_CASE("VertexMatrixView empty matrix B", "[vertex_matrix_view]") +{ + Eigen::MatrixXd A(3, 3); + A << 1, 2, 3, 4, 5, 6, 7, 8, 9; + Eigen::MatrixXd B(0, 3); + + VertexMatrixView<3> view(A, B); + + REQUIRE(view.rows() == 3); + REQUIRE(view.m_n_b_rows == 0); + + for (index_t i = 0; i < 3; i++) { + auto row = view(i); + for (int j = 0; j < 3; j++) { + CHECK(row(j) == Approx(A(i, j))); + } + } +} + +TEST_CASE( + "VertexMatrixView reads strided and row-major matrices", + "[vertex_matrix_view]") +{ + const Eigen::MatrixXd A = Eigen::MatrixXd::Random(4, 3); + const Eigen::MatrixXd B = Eigen::MatrixXd::Random(2, 3); + Eigen::MatrixXd AB(6, 3); + AB << A, B; + + // Embed A and B in taller matrices so each column is further from the + // next than the number of rows the view sees. + Eigen::MatrixXd A_big = Eigen::MatrixXd::Random(10, 3); + A_big.topRows(4) = A; + Eigen::MatrixXd B_big = Eigen::MatrixXd::Random(3, 5); + B_big.block<2, 3>(1, 2) = B; + const Eigen::Matrix A_row_major = A; + + const auto check_matches = [](const VertexMatrixView<3>& view, + const Eigen::MatrixXd& expected) { + REQUIRE(view.rows() == expected.rows()); + for (index_t i = 0; i < view.rows(); i++) { + CHECK(view(i) == expected.row(i)); + } + }; + + SECTION("Block") + { + check_matches(VertexMatrixView<3>(A_big.topRows(4)), A); + check_matches( + VertexMatrixView<3>(A_big.topRows(4), B_big.block<2, 3>(1, 2)), AB); + } + SECTION("Ref") + { + // A const Ref binds blocks without a copy, so the view must use the + // Ref's outer stride. + const Eigen::Ref A_ref = A_big.topRows(4); + REQUIRE(A_ref.data() == A_big.data()); + const VertexMatrixView<3> view(A_ref, B); + CHECK(view.m_a == A_big.data()); + check_matches(view, AB); + } + SECTION("Map with outer stride") + { + const Eigen::Map> A_map( + A_big.data(), 4, 3, Eigen::OuterStride<>(A_big.rows())); + check_matches(VertexMatrixView<3>(A_map), A); + } + SECTION("Row-major") + { + const VertexMatrixView<3> view(A_row_major, B); + CHECK(view.m_a == A_row_major.data()); + check_matches(view, AB); + } + SECTION("Row vector") + { + const Eigen::RowVector3d q = B.row(1); + const VertexMatrixView<3> view(A_big.topRows(4), q); + CHECK(view.m_b == q.data()); + Eigen::MatrixXd Aq(5, 3); + Aq << A, q; + check_matches(view, Aq); + } +}