diff --git a/CMakeLists.txt b/CMakeLists.txt index 66b98594c..54a12d61c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,6 +100,7 @@ option(IPC_TOOLKIT_WITH_ABSEIL "Use Abseil's hash functions" option(IPC_TOOLKIT_WITH_FILIB "Use filib for interval arithmetic" ON) option(IPC_TOOLKIT_WITH_INEXACT_CCD "Use the original inexact CCD method of IPC" OFF) option(IPC_TOOLKIT_WITH_PROFILER "Enable performance profiler" OFF) +option(IPC_TOOLKIT_WITH_GEOGRAM "Use geogram for predicates / expansion types" ON) option(IPC_TOOLKIT_WITH_TRACY "Enable Tracy frame profiler" OFF) option(IPC_TOOLKIT_WITH_MESHFEM_SPARSE "Use MeshFEMSparse for block-accelerated assembly" ON) @@ -302,6 +303,12 @@ if(IPC_TOOLKIT_WITH_FILIB) target_link_libraries(ipc_toolkit PUBLIC filib::filib) endif() +# Geogram +if(IPC_TOOLKIT_WITH_GEOGRAM) + include(geogram) + target_link_libraries(ipc_toolkit PRIVATE geogram::geogram) +endif() + # 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/recipes/geogram.cmake b/cmake/recipes/geogram.cmake new file mode 100644 index 000000000..cc30d14f1 --- /dev/null +++ b/cmake/recipes/geogram.cmake @@ -0,0 +1,24 @@ +# geogram (https://github.com/BrunoLevy/geogram) +# License: BSD 3-Clause License +if(TARGET geogram::geogram) + return() +endif() + +message(STATUS "Third-party: creating target 'geogram::geogram'") + +include(CPM) +CPMAddPackage( + URI "gh:BrunoLevy/geogram@1.10.1" + OPTIONS + "GEOGRAM_WITH_GRAPHICS OFF" + "GEOGRAM_WITH_LEGACY_NUMERICS OFF" + "GEOGRAM_WITH_HLBFGS OFF" + "GEOGRAM_WITH_TETGEN OFF" + "GEOGRAM_WITH_TRIANGLE OFF" + "GEOGRAM_WITH_LUA OFF" + "GEOGRAM_LIB_ONLY ON" +) + +if(NOT TARGET geogram::geogram AND TARGET geogram) + add_library(geogram::geogram ALIAS geogram) +endif() diff --git a/docs/source/_static/graphviz/dependencies.dot b/docs/source/_static/graphviz/dependencies.dot index f76aba605..5213cc2c1 100644 --- a/docs/source/_static/graphviz/dependencies.dot +++ b/docs/source/_static/graphviz/dependencies.dot @@ -98,4 +98,7 @@ digraph "IPC Toolkit Dependencies" { // MeshFEMSparse -> Eigen3_Eigen "node17" -> "node9" [color = "#8FB976";]; // MeshFEMSparse -> tbb + "node18" [label = "geogram\n(geogram::geogram)";shape = box;style = "rounded,filled";fillcolor = "#CCE7F8";color = "#6596B2";]; + "node5" -> "node18" [color = "#BE6562"; style = "dashed";]; + // ipc_toolkit -> geogram (IPC_TOOLKIT_WITH_GEOGRAM) } \ No newline at end of file diff --git a/docs/source/about/dependencies.rst b/docs/source/about/dependencies.rst index 8205991ce..d3149d78a 100644 --- a/docs/source/about/dependencies.rst +++ b/docs/source/about/dependencies.rst @@ -93,6 +93,12 @@ Additionally, IPC Toolkit may optionally use the following libraries: - `github.com/MeshFEM/MeshFEMSparse `_ - |:white_check_mark:| - ``IPC_TOOLKIT_WITH_MESHFEM_SPARSE`` + * - geogram + - Exact geometric predicates for the exact distance-type classification used by the ESP potential + - BSD-3-Clause + - `github.com/BrunoLevy/geogram `_ + - |:white_check_mark:| + - ``IPC_TOOLKIT_WITH_GEOGRAM`` * - nlohmann/json - JSON parsing for profiler and tests - MIT @@ -128,6 +134,9 @@ 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. With ``IPC_TOOLKIT_WITH_GEOGRAM`` set to ``OFF``, the same predicates are evaluated in floating point, still consistently across the classifiers, and a warning is logged the first time they are used. + .. 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/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 06206814d..a61c74585 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -98,6 +98,7 @@ PYBIND11_MODULE(ipctk, m) define_smooth_mu(m); define_smooth_potential(m); + define_esp_potential(m); // geometry define_angle(m); diff --git a/python/src/candidates/candidates.cpp b/python/src/candidates/candidates.cpp index be82f62cc..21601be7b 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,15 +21,16 @@ 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< const CollisionMesh&, Eigen::ConstRef, - Eigen::ConstRef, const double, BroadPhase*>( - &Candidates::build), + Eigen::ConstRef, const double, BroadPhase*, + const bool>(&Candidates::build), R"ipc_Qu8mg5v7( Initialize the set of continuous collision detection candidates. @@ -42,9 +43,11 @@ void define_candidates(py::module_& m) vertices_t1: Surface vertex ending 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_t0"_a, "vertices_t1"_a, - "inflation_radius"_a = 0, "broad_phase"_a = nullptr) + "inflation_radius"_a = 0, "broad_phase"_a = nullptr, + "all_types"_a = false) .def("__len__", &Candidates::size) .def("empty", &Candidates::empty) .def("clear", &Candidates::clear) diff --git a/python/src/collisions/normal/normal_collisions.cpp b/python/src/collisions/normal/normal_collisions.cpp index d394cbc98..7c15053c8 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."); } @@ -272,10 +320,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 +337,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. @@ -300,13 +348,19 @@ void define_normal_collisions(py::module_& m) The index of the primitive. )ipc_Qu8mg5v7", "i"_a); + // Deprecated name from before the smooth_contact -> gcp rename + m.attr("SmoothCollision2") = m.attr("GCPCollision2"); 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"); + // Deprecated name from before the smooth_contact -> gcp rename + m.attr("SmoothCollisions") = m.attr("GCPCollisions"); + + define_esp_collisions(m); } diff --git a/python/src/potentials/barrier_potential.cpp b/python/src/potentials/barrier_potential.cpp index 25224fc9c..dea954813 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,23 @@ 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."); + // Deprecated name from before the smooth_contact -> gcp rename + m.attr("SmoothContactParameters") = m.attr("GCPParameters"); - py::class_(m, "SmoothContactPotential") + py::class_(m, "GCPPotential") .def( - py::init(), + py::init(), R"ipc_Qu8mg5v7( Construct a smooth barrier potential. @@ -130,9 +132,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 +150,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 +168,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 +188,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 +204,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 +220,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. @@ -233,4 +234,116 @@ void define_smooth_potential(py::module_& m) )ipc_Qu8mg5v7", "collision"_a, "x"_a, "project_hessian_to_psd"_a = PSDProjectionMethod::NONE); + // Deprecated name from before the smooth_contact -> gcp rename + m.attr("SmoothContactPotential") = m.attr("GCPPotential"); +} + +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. + )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..566e5203d 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,53 @@ 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"), + ]: + 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..f1f88f7e4 100644 --- a/src/ipc/barrier/barrier.cpp +++ b/src/ipc/barrier/barrier.cpp @@ -5,9 +5,13 @@ // inequality constraints on a function. #include "barrier.hpp" +#include #include #include +#include +#include + namespace ipc { // ============================================================================ @@ -211,6 +215,147 @@ T TwoStageBarrier::second_derivative(const T d, const T dhat) const [&] { return T(0); }); } +// ============================================================================ + +void InversePowerBarrier::h_and_derivs( + const double d, const double dhat, double& h, double& dh, double& ddh) +{ + const double t = 2.0 * d / dhat; + double B, dB, ddB; + if (t < 1.0) { + B = 2.0 / 3.0 - t * t + 0.5 * t * t * t; + dB = -2.0 * t + 1.5 * t * t; + ddB = -2.0 + 3.0 * t; + } else if (t < 2.0) { + const double s = 2.0 - t; + B = s * s * s / 6.0; + dB = -s * s / 2.0; + ddB = s; + } else { + h = dh = ddh = 0.0; + return; + } + // h = 2*B(t), dh/dd = 2*B'(t)*(dt/dd) = 2*B'*(2/dhat) = 4/dhat * B' + h = 2.0 * B; + dh = 4.0 / dhat * dB; + ddh = 8.0 / (dhat * dhat) * ddB; +} + +double InversePowerBarrier::operator()(const double d, const double dhat) const +{ + if (d <= 0.0) { + return std::numeric_limits::infinity(); + } + double h, dh, ddh; + h_and_derivs(d, dhat, h, dh, ddh); + if (h == 0.0) { + return 0.0; + } + return h / std::pow(d, m_power); +} + +double +InversePowerBarrier::first_derivative(const double d, const double dhat) const +{ + if (d <= 0.0 || d >= dhat) { + return 0.0; + } + double h, dh, ddh; + h_and_derivs(d, dhat, h, dh, ddh); + // b'(d) = (dh·d − p·h) / d^(p+1) + return (dh * d - m_power * h) / std::pow(d, m_power + 1.0); +} + +double +InversePowerBarrier::second_derivative(const double d, const double dhat) const +{ + if (d <= 0.0 || d >= dhat) { + return 0.0; + } + double 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) + const double d2 = d * d; + return (ddh * d2 - 2.0 * m_power * dh * d + m_power * (m_power + 1.0) * h) + / std::pow(d, m_power + 2.0); +} + +// ============================================================================ + +double NearFarBarrier::near_value(const double d, const double dhat) const +{ + const double dhat_end = m_alpha * dhat; + const double dhat_start = -dhat_end / 2.0; + return (*m_base_barrier)(d, dhat) + * (1.0 - Math::smooth_heaviside(d, dhat_start, dhat_end)); +} + +double NearFarBarrier::far_value(const double d, const double dhat) const +{ + const double dhat_end = m_alpha * dhat; + const double dhat_start = -dhat_end / 2.0; + return (*m_base_barrier)(d, dhat) + * Math::smooth_heaviside(d, dhat_start, dhat_end); +} + +double +NearFarBarrier::first_derivative_near(const double d, const double dhat) const +{ + const double dhat_end = m_alpha * dhat; + const double dhat_start = -dhat_end / 2.0; + const double b = (*m_base_barrier)(d, dhat); + const double bp = m_base_barrier->first_derivative(d, dhat); + const double w = Math::smooth_heaviside(d, dhat_start, dhat_end); + const double wp = + Math::smooth_heaviside_grad(d, dhat_start, dhat_end); + return bp * (1.0 - w) - b * wp; +} + +double +NearFarBarrier::first_derivative_far(const double d, const double dhat) const +{ + const double dhat_end = m_alpha * dhat; + const double dhat_start = -dhat_end / 2.0; + const double b = (*m_base_barrier)(d, dhat); + const double bp = m_base_barrier->first_derivative(d, dhat); + const double w = Math::smooth_heaviside(d, dhat_start, dhat_end); + const double wp = + Math::smooth_heaviside_grad(d, dhat_start, dhat_end); + return bp * w + b * wp; +} + +double +NearFarBarrier::second_derivative_near(const double d, const double dhat) const +{ + const double dhat_end = m_alpha * dhat; + const double dhat_start = -dhat_end / 2.0; + const double b = (*m_base_barrier)(d, dhat); + const double bp = m_base_barrier->first_derivative(d, dhat); + const double bpp = m_base_barrier->second_derivative(d, dhat); + const double w = Math::smooth_heaviside(d, dhat_start, dhat_end); + const double wp = + Math::smooth_heaviside_grad(d, dhat_start, dhat_end); + const double wpp = + Math::smooth_heaviside_hess(d, dhat_start, dhat_end); + return bpp * (1.0 - w) - 2.0 * bp * wp - b * wpp; +} + +double +NearFarBarrier::second_derivative_far(const double d, const double dhat) const +{ + const double dhat_end = m_alpha * dhat; + const double dhat_start = -dhat_end / 2.0; + const double b = (*m_base_barrier)(d, dhat); + const double bp = m_base_barrier->first_derivative(d, dhat); + const double bpp = m_base_barrier->second_derivative(d, dhat); + const double w = Math::smooth_heaviside(d, dhat_start, dhat_end); + const double wp = + Math::smooth_heaviside_grad(d, dhat_start, dhat_end); + const double wpp = + Math::smooth_heaviside_hess(d, dhat_start, dhat_end); + return bpp * w + 2.0 * bp * wp + b * wpp; +} + // ============================================================================ // Explicit template instantiations /// @cond DOXYGEN_SKIP diff --git a/src/ipc/barrier/barrier.hpp b/src/ipc/barrier/barrier.hpp index de7456e32..b9810b948 100644 --- a/src/ipc/barrier/barrier.hpp +++ b/src/ipc/barrier/barrier.hpp @@ -6,6 +6,9 @@ #include +#include +#include + namespace ipc { /// Base class for barrier functions. @@ -399,4 +402,184 @@ 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$. +class InversePowerBarrier : public Barrier { +public: + /// @param power The power \f$p > 0\f$ controlling the singularity at d = 0. + explicit InversePowerBarrier(const double power) : m_power(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. + double operator()(const double d, const double 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. + double first_derivative(const double d, const double 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. + double second_derivative(const double d, const double 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. + double units(const double dhat) const override + { + return 1.0 / std::pow(dhat, m_power); + } + + /// @brief The power p used in the barrier. + double power() const { return m_power; } + +private: + double m_power; ///< p > 0 + + /// @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(double d, double dhat, double& h, double& dh, double& ddh); +}; + +/// @brief Near-Far barrier function. +/// This barrier function takes another "base" barrier object as argument. +/// All operations defined in Barrier 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$. 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. +class NearFarBarrier : public Barrier { +public: + /// @brief Construct a NearFarBarrier. + /// @param base_barrier The base barrier function to use. + /// @param alpha Split parameter: the far weight H rises from 0 at + /// d = alpha * dhat / 2 to 1 at d = alpha * dhat. + NearFarBarrier(const Barrier* const base_barrier, const double alpha) + : m_base_barrier(base_barrier) + , m_alpha(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_owned(std::move(base_barrier)) + , m_base_barrier(m_base_barrier_owned.get()) + , m_alpha(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. + double operator()(const double d, const double 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. + double first_derivative(const double d, const double 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. + double second_derivative(const double d, const double 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. + double units(const double 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. + double near_value(const double d, const double 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. + double far_value(const double d, const double 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. + double first_derivative_near(const double d, const double 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. + double first_derivative_far(const double d, const double 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. + double second_derivative_near(const double d, const double 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. + double second_derivative_far(const double d, const double dhat) const; + +private: + // Optional shared ownership; null when constructed from a raw pointer. + const std::shared_ptr m_base_barrier_owned; + const Barrier* const 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/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index 6d8bc9685..d45e3beb6 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -23,6 +24,22 @@ namespace ipc { +// Definition of the pimpl declared in candidates.hpp. Kept here so the public +// header does not need to include Abseil (a private dependency). +struct Candidates::AdjacencySets { + unordered_map> vv; + unordered_map> ve; + unordered_map> vf; + + unordered_map> ev; + unordered_map> ee; + unordered_map> ef; + + unordered_map> fv; + unordered_map> fe; + unordered_map> ff; +}; + namespace { // Pad codim_edges because remove_unreferenced requires a N×3 matrix. Eigen::MatrixXi pad_edges(Eigen::ConstRef E) @@ -44,7 +61,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,7 +78,7 @@ 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); // Codim. vertices to codim. vertices: if (mesh.num_codim_vertices()) { @@ -130,7 +148,8 @@ void Candidates::build( Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, const double inflation_radius, - BroadPhase* broad_phase) + BroadPhase* broad_phase, + const bool all_types) { IPC_TOOLKIT_PROFILE_BLOCK("Candidates::build(dynamic)"); @@ -493,7 +512,10 @@ void Candidates::clear() ev_candidates.clear(); ee_candidates.clear(); fv_candidates.clear(); + ef_candidates.clear(); + ff_candidates.clear(); pv_candidates.clear(); + m_sets.reset(); } CollisionStencil& Candidates::operator[](size_t i) @@ -717,4 +739,208 @@ bool Candidates::save_obj( return true; } +void Candidates::convert_candidates_to_sets() +{ + m_sets = std::make_shared(); + + for (const auto& vv : vv_candidates) { + m_sets->vv[vv.vertex0_id].insert(vv.vertex1_id); + m_sets->vv[vv.vertex1_id].insert(vv.vertex0_id); + } + for (const auto& ee : ee_candidates) { + m_sets->ee[ee.edge0_id].insert(ee.edge1_id); + m_sets->ee[ee.edge1_id].insert(ee.edge0_id); + } + for (const auto& ff : ff_candidates) { + m_sets->ff[ff.face0_id].insert(ff.face1_id); + m_sets->ff[ff.face1_id].insert(ff.face0_id); + } + for (const auto& ev : ev_candidates) { + m_sets->ev[ev.edge_id].insert(ev.vertex_id); + m_sets->ve[ev.vertex_id].insert(ev.edge_id); + } + for (const auto& fv : fv_candidates) { + m_sets->fv[fv.face_id].insert(fv.vertex_id); + m_sets->vf[fv.vertex_id].insert(fv.face_id); + } + for (const auto& ef : ef_candidates) { + m_sets->ef[ef.edge_id].insert(ef.face_id); + m_sets->fe[ef.face_id].insert(ef.edge_id); + } +} + +std::set +Candidates::vv_set(const CollisionMesh& mesh, index_t id) const +{ + if (!m_sets) { + return {}; + } + + std::set out; + if (auto iter = m_sets->vv.find(id); iter != m_sets->vv.end()) { + out = iter->second; + } + + if (mesh.dim() == 2) { + for (const index_t ej : ve_set(id)) { + out.insert(mesh.edges()(ej, 0)); + out.insert(mesh.edges()(ej, 1)); + } + } + out.erase(id); + return out; +} +std::set Candidates::ve_set(index_t id) const +{ + if (!m_sets) { + return {}; + } + + if (auto iter = m_sets->ve.find(id); iter != m_sets->ve.end()) { + return iter->second; + } + return {}; +} +std::set Candidates::vf_set(index_t id) const +{ + if (!m_sets) { + return {}; + } + + if (auto iter = m_sets->vf.find(id); iter != m_sets->vf.end()) { + return iter->second; + } + return {}; +} + +std::set +Candidates::ev_set(const CollisionMesh& mesh, index_t id) const +{ + if (!m_sets) { + return {}; + } + + std::set out; + if (auto iter = m_sets->ev.find(id); iter != m_sets->ev.end()) { + out = iter->second; + } + for (index_t lv = 0; lv < 2; ++lv) { + out.insert(mesh.edges()(id, lv)); + } + return out; +} +std::set +Candidates::ee_set(const CollisionMesh& mesh, index_t id) const +{ + if (!m_sets) { + return {}; + } + + std::set out; + if (auto iter = m_sets->ee.find(id); iter != m_sets->ee.end()) { + out = iter->second; + } + for (index_t lv = 0; lv < 2; ++lv) { + for (index_t eid : mesh.vertices_to_edges()[mesh.edges()(id, lv)]) { + out.insert(eid); + } + } + // In 2D, EE candidates are never built by the broad phase. Reconstruct + // them from EV candidates symmetrically: + // (a) edges adjacent to vertices that are close to edge id (via ev_set) + // (b) edges that id's own endpoints are close to (via ve_set) + if (mesh.dim() == 2) { + for (const index_t vj : ev_set(mesh, id)) { + for (const index_t ej : mesh.vertices_to_edges()[vj]) { + out.insert(ej); + } + } + for (index_t lv = 0; lv < 2; ++lv) { + const index_t vi = mesh.edges()(id, lv); + for (const index_t ej : ve_set(vi)) { + out.insert(ej); + } + } + } + out.erase(id); + return out; +} +std::set +Candidates::ef_set(const CollisionMesh& mesh, index_t id) const +{ + if (!m_sets) { + return {}; + } + + std::set out; + if (auto iter = m_sets->ef.find(id); iter != m_sets->ef.end()) { + out = iter->second; + } + for (index_t lv = 0; lv < 2; ++lv) { + const auto& faces = mesh.vertices_to_faces()[mesh.edges()(id, lv)]; + for (int fid : faces) { + out.insert(fid); + } + } + for (const index_t fid : mesh.edges_to_faces()[id]) { + out.erase(fid); + } + return out; +} + +std::set +Candidates::fv_set(const CollisionMesh& mesh, index_t id) const +{ + if (!m_sets) { + return {}; + } + + std::set out; + if (auto iter = m_sets->fv.find(id); iter != m_sets->fv.end()) { + out = iter->second; + } + for (index_t lv = 0; lv < 3; ++lv) { + out.insert(mesh.faces()(id, lv)); + } + return out; +} +std::set +Candidates::fe_set(const CollisionMesh& mesh, index_t id) const +{ + if (!m_sets) { + return {}; + } + + std::set out; + if (auto iter = m_sets->fe.find(id); iter != m_sets->fe.end()) { + out = iter->second; + } + for (index_t lv = 0; lv < 3; ++lv) { + for (index_t eid : mesh.vertices_to_edges()[mesh.faces()(id, lv)]) { + out.insert(eid); + } + } + return out; +} +std::set +Candidates::ff_set(const CollisionMesh& mesh, index_t id) const +{ + if (!m_sets) { + return {}; + } + + std::set out; + if (auto iter = m_sets->ff.find(id); iter != m_sets->ff.end()) { + out = iter->second; + } + for (index_t lv = 0; lv < 3; ++lv) { + const index_t vid = mesh.faces()(id, lv); + for (index_t fid : mesh.vertices_to_faces()[vid]) { + out.insert(fid); + } + } + out.erase(id); + return out; +} + } // namespace ipc diff --git a/src/ipc/candidates/candidates.hpp b/src/ipc/candidates/candidates.hpp index a52ae6bb0..9a2c07495 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -9,6 +9,8 @@ #include +#include +#include #include namespace ipc { @@ -23,11 +25,14 @@ 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). 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. @@ -36,12 +41,15 @@ class Candidates { /// @param vertices_t1 Surface vertex ending 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). void build( const CollisionMesh& mesh, Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, const double inflation_radius = 0, - BroadPhase* broad_phase = nullptr); + BroadPhase* broad_phase = nullptr, + const bool all_types = false); /// @brief Get the number of collision candidates. /// @return The number of collision candidates. @@ -236,6 +244,31 @@ class Candidates { Eigen::ConstRef edges, Eigen::ConstRef faces) const; + /// @brief Build per-primitive adjacency sets from the candidates. + /// @note Required before calling any of the *_set() queries below. + void convert_candidates_to_sets(); + + /// @brief Vertices near vertex id (2D: also the endpoints of nearby edges). + std::set vv_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Edges near vertex id. + std::set ve_set(index_t id) const; + /// @brief Faces near vertex id. + std::set vf_set(index_t id) const; + + /// @brief Vertices near edge id, including its endpoints. + std::set ev_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Edges near or adjacent to edge id, excluding id. + std::set ee_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Faces near or adjacent to edge id, excluding faces containing id. + std::set ef_set(const CollisionMesh& mesh, index_t id) const; + + /// @brief Vertices near face id, including its corners. + std::set fv_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Edges near or adjacent to face id. + std::set fe_set(const CollisionMesh& mesh, index_t id) const; + /// @brief Faces near or adjacent to face id, excluding id. + std::set ff_set(const CollisionMesh& mesh, index_t id) const; + public: std::vector vv_candidates; std::vector ev_candidates; @@ -243,8 +276,20 @@ class Candidates { std::vector fv_candidates; std::vector pv_candidates; + std::vector ef_candidates; + std::vector ff_candidates; + private: static bool default_is_active(double candidate) { return true; } + + /// @brief Adjacency sets built by convert_candidates_to_sets(). + /// + /// Held behind a pointer so this public header does not need + /// ipc/utils/unordered_map_and_set.hpp, which pulls in Abseil -- a private + /// dependency of the library that consumers (e.g. the Python bindings) do + /// not link against. + struct AdjacencySets; + std::shared_ptr m_sets; }; } // 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 c57fa15cd..04d114d72 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -32,6 +32,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 +100,12 @@ CollisionMesh::CollisionMesh( m_is_orient_vertex[i] = orient_vertex[m_vertex_to_full_vertex[i]]; } + assert(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 +349,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 +360,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; } } } diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index 402d0a0a3..fa19f439b 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -38,6 +38,34 @@ 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. + /// @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. + 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 +138,45 @@ 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 This checks if all vertices of the edge are obstacle vertices. + /// @throws std::runtime_error if some but not all vertices are obstacle vertices. + bool is_obstacle_edge(const index_t i) const + { + const auto& edge_v_indices = edges().row(i); + const bool v0_is_obstacle = is_obstacle_vertex(edge_v_indices(0)); + const bool v1_is_obstacle = is_obstacle_vertex(edge_v_indices(1)); + + if (v0_is_obstacle != v1_is_obstacle) { + throw std::runtime_error( + "Edge has a mix of obstacle and non-obstacle vertices."); + } + return v0_is_obstacle; + } + + /// @brief Check if face i is from an obstacle. + /// @note This checks if all vertices of the face are obstacle vertices. + /// @throws std::runtime_error if some but not all vertices are obstacle vertices. + bool is_obstacle_face(const index_t i) const + { + const auto& face_v_indices = faces().row(i); + const bool v0_is_obstacle = is_obstacle_vertex(face_v_indices(0)); + const bool v1_is_obstacle = is_obstacle_vertex(face_v_indices(1)); + const bool v2_is_obstacle = is_obstacle_vertex(face_v_indices(2)); + if ((v0_is_obstacle != v1_is_obstacle) + || (v1_is_obstacle != v2_is_obstacle)) { + throw std::runtime_error( + "Face has a mix of obstacle and non-obstacle vertices."); + } + return v0_is_obstacle; + } + /// @brief Get the indices of codimensional edges of the collision mesh (|CE| × 1). const Eigen::VectorXi& codim_edges() const { return m_codim_edges; } @@ -288,6 +355,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 Get the gradient of the barycentric area of a vertex wrt the rest positions of all points. /// @param vi Vertex ID. /// @return Gradient of the barycentric area of vertex vi wrt the rest positions of all points. @@ -391,6 +461,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|). @@ -452,6 +524,10 @@ class CollisionMesh { /// 3D: 1/3 sum of area of connected triangles Eigen::VectorXd m_edge_areas; + /// @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..ae9526aa4 100644 --- a/src/ipc/collisions/normal/normal_collisions.cpp +++ b/src/ipc/collisions/normal/normal_collisions.cpp @@ -52,7 +52,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) { diff --git a/src/ipc/collisions/normal/normal_collisions.hpp b/src/ipc/collisions/normal/normal_collisions.hpp index 47347e04d..783148074 100644 --- a/src/ipc/collisions/normal/normal_collisions.hpp +++ b/src/ipc/collisions/normal/normal_collisions.hpp @@ -188,10 +188,26 @@ 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: pairs whose + /// vertex is an obstacle and edge-edge pairs between two obstacle edges + /// are dropped, and edge-edge pairs with one obstacle edge get half weight. + /// @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..48156bf2c 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) { } @@ -64,6 +66,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 +152,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()); @@ -178,7 +191,7 @@ void NormalCollisionsBuilder::add_edge_edge_collision( // ÷ 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 + double weight = use_area_weighting ? (0.25 * (mesh.edge_area(eai) + mesh.edge_area(ebi))) : 1; @@ -190,6 +203,13 @@ void NormalCollisionsBuilder::add_edge_edge_collision( : Eigen::SparseVector(vertices.size()); } + if (is_obstacle_ea || is_obstacle_eb) { + weight /= 2; + if (enable_shape_derivatives) { + weight_gradient /= 2; + } + } + switch (dtype) { case EdgeEdgeDistanceType::EA0_EB0: add_vertex_vertex_collision(ea0i, eb0i, weight, weight_gradient); @@ -243,6 +263,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); 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..128ea0e6d 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.cpp +++ b/src/ipc/collisions/tangential/tangential_collisions.cpp @@ -173,8 +173,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 +201,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 +217,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 +243,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 +282,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 +308,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 +328,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 +354,7 @@ void TangentialCollisions::build( ptr = &(FC_ev.back()); } if (ptr) { - ptr->smooth_collision = collisions.collisions[i]; + ptr->gcp_collision = collisions.collisions[i]; } } } diff --git a/src/ipc/collisions/tangential/tangential_collisions.hpp b/src/ipc/collisions/tangential/tangential_collisions.hpp index e3d9ff8ed..85d08c102 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,11 +94,36 @@ 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 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. + /// @param blend_mu Function to blend vertex-based coefficients of friction. Defaults to average. + 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 bool normalize_weights = true, const std::function& blend_mu = default_blend_mu); diff --git a/src/ipc/config.hpp.in b/src/ipc/config.hpp.in index e901e9eb4..7bcb7c9e5 100644 --- a/src/ipc/config.hpp.in +++ b/src/ipc/config.hpp.in @@ -19,6 +19,7 @@ #cmakedefine IPC_TOOLKIT_WITH_FILIB #cmakedefine IPC_TOOLKIT_WITH_PROFILER #cmakedefine IPC_TOOLKIT_WITH_TRACY +#cmakedefine IPC_TOOLKIT_WITH_GEOGRAM #cmakedefine IPC_TOOLKIT_WITH_MESHFEM_SPARSE // #define IPC_TOOLKIT_DEBUG_AUTODIFF diff --git a/src/ipc/distance/CMakeLists.txt b/src/ipc/distance/CMakeLists.txt index 9ede9e1e7..8c3159cc4 100644 --- a/src/ipc/distance/CMakeLists.txt +++ b/src/ipc/distance/CMakeLists.txt @@ -1,5 +1,7 @@ set(SOURCES distance_type.hpp + distance_type_exact.cpp + distance_type_exact.hpp edge_edge.hpp edge_edge_mollifier.hpp line_line.hpp diff --git a/src/ipc/distance/distance_type_exact.cpp b/src/ipc/distance/distance_type_exact.cpp new file mode 100644 index 000000000..ea7235f3e --- /dev/null +++ b/src/ipc/distance/distance_type_exact.cpp @@ -0,0 +1,402 @@ +#include "distance_type_exact.hpp" + +#include + +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_GEOGRAM +#include "fp_filters.h" + +#include +// geogram 1.10 no longer pulls this in transitively via exact_geometry.h. +#include +#endif + +namespace ipc { + +namespace { + +#ifdef IPC_TOOLKIT_WITH_GEOGRAM + // 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 geogram's PCK + // 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 = GEO::expansion_nt; // exact scalar type + using ExVec3 = GEO::vec3E; // exact vector type + + void init_predicates() + { + struct PckInit { + PckInit() { GEO::PCK::initialize(); } + }; + static PckInit _; + } + + /// @brief Exact copy of a 2D or 3D point (z = 0 in 2D). + ExVec3 make_exact(const double* v, const int dim) + { + return ExVec3(ExReal(v[0]), ExReal(v[1]), ExReal(dim == 3 ? v[2] : 0)); + } + + int exact_sign(const ExReal& s) { return (s > 0) ? 1 : ((s < 0) ? -1 : 0); } + + // -- 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(dot(p1 - p0, 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( + dot(cross(p1 - p0, p2 - p0), cross(p3 - p0, 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( + dot(cross(p1 - p0, p2 - p0), cross(p3 - p0, 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); + } + +#else + // Without geogram, the same predicates are evaluated in double precision. + // Their signs are not exact, but the classifiers below are built only from + // them, so the classifications stay mutually consistent as long as each + // predicate returns the same sign for the same arguments at every call + // site. E.g., a point-triangle P_E0 implies a point-edge P_E on that edge, + // which ESP asserts. Each predicate is kept out of line so that the + // compiler cannot contract (FMA) its arithmetic differently per call site. + +#if defined(_MSC_VER) +#define IPC_DISTANCE_TYPE_NOINLINE __declspec(noinline) +#else +#define IPC_DISTANCE_TYPE_NOINLINE __attribute__((noinline)) +#endif + + void init_predicates() + { + static const bool warned = [] { + logger().warn( + "IPC Toolkit was built without geogram, so the " + "*_distance_type_exact functions use floating-point " + "predicates: consistent with each other, but not exact. " + "Build with IPC_TOOLKIT_WITH_GEOGRAM=ON for exact " + "predicates."); + return true; + }(); + (void)warned; + } + + int sign(const double x) { return (x > 0) - (x < 0); } + + /// @brief Sign of dot(p1-p0, p2-p0). + template + IPC_DISTANCE_TYPE_NOINLINE int + dot3(const double* p0, const double* p1, const double* p2) + { + double s = 0; + for (int i = 0; i < dim; ++i) { + s += (p1[i] - p0[i]) * (p2[i] - p0[i]); + } + return sign(s); + } + + using Vec3 = std::array; + + /// @brief b - a. + Vec3 sub(const double* b, const double* a) + { + return { { b[0] - a[0], b[1] - a[1], b[2] - a[2] } }; + } + + /// @brief Sign of dot(cross(a, b), cross(c, d)). + int + cross_dot_cross(const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& d) + { + const double n0 = a[1] * b[2] - a[2] * b[1]; + const double n1 = a[2] * b[0] - a[0] * b[2]; + const double n2 = a[0] * b[1] - a[1] * b[0]; + const double m0 = c[1] * d[2] - c[2] * d[1]; + const double m1 = c[2] * d[0] - c[0] * d[2]; + const double m2 = c[0] * d[1] - c[1] * d[0]; + return sign(n0 * m0 + n1 * m1 + n2 * m2); + } + + /// @brief Sign of dot(cross(p1-p0, p2-p0), cross(p3-p0, p1-p0)). + IPC_DISTANCE_TYPE_NOINLINE int cross_dot_cross_1( + const double* p0, const double* p1, const double* p2, const double* p3) + { + const Vec3 u = sub(p1, p0); + return cross_dot_cross(u, sub(p2, p0), sub(p3, p0), u); + } + + /// @brief Sign of dot(cross(p1-p0, p2-p0), cross(p3-p0, p1-p2)). + IPC_DISTANCE_TYPE_NOINLINE int cross_dot_cross_2( + const double* p0, const double* p1, const double* p2, const double* p3) + { + return cross_dot_cross( + sub(p1, p0), sub(p2, p0), sub(p3, p0), sub(p1, p2)); + } + +#undef IPC_DISTANCE_TYPE_NOINLINE +#endif + + // -- 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) +{ +#ifdef IPC_TOOLKIT_WITH_GEOGRAM + 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); + const ExReal cross_norm_sqr = cross(ea1 - ea0, eb1 - eb0).length2(); + return cross_norm_sqr == 0; + } else { + return is_almost_parallel_edge_edge(_ea0, _ea1, _eb0, _eb1); + } +#else + // Without geogram the exact test is unavailable; PARALLEL_THRESHOLD must + // be non-zero for the thresholded test to be meaningful. + static_assert( + PARALLEL_THRESHOLD != 0.0, + "PARALLEL_THRESHOLD == 0 requires the exact predicates (geogram)."); + return is_almost_parallel_edge_edge(_ea0, _ea1, _eb0, _eb1); +#endif +} + +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..e98ce14ff --- /dev/null +++ b/src/ipc/distance/distance_type_exact.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +constexpr double PARALLEL_THRESHOLD { + 2.5e-16 +}; // TODO set to zero eventually (requires geogram) + +/// @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 when IPC Toolkit is built with geogram +/// (IPC_TOOLKIT_WITH_GEOGRAM) and evaluated in floating point otherwise. Either +/// way, 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 sign +/// predicates that are exact when geogram is available (see +/// DistanceTypeConfig), 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 sign +/// predicates that are exact when geogram is available (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 sign predicates +/// that are exact when geogram is available (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/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..e9f1742bc --- /dev/null +++ b/src/ipc/esp/CMakeLists.txt @@ -0,0 +1,27 @@ +set(SOURCES + arbitrary_point_bvh.cpp + arbitrary_point_bvh.hpp + 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 + 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_bvh.cpp b/src/ipc/esp/arbitrary_point_bvh.cpp new file mode 100644 index 000000000..d2b8bc06f --- /dev/null +++ b/src/ipc/esp/arbitrary_point_bvh.cpp @@ -0,0 +1,121 @@ +#include "arbitrary_point_bvh.hpp" + +#include +#include + +namespace ipc { + +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 aabb_min/aabb_max are meaningful; the union fields are unused + /// by intersects() but are set to keep the node well-defined. + /// + /// A 2D point is padded to (x, y, 0), which is what AABB does to 2D + /// primitives (it stores Array3d and zero-fills the unused component), + /// so the resulting z range [-radius, radius] straddles the tree's. + LBVH::Node + point_query_node(Eigen::ConstRef p, 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 Stack-based traversal of a single LBVH tree against one query + /// box, collecting the primitive ids of every leaf whose AABB + /// intersects the query. LBVH's own traversal (lbvh.cpp) is a + /// file-local template used for tree-vs-tree queries; this is the + /// tree-vs-single-external-box equivalent, which LBVH does not expose. + void query_point_vs_bvh( + const LBVH::Nodes& bvh, + const LBVH::Node& query, + std::vector& hits) + { + if (bvh.empty()) { + return; + } + + // A single-primitive tree is one node, and that node is a leaf, so + // the descend-from-an-inner-root loop below would read the union's + // left/right through a leaf. Easy to hit in 2D (a one-edge mesh). + if (bvh.size() == 1) { + if (bvh[0].intersects(query)) { + hits.push_back(bvh[0].primitive_id); + } + return; + } + + constexpr int MAX_STACK = 64; + int stack[MAX_STACK]; + int sp = 0; + stack[sp++] = LBVH::Node::INVALID_POINTER; + + int node_idx = 0; // root is always index 0 + do { + const LBVH::Node& node = bvh[node_idx]; + assert(node.is_inner()); + + const LBVH::Node& cl = bvh[node.left]; + const LBVH::Node& cr = bvh[node.right]; + const bool hl = cl.intersects(query); + const bool hr = cr.intersects(query); + + if (hl && cl.is_leaf()) { + hits.push_back(cl.primitive_id); + } + if (hr && cr.is_leaf()) { + hits.push_back(cr.primitive_id); + } + + const bool tl = hl && !cl.is_leaf(); + const bool tr = hr && !cr.is_leaf(); + + if (!tl && !tr) { + node_idx = stack[--sp]; + } else { + node_idx = tl ? node.left : node.right; + if (tl && tr) { + assert(sp < MAX_STACK); + stack[sp++] = node.right; + } + } + } while (node_idx != LBVH::Node::INVALID_POINTER); + } + +} // namespace + +void ArbitraryPointBVH::update( + Eigen::ConstRef V, const CollisionMesh& mesh) +{ + // Zero inflation: the query radius is applied to the query point's own + // box at query time instead, so a single build serves queries at any + // radius. + bvh.build(V, mesh.edges(), mesh.faces(), /*inflation_radius=*/0.0); +} + +void ArbitraryPointBVH::query_point( + Eigen::ConstRef q, + double radius, + std::vector& vertex_ids, + std::vector& edge_ids, + std::vector& face_ids) const +{ + vertex_ids.clear(); + edge_ids.clear(); + face_ids.clear(); + + const LBVH::Node qnode = point_query_node(q, radius); + query_point_vs_bvh(bvh.vertex_nodes(), qnode, vertex_ids); + query_point_vs_bvh(bvh.edge_nodes(), qnode, edge_ids); + query_point_vs_bvh(bvh.face_nodes(), qnode, face_ids); +} + +} // namespace ipc diff --git a/src/ipc/esp/arbitrary_point_bvh.hpp b/src/ipc/esp/arbitrary_point_bvh.hpp new file mode 100644 index 000000000..c417fd67d --- /dev/null +++ b/src/ipc/esp/arbitrary_point_bvh.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include + +namespace ipc { + +/// @brief Broad-phase index over a mesh's vertices/edges/faces, built once +/// per vertex configuration and queried many times for arbitrary points in +/// space. +/// +/// Wraps ipc::LBVH, reusing its Node/Nodes data structures and build() as +/// they are; adds the point-vs-tree query LBVH's public API does not expose +/// (LBVH's own traversal is a file-local template, not part of its public +/// interface). +/// +/// The tree is built with zero box inflation (mirroring the pattern used by +/// the potential-sampling tools this class is ported from): the query +/// radius is applied to the query point's own box instead. This decouples +/// the built tree from any particular dhat, so one build serves queries at +/// any radius. +class ArbitraryPointBVH { +public: + /// @brief Rebuild the tree. O(n log n). Call once per vertex + /// configuration (i.e. whenever V changes). + void update(Eigen::ConstRef V, const CollisionMesh& mesh); + + /// @brief Find real mesh primitives whose (uninflated) AABB is within + /// `radius` of q, i.e. intersects a box of half-width `radius` centered + /// at q. Broad-phase only: callers still need to check exact distances. + /// @param q Query point (2D or 3D, matching the mesh). + /// @param radius Half-width of the query box around q. + /// @param[out] vertex_ids Real vertex ids found nearby. + /// @param[out] edge_ids Real edge ids found nearby. + /// @param[out] face_ids Real face ids found nearby. Always empty in 2D, + /// where mesh.faces() is empty and 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; + +private: + LBVH bvh; +}; + +} // namespace ipc diff --git a/src/ipc/esp/arbitrary_point_esp.cpp b/src/ipc/esp/arbitrary_point_esp.cpp new file mode 100644 index 000000000..072a8c74a --- /dev/null +++ b/src/ipc/esp/arbitrary_point_esp.cpp @@ -0,0 +1,485 @@ +#include "arbitrary_point_esp.hpp" + +#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; + case PointTriangleDistanceType::P_T: + insert_by_value(s.fv, FV(fi, vid, mesh), weight); + break; + case PointTriangleDistanceType::AUTO: + default: + assert(false); + 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 ArbitraryPointBVH& 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) + : mesh(_mesh) + , params(std::move(_params)) +{ + if (mesh.dim() != dim) { + log_and_throw_error( + "ArbitraryPointESP<{}> requires a {}D mesh (got {}D)!", dim, dim, + mesh.dim()); + } + + // Inclusion-exclusion weights (supplemental S2): the weights of the + // elements containing any point of the mesh sum to one. Faces take one, so + // an edge takes 1 - (faces on it) and a vertex 1 - (edges at it) + (faces + // at it), each face at a vertex having two of its edges there. A closed + // mesh gets the alternating +1/-1/+1 (2D: edges +1, vertices -1). A + // boundary edge or vertex, and an open polyline's end, gets zero (S4); + // with the closed-mesh signs instead, the potential is exactly zero beyond + // a straight boundary edge of an open sheet, where it should be b(d). An + // edge in no face and an isolated vertex get one. + 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)]; + } +} + +template +void ArbitraryPointESP::update(Eigen::ConstRef V) +{ + point_bvh.update(V, mesh); +} + +template +double ArbitraryPointESP::operator()( + Eigen::ConstRef V, Eigen::ConstRef q) const +{ + const PointCollisions& collisions = build_point_collisions( + point_bvh, mesh, params, edge_weights, 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), params); + }); + return value; +} + +template +auto ArbitraryPointESP::gradient( + Eigen::ConstRef V, Eigen::ConstRef q) const + -> Gradient +{ + const PointCollisions& collisions = build_point_collisions( + point_bvh, mesh, params, edge_weights, 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), 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( + point_bvh, mesh, params, edge_weights, 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), 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( + point_bvh, mesh, params, edge_weights, 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, params); + + const VectorMax g = + cc.weight * cc.gradient(dof, params); + grad += g.template segment(j); + + H += (cc.weight * cc.hessian(dof, 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..fb9193088 --- /dev/null +++ b/src/ipc/esp/arbitrary_point_esp.hpp @@ -0,0 +1,100 @@ +#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 ArbitraryPointBVH +/// 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& mesh; + ESPParameters params; + ArbitraryPointBVH point_bvh; + /// Inclusion-exclusion weight of each edge and each vertex (every face + /// has weight one), from the mesh connectivity; see the constructor. + std::vector edge_weights, 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..637207b06 --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision.cpp @@ -0,0 +1,56 @@ +#include "esp_collision.hpp" + +#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 { + throw std::runtime_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..f0ec82c0d --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision.hpp @@ -0,0 +1,160 @@ +#pragma once + +#include "esp_primitives.hpp" +#include "vertex_matrix_view.hpp" + +#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: + static constexpr int MAX_VERT_3D = 20 * 2; + static constexpr int ELEMENT_SIZE = 3 * MAX_VERT_3D; + + 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 centers 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 + { + return { 0.0, 0.0 }; + } + + virtual std:: + pair, VectorMax> + gradient_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier* nf_barrier) const + { + VectorMax zero = + VectorMax::Zero(positions.size()); + return { zero, zero }; + } + + virtual std::pair< + MatrixMax, + MatrixMax> + hessian_nearfar( + Eigen::ConstRef> positions, + const ESPParameters& params, + const NearFarBarrier* nf_barrier) const + { + int n = positions.size(); + MatrixMax zero = + MatrixMax::Zero(n, n); + return { zero, zero }; + } + + 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..5bf77fb95 --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision_dict.cpp @@ -0,0 +1,184 @@ +#include "esp_collision_dict.hpp" + +#include + +#include +#include + +namespace ipc { +template +void ESPCollisionDict::initialize( + const std::vector& primitive_ids, + const std::vector& primary_vertex_ids, + const ESPCollisionPairMap& map) +{ + 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(m_primitive_ids.size() <= m_primary_vertex_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]); + } + + // 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] = m_vertex_ids[i] * DIMENSION + 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(i * DIMENSION + 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); + vv_collisions.push_back(*ptr); + } else { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + vv_collisions.push_back(*ptr); + } + break; + case ESPCollisionType::EDGE_VERTEX: + if constexpr (DIM == 2) { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + ev_collisions.push_back(*ptr); + } else { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + ev_collisions.push_back(*ptr); + } + break; + case ESPCollisionType::FACE_VERTEX: + if constexpr (DIM == 3) { + auto ptr = std::dynamic_pointer_cast< + ESPCollisionTemplate>(val); + assert(ptr); + 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 < vv_collisions.size()) { + return vv_collisions[i]; + } else { + i -= vv_collisions.size(); + if (i < ev_collisions.size()) { + return ev_collisions[i]; + } else { + i -= ev_collisions.size(); + if (i < fv_collisions.size()) { + return 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..266b7bd03 --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision_dict.hpp @@ -0,0 +1,143 @@ +#pragma once +#include "esp_collision_template.hpp" + +#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; + + void initialize( + const std::vector& primitive_ids, + const std::vector& primary_vertex_ids, + const ESPCollisionPairMap& map); + + 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 vv_collisions.size() + ev_collisions.size() + + 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 + 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 vv_collisions; + std::vector ev_collisions; + std::vector> + 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..03364d430 --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision_template.cpp @@ -0,0 +1,892 @@ +#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) + : primitive_a(_primitive0, mesh) + , primitive_b(_primitive1, mesh) +{ +} + +template <> +ESPCollisionTemplate::ESPCollisionTemplate( + index_t _primitive0, index_t _primitive1, const CollisionMesh& mesh) + : primitive_a(std::min(_primitive0, _primitive1), mesh) + , primitive_b(std::max(_primitive0, _primitive1), mesh) +{ +} + +// ---- vertex_id ---- + +template +index_t ESPCollisionTemplate::vertex_id(index_t i) const +{ + if (i < static_cast(primitive_a.n_vertices())) { + return primitive_a.vertex_id(i); + } + i -= primitive_a.n_vertices(); + assert((index_t)primitive_b.n_vertices() > i); + return 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 VectorMax zero = + VectorMax::Zero(positions.size()); + return { zero, zero }; +} + +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(); + const MatrixMax zero = + MatrixMax::Zero(n, n); + return { zero, zero }; +} + +// ---- 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 edge_edge_distance( + vertices.row(vertex_id(0)), vertices.row(vertex_id(1)), + vertices.row(vertex_id(2)), vertices.row(vertex_id(3))); + } + 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; + params.record_dist(dist); + 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))); + params.record_dist(dist); + 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))); + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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>()); + VectorMax g_near(6), g_far(6); + g_near.head(6) = deriv_near * g; + g_far.head(6) = deriv_far * g; + return { g_near, g_far }; +} + +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; + params.record_dist(dist); + 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); + Vector9d g_near = deriv_near * g; + Vector9d g_far = deriv_far * g; + g_near = g_near({ 3, 4, 5, 6, 7, 8, 0, 1, 2 }).eval(); + g_far = g_far({ 3, 4, 5, 6, 7, 8, 0, 1, 2 }).eval(); + VectorMax result_near(9), result_far(9); + result_near.head(9) = g_near; + result_far.head(9) = g_far; + return { result_near, result_far }; +} + +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; + params.record_dist(dist); + 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); + Vector12d g_near = deriv_near * g; + Vector12d g_far = deriv_far * g; + g_near = g_near({ 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2 }).eval(); + g_far = g_far({ 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2 }).eval(); + VectorMax result_near(12), + result_far(12); + result_near.head(12) = g_near; + result_far.head(12) = g_far; + return { result_near, result_far }; +} + +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; + params.record_dist(dist); + 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>()); + Matrix6d hess_near = g * deriv2_near * g.transpose() + h * deriv1_near; + Matrix6d hess_far = g * deriv2_far * g.transpose() + h * deriv1_far; + MatrixMax + result_near(6, 6), result_far(6, 6); + result_near.block<6, 6>(0, 0) = hess_near; + result_far.block<6, 6>(0, 0) = hess_far; + return { result_near, result_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; + params.record_dist(dist); + 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); + Matrix9d hess_near = g * deriv2_near * g.transpose() + h * deriv1_near; + Matrix9d hess_far = g * deriv2_far * g.transpose() + h * deriv1_far; + std::vector reorder { 3, 4, 5, 6, 7, 8, 0, 1, 2 }; + hess_near = hess_near(reorder, reorder).eval(); + hess_far = hess_far(reorder, reorder).eval(); + MatrixMax + result_near(9, 9), result_far(9, 9); + result_near.block<9, 9>(0, 0) = hess_near; + result_far.block<9, 9>(0, 0) = hess_far; + return { result_near, result_far }; +} + +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; + params.record_dist(dist); + 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); + Matrix12d hess_near = g * deriv2_near * g.transpose() + h * deriv1_near; + Matrix12d hess_far = g * deriv2_far * g.transpose() + h * deriv1_far; + std::vector reorder { 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2 }; + hess_near = hess_near(reorder, reorder).eval(); + hess_far = hess_far(reorder, reorder).eval(); + MatrixMax + result_near(12, 12), result_far(12, 12); + result_near.block<12, 12>(0, 0) = hess_near; + result_far.block<12, 12>(0, 0) = hess_far; + return { result_near, result_far }; +} + +// ---- 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; + params.record_dist(dist); + 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))); + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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; + params.record_dist(dist); + 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..a6d717acc --- /dev/null +++ b/src/ipc/esp/collisions/esp_collision_template.hpp @@ -0,0 +1,122 @@ +#pragma once +#include "esp_collision.hpp" +#include "esp_primitives.hpp" + +#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; + + ESPCollisionTemplate( + index_t primitive0, index_t primitive1, const CollisionMesh& mesh); + + virtual ~ESPCollisionTemplate() = default; + + std::string name() const override; + + int n_dofs() const override + { + return primitive_a.n_dofs() + primitive_b.n_dofs(); + } + ESPCollisionType type() const override; + + std::pair get_hash() const override + { + return std::make_pair(primitive_a.id(), primitive_b.id()); + } + + std::array get_typed_hash() const override + { + return { { static_cast(type()), primitive_a.id(), + primitive_b.id() } }; + } + + index_t operator[](int idx) const override + { + if (idx == 0) { + return primitive_a.id(); + } else if (idx == 1) { + return primitive_b.id(); + } else { + throw std::runtime_error("Invalid index in ESP collision!"); + } + } + + int num_vertices() const override + { + return primitive_a.n_vertices() + primitive_b.n_vertices(); + } + + index_t vertex_id(index_t i) const override; + + size_t n_vertices_a() const override { return primitive_a.n_vertices(); } + size_t n_vertices_b() const override { return 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 primitive_a; + PrimitiveB primitive_b; +}; + +// 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..d9caac34d --- /dev/null +++ b/src/ipc/esp/collisions/esp_primitives.hpp @@ -0,0 +1,192 @@ +#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; +}; + +namespace { + // Helper function to find the vertices adjacent to a given vertex in a 2D + // mesh. + std::vector + find_vertex_neighbors_2D(const CollisionMesh& mesh, const index_t v_id) + { + assert(mesh.dim() == 2); + std::array neighbors; + std::fill(neighbors.begin(), neighbors.end(), v_id); + for (const auto& edge_id : mesh.vertex_edge_adjacencies()[v_id]) { + const auto& edge = mesh.edges().row(edge_id); + if (edge[0] == v_id) { + neighbors[0] = edge[1]; + } else { + neighbors[1] = edge[0]; + } + } + std::vector neighbors_ordered; + for (index_t n : neighbors) { + if (n != v_id) { + neighbors_ordered.push_back(n); + } + } + return neighbors_ordered; + } +} // namespace + +/// @brief 2D vertex primitive with neighbor storage, for OGC. +class Vertex2ogc : 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; + + Vertex2ogc( + const index_t id, const CollisionMesh& mesh, const Eigen::MatrixXd& V) + : ESPPrimitive(id) + { + n_verts = 0; + m_vertex_ids[n_verts++] = id; + std::vector neighbors = find_vertex_neighbors_2D(mesh, id); + for (const auto& neighbor_id : neighbors) { + m_vertex_ids[n_verts++] = neighbor_id; + } + } + + int n_vertices() const override { return n_verts; } + int n_dofs() const override { return n_vertices() * DIM; } + +private: + int n_verts; +}; + +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..be0ceb7d8 --- /dev/null +++ b/src/ipc/esp/collisions/esp_quadrature.hpp @@ -0,0 +1,773 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ipc { +// 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; + +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) { + throw std::runtime_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 <= ORDER ) WEIGHt[I) * F ( XTAb[I) ) + + The quadrature rule will integrate exactly all polynomials up to + X**(2*ORDER-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 GNU LGPL license. + + Modified: + + 30 April 2006 + + Author: + + John Burkardt + + 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 ORDER, the order. + ORDER must be between 2 and 20. + + Output, double XTAB[ORDER], the abscissas. + + Output, double WEIGHT[ORDER], 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: { + throw std::domain_error( + "Legal values for lobatto_set are between 2 and 20.\n"); + } + } +} + +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 GNU LGPL license. + + Modified: + + 04 February 2007 + + Author: + + Original MATLAB version by Greg von Winckel. + C version by John Burkardt. + + 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. +*/ +{ + int i; + int j; + double test, error; + double tolerance; + const int n = n1 + 1; + + if (n < 2) { + std::ostringstream oss; + oss << "Requested Gauss Lobatto rule with " << n + << " points, but n>=2 is required.\n"; + throw std::runtime_error(oss.str()); + } + + 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]); + } + + error = 0.0; + for (i = 0; i < n; i++) { + test = fabs(x[i] - xold[i]); + if (test > error) { + error = test; + } + } + + } while (tolerance < error); + + // 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..01bde978d --- /dev/null +++ b/src/ipc/esp/collisions/vertex_matrix_view.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include +#include + +#include + +#include + +namespace ipc { + +/// @brief A non-owning view that presents one or two column-major matrices +/// (with the same number of columns) as a single vertically concatenated +/// matrix. +template class VertexMatrixView { +public: + /// @brief Construct a view concatenating two matrices vertically. + /// @param A The top matrix. + /// @param B The bottom matrix. + VertexMatrixView( + Eigen::ConstRef A, Eigen::ConstRef B) + : m_n_a_rows(A.rows()) + , m_n_b_rows(B.rows()) + , m_a(A.data()) + , m_b(B.data()) + { + if (A.cols() != ncols || B.cols() != ncols) { + log_and_throw_error("Incompatible matrix columns!"); + } + } + + /// @brief Construct a view wrapping a single matrix (no concatenation). + explicit VertexMatrixView(Eigen::ConstRef A) + : m_n_a_rows(A.rows()) + , m_n_b_rows(0) + , m_a(A.data()) + , m_b(nullptr) + { + if (A.cols() != ncols) { + log_and_throw_error("Incompatible matrix columns!"); + } + } + + /// @brief Access row i of the concatenated matrix. + Eigen::RowVector operator()(index_t i) const + { + assert(i < rows()); + Eigen::RowVector row; + const double* src = (i < m_n_a_rows) ? m_a : m_b; + const index_t nrows = (i < m_n_a_rows) ? m_n_a_rows : m_n_b_rows; + const index_t li = (i < m_n_a_rows) ? i : (i - m_n_a_rows); + for (int d = 0; d < ncols; ++d) { + row[d] = src[li + d * nrows]; + } + 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; +}; + +} // 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..715ba7da3 --- /dev/null +++ b/src/ipc/esp/esp_collisions.cpp @@ -0,0 +1,259 @@ +#include "esp_collisions.hpp" + +#include "esp_collision_maps.hpp" +#include "esp_collisions_builder.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include // std::out_of_range +#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(vertices.rows() == mesh.num_vertices()); + + clear(); + + if (&_candidates != &m_candidates) { + m_candidates = _candidates; + } + m_candidates.convert_candidates_to_sets(); + const Candidates& candidates = m_candidates; + + if (mesh.dim() == 2) { + tbb::enumerable_thread_specific> storage { + ESPCollisionsBuilder<2>() + }; + + // 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( + mesh, vertices, candidates, params, r.begin(), r.end()); + }); + ESPCollisionsBuilder<2>::merge(storage, *this); + } else { + // Compute vertex mask: which vertices to process. + std::vector vertex_mask(mesh.num_vertices(), false); + + // Standard mode: only process vertices in face-vertex candidates. + for (const auto& candidate : candidates.fv_candidates) { + vertex_mask[candidate.vertex_id] = true; + } + + // Face quadrature includes the face vertices, so vertices are only + // integrated separately when there is no face quadrature rule. + const bool use_face_quadrature = !params.get_quad_rule().empty(); + + std::vector vertices_to_process; + if (!use_face_quadrature) { + vertices_to_process.reserve(mesh.num_vertices()); + for (int i = 0; i < mesh.num_vertices(); ++i) { + if (vertex_mask[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)); + + if (!use_face_quadrature) { + 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(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; +} + +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); + } + } + } + for (const auto& ccs : m_maps->face_collisions) { + for (const auto& dict_ptr : ccs.second) { + for (int i = 0; i < dict_ptr->size(); i++) { + const auto& cc = (*dict_ptr)[i]; + ss << "\n"; + { + ss << fmt::format( + "face [{}]: ({} {}) 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()); + } + } + } + } + + return ss.str(); +} + +// NOTE: Actually distance squared +double ESPCollisions::compute_minimum_distance( + const CollisionMesh& mesh, Eigen::ConstRef vertices) const +{ + assert(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..da5350c10 --- /dev/null +++ b/src/ipc/esp/esp_collisions.hpp @@ -0,0 +1,86 @@ +#pragma once + +#include "collisions/esp_collision.hpp" +#include "collisions/esp_collision_dict.hpp" + +#include +#include +#include + +#include + +namespace ipc { +class ESPCollisions { +public: + 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). + 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. + 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 + 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(); } + +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; + Maps& maps() { return *m_maps; } + const Maps& maps() const { return *m_maps; } + +private: + std::unique_ptr m_maps; +}; +} // 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..c61cbc553 --- /dev/null +++ b/src/ipc/esp/esp_collisions_builder.cpp @@ -0,0 +1,531 @@ +#include "esp_collisions_builder.hpp" + +#include "collisions/esp_quadrature.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace ipc { + +using IntegrationType = ESPParameters::IntegrationType; + +void ESPCollisionsBuilder<2>::build_edge_collisions( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const Candidates& candidates, + const ESPParameters& params, + size_t start, + size_t end) +{ + const PointPotential pp(mesh, candidates, 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 (candidates.ev_set(mesh, ei).empty() + && candidates.ee_set(mesh, ei).empty()) { + continue; + } + + 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)) { + const auto& ev = candidates.ev_set(mesh, ei); + const bool has_non_obstacle = + std::any_of(ev.begin(), ev.end(), [&](index_t v) { + return !mesh.is_obstacle_vertex(v); + }); + 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 = pp.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)); + } + + 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))); + } + } + + 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); + + case PointTriangleDistanceType::P_T: + return std::make_shared>( + fi, vi, mesh); + + case PointTriangleDistanceType::AUTO: + default: + assert(false); + 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); + case PointEdgeDistanceType::P_E: + return std::make_shared>( + ei, vi, mesh); + default: + assert(false); + 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; + +QuadratureCollisionsBuilder::QuadratureCollisionsBuilder( + const QuadratureCollisionsBuilder& other) +{ + point_potential = other.point_potential; + vertex_collisions.clear(); + for (const auto& cc : other.vertex_collisions) { + vertex_collisions.push_back( + std::make_unique>(*cc)); + } + edge_edge_collisions.clear(); + for (const auto& cc : other.edge_edge_collisions) { + edge_edge_collisions.push_back( + std::make_unique>(*cc)); + } + face_collisions.clear(); + for (const auto& [fi, dicts] : other.face_collisions) { + std::vector>> copied; + for (const auto& d : dicts) { + copied.push_back( + std::make_unique>(*d)); + } + face_collisions.push_back({ fi, std::move(copied) }); + } +} +QuadratureCollisionsBuilder& +QuadratureCollisionsBuilder::operator=(const QuadratureCollisionsBuilder& other) +{ + point_potential = other.point_potential; + vertex_collisions.clear(); + for (const auto& cc : other.vertex_collisions) { + vertex_collisions.push_back( + std::make_unique>(*cc)); + } + edge_edge_collisions.clear(); + for (const auto& cc : other.edge_edge_collisions) { + edge_edge_collisions.push_back( + std::make_unique>(*cc)); + } + face_collisions.clear(); + for (const auto& [fi, dicts] : other.face_collisions) { + std::vector>> copied; + for (const auto& d : dicts) { + copied.push_back( + std::make_unique>(*d)); + } + face_collisions.push_back({ fi, std::move(copied) }); + } + return *this; +} + +void QuadratureCollisionsBuilder::build_vertex_collisions( + const Eigen::MatrixXd& 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( + const Eigen::MatrixXd& 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.get_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( + const Eigen::MatrixXd& 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; + + // Returns true if edge e (which is an obstacle) has at least one + // non-obstacle candidate. Used in NORMAL mode to skip placing a QP on an + // obstacle edge with only obstacle candidates. + auto obstacle_edge_has_non_obstacle_candidates = [&](index_t e) -> bool { + const auto v_set = point_potential->candidates.ev_set(mesh, e); + const auto e_set = point_potential->candidates.ee_set(mesh, e); + const auto f_set = point_potential->candidates.ef_set(mesh, e); + return 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 e2) { return !mesh.is_obstacle_edge(e2); }) + || std::any_of(f_set.begin(), f_set.end(), [&](index_t f) { + return !mesh.is_obstacle_face(f); + }); + }; + + 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; + } + + // ESPPotential only ever evaluates dicts whose stored + // dtype is EA_EB (see the `if (dtype != EA_EB) continue;` guards in + // esp_potential.cpp). All other edge-edge distance + // types are captured through vertex_collisions at the relevant + // endpoint, so building EA_EB0/EA_EB1/EA0_EB/EA1_EB dicts here is + // dead work. + if (dtype != EdgeEdgeDistanceType::EA_EB) { + continue; + } + + const bool ei_is_obs = mesh.is_obstacle_edge(ei); + const bool ej_is_obs = mesh.is_obstacle_edge(ej); + + if ((params.integration_type != IntegrationType::NO_OBST || !ei_is_obs) + && (!ei_is_obs + || params.integration_type == IntegrationType::BRUTE_FORCE + || obstacle_edge_has_non_obstacle_candidates(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 ((params.integration_type != IntegrationType::NO_OBST || !ej_is_obs) + && (!ej_is_obs + || params.integration_type == IntegrationType::BRUTE_FORCE + || obstacle_edge_has_non_obstacle_candidates(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..872bfb4ef --- /dev/null +++ b/src/ipc/esp/esp_collisions_builder.hpp @@ -0,0 +1,201 @@ +#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. + void build_edge_collisions( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const Candidates& candidates, + const ESPParameters& params, + 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; +}; + +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, + const Eigen::MatrixXd& 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, + const Eigen::MatrixXd& 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, + const Eigen::MatrixXd& 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, + const Eigen::MatrixXd& 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, + const Eigen::MatrixXd& 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, + const Eigen::MatrixXd& 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); + ~QuadratureCollisionsBuilder(); + + void build_vertex_collisions( + const Eigen::MatrixXd& vertices, + const std::vector& vertex_indices, + size_t start, + size_t end); + + void build_face_collisions( + const Eigen::MatrixXd& vertices, + const std::vector& face_indices, + size_t start, + size_t end); + + void build_edge_edge_collisions( + const Eigen::MatrixXd& 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..53f4840a1 --- /dev/null +++ b/src/ipc/esp/esp_distance.hpp @@ -0,0 +1,186 @@ +#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 point_point_sqr_distance( + Eigen::ConstRef> a, Eigen::ConstRef> b) +{ + return (a - b).squaredNorm(); +} + +template +T point_line_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) +{ + return (e0 - p).cross(e1 - p).squaredNorm() / (e1 - e0).squaredNorm(); +} + +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 +scalar edge_edge_sqr_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype) +{ + if constexpr (std::is_same::value) { + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } + } + + switch (dtype) { + case EdgeEdgeDistanceType::EA0_EB0: + return point_point_sqr_distance(ea0, eb0); + + case EdgeEdgeDistanceType::EA0_EB1: + return point_point_sqr_distance(ea0, eb1); + + case EdgeEdgeDistanceType::EA1_EB0: + return point_point_sqr_distance(ea1, eb0); + + case EdgeEdgeDistanceType::EA1_EB1: + return point_point_sqr_distance(ea1, eb1); + + case EdgeEdgeDistanceType::EA_EB0: + return point_line_sqr_distance(eb0, ea0, ea1); + + case EdgeEdgeDistanceType::EA_EB1: + return point_line_sqr_distance(eb1, ea0, ea1); + + case EdgeEdgeDistanceType::EA0_EB: + return point_line_sqr_distance(ea0, eb0, eb1); + + case EdgeEdgeDistanceType::EA1_EB: + return point_line_sqr_distance(ea1, eb0, eb1); + + case EdgeEdgeDistanceType::EA_EB: + return line_line_sqr_distance(ea0, ea1, eb0, eb1); + + default: + throw std::invalid_argument( + "Invalid distance type for edge-edge distance!"); + } +} + +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; + + const T a = u.squaredNorm(); + const T b = u.dot(v); + const T c = v.squaredNorm(); + const T d = u.dot(w); + const T e = v.dot(w); + + const T sN = (b * e - c * d); + const T tN = (a * e - b * d); + const T fac = a * c - pow(b, 2); + assert(fac > 0); + + return Eigen::Vector(sN, tN) / fac; +} + +// Compute the closest point local coordinate on edge (e0, e1) with respect to +// edge (e2, e3) This function is written in a consistent way as the edge-edge +// distance type classification +template +T closest_point_uv( + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + Eigen::ConstRef> e2, + Eigen::ConstRef> e3, + EdgeEdgeDistanceType dtype) +{ + Eigen::Vector u = e1 - e0; + Eigen::Vector v = e3 - e2; + + T uv(0.); + if (dtype == EdgeEdgeDistanceType::EA_EB) { + Eigen::Vector2 uvs = + line_line_closest_point_pairs_uv(e0, e1, e2, e3); + + uv = uvs(0); + } else if (dtype == EdgeEdgeDistanceType::EA_EB0) { + const T a = u.squaredNorm(); + const T d = u.dot(e0 - e2); + uv = (-d) / a; + } else if (dtype == EdgeEdgeDistanceType::EA_EB1) { + const T a = u.squaredNorm(); + const T b = u.dot(v); + const T d = u.dot(e0 - e2); + uv = (-d + b) / a; + } else { + log_and_throw_error( + "edge-edge dtype {} cannot handle!", static_cast(dtype)); + } + + 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..500131210 --- /dev/null +++ b/src/ipc/esp/esp_parameters.hpp @@ -0,0 +1,176 @@ +#pragma once + +#include +#include + +#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 dbar_factor_value 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. + /// @throws std::invalid_argument if dbar_factor_value is not in [0, 2]. + ESPParameters( + const double _dhat, + const double dbar_factor_value = 0.2, + const int _quad_order = 1, + bool _area_weights = true, + const IntegrationType _integration_type = IntegrationType::NORMAL) + : dhat(_dhat) + , dbar(dbar_factor_value * dhat) + , dbar_factor_value(dbar_factor_value) + , quad_order(_quad_order) + , area_weights(_area_weights) + , integration_type(_integration_type) + { + if (std::isnan(dbar_factor_value) || dbar_factor_value < 0 + || dbar_factor_value > 2) { + throw std::invalid_argument( + "dbar_factor " + std::to_string(dbar_factor_value) + + " is not in [0, 2]."); + } else if (dbar_factor_value == 0) { + logger().warn("dbar_factor = 0 disables edge-edge contact."); + } + } + + const double dhat; + const double dbar; + const double dbar_factor_value; + + /// 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. Unused in 3D. + const int quad_order; + /// Weight each face by its area / 9 (3D), or each edge by its area + /// (CollisionMesh::edge_area(), 2D). If false, every weight is 1. + bool area_weights; + const IntegrationType integration_type; + + double dbar_factor() const { return dbar_factor_value; } + + /// @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& get_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) { + throw std::invalid_argument( + fmt::format( + "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) { + throw std::invalid_argument( + fmt::format( + "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) { + throw std::invalid_argument("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); + } + + /// Record a distance passed to the barrier; tracks the running minimum + /// across all threads. Copies of this struct share the same tracker + /// (shared_ptr), so pass-by-value sites still update the original. + void record_dist(double d) const + { + auto& a = *m_min_dist_seen; + double cur = a.load(std::memory_order_relaxed); + while (d < cur + && !a.compare_exchange_weak(cur, d, std::memory_order_relaxed)) { + } + } + + double min_dist_seen() const + { + return m_min_dist_seen->load(std::memory_order_relaxed); + } + + void reset_min_dist() const + { + m_min_dist_seen->store( + std::numeric_limits::infinity(), std::memory_order_relaxed); + } + +private: + FaceQuadRule m_face_quad_rule; + + std::shared_ptr> m_min_dist_seen = + std::make_shared>( + std::numeric_limits::infinity()); +}; + +} // namespace ipc diff --git a/src/ipc/esp/esp_potential.cpp b/src/ipc/esp/esp_potential.cpp new file mode 100644 index 000000000..6a1027d9b --- /dev/null +++ b/src/ipc/esp/esp_potential.cpp @@ -0,0 +1,1661 @@ +#include "esp_potential.hpp" + +#include "ipc/barrier/barrier.hpp" +#include "ipc/distance/edge_edge.hpp" +#include "ipc/distance/edge_edge_mollifier.hpp" +#include "ipc/esp/collisions/esp_quadrature.hpp" +#include "ipc/esp/collisions/vertex_matrix_view.hpp" +#include "ipc/esp/esp_collision_maps.hpp" +#include "ipc/esp/esp_distance.hpp" +#include "ipc/esp/quadrature_potential.hpp" +#include "ipc/gcp/distance/mollifier.hpp" + +#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; + } +} // namespace + +double ESPPotential::operator()( + const ESPCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const +{ + assert(X.rows() == mesh.num_vertices()); + + 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(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 = 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, 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 = params.dbar_factor(); + const bool use_nf = 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( + params.barrier, dbar_factor); + } + + auto loop_body = [&](const tbb::blocked_range& r) { + double& total = potential_storage.local(); + for (index_t f = r.begin(); f < r.end(); f++) { + const double area = mesh.face_areas()(f); + const double w = params.area_weights ? (area / 9.) : 1.; + + 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 (index_t le = 0; le < 3; le++) { + const index_t edge_id = mesh.faces_to_edges()(f, 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(); + + // Skip non EA_EB collision types + if (dtype != EdgeEdgeDistanceType::EA_EB) { + continue; + } + + 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), + dtype); + + 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 / 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( + 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), 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), params, dtype); + total_w += mollifier; + total_p += mollifier * P_val; + } + } + } + } + + // Face-interior quadrature points from + // params.get_quad_rule(). + const auto& face_quad_rule = params.get_quad_rule(); + { + auto iter = collisions.maps().face_collisions.find(f); + 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()(f, 0)) + + qp.lambda[1] * X.row(mesh.faces()(f, 1)) + + qp.lambda[2] * X.row(mesh.faces()(f, 2)); + if (use_nf) { + auto [fq_near, fq_far] = PointPotentialHelper:: + evaluate_potential_at_face_center_with_cached_collisions_nearfar( + VertexMatrixView<3>(X, q_pos), + *iter->second[qi], 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_center_with_cached_collisions( + VertexMatrixView<3>(X, q_pos), + *iter->second[qi], params); + total_p += FACE_QUADRATURE_WEIGHT_SCALE + * qp.weight * fq_val; + } + } + } + } + + // Only integrate on vertices explicitly if there is no + // ESP quadrature, since that includes verts + if (face_quad_rule.empty()) { + for (index_t lv = 0; lv < 3; lv++) { + const index_t v = mesh.faces()(f, 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), 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), params); + total_p += vt_val; + } + } + } + } + + if (use_nf) { + assert(total_w_near >= total_w_far - 1e-14); + if (total_w_far == 0) { + assert(total_p_far == 0); + } + if (total_w_near > 0 && total_w_far > 0) { + total += w + * (total_p_near / total_w_near + + total_p_far / total_w_far); + } else if (total_w_near > 0) { + total += w * (total_p_near / total_w_near); + } + } else if (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, mesh.num_faces()), 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(X.rows() == mesh.num_vertices()); + + 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(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 = 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, 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 = params.dbar_factor(); + const bool use_nf_grad = 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 (index_t f = r.begin(); f < r.end(); f++) { + const double area = mesh.face_areas()(f); + const double w = params.area_weights ? (area / 9.) : 1.; + // 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_p = 0; + double total_w_near = 0, total_p_near = 0; + double total_w_far = 0; + [[maybe_unused]] double total_p_far = 0; // asserts only + + std::unique_ptr nf_barrier; + if (use_nf_grad) { + nf_barrier = std::make_unique( + params.barrier, params.dbar_factor()); + } + + for (index_t le = 0; le < 3; le++) { + const index_t edge_id = mesh.faces_to_edges()(f, 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(); + + // Skip non EA_EB collision types + if (dtype != EdgeEdgeDistanceType::EA_EB) { + continue; + } + + 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( + edge_edge_sqr_distance( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3), + dtype)); + + const T uv = closest_point_uv( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3), + dtype); + + 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 = edge_edge_sqr_distance( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3), + 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.val); + + T mollifier = Math::cubic_spline( + dist / 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( + 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, params, dtype, + *nf_barrier); + } else { + P = PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions( + X_extended, dict, 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, 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, params, + ee_closest_point_T); + } + + ee_cache.push_back( + { &dict, mollifier.val, mollifier.grad, P, + grad_p }); + total_w += mollifier.val; + total_p += mollifier.val * P; + if (use_nf_grad) { + total_w_near += mollifier.val; + total_p_near += mollifier.val * P; + } + } + } + } + + // Face-interior quadrature points + const auto& face_quad_rule = params.get_quad_rule(); + { + auto iter = collisions.maps().face_collisions.find(f); + 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()(f, 0)) + + qp.lambda[1] * X.row(mesh.faces()(f, 1)) + + qp.lambda[2] * X.row(mesh.faces()(f, 2)); + VertexMatrixView<3> X_qp(X, q_pos); + + if (use_nf_grad) { + auto [P_n, P_f] = PointPotentialHelper:: + evaluate_potential_at_face_center_with_cached_collisions_nearfar( + X_qp, dict, params, *nf_barrier); + auto [grad_n, grad_f] = PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions_nearfar( + X_qp, dict, 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; + total_p_far += qp_weight_scale * P_f; + } else { + const double P = PointPotentialHelper:: + evaluate_potential_at_face_center_with_cached_collisions( + X_qp, dict, params); + const Eigen::VectorXd grad_p = + PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions( + X_qp, dict, params, qp.lambda); + const_cache.push_back( + ConstGradEntry { + &dict.dofs(), + qp_weight_scale * grad_p, + Eigen::VectorXd::Zero(0), + qp_weight_scale * P, 0 }); + total_p += qp_weight_scale * P; + } + } + } + } + + if (face_quad_rule.empty()) { + for (index_t lv = 0; lv < 3; lv++) { + const index_t v = mesh.faces()(f, 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), params, + *nf_barrier); + auto [grad_n, grad_f] = PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions_nearfar( + X, (*iter->second), params, + *nf_barrier); + const_cache.push_back( + ConstGradEntry { + &(*iter->second).dofs(), grad_n, + grad_f, P_n, P_f }); + total_p_near += P_n; + total_p_far += P_f; + } else { + const double P = PointPotentialHelper:: + evaluate_potential_at_vertex_with_cached_collisions( + X, (*iter->second), params); + const Eigen::VectorXd grad_p = + PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions( + X, (*iter->second), params); + const_cache.push_back( + ConstGradEntry { + &(*iter->second).dofs(), grad_p, + Eigen::VectorXd::Zero(0), P, 0 }); + total_p += P; + } + } + } + } + + // Pass 2: apply gradient + if (use_nf_grad) { + assert(total_w_near > 0); + if (total_w_far == 0) { + assert(total_p_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) { + if (total_w_far > 0) { + grad(*e.dofs) += + (w / total_w_near) * e.grad_p_near + + (w / total_w_far) * e.grad_p_far; + } else { + grad(*e.dofs) += + (w / total_w_near) * e.grad_p_near; + } + } + } else if (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); + const double avg_P = total_p / total_w; + for (const auto& e : ee_cache) { + grad(e.dict->dofs()) += + (w / total_w * e.mol_val) * e.grad_p; + grad(e.dict->primary_dofs()) += + (w / total_w * (e.P - avg_P)) * e.mol_grad; + } + 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, mesh.num_faces()), 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(X.rows() == mesh.num_vertices()); + + if (collisions.empty()) { + return Eigen::SparseMatrix(X.size(), X.size()); + } + + const int dim = X.cols(); + 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(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 = 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, params, lambda, + project_hessian_to_psd); + + local_hessian_to_global_triplets( + local_hess, dict.vertex_ids(), dim, + *(hess_triplets.cache)); + } + } + }); + } 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 = params.dbar_factor(); + const bool use_nf_hess = 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 (index_t f = r.begin(); f < r.end(); f++) { + const double area = mesh.face_areas()(f); + const double w = params.area_weights ? (area / 9.) : 1.; + + // 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_p = 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( + params.barrier, params.dbar_factor()); + } + + for (index_t le = 0; le < 3; le++) { + const index_t edge_id = mesh.faces_to_edges()(f, 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(); + + // Skip non EA_EB collision types + if (dtype != EdgeEdgeDistanceType::EA_EB) { + continue; + } + + 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( + edge_edge_sqr_distance( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3), + dtype)); + + const T uv = closest_point_uv( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3), + dtype); + + 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 = edge_edge_sqr_distance( + positionsT.row(0), positionsT.row(1), + positionsT.row(2), positionsT.row(3), + 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.val); + + T mollifier = Math::cubic_spline( + dist / 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( + 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, params, dtype, + *nf_barrier); + grad_p = PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions_near< + T>( + X_extended, dict, 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, params, + ee_closest_point_T, *nf_barrier); + } else { + P = PointPotentialHelper:: + evaluate_potential_at_edge_edge_closest_point_with_cached_collisions( + X_extended, dict, params, dtype); + grad_p = PointPotentialHelper:: + evaluate_potential_gradient_at_edge_edge_closest_point_with_cached_collisions< + T>( + X_extended, dict, params, + ee_closest_point_T); + base_hess = PointPotentialHelper:: + evaluate_potential_hessian_at_edge_edge_closest_point_with_cached_collisions( + X_extended, dict, 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; + total_p += mollifier.val * P; + if (use_nf_hess) { + total_w_near += mollifier.val; + total_p_near += mollifier.val * P; + } + } + } + } + + // Face-interior quadrature points + const auto& face_quad_rule = params.get_quad_rule(); + { + auto iter = collisions.maps().face_collisions.find(f); + 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()(f, 0)) + + qp.lambda[1] * X.row(mesh.faces()(f, 1)) + + qp.lambda[2] * X.row(mesh.faces()(f, 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_center_with_cached_collisions_nearfar( + X_qp, dict, params, *nf_barrier); + auto [grad_n, grad_f] = PointPotentialHelper:: + evaluate_potential_gradient_at_face_interior_point_with_cached_collisions_nearfar( + X_qp, dict, 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, 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_center_with_cached_collisions( + X_qp, dict, 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, 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, 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); + total_p += entry.p_near; + } + const_cache.push_back(std::move(entry)); + } + } + } + + if (face_quad_rule.empty()) { + for (index_t lv = 0; lv < 3; lv++) { + const index_t v = mesh.faces()(f, 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, params, *nf_barrier); + auto [grad_n, grad_f] = PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions_nearfar( + X, dict, params, *nf_barrier); + auto [hess_n, hess_f] = PointPotentialHelper:: + evaluate_potential_hessian_at_vertex_with_cached_collisions_nearfar( + X, dict, 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, params); + entry.grad_p_near = PointPotentialHelper:: + evaluate_potential_gradient_at_vertex_with_cached_collisions( + X, dict, params); + entry + .local_hess_near = PointPotentialHelper:: + evaluate_potential_hessian_at_vertex_with_cached_collisions( + X, dict, 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); + total_p += entry.p_near; + } + 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) to its + // local index inside H_face. + auto global_dof_to_local = [&](index_t gd) { + return vid_to_local.at(gd / dim) * dim + + static_cast(gd % dim); + }; + + // 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)); + } 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)); + } + 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)); + 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)); + } + } + + // 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)); + } + + // 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 (use_near_far) { + // Normalized (use_near_far=true) but without + // NearFarBarrier splitting (only reached at + // dbar_factor = 0: no edge-edge terms, so B = C = 0) + assert(total_w > 0); + const double avg_P = total_p / total_w; + const double scale_C = -(w / (total_w * total_w)); + + auto add_sym_correction_norm = + [&](const std::vector& g_dofs, + const Eigen::Ref& g_vec, + const std::vector& gradz_dofs, + const Eigen::Ref& + gradz_vec) { + 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_C * 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) * H(p_sum) + for (const auto& e : ee_cache) { + local_hessian_to_global_triplets( + (w / total_w) * e.local_hess, + e.dict->vertex_ids(), dim, + *(hess_triplets.cache)); + } + 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)); + } + + // Term B: -(w*avg_P/total_w) * Σ_i H(mol_i) + for (const auto& e : ee_cache) { + local_hessian_to_global_triplets( + -(w * avg_P / total_w) * e.mol_hess, + e.dict->primary_vertex_ids(), dim, + *(hess_triplets.cache)); + } + + // Term C: -(w/total_w²) * sym(G ⊗ ∇Z) + for (const auto& ei : ee_cache) { + const auto& prim_dofs_i = ei.dict->primary_dofs(); + const Eigen::Vector& mol_grad_i = + ei.mol_grad; + for (const auto& ek : ee_cache) { + add_sym_correction_norm( + ek.dict->primary_dofs(), + (ek.P - avg_P) * ek.mol_grad, prim_dofs_i, + mol_grad_i); + add_sym_correction_norm( + ek.dict->dofs(), ek.mol_val * ek.grad_p, + prim_dofs_i, mol_grad_i); + } + for (const auto& ej : const_cache) { + add_sym_correction_norm( + *ej.dofs, ej.grad_p_near, prim_dofs_i, + mol_grad_i); + } + } + } 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)); + } + for (const auto& e : const_cache) { + local_hessian_to_global_triplets( + w * e.local_hess_near, *e.vertex_ids, dim, + *(hess_triplets.cache)); + } + } + } + }; + + tbb::parallel_for( + tbb::blocked_range(0, mesh.num_faces()), 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 Eigen::SparseMatrix(); + } + + // 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, params); +} + +Eigen::VectorXd ESPPotential::gradient( + const ESPCollision& collision, + Eigen::ConstRef positions) const +{ + return collision.weight * collision.gradient(positions, 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, 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..5c29bc0b0 --- /dev/null +++ b/src/ipc/esp/esp_potential.hpp @@ -0,0 +1,100 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +class ESPPotential { +public: + ESPPotential(const ESPParameters& _params, const bool _use_near_far = true) + : params(_params) + , 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. + 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|. + 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|. + 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; + + bool get_use_near_far() const { return use_near_far; } + +protected: + /// @brief ESP parameters for collision potential + ESPParameters params; + /// @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. + const bool 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..1be4a3101 --- /dev/null +++ b/src/ipc/esp/esp_tangential_collisions.cpp @@ -0,0 +1,793 @@ +#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 bool normalize_weights, + const std::function& blend_mu) +{ + assert(mu_s.size() == vertices.rows()); + assert(mu_k.size() == vertices.rows()); + + 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.get_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) { + if (dict_ptr->ee_dtype() != EdgeEdgeDistanceType::EA_EB) { + continue; + } + 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); + if (e00 == e10 || e00 == e11 || e01 == e10 || e01 == e11) { + continue; + } + 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 ESP potential only contributes for EA_EB; skip otherwise so + // friction matches exactly. + if (dtype != EdgeEdgeDistanceType::EA_EB) { + continue; + } + + // Compute virtual vertex position on edge e0 + // (same logic as quadrature_potential.cpp) + double closest_uv = line_line_closest_point_pairs_uv( + vertices.row(e00).transpose(), vertices.row(e01).transpose(), + vertices.row(e10).transpose(), + vertices.row(e11).transpose())(0); + 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; + } + } + } + } + } +} + +} // namespace ipc diff --git a/src/ipc/esp/quadrature_potential.cpp b/src/ipc/esp/quadrature_potential.cpp new file mode 100644 index 000000000..0031e182e --- /dev/null +++ b/src/ipc/esp/quadrature_potential.cpp @@ -0,0 +1,1735 @@ +#include "quadrature_potential.hpp" + +#include "ipc/candidates/candidates.hpp" +#include "ipc/distance/distance_type_exact.hpp" +#include "ipc/distance/edge_edge.hpp" +#include "ipc/distance/point_edge.hpp" +#include "ipc/distance/point_point.hpp" +#include "ipc/distance/point_triangle.hpp" +#include "ipc/esp/esp_collision_maps.hpp" +#include "ipc/esp/esp_collisions_builder.hpp" + +#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( + const Eigen::MatrixXd& 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); + + const bool src_is_obstacle = mesh.is_obstacle_vertex(vid); + const bool filter_obstacles = src_is_obstacle + && params.integration_type + != ESPParameters::IntegrationType::BRUTE_FORCE; + + 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)); + } + } + + for (const auto& other_e : e_set) { + if (filter_obstacles && mesh.is_obstacle_edge(other_e)) { + 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 = -1; + insert_pair(pairs, std::move(pair)); + } + } + + for (const auto& other_v : v_set) { + if (filter_obstacles && mesh.is_obstacle_vertex(other_v)) { + continue; + } + if ((V.row(vid) - V.row(other_v)).squaredNorm() + >= params.dhat * params.dhat) { + continue; + } + std::shared_ptr pair = + std::make_shared>( + vid, other_v, mesh); + ++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); + return collisions; +} + +double +PointPotentialHelper::evaluate_potential_at_vertex_with_cached_collisions( + const Eigen::MatrixXd& 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( + const Eigen::MatrixXd& 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( + const Eigen::MatrixXd& 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( + const Eigen::MatrixXd& 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) { + closest_uv = line_line_closest_point_pairs_uv( + V.row(e00), V.row(e01), V.row(e10), V.row(e11))(0); + } 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 src_is_obstacle_e = mesh.is_obstacle_edge(e0); + const bool filter_obstacles_e = src_is_obstacle_e + && params.integration_type + != ESPParameters::IntegrationType::BRUTE_FORCE; + + for (const auto& other_v : v_set) { + if (filter_obstacles_e && mesh.is_obstacle_vertex(other_v)) { + continue; + } + if ((V_view(vid) - V_view(other_v)).squaredNorm() + >= params.dhat * params.dhat) { + continue; + } + auto pair = + std::make_shared>( + vid, other_v, mesh); + ++num_collision_pairs; + insert_pair(pairs, std::shared_ptr(pair)); + } + + for (const auto& other_e : e_set) { + if (other_e == e0) { + continue; + } + if (filter_obstacles_e && mesh.is_obstacle_edge(other_e)) { + 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 = -1; + 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 = -1; + 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 = -1; + insert_pair(pairs, std::shared_ptr(pair)); + break; + } + default: + assert(false); + break; + } + } + + const auto& e0_faces = mesh.edges_to_faces()[e0]; + for (const auto& other_f : f_set) { + if (std::find(e0_faces.begin(), e0_faces.end(), other_f) + != e0_faces.end()) { + continue; + } + 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); + 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_center( + const Eigen::MatrixXd& V, + const index_t fid, + size_t& num_collision_pairs) const +{ + // the fake vertex id + const index_t vid = V.rows(); + + // Use VertexMatrixView to avoid deep-copying the entire vertex matrix + const Eigen::RowVector3d face_center = + (V.row(mesh.faces()(fid, 0)) + V.row(mesh.faces()(fid, 1)) + + V.row(mesh.faces()(fid, 2))) + / 3.; + VertexMatrixView<3> V_view(V, face_center); + + 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); + + for (const auto& other_f : f_set) { + assert(other_f != fid); + ++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) { + ++num_collision_pairs; + if (auto pair = ESPCollisionsBuilder<3>::reduce_point_edge_collision( + EdgeVertexCandidate(other_e, vid), params, mesh, V_view)) { + pair->weight = -1; + insert_pair(pairs, std::shared_ptr(pair)); + } + } + + for (const auto& other_v : v_set) { + 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); + ++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); + return collisions; +} + +std::unique_ptr> +PointPotential::build_collisions_at_face_interior_point( + const Eigen::MatrixXd& 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 src_is_obstacle_f = mesh.is_obstacle_face(fid); + const bool filter_obstacles_f = src_is_obstacle_f + && params.integration_type + != ESPParameters::IntegrationType::BRUTE_FORCE; + + 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; + } + ++num_collision_pairs; + if (auto pair = ESPCollisionsBuilder<3>::reduce_point_edge_collision( + EdgeVertexCandidate(other_e, vid), params, mesh, V_view)) { + pair->weight = -1; + 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) { + 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); + ++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); + return collisions; +} + +Eigen::VectorXd PointPotentialHelper:: + evaluate_potential_gradient_at_face_center_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params) +{ + 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) { + // distribute grad wrt virtual vertex to real face vertices + for (index_t lv = 0; lv < 3; lv++) { + grad.segment<3>(collisions.primary_local_ids()[lv] * 3) += + g.segment<3>(3 * i) / 3.; + } + } 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_center_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + 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); + // if (project_to_psd != PSDProjectionMethod::NONE) { + // h = ipc::project_to_psd(h, project_to_psd); + // } + 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) { + // distribute grad wrt virtual vertex to real face vertices + 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) / 9.; + } + } + } else if (gi == n_real_vertices) { + 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) / 3.; + } + } else if (gj == n_real_vertices) { + 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) / 3.; + } + } 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; +} + +double +PointPotentialHelper::evaluate_potential_at_face_center_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( + const Eigen::MatrixXd& 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 src_is_obstacle = mesh.is_obstacle_edge(ei); + const bool filter_obstacles = src_is_obstacle + && params.integration_type + != ESPParameters::IntegrationType::BRUTE_FORCE; + + ESPCollisionPairMap pairs; + num_collision_pairs = 0; + const double dhat2 = dhat * dhat; + + // VV pairs (weight=-1): for each nearby vertex within dhat of the QP. + for (const index_t vj : candidates.ev_set(mesh, ei)) { + if (vj == corner_vertex) { + 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 = -1; + insert_pair(pairs, std::move(vv_pair)); + } + + // EV pairs (weight=+1): 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); + 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( + const Eigen::MatrixXd& 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( + const Eigen::MatrixXd& 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( + const Eigen::MatrixXd& 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>::value, 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) +{ + 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 <> +std::enable_if_t>::value, 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) +{ + 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; +} + +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_center_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_center_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + 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) { + grad_near.template segment<3>(0) += + cc.weight * (1.0 / 3.0) * gn.template segment<3>(i * 3); + grad_far.template segment<3>(0) += + cc.weight * (1.0 / 3.0) * 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_center_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + 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_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) { + H_near.block<3, 3>(0, 0) += + cc.weight * (1.0 / 9.0) * hn.block<3, 3>(i * 3, j * 3); + H_far.block<3, 3>(0, 0) += + cc.weight * (1.0 / 9.0) * hf.block<3, 3>(i * 3, j * 3); + } else if (i_virtual) { + const index_t local_j = + collisions.vertex_ids_inverse(global_id_j); + H_near.block<3, 3>(0, local_j * 3) += + cc.weight * (1.0 / 3.0) * hn.block<3, 3>(i * 3, j * 3); + H_far.block<3, 3>(0, local_j * 3) += + cc.weight * (1.0 / 3.0) * hf.block<3, 3>(i * 3, j * 3); + } else if (j_virtual) { + const index_t local_i = + collisions.vertex_ids_inverse(global_id_i); + H_near.block<3, 3>(local_i * 3, 0) += + cc.weight * (1.0 / 3.0) * hn.block<3, 3>(i * 3, j * 3); + H_far.block<3, 3>(local_i * 3, 0) += + cc.weight * (1.0 / 3.0) * 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 }; +} + +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..35f0b2f58 --- /dev/null +++ b/src/ipc/esp/quadrature_potential.hpp @@ -0,0 +1,274 @@ +#pragma once +#include "ipc/candidates/edge_edge.hpp" +#include "ipc/collision_mesh.hpp" +#include "ipc/distance/point_point.hpp" +#include "ipc/distance/point_triangle.hpp" +#include "ipc/esp/esp_collisions.hpp" +#include "ipc/esp/esp_distance.hpp" + +#include + +namespace ipc { +namespace PointPotentialHelper { + double evaluate_potential_at_vertex_with_cached_collisions( + const Eigen::MatrixXd& V, + const ESPCollisionDict& collisions, + const ESPParameters& params); + + Eigen::VectorXd + evaluate_potential_gradient_at_vertex_with_cached_collisions( + const Eigen::MatrixXd& V, + const ESPCollisionDict& collisions, + const ESPParameters& params); + + Eigen::MatrixXd evaluate_potential_hessian_at_vertex_with_cached_collisions( + const Eigen::MatrixXd& V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + PSDProjectionMethod project_to_psd); + + std::pair + evaluate_potential_at_vertex_with_cached_collisions_nearfar( + const Eigen::MatrixXd& V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier& nf_barrier); + + std::pair + evaluate_potential_gradient_at_vertex_with_cached_collisions_nearfar( + const Eigen::MatrixXd& V, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier& nf_barrier); + + std::pair + evaluate_potential_hessian_at_vertex_with_cached_collisions_nearfar( + const Eigen::MatrixXd& 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_center_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params); + + std::pair + evaluate_potential_at_face_center_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier& nf_barrier); + + Eigen::VectorXd + evaluate_potential_gradient_at_face_center_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params); + + Eigen::MatrixXd + evaluate_potential_hessian_at_face_center_with_cached_collisions( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + PSDProjectionMethod project_to_psd); + + std::pair + evaluate_potential_gradient_at_face_center_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + const NearFarBarrier& nf_barrier); + + std::pair + evaluate_potential_hessian_at_face_center_with_cached_collisions_nearfar( + VertexMatrixView<3> V_extended, + const ESPCollisionDict& collisions, + const ESPParameters& params, + PSDProjectionMethod project_to_psd, + 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) + { + } + + std::unique_ptr> + build_collisions_at_vertex( + const Eigen::MatrixXd& V, + index_t vid, + size_t& num_collision_pairs) const; + + std::unique_ptr> + build_collisions_at_edge_edge_closest_point( + const Eigen::MatrixXd& V, + index_t e0, + index_t e1, + EdgeEdgeDistanceType dtype, + size_t& num_collision_pairs) const; + + std::unique_ptr> + build_collisions_at_face_center( + const Eigen::MatrixXd& V, + index_t fid, + size_t& num_collision_pairs) const; + + std::unique_ptr> + build_collisions_at_face_interior_point( + const Eigen::MatrixXd& 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( + const Eigen::MatrixXd& 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; +}; +} // 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..ed3721380 100644 --- a/src/ipc/smooth_contact/collisions/smooth_collision.cpp +++ b/src/ipc/gcp/collisions/gcp_collision.cpp @@ -1,28 +1,28 @@ -#include "smooth_collision.hpp" +#include "gcp_collision.hpp" #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); @@ -41,7 +41,7 @@ Eigen::VectorXd SmoothCollision::dof(Eigen::ConstRef X) const } template -auto SmoothCollisionTemplate::get_core_indices() const +auto GCPCollisionTemplate::get_core_indices() const -> Eigen::Vector { Eigen::Vector core_indices; @@ -54,15 +54,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 +103,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 +142,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 +255,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 +469,7 @@ auto SmoothCollisionTemplate::hessian( // ---- distance ---- template -double SmoothCollisionTemplate::compute_distance( +double GCPCollisionTemplate::compute_distance( Eigen::ConstRef vertices) const { VectorMax positions = dof(vertices); @@ -485,7 +484,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 +496,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..ebe415282 100644 --- a/src/ipc/smooth_contact/collisions/smooth_collision.hpp +++ b/src/ipc/gcp/collisions/gcp_collision.hpp @@ -1,8 +1,8 @@ #pragma once #include -#include -#include +#include +#include namespace ipc { @@ -14,12 +14,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 +30,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 +82,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 +112,7 @@ class SmoothCollision { } else if (idx == 1) { return primitive1; } else { - throw std::runtime_error("Invalid index in smooth_collision!"); + throw std::runtime_error("Invalid index in gcp_collision!"); } } @@ -130,11 +130,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 +150,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 +191,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 +199,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 +207,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 +222,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 100% rename from src/ipc/smooth_contact/distance/edge_edge.cpp rename to src/ipc/gcp/distance/edge_edge.cpp 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 100% rename from src/ipc/smooth_contact/distance/point_edge.cpp rename to src/ipc/gcp/distance/point_edge.cpp 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 100% rename from src/ipc/smooth_contact/distance/point_face.cpp rename to src/ipc/gcp/distance/point_face.cpp 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 88% rename from src/ipc/smooth_contact/smooth_collisions.cpp rename to src/ipc/gcp/gcp_collisions.cpp index bafc55027..ea0450ee8 100644 --- a/src/ipc/smooth_contact/smooth_collisions.cpp +++ b/src/ipc/gcp/gcp_collisions.cpp @@ -1,6 +1,6 @@ -#include "smooth_collisions.hpp" +#include "gcp_collisions.hpp" -#include "smooth_collisions_builder.hpp" +#include "gcp_collisions_builder.hpp" #include #include @@ -17,10 +17,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()); @@ -112,10 +112,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 +128,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 +164,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 +177,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,17 +201,17 @@ 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]; @@ -219,7 +219,7 @@ SmoothCollision& SmoothCollisions::operator[](size_t i) throw std::out_of_range("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]; @@ -227,10 +227,10 @@ const SmoothCollision& SmoothCollisions::operator[](size_t i) const throw std::out_of_range("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 +247,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 +274,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 97% rename from src/ipc/smooth_contact/primitives/point2.cpp rename to src/ipc/gcp/primitives/point2.cpp index 9d7e0ba79..05420382d 100644 --- a/src/ipc/smooth_contact/primitives/point2.cpp +++ b/src/ipc/gcp/primitives/point2.cpp @@ -28,7 +28,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 +60,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 +91,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 +108,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/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/potentials/barrier_potential.cpp b/src/ipc/potentials/barrier_potential.cpp index 5a0ef8004..a84aade59 100644 --- a/src/ipc/potentials/barrier_potential.cpp +++ b/src/ipc/potentials/barrier_potential.cpp @@ -3,6 +3,9 @@ #include #include +#include +#include + namespace ipc { BarrierPotential::BarrierPotential( @@ -19,11 +22,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 +38,12 @@ BarrierPotential::BarrierPotential( double BarrierPotential::force_magnitude( const double distance_squared, const double dmin) const { + if (!m_use_squared_distance) { + throw std::runtime_error( + "BarrierPotential: force_magnitude not implemented for " + "use_squared_distance=false"); + } + double N = barrier_force_magnitude( distance_squared, barrier(), dhat(), stiffness(), dmin); @@ -48,6 +59,12 @@ VectorMax12d BarrierPotential::force_magnitude_gradient( Eigen::ConstRef distance_squared_gradient, const double dmin) const { + if (!m_use_squared_distance) { + throw std::runtime_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 +79,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 b; + } + double b = barrier()(distance_squared - dmin * dmin, (2 * dmin + dhat()) * dhat()); @@ -75,6 +101,15 @@ 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); + double db = barrier().first_derivative(d - dmin, dhat()) / (2.0 * d); + if (use_physical_barrier()) { + db *= dhat() / barrier().units(dhat()); + } + return db; + } + double db = barrier().first_derivative( distance_squared - dmin * dmin, (2 * dmin + dhat()) * dhat()); @@ -88,6 +123,18 @@ 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); + 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 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..74be2b420 100644 --- a/src/ipc/potentials/barrier_potential.hpp +++ b/src/ipc/potentials/barrier_potential.hpp @@ -31,11 +31,17 @@ 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 actual Euclidean distance d, with chain- + /// rule corrections applied internally so NormalPotential is + /// unchanged. 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 +103,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 +162,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 actual distance d, with + /// chain-rule corrections applied so NormalPotential is unchanged. + bool m_use_squared_distance = true; }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/potentials/tangential_potential.cpp b/src/ipc/potentials/tangential_potential.cpp index ad1e53e59..6fb127219 100644 --- a/src/ipc/potentials/tangential_potential.cpp +++ b/src/ipc/potentials/tangential_potential.cpp @@ -583,7 +583,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 +604,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 +612,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 +647,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 +661,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 +670,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 +694,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 +758,7 @@ TangentialPotential::VectorMaxNd TangentialPotential::smooth_contact_force( * mu_f1_over_norm_tau * T * tau_aniso; } -TangentialPotential::MatrixMaxNd -TangentialPotential::smooth_contact_force_jacobian_unit( +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..de3cf28df 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,7 +163,7 @@ class TangentialPotential : public Potential { const bool no_mu = false, const bool no_contact_force_multiplier = false) const; - Eigen::MatrixXd smooth_contact_force_jacobian( + Eigen::MatrixXd gcp_force_jacobian( const TangentialCollision& collision, Eigen::ConstRef rest_positions, // = x Eigen::ConstRef lagged_displacements, // = u @@ -178,13 +178,77 @@ 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_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..405104dc5 100644 --- a/src/ipc/smooth_contact/smooth_collisions.hpp +++ b/src/ipc/smooth_contact/smooth_collisions.hpp @@ -1,162 +1,4 @@ #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. +#include diff --git a/src/ipc/smooth_contact/smooth_contact_potential.hpp b/src/ipc/smooth_contact/smooth_contact_potential.hpp index 5e755ecf3..4a2612482 100644 --- a/src/ipc/smooth_contact/smooth_contact_potential.hpp +++ b/src/ipc/smooth_contact/smooth_contact_potential.hpp @@ -1,87 +1,4 @@ #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. +#include diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7cfa3e9ef..1f0e7ad6e 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -70,6 +70,11 @@ 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 directly. +if(IPC_TOOLKIT_WITH_GEOGRAM) + target_link_libraries(ipc_toolkit_tests PRIVATE geogram::geogram) +endif() + 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..02ac637ed 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 @@ -249,7 +249,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 +340,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, @@ -436,6 +436,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 +486,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/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_obstacles.cpp b/tests/src/tests/collisions/test_obstacles.cpp new file mode 100644 index 000000000..edadcbf3c --- /dev/null +++ b/tests/src/tests/collisions/test_obstacles.cpp @@ -0,0 +1,219 @@ +#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); +} + +const std::vector LOWER_OBSTACLE = { false, false, false, false, + true, true, true, true }; + +constexpr double DHAT = 0.6; + +} // 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("Mixed edges and faces throw") + { + std::vector obstacle = LOWER_OBSTACLE; + obstacle[0] = true; + const CollisionMesh mesh = star_mesh(obstacle); + for (int e = 0; e < mesh.num_edges(); e++) { + const bool v0 = mesh.edges()(e, 0) == 0; + const bool v1 = mesh.edges()(e, 1) == 0; + if (v0 || v1) { + CHECK_THROWS(mesh.is_obstacle_edge(e)); + } + } + for (int f = 0; f < 4; f++) { + if ((mesh.faces().row(f).array() == 0).any()) { + CHECK_THROWS(mesh.is_obstacle_face(f)); + } + } + } +} + +TEST_CASE("Normal collisions skip obstacles", "[collisions][obstacle]") +{ + const CollisionMesh mesh = star_mesh(LOWER_OBSTACLE); + const Eigen::MatrixXd& V = mesh.rest_positions(); + + 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); + } + return n; + }; + REQUIRE(count_obstacle_vertices(all) > 0); + 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("ESP integration types", "[esp_potential][obstacle]") +{ + using IntegrationType = ESPParameters::IntegrationType; + + const auto potential_value = [](const std::vector& obstacle_vertex, + const IntegrationType type) { + const CollisionMesh mesh = star_mesh(obstacle_vertex); + const ESPParameters params(DHAT, 1.0, 1, true, type); + 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); +} 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..1237f91eb --- /dev/null +++ b/tests/src/tests/distance/distance_type_reference.hpp @@ -0,0 +1,259 @@ +#pragma once +#include +#include +#include +#include +// geogram 1.10 no longer pulls this in transitively via exact_geometry.h. +#include + +using namespace ipc; +using ExReal = GEO::expansion_nt; // exact scalar type +using ExVec3 = GEO::vec3E; // 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() +{ // TODO init once in main + static bool initialized = false; + if (!initialized) { + GEO::PCK::initialize(); + initialized = true; + } +} +inline ExVec3 make_exact(Eigen::ConstRef v) +{ + ExReal x { v.x() }; + ExReal y { v.y() }; + ExReal z { v.size() < 3 ? 0 : v.z() }; // compatibility with 2D vectors + return ExVec3(std::move(x), std::move(y), std::move(z)); +} +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 (dot(e, p - e0) <= 0) { + return PointEdgeDistanceType::P_E0; // PP (p-e0) + } else if (dot(-e, 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 (dot(r0, e0) <= 0 && dot(r0, e2) >= 0) { + return PointTriangleDistanceType::P_T0; + } + if (dot(r1, e1) <= 0 && dot(r1, e0) >= 0) { + return PointTriangleDistanceType::P_T1; + } + if (dot(r2, e2) <= 0 && dot(r2, e1) >= 0) { + return PointTriangleDistanceType::P_T2; + } + + const ExVec3 n = cross(e0, -e1); + + if (dot(n, cross(r0, e0)) <= 0 && dot(r0, e0) > 0 && dot(r1, e0) < 0) { + return PointTriangleDistanceType::P_E0; + } + if (dot(n, cross(r1, e1)) <= 0 && dot(r1, e1) > 0 && dot(r2, e1) < 0) { + return PointTriangleDistanceType::P_E1; + } + if (dot(n, cross(r2, e2)) <= 0 && dot(r2, e2) > 0 && dot(r0, 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 = cross(u, v).length2(); + if constexpr (REFERENCE_PARALLEL_THRESHOLD == 0.0) + return cross_norm_sqr == 0; + const ExReal a = u.length2(); + const ExReal c = v.length2(); + 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.length2(); + const ExReal alpha_N = dot(eb0 - ea0, ea); + const ExReal beta_N = dot(eb1 - ea0, 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.length2(); // always ≥ 0 + const ExReal b = dot(u, v); + const ExReal c = v.length2(); // always ≥ 0 + const ExReal d = dot(u, w); + const ExReal e = dot(v, 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 = cross(u, v).length2(); + 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..a7f7894c3 100644 --- a/tests/src/tests/distance/test_distance_type.cpp +++ b/tests/src/tests/distance/test_distance_type.cpp @@ -6,9 +6,16 @@ #include #include +#include #include #include +#ifdef IPC_TOOLKIT_WITH_GEOGRAM +#include "distance_type_reference.hpp" +#endif + +#include + using namespace ipc; TEST_CASE("Point-edge distance type", "[distance][distance-type][point-edge]") @@ -55,6 +62,224 @@ TEST_CASE("Point-edge distance type", "[distance][distance-type][point-edge]") } } +#ifdef IPC_TOOLKIT_WITH_GEOGRAM +// These compare the shipped classifiers, standard and exact, against an +// exact-arithmetic reference, which is only available when geogram is enabled. +TEST_CASE( + "Point-edge distance type random", + "[distance][distance-type][point-edge][exact]") +{ + const int num_random_tests = 100000; + + 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 = 100000; + + 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 = 100000; + + 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. ipc::edge_edge_distance_type unconditionally +// uses the thresholded analytic classifier (no exact-predicate mode), so the +// reference must be called with the *same* threshold to stay comparable: +// with a threshold of 0 (a fully exact reference), this comparison would fail +// on ~20% of samples, because the reference would then only take the +// parallel-classifier branch for exactly-parallel edges while the shipped +// classifier takes it for the much larger near-parallel set. +TEST_CASE( + "Edge-edge distance type random parallel", + "[distance][distance-type][edge-edge][exact][parallel]") +{ + const int num_random_tests = 100000; + + 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; + + Eigen::Vector3d eb0 = ea0; + Eigen::Vector3d eb1 = ea1; + + const double amount = Eigen::Vector3d::Random()(0); + const int axis = i % 3; + eb0[axis] += amount; + eb1[axis] += amount; + + if (i % 2 == 0) + eb1 += Eigen::Vector3d::Random() * ((ea1 - ea0).norm() * 1e-20); + + const EdgeEdgeDistanceType dtype_ref = + edge_edge_distance_type_reference(ea0, ea1, eb0, eb1); + + CAPTURE( + ea0.transpose(), ea1.transpose(), eb0.transpose(), eb1.transpose()); + CHECK(edge_edge_distance_type(ea0, ea1, eb0, eb1) == dtype_ref); + // The exact classifier has no parallel branch; its callers skip the + // pairs is_parallel_edge_edge flags. + if (!is_parallel_edge_edge(ea0, ea1, eb0, eb1)) { + CHECK( + edge_edge_distance_type_exact(ea0, ea1, eb0, eb1) == dtype_ref); + } + } +} + +#endif // IPC_TOOLKIT_WITH_GEOGRAM + +// The *_exact classifiers must agree with each other whether or not their +// predicates are exact (geogram), 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; 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/friction/friction_data_generator.cpp b/tests/src/tests/friction/friction_data_generator.cpp index 29d4472c2..12799ddac 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,76 @@ 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 }; + } + + 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..69e71caff 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". +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..1921ae0b3 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,160 @@ 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.get_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, 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); + 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( "Smooth friction force no_mu and no_contact_force_multiplier", "[friction-smooth][force][no-mu]") @@ -740,7 +892,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 +917,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 +938,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,11 +952,10 @@ 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); } @@ -815,7 +966,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 +980,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..c46e9767b 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 @@ -25,4 +27,4 @@ target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) ################################################################################ # Subfolders -################################################################################ \ No newline at end of file +################################################################################ diff --git a/tests/src/tests/potential/sphere.obj b/tests/src/tests/potential/sphere.obj new file mode 100644 index 000000000..65532259f --- /dev/null +++ b/tests/src/tests/potential/sphere.obj @@ -0,0 +1,1447 @@ +# Blender 4.2.0 +# www.blender.org +mtllib untitled.mtl +o Sphere +v 0.000000 0.733886 1.098338 +v 0.000000 1.098338 0.733886 +v 0.000000 1.295578 0.257706 +v 0.000000 1.320960 0.000000 +v 0.000000 1.295578 -0.257706 +v 0.000000 1.098338 -0.733886 +v 0.050276 0.252755 1.295578 +v 0.098620 0.495796 1.220408 +v 0.143174 0.719785 1.098338 +v 0.182226 0.916112 0.934060 +v 0.214275 1.077234 0.733886 +v 0.238090 1.196958 0.505509 +v 0.252755 1.270684 0.257706 +v 0.257706 1.295578 0.000000 +v 0.252755 1.270684 -0.257706 +v 0.238090 1.196958 -0.505509 +v 0.214275 1.077234 -0.733886 +v 0.182226 0.916112 -0.934060 +v 0.143174 0.719785 -1.098338 +v 0.098620 0.495796 -1.220408 +v 0.050276 0.252755 -1.295578 +v 0.098620 0.238090 1.295578 +v 0.193450 0.467030 1.220408 +v 0.280846 0.678022 1.098338 +v 0.357449 0.862959 0.934060 +v 0.420316 1.014732 0.733886 +v 0.467030 1.127510 0.505509 +v 0.495796 1.196958 0.257706 +v 0.505509 1.220408 0.000000 +v 0.495796 1.196958 -0.257706 +v 0.467030 1.127510 -0.505509 +v 0.420316 1.014732 -0.733886 +v 0.357449 0.862959 -0.934060 +v 0.280846 0.678022 -1.098338 +v 0.193450 0.467030 -1.220408 +v 0.098620 0.238090 -1.295578 +v 0.143174 0.214275 1.295578 +v 0.280846 0.420316 1.220408 +v 0.407725 0.610204 1.098338 +v 0.518936 0.776642 0.934060 +v 0.610204 0.913235 0.733886 +v 0.678022 1.014732 0.505509 +v 0.719785 1.077234 0.257706 +v 0.733886 1.098338 0.000000 +v 0.719785 1.077234 -0.257706 +v 0.678022 1.014732 -0.505509 +v 0.610204 0.913235 -0.733886 +v 0.518936 0.776642 -0.934060 +v 0.407725 0.610204 -1.098338 +v 0.280846 0.420316 -1.220408 +v 0.143174 0.214275 -1.295578 +v 0.182226 0.182226 1.295578 +v 0.357449 0.357449 1.220408 +v 0.518936 0.518936 1.098338 +v 0.660480 0.660480 0.934060 +v 0.776642 0.776642 0.733886 +v 0.862959 0.862959 0.505509 +v 0.916112 0.916112 0.257706 +v 0.934060 0.934059 0.000000 +v 0.916112 0.916112 -0.257706 +v 0.862959 0.862959 -0.505509 +v 0.776642 0.776642 -0.733886 +v 0.660480 0.660480 -0.934060 +v 0.518936 0.518936 -1.098338 +v 0.357449 0.357449 -1.220408 +v 0.182226 0.182226 -1.295578 +v 0.214275 0.143174 1.295578 +v 0.420316 0.280846 1.220408 +v 0.610204 0.407725 1.098338 +v 0.776642 0.518936 0.934060 +v 0.913235 0.610204 0.733886 +v 1.014732 0.678022 0.505509 +v 1.077234 0.719784 0.257706 +v 1.098338 0.733886 0.000000 +v 1.077234 0.719784 -0.257706 +v 1.014732 0.678022 -0.505509 +v 0.913235 0.610204 -0.733886 +v 0.776642 0.518936 -0.934060 +v 0.610204 0.407725 -1.098338 +v 0.420316 0.280846 -1.220408 +v 0.214275 0.143174 -1.295578 +v 0.000000 0.000000 1.320960 +v 0.238090 0.098620 1.295578 +v 0.467030 0.193450 1.220408 +v 0.678022 0.280846 1.098338 +v 0.862959 0.357449 0.934060 +v 1.014732 0.420316 0.733886 +v 1.127510 0.467030 0.505509 +v 1.196958 0.495796 0.257706 +v 1.220408 0.505509 0.000000 +v 1.196958 0.495796 -0.257706 +v 1.127510 0.467030 -0.505509 +v 1.014732 0.420316 -0.733886 +v 0.862959 0.357449 -0.934060 +v 0.678022 0.280846 -1.098338 +v 0.467030 0.193450 -1.220408 +v 0.238090 0.098620 -1.295578 +v 0.252755 0.050276 1.295578 +v 0.495796 0.098620 1.220408 +v 0.719785 0.143174 1.098338 +v 0.916112 0.182226 0.934060 +v 1.077234 0.214275 0.733886 +v 1.196958 0.238090 0.505509 +v 1.270684 0.252755 0.257706 +v 1.295578 0.257706 0.000000 +v 1.270684 0.252755 -0.257706 +v 1.196958 0.238090 -0.505509 +v 1.077234 0.214275 -0.733886 +v 0.916112 0.182226 -0.934060 +v 0.719785 0.143174 -1.098338 +v 0.495796 0.098620 -1.220408 +v 0.252755 0.050276 -1.295578 +v 0.257706 -0.000000 1.295578 +v 0.505509 -0.000000 1.220408 +v 0.733886 -0.000000 1.098338 +v 0.934060 0.000000 0.934060 +v 1.098338 -0.000000 0.733886 +v 1.220408 0.000000 0.505509 +v 1.295578 -0.000000 0.257706 +v 1.320960 -0.000000 0.000000 +v 1.295578 -0.000000 -0.257706 +v 1.220408 0.000000 -0.505509 +v 1.098338 -0.000000 -0.733886 +v 0.934060 0.000000 -0.934060 +v 0.733886 -0.000000 -1.098338 +v 0.505509 -0.000000 -1.220408 +v 0.257706 -0.000000 -1.295578 +v 0.252755 -0.050276 1.295578 +v 0.495796 -0.098620 1.220408 +v 0.719784 -0.143174 1.098338 +v 0.916112 -0.182226 0.934060 +v 1.077233 -0.214275 0.733886 +v 1.196958 -0.238090 0.505509 +v 1.270684 -0.252755 0.257706 +v 1.295578 -0.257707 0.000000 +v 1.270684 -0.252755 -0.257706 +v 1.196958 -0.238090 -0.505509 +v 1.077233 -0.214275 -0.733886 +v 0.916112 -0.182226 -0.934060 +v 0.719784 -0.143174 -1.098338 +v 0.495796 -0.098620 -1.220408 +v 0.252755 -0.050276 -1.295578 +v 0.238090 -0.098620 1.295578 +v 0.467030 -0.193450 1.220408 +v 0.678022 -0.280846 1.098338 +v 0.862959 -0.357449 0.934060 +v 1.014732 -0.420316 0.733886 +v 1.127510 -0.467030 0.505509 +v 1.196958 -0.495796 0.257706 +v 1.220407 -0.505509 0.000000 +v 1.196958 -0.495796 -0.257706 +v 1.127510 -0.467030 -0.505509 +v 1.014732 -0.420316 -0.733886 +v 0.862959 -0.357449 -0.934060 +v 0.678022 -0.280846 -1.098338 +v 0.467030 -0.193450 -1.220408 +v 0.238090 -0.098620 -1.295578 +v 0.214275 -0.143174 1.295578 +v 0.420316 -0.280846 1.220408 +v 0.610204 -0.407725 1.098338 +v 0.776642 -0.518936 0.934060 +v 0.913234 -0.610204 0.733886 +v 1.014732 -0.678022 0.505509 +v 1.077233 -0.719785 0.257706 +v 1.098337 -0.733886 0.000000 +v 1.077233 -0.719785 -0.257706 +v 1.014732 -0.678022 -0.505509 +v 0.913234 -0.610204 -0.733886 +v 0.776642 -0.518936 -0.934060 +v 0.610204 -0.407725 -1.098338 +v 0.420316 -0.280846 -1.220408 +v 0.214275 -0.143174 -1.295578 +v 0.182226 -0.182226 1.295578 +v 0.357449 -0.357449 1.220408 +v 0.518936 -0.518936 1.098338 +v 0.660480 -0.660480 0.934060 +v 0.776642 -0.776642 0.733886 +v 0.862959 -0.862958 0.505509 +v 0.916112 -0.916112 0.257706 +v 0.934059 -0.934060 0.000000 +v 0.916112 -0.916112 -0.257706 +v 0.862959 -0.862958 -0.505509 +v 0.776642 -0.776642 -0.733886 +v 0.660480 -0.660480 -0.934060 +v 0.518936 -0.518936 -1.098338 +v 0.357449 -0.357449 -1.220408 +v 0.182226 -0.182226 -1.295578 +v 0.143174 -0.214275 1.295578 +v 0.280846 -0.420316 1.220408 +v 0.407725 -0.610204 1.098338 +v 0.518936 -0.776642 0.934060 +v 0.610204 -0.913234 0.733886 +v 0.678022 -1.014732 0.505509 +v 0.719784 -1.077234 0.257706 +v 0.733885 -1.098338 0.000000 +v 0.719784 -1.077234 -0.257706 +v 0.678022 -1.014732 -0.505509 +v 0.610204 -0.913234 -0.733886 +v 0.518936 -0.776642 -0.934060 +v 0.407725 -0.610204 -1.098338 +v 0.280846 -0.420316 -1.220408 +v 0.143174 -0.214275 -1.295578 +v 0.098620 -0.238090 1.295578 +v 0.193450 -0.467030 1.220408 +v 0.280846 -0.678022 1.098338 +v 0.357449 -0.862959 0.934060 +v 0.420316 -1.014732 0.733886 +v 0.467030 -1.127510 0.505509 +v 0.495796 -1.196958 0.257706 +v 0.505509 -1.220407 0.000000 +v 0.495796 -1.196958 -0.257706 +v 0.467030 -1.127510 -0.505509 +v 0.420316 -1.014732 -0.733886 +v 0.357449 -0.862959 -0.934060 +v 0.280846 -0.678022 -1.098338 +v 0.193450 -0.467030 -1.220408 +v 0.098620 -0.238090 -1.295578 +v 0.050276 -0.252755 1.295578 +v 0.098620 -0.495796 1.220408 +v 0.143174 -0.719784 1.098338 +v 0.182226 -0.916112 0.934060 +v 0.214275 -1.077233 0.733886 +v 0.238090 -1.196958 0.505509 +v 0.252754 -1.270684 0.257706 +v 0.257706 -1.295577 0.000000 +v 0.252754 -1.270684 -0.257706 +v 0.238090 -1.196958 -0.505509 +v 0.214275 -1.077233 -0.733886 +v 0.182226 -0.916112 -0.934060 +v 0.143174 -0.719784 -1.098338 +v 0.098620 -0.495796 -1.220408 +v 0.050276 -0.252755 -1.295578 +v -0.000000 -0.257706 1.295578 +v -0.000000 -0.505509 1.220408 +v -0.000000 -0.733886 1.098338 +v -0.000000 -0.934060 0.934060 +v -0.000000 -1.098338 0.733886 +v 0.000000 -1.220407 0.505509 +v -0.000000 -1.295578 0.257706 +v -0.000000 -1.320959 0.000000 +v -0.000000 -1.295578 -0.257706 +v 0.000000 -1.220407 -0.505509 +v -0.000000 -1.098338 -0.733886 +v -0.000000 -0.934060 -0.934060 +v -0.000000 -0.733886 -1.098338 +v -0.000000 -0.505509 -1.220408 +v -0.000000 -0.257706 -1.295578 +v -0.050276 -0.252755 1.295578 +v -0.098620 -0.495796 1.220408 +v -0.143174 -0.719784 1.098338 +v -0.182226 -0.916112 0.934060 +v -0.214275 -1.077233 0.733886 +v -0.238090 -1.196958 0.505509 +v -0.252755 -1.270683 0.257706 +v -0.257707 -1.295577 0.000000 +v -0.252755 -1.270683 -0.257706 +v -0.238090 -1.196958 -0.505509 +v -0.214275 -1.077233 -0.733886 +v -0.182226 -0.916112 -0.934060 +v -0.143174 -0.719784 -1.098338 +v -0.098620 -0.495796 -1.220408 +v -0.050276 -0.252755 -1.295578 +v -0.098620 -0.238090 1.295578 +v -0.193450 -0.467030 1.220408 +v -0.280846 -0.678022 1.098338 +v -0.357449 -0.862958 0.934060 +v -0.420316 -1.014732 0.733886 +v -0.467030 -1.127509 0.505509 +v -0.495796 -1.196957 0.257706 +v -0.505510 -1.220407 0.000000 +v -0.495796 -1.196957 -0.257706 +v -0.467030 -1.127509 -0.505509 +v -0.420316 -1.014732 -0.733886 +v -0.357449 -0.862958 -0.934060 +v -0.280846 -0.678022 -1.098338 +v -0.193450 -0.467030 -1.220408 +v -0.098620 -0.238090 -1.295578 +v -0.143174 -0.214275 1.295578 +v -0.280846 -0.420316 1.220408 +v -0.407725 -0.610203 1.098338 +v -0.518936 -0.776642 0.934060 +v -0.610204 -0.913234 0.733886 +v -0.678022 -1.014732 0.505509 +v -0.719785 -1.077233 0.257706 +v -0.733886 -1.098337 0.000000 +v -0.719785 -1.077233 -0.257706 +v -0.678022 -1.014732 -0.505509 +v -0.610204 -0.913234 -0.733886 +v -0.518936 -0.776642 -0.934060 +v -0.407725 -0.610203 -1.098338 +v -0.280846 -0.420316 -1.220408 +v -0.143174 -0.214275 -1.295578 +v -0.182226 -0.182226 1.295578 +v -0.357449 -0.357449 1.220408 +v -0.518936 -0.518935 1.098338 +v -0.660480 -0.660480 0.934060 +v -0.776642 -0.776642 0.733886 +v -0.862958 -0.862958 0.505509 +v -0.916112 -0.916112 0.257706 +v -0.934059 -0.934059 0.000000 +v -0.916112 -0.916112 -0.257706 +v -0.862958 -0.862958 -0.505509 +v -0.776642 -0.776642 -0.733886 +v -0.660480 -0.660480 -0.934060 +v -0.518936 -0.518935 -1.098338 +v -0.357449 -0.357449 -1.220408 +v -0.182226 -0.182226 -1.295578 +v 0.000000 0.000000 -1.320960 +v -0.214275 -0.143174 1.295578 +v -0.420316 -0.280846 1.220408 +v -0.610204 -0.407725 1.098338 +v -0.776642 -0.518936 0.934060 +v -0.913234 -0.610203 0.733886 +v -1.014732 -0.678022 0.505509 +v -1.077233 -0.719784 0.257706 +v -1.098337 -0.733885 0.000000 +v -1.077233 -0.719784 -0.257706 +v -1.014732 -0.678022 -0.505509 +v -0.913234 -0.610203 -0.733886 +v -0.776642 -0.518936 -0.934060 +v -0.610204 -0.407725 -1.098338 +v -0.420316 -0.280846 -1.220408 +v -0.214275 -0.143174 -1.295578 +v -0.238090 -0.098620 1.295578 +v -0.467030 -0.193450 1.220408 +v -0.678022 -0.280846 1.098338 +v -0.862958 -0.357449 0.934060 +v -1.014732 -0.420315 0.733886 +v -1.127509 -0.467030 0.505509 +v -1.196958 -0.495796 0.257706 +v -1.220407 -0.505508 0.000000 +v -1.196958 -0.495796 -0.257706 +v -1.127509 -0.467030 -0.505509 +v -1.014732 -0.420315 -0.733886 +v -0.862958 -0.357449 -0.934060 +v -0.678022 -0.280846 -1.098338 +v -0.467030 -0.193450 -1.220408 +v -0.238090 -0.098620 -1.295578 +v -0.252755 -0.050276 1.295578 +v -0.495796 -0.098620 1.220408 +v -0.719784 -0.143174 1.098338 +v -0.916112 -0.182226 0.934060 +v -1.077233 -0.214275 0.733886 +v -1.196958 -0.238090 0.505509 +v -1.270683 -0.252754 0.257706 +v -1.295577 -0.257706 0.000000 +v -1.270683 -0.252754 -0.257706 +v -1.196958 -0.238090 -0.505509 +v -1.077233 -0.214275 -0.733886 +v -0.916112 -0.182226 -0.934060 +v -0.719784 -0.143174 -1.098338 +v -0.495796 -0.098620 -1.220408 +v -0.252755 -0.050276 -1.295578 +v -0.257706 0.000000 1.295578 +v -0.505509 0.000000 1.220408 +v -0.733885 0.000000 1.098338 +v -0.934059 0.000000 0.934060 +v -1.098337 0.000000 0.733886 +v -1.220407 0.000000 0.505509 +v -1.295577 0.000000 0.257706 +v -1.320959 0.000001 0.000000 +v -1.295577 0.000000 -0.257706 +v -1.220407 0.000000 -0.505509 +v -1.098337 0.000000 -0.733886 +v -0.934059 0.000000 -0.934060 +v -0.733885 0.000000 -1.098338 +v -0.505509 0.000000 -1.220408 +v -0.257706 0.000000 -1.295578 +v -0.252755 0.050276 1.295578 +v -0.495796 0.098620 1.220408 +v -0.719784 0.143174 1.098338 +v -0.916112 0.182226 0.934060 +v -1.077233 0.214275 0.733886 +v -1.196957 0.238090 0.505509 +v -1.270683 0.252755 0.257706 +v -1.295577 0.257707 0.000000 +v -1.270683 0.252755 -0.257706 +v -1.196957 0.238090 -0.505509 +v -1.077233 0.214275 -0.733886 +v -0.916112 0.182226 -0.934060 +v -0.719784 0.143174 -1.098338 +v -0.495796 0.098620 -1.220408 +v -0.252755 0.050276 -1.295578 +v -0.238090 0.098620 1.295578 +v -0.467030 0.193450 1.220408 +v -0.678022 0.280846 1.098338 +v -0.862958 0.357449 0.934060 +v -1.014731 0.420316 0.733886 +v -1.127509 0.467030 0.505509 +v -1.196957 0.495796 0.257706 +v -1.220406 0.505510 0.000000 +v -1.196957 0.495796 -0.257706 +v -1.127509 0.467030 -0.505509 +v -1.014731 0.420316 -0.733886 +v -0.862958 0.357449 -0.934060 +v -0.678022 0.280846 -1.098338 +v -0.467030 0.193450 -1.220408 +v -0.238090 0.098620 -1.295578 +v -0.214275 0.143174 1.295578 +v -0.420316 0.280846 1.220408 +v -0.610203 0.407725 1.098338 +v -0.776642 0.518936 0.934060 +v -0.913234 0.610204 0.733886 +v -1.014732 0.678022 0.505509 +v -1.077233 0.719785 0.257706 +v -1.098337 0.733886 0.000000 +v -1.077233 0.719785 -0.257706 +v -1.014732 0.678022 -0.505509 +v -0.913234 0.610204 -0.733886 +v -0.776642 0.518936 -0.934060 +v -0.610203 0.407725 -1.098338 +v -0.420316 0.280846 -1.220408 +v -0.214275 0.143174 -1.295578 +v -0.182226 0.182226 1.295578 +v -0.357449 0.357449 1.220408 +v -0.518935 0.518935 1.098338 +v -0.660480 0.660480 0.934060 +v -0.776642 0.776642 0.733886 +v -0.862958 0.862958 0.505509 +v -0.916111 0.916112 0.257706 +v -0.934058 0.934059 0.000000 +v -0.916111 0.916112 -0.257706 +v -0.862958 0.862958 -0.505509 +v -0.776642 0.776642 -0.733886 +v -0.660480 0.660480 -0.934060 +v -0.518935 0.518935 -1.098338 +v -0.357449 0.357449 -1.220408 +v -0.182226 0.182226 -1.295578 +v -0.143174 0.214275 1.295578 +v -0.280846 0.420316 1.220408 +v -0.407725 0.610203 1.098338 +v -0.518936 0.776642 0.934060 +v -0.610203 0.913234 0.733886 +v -0.678022 1.014732 0.505509 +v -0.719784 1.077233 0.257706 +v -0.733885 1.098337 0.000000 +v -0.719784 1.077233 -0.257706 +v -0.678022 1.014732 -0.505509 +v -0.610203 0.913234 -0.733886 +v -0.518936 0.776642 -0.934060 +v -0.407725 0.610203 -1.098338 +v -0.280846 0.420316 -1.220408 +v -0.143174 0.214275 -1.295578 +v -0.098620 0.238090 1.295578 +v -0.193450 0.467030 1.220408 +v -0.280845 0.678022 1.098338 +v -0.357449 0.862958 0.934060 +v -0.420315 1.014731 0.733886 +v -0.467030 1.127509 0.505509 +v -0.495796 1.196957 0.257706 +v -0.505508 1.220407 0.000000 +v -0.495796 1.196957 -0.257706 +v -0.467030 1.127509 -0.505509 +v -0.420315 1.014731 -0.733886 +v -0.357449 0.862958 -0.934060 +v -0.280845 0.678022 -1.098338 +v -0.193450 0.467030 -1.220408 +v -0.098620 0.238090 -1.295578 +v -0.050276 0.252754 1.295578 +v -0.098620 0.495796 1.220408 +v -0.143174 0.719784 1.098338 +v -0.182226 0.916112 0.934060 +v -0.214275 1.077233 0.733886 +v -0.238090 1.196957 0.505509 +v -0.252754 1.270683 0.257706 +v -0.257706 1.295577 0.000000 +v -0.252754 1.270683 -0.257706 +v -0.238090 1.196957 -0.505509 +v -0.214275 1.077233 -0.733886 +v -0.182226 0.916112 -0.934060 +v -0.143174 0.719784 -1.098338 +v -0.098620 0.495796 -1.220408 +v -0.050276 0.252754 -1.295578 +v 0.000000 0.257706 1.295578 +v 0.000000 0.505509 1.220408 +v 0.000000 0.934059 0.934060 +v 0.000000 1.220407 0.505509 +v 0.000000 1.220407 -0.505509 +v 0.000000 0.934059 -0.934060 +v 0.000000 0.733885 -1.098338 +v 0.000000 0.505509 -1.220408 +v 0.000000 0.257706 -1.295578 +s 0 +f 480 20 481 +f 3 12 13 +f 481 21 482 +f 3 14 4 +f 474 82 7 +f 308 482 21 +f 4 15 5 +f 475 7 8 +f 5 16 478 +f 1 8 9 +f 478 17 6 +f 476 9 10 +f 6 18 479 +f 2 10 11 +f 479 19 480 +f 477 11 12 +f 19 33 34 +f 11 27 12 +f 20 34 35 +f 13 27 28 +f 20 36 21 +f 13 29 14 +f 7 82 22 +f 308 21 36 +f 14 30 15 +f 7 23 8 +f 15 31 16 +f 8 24 9 +f 16 32 17 +f 9 25 10 +f 17 33 18 +f 10 26 11 +f 22 38 23 +f 30 46 31 +f 23 39 24 +f 32 46 47 +f 24 40 25 +f 33 47 48 +f 25 41 26 +f 34 48 49 +f 26 42 27 +f 34 50 35 +f 27 43 28 +f 35 51 36 +f 28 44 29 +f 22 82 37 +f 308 36 51 +f 30 44 45 +f 41 57 42 +f 49 65 50 +f 43 57 58 +f 50 66 51 +f 44 58 59 +f 37 82 52 +f 308 51 66 +f 44 60 45 +f 37 53 38 +f 45 61 46 +f 38 54 39 +f 47 61 62 +f 39 55 40 +f 47 63 48 +f 40 56 41 +f 48 64 49 +f 60 76 61 +f 53 69 54 +f 62 76 77 +f 54 70 55 +f 62 78 63 +f 56 70 71 +f 63 79 64 +f 56 72 57 +f 65 79 80 +f 58 72 73 +f 65 81 66 +f 59 73 74 +f 52 82 67 +f 308 66 81 +f 59 75 60 +f 52 68 53 +f 79 96 80 +f 73 88 89 +f 81 96 97 +f 74 89 90 +f 67 82 83 +f 308 81 97 +f 74 91 75 +f 67 84 68 +f 75 92 76 +f 68 85 69 +f 77 92 93 +f 69 86 70 +f 77 94 78 +f 71 86 87 +f 78 95 79 +f 71 88 72 +f 93 107 108 +f 85 101 86 +f 93 109 94 +f 87 101 102 +f 95 109 110 +f 87 103 88 +f 96 110 111 +f 89 103 104 +f 96 112 97 +f 90 104 105 +f 83 82 98 +f 308 97 112 +f 90 106 91 +f 84 98 99 +f 91 107 92 +f 84 100 85 +f 111 127 112 +f 105 119 120 +f 98 82 113 +f 308 112 127 +f 105 121 106 +f 99 113 114 +f 106 122 107 +f 99 115 100 +f 108 122 123 +f 100 116 101 +f 108 124 109 +f 102 116 117 +f 110 124 125 +f 102 118 103 +f 110 126 111 +f 104 118 119 +f 115 131 116 +f 123 139 124 +f 117 131 132 +f 125 139 140 +f 117 133 118 +f 125 141 126 +f 119 133 134 +f 126 142 127 +f 120 134 135 +f 113 82 128 +f 308 127 142 +f 120 136 121 +f 113 129 114 +f 121 137 122 +f 114 130 115 +f 123 137 138 +f 135 149 150 +f 128 82 143 +f 308 142 157 +f 135 151 136 +f 128 144 129 +f 136 152 137 +f 130 144 145 +f 138 152 153 +f 130 146 131 +f 138 154 139 +f 132 146 147 +f 140 154 155 +f 132 148 133 +f 140 156 141 +f 134 148 149 +f 142 156 157 +f 153 169 154 +f 147 161 162 +f 155 169 170 +f 147 163 148 +f 155 171 156 +f 149 163 164 +f 156 172 157 +f 150 164 165 +f 143 82 158 +f 308 157 172 +f 150 166 151 +f 143 159 144 +f 151 167 152 +f 145 159 160 +f 153 167 168 +f 145 161 146 +f 158 82 173 +f 308 172 187 +f 165 181 166 +f 158 174 159 +f 166 182 167 +f 159 175 160 +f 168 182 183 +f 160 176 161 +f 168 184 169 +f 162 176 177 +f 169 185 170 +f 162 178 163 +f 171 185 186 +f 164 178 179 +f 172 186 187 +f 165 179 180 +f 177 191 192 +f 185 199 200 +f 177 193 178 +f 185 201 186 +f 179 193 194 +f 186 202 187 +f 180 194 195 +f 173 82 188 +f 308 187 202 +f 180 196 181 +f 174 188 189 +f 181 197 182 +f 175 189 190 +f 183 197 198 +f 175 191 176 +f 183 199 184 +f 195 211 196 +f 188 204 189 +f 196 212 197 +f 189 205 190 +f 198 212 213 +f 190 206 191 +f 198 214 199 +f 192 206 207 +f 200 214 215 +f 192 208 193 +f 200 216 201 +f 194 208 209 +f 201 217 202 +f 195 209 210 +f 188 82 203 +f 308 202 217 +f 215 229 230 +f 207 223 208 +f 215 231 216 +f 209 223 224 +f 216 232 217 +f 210 224 225 +f 203 82 218 +f 308 217 232 +f 210 226 211 +f 203 219 204 +f 211 227 212 +f 204 220 205 +f 213 227 228 +f 205 221 206 +f 213 229 214 +f 207 221 222 +f 226 242 227 +f 220 234 235 +f 228 242 243 +f 220 236 221 +f 228 244 229 +f 222 236 237 +f 230 244 245 +f 222 238 223 +f 230 246 231 +f 224 238 239 +f 232 246 247 +f 225 239 240 +f 218 82 233 +f 308 232 247 +f 225 241 226 +f 218 234 219 +f 245 261 246 +f 239 253 254 +f 246 262 247 +f 240 254 255 +f 233 82 248 +f 308 247 262 +f 240 256 241 +f 233 249 234 +f 241 257 242 +f 234 250 235 +f 243 257 258 +f 235 251 236 +f 243 259 244 +f 237 251 252 +f 245 259 260 +f 237 253 238 +f 249 265 250 +f 258 272 273 +f 250 266 251 +f 258 274 259 +f 252 266 267 +f 260 274 275 +f 252 268 253 +f 260 276 261 +f 254 268 269 +f 261 277 262 +f 255 269 270 +f 248 82 263 +f 308 262 277 +f 255 271 256 +f 249 263 264 +f 256 272 257 +f 269 283 284 +f 277 291 292 +f 270 284 285 +f 263 82 278 +f 308 277 292 +f 270 286 271 +f 264 278 279 +f 271 287 272 +f 264 280 265 +f 273 287 288 +f 265 281 266 +f 273 289 274 +f 267 281 282 +f 275 289 290 +f 267 283 268 +f 275 291 276 +f 288 302 303 +f 280 296 281 +f 288 304 289 +f 282 296 297 +f 290 304 305 +f 282 298 283 +f 290 306 291 +f 284 298 299 +f 292 306 307 +f 285 299 300 +f 278 82 293 +f 308 292 307 +f 285 301 286 +f 278 294 279 +f 286 302 287 +f 279 295 280 +f 306 323 307 +f 300 315 316 +f 293 82 309 +f 308 307 323 +f 300 317 301 +f 293 310 294 +f 301 318 302 +f 294 311 295 +f 303 318 319 +f 295 312 296 +f 303 320 304 +f 297 312 313 +f 305 320 321 +f 297 314 298 +f 305 322 306 +f 299 314 315 +f 311 327 312 +f 319 335 320 +f 312 328 313 +f 321 335 336 +f 313 329 314 +f 321 337 322 +f 315 329 330 +f 322 338 323 +f 316 330 331 +f 309 82 324 +f 308 323 338 +f 316 332 317 +f 309 325 310 +f 317 333 318 +f 310 326 311 +f 319 333 334 +f 331 345 346 +f 324 82 339 +f 308 338 353 +f 331 347 332 +f 324 340 325 +f 332 348 333 +f 325 341 326 +f 334 348 349 +f 326 342 327 +f 334 350 335 +f 328 342 343 +f 336 350 351 +f 328 344 329 +f 336 352 337 +f 330 344 345 +f 337 353 338 +f 349 365 350 +f 343 357 358 +f 351 365 366 +f 343 359 344 +f 351 367 352 +f 345 359 360 +f 352 368 353 +f 346 360 361 +f 339 82 354 +f 308 353 368 +f 346 362 347 +f 339 355 340 +f 347 363 348 +f 341 355 356 +f 349 363 364 +f 341 357 342 +f 308 368 383 +f 361 377 362 +f 354 370 355 +f 362 378 363 +f 356 370 371 +f 364 378 379 +f 356 372 357 +f 364 380 365 +f 358 372 373 +f 366 380 381 +f 358 374 359 +f 366 382 367 +f 360 374 375 +f 367 383 368 +f 361 375 376 +f 354 82 369 +f 381 395 396 +f 373 389 374 +f 381 397 382 +f 375 389 390 +f 382 398 383 +f 376 390 391 +f 369 82 384 +f 308 383 398 +f 376 392 377 +f 369 385 370 +f 377 393 378 +f 371 385 386 +f 379 393 394 +f 371 387 372 +f 379 395 380 +f 373 387 388 +f 385 399 400 +f 392 408 393 +f 385 401 386 +f 394 408 409 +f 386 402 387 +f 394 410 395 +f 388 402 403 +f 396 410 411 +f 388 404 389 +f 397 411 412 +f 390 404 405 +f 397 413 398 +f 391 405 406 +f 384 82 399 +f 308 398 413 +f 391 407 392 +f 403 419 404 +f 411 427 412 +f 405 419 420 +f 412 428 413 +f 406 420 421 +f 399 82 414 +f 308 413 428 +f 406 422 407 +f 399 415 400 +f 407 423 408 +f 401 415 416 +f 409 423 424 +f 401 417 402 +f 409 425 410 +f 403 417 418 +f 411 425 426 +f 422 438 423 +f 416 430 431 +f 424 438 439 +f 416 432 417 +f 424 440 425 +f 418 432 433 +f 426 440 441 +f 418 434 419 +f 426 442 427 +f 420 434 435 +f 427 443 428 +f 421 435 436 +f 414 82 429 +f 308 428 443 +f 421 437 422 +f 414 430 415 +f 441 457 442 +f 435 449 450 +f 442 458 443 +f 436 450 451 +f 429 82 444 +f 308 443 458 +f 436 452 437 +f 430 444 445 +f 437 453 438 +f 430 446 431 +f 439 453 454 +f 431 447 432 +f 439 455 440 +f 433 447 448 +f 441 455 456 +f 433 449 434 +f 446 460 461 +f 454 468 469 +f 446 462 447 +f 454 470 455 +f 448 462 463 +f 456 470 471 +f 448 464 449 +f 456 472 457 +f 450 464 465 +f 458 472 473 +f 451 465 466 +f 444 82 459 +f 308 458 473 +f 451 467 452 +f 444 460 445 +f 452 468 453 +f 465 477 3 +f 472 482 473 +f 466 3 4 +f 459 82 474 +f 308 473 482 +f 466 5 467 +f 460 474 475 +f 467 478 468 +f 461 475 1 +f 468 6 469 +f 462 1 476 +f 469 479 470 +f 463 476 2 +f 471 479 480 +f 464 2 477 +f 471 481 472 +f 480 19 20 +f 3 477 12 +f 481 20 21 +f 3 13 14 +f 4 14 15 +f 475 474 7 +f 5 15 16 +f 1 475 8 +f 478 16 17 +f 476 1 9 +f 6 17 18 +f 2 476 10 +f 479 18 19 +f 477 2 11 +f 19 18 33 +f 11 26 27 +f 20 19 34 +f 13 12 27 +f 20 35 36 +f 13 28 29 +f 14 29 30 +f 7 22 23 +f 15 30 31 +f 8 23 24 +f 16 31 32 +f 9 24 25 +f 17 32 33 +f 10 25 26 +f 22 37 38 +f 30 45 46 +f 23 38 39 +f 32 31 46 +f 24 39 40 +f 33 32 47 +f 25 40 41 +f 34 33 48 +f 26 41 42 +f 34 49 50 +f 27 42 43 +f 35 50 51 +f 28 43 44 +f 30 29 44 +f 41 56 57 +f 49 64 65 +f 43 42 57 +f 50 65 66 +f 44 43 58 +f 44 59 60 +f 37 52 53 +f 45 60 61 +f 38 53 54 +f 47 46 61 +f 39 54 55 +f 47 62 63 +f 40 55 56 +f 48 63 64 +f 60 75 76 +f 53 68 69 +f 62 61 76 +f 54 69 70 +f 62 77 78 +f 56 55 70 +f 63 78 79 +f 56 71 72 +f 65 64 79 +f 58 57 72 +f 65 80 81 +f 59 58 73 +f 59 74 75 +f 52 67 68 +f 79 95 96 +f 73 72 88 +f 81 80 96 +f 74 73 89 +f 74 90 91 +f 67 83 84 +f 75 91 92 +f 68 84 85 +f 77 76 92 +f 69 85 86 +f 77 93 94 +f 71 70 86 +f 78 94 95 +f 71 87 88 +f 93 92 107 +f 85 100 101 +f 93 108 109 +f 87 86 101 +f 95 94 109 +f 87 102 103 +f 96 95 110 +f 89 88 103 +f 96 111 112 +f 90 89 104 +f 90 105 106 +f 84 83 98 +f 91 106 107 +f 84 99 100 +f 111 126 127 +f 105 104 119 +f 105 120 121 +f 99 98 113 +f 106 121 122 +f 99 114 115 +f 108 107 122 +f 100 115 116 +f 108 123 124 +f 102 101 116 +f 110 109 124 +f 102 117 118 +f 110 125 126 +f 104 103 118 +f 115 130 131 +f 123 138 139 +f 117 116 131 +f 125 124 139 +f 117 132 133 +f 125 140 141 +f 119 118 133 +f 126 141 142 +f 120 119 134 +f 120 135 136 +f 113 128 129 +f 121 136 137 +f 114 129 130 +f 123 122 137 +f 135 134 149 +f 135 150 151 +f 128 143 144 +f 136 151 152 +f 130 129 144 +f 138 137 152 +f 130 145 146 +f 138 153 154 +f 132 131 146 +f 140 139 154 +f 132 147 148 +f 140 155 156 +f 134 133 148 +f 142 141 156 +f 153 168 169 +f 147 146 161 +f 155 154 169 +f 147 162 163 +f 155 170 171 +f 149 148 163 +f 156 171 172 +f 150 149 164 +f 150 165 166 +f 143 158 159 +f 151 166 167 +f 145 144 159 +f 153 152 167 +f 145 160 161 +f 165 180 181 +f 158 173 174 +f 166 181 182 +f 159 174 175 +f 168 167 182 +f 160 175 176 +f 168 183 184 +f 162 161 176 +f 169 184 185 +f 162 177 178 +f 171 170 185 +f 164 163 178 +f 172 171 186 +f 165 164 179 +f 177 176 191 +f 185 184 199 +f 177 192 193 +f 185 200 201 +f 179 178 193 +f 186 201 202 +f 180 179 194 +f 180 195 196 +f 174 173 188 +f 181 196 197 +f 175 174 189 +f 183 182 197 +f 175 190 191 +f 183 198 199 +f 195 210 211 +f 188 203 204 +f 196 211 212 +f 189 204 205 +f 198 197 212 +f 190 205 206 +f 198 213 214 +f 192 191 206 +f 200 199 214 +f 192 207 208 +f 200 215 216 +f 194 193 208 +f 201 216 217 +f 195 194 209 +f 215 214 229 +f 207 222 223 +f 215 230 231 +f 209 208 223 +f 216 231 232 +f 210 209 224 +f 210 225 226 +f 203 218 219 +f 211 226 227 +f 204 219 220 +f 213 212 227 +f 205 220 221 +f 213 228 229 +f 207 206 221 +f 226 241 242 +f 220 219 234 +f 228 227 242 +f 220 235 236 +f 228 243 244 +f 222 221 236 +f 230 229 244 +f 222 237 238 +f 230 245 246 +f 224 223 238 +f 232 231 246 +f 225 224 239 +f 225 240 241 +f 218 233 234 +f 245 260 261 +f 239 238 253 +f 246 261 262 +f 240 239 254 +f 240 255 256 +f 233 248 249 +f 241 256 257 +f 234 249 250 +f 243 242 257 +f 235 250 251 +f 243 258 259 +f 237 236 251 +f 245 244 259 +f 237 252 253 +f 249 264 265 +f 258 257 272 +f 250 265 266 +f 258 273 274 +f 252 251 266 +f 260 259 274 +f 252 267 268 +f 260 275 276 +f 254 253 268 +f 261 276 277 +f 255 254 269 +f 255 270 271 +f 249 248 263 +f 256 271 272 +f 269 268 283 +f 277 276 291 +f 270 269 284 +f 270 285 286 +f 264 263 278 +f 271 286 287 +f 264 279 280 +f 273 272 287 +f 265 280 281 +f 273 288 289 +f 267 266 281 +f 275 274 289 +f 267 282 283 +f 275 290 291 +f 288 287 302 +f 280 295 296 +f 288 303 304 +f 282 281 296 +f 290 289 304 +f 282 297 298 +f 290 305 306 +f 284 283 298 +f 292 291 306 +f 285 284 299 +f 285 300 301 +f 278 293 294 +f 286 301 302 +f 279 294 295 +f 306 322 323 +f 300 299 315 +f 300 316 317 +f 293 309 310 +f 301 317 318 +f 294 310 311 +f 303 302 318 +f 295 311 312 +f 303 319 320 +f 297 296 312 +f 305 304 320 +f 297 313 314 +f 305 321 322 +f 299 298 314 +f 311 326 327 +f 319 334 335 +f 312 327 328 +f 321 320 335 +f 313 328 329 +f 321 336 337 +f 315 314 329 +f 322 337 338 +f 316 315 330 +f 316 331 332 +f 309 324 325 +f 317 332 333 +f 310 325 326 +f 319 318 333 +f 331 330 345 +f 331 346 347 +f 324 339 340 +f 332 347 348 +f 325 340 341 +f 334 333 348 +f 326 341 342 +f 334 349 350 +f 328 327 342 +f 336 335 350 +f 328 343 344 +f 336 351 352 +f 330 329 344 +f 337 352 353 +f 349 364 365 +f 343 342 357 +f 351 350 365 +f 343 358 359 +f 351 366 367 +f 345 344 359 +f 352 367 368 +f 346 345 360 +f 346 361 362 +f 339 354 355 +f 347 362 363 +f 341 340 355 +f 349 348 363 +f 341 356 357 +f 361 376 377 +f 354 369 370 +f 362 377 378 +f 356 355 370 +f 364 363 378 +f 356 371 372 +f 364 379 380 +f 358 357 372 +f 366 365 380 +f 358 373 374 +f 366 381 382 +f 360 359 374 +f 367 382 383 +f 361 360 375 +f 381 380 395 +f 373 388 389 +f 381 396 397 +f 375 374 389 +f 382 397 398 +f 376 375 390 +f 376 391 392 +f 369 384 385 +f 377 392 393 +f 371 370 385 +f 379 378 393 +f 371 386 387 +f 379 394 395 +f 373 372 387 +f 385 384 399 +f 392 407 408 +f 385 400 401 +f 394 393 408 +f 386 401 402 +f 394 409 410 +f 388 387 402 +f 396 395 410 +f 388 403 404 +f 397 396 411 +f 390 389 404 +f 397 412 413 +f 391 390 405 +f 391 406 407 +f 403 418 419 +f 411 426 427 +f 405 404 419 +f 412 427 428 +f 406 405 420 +f 406 421 422 +f 399 414 415 +f 407 422 423 +f 401 400 415 +f 409 408 423 +f 401 416 417 +f 409 424 425 +f 403 402 417 +f 411 410 425 +f 422 437 438 +f 416 415 430 +f 424 423 438 +f 416 431 432 +f 424 439 440 +f 418 417 432 +f 426 425 440 +f 418 433 434 +f 426 441 442 +f 420 419 434 +f 427 442 443 +f 421 420 435 +f 421 436 437 +f 414 429 430 +f 441 456 457 +f 435 434 449 +f 442 457 458 +f 436 435 450 +f 436 451 452 +f 430 429 444 +f 437 452 453 +f 430 445 446 +f 439 438 453 +f 431 446 447 +f 439 454 455 +f 433 432 447 +f 441 440 455 +f 433 448 449 +f 446 445 460 +f 454 453 468 +f 446 461 462 +f 454 469 470 +f 448 447 462 +f 456 455 470 +f 448 463 464 +f 456 471 472 +f 450 449 464 +f 458 457 472 +f 451 450 465 +f 451 466 467 +f 444 459 460 +f 452 467 468 +f 465 464 477 +f 472 481 482 +f 466 465 3 +f 466 4 5 +f 460 459 474 +f 467 5 478 +f 461 460 475 +f 468 478 6 +f 462 461 1 +f 469 6 479 +f 463 462 476 +f 471 470 479 +f 464 463 2 +f 471 480 481 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..983d1b63e --- /dev/null +++ b/tests/src/tests/potential/test_arbitrary_point_esp.cpp @@ -0,0 +1,426 @@ +#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()); +} + +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))); + } +} + +TEST_CASE( + "Arbitrary Point ESP: inclusion-exclusion weights on non-closed inputs", + "[esp_potential],[arbitrary_point_esp]") +{ + // 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. + const double dhat = 0.5; + ESPParameters params(dhat); + + SECTION("3D sheet") + { + // 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 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 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); + } + + SECTION("2D polyline") + { + // 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 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 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))); + } + + SECTION("3D polyline in no face") + { + // 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 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 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))); + } + + SECTION("isolated vertices") + { + 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))); + } + + SECTION("2D junction") + { + // 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); + + 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); + + CHECK(potential(V, q) == Catch::Approx((*params.barrier)(-q(1), dhat))); + } +} diff --git a/tests/src/tests/potential/test_barrier_potential.cpp b/tests/src/tests/potential/test_barrier_potential.cpp index 5a82f0403..9262bb4ac 100644 --- a/tests/src/tests/potential/test_barrier_potential.cpp +++ b/tests/src/tests/potential/test_barrier_potential.cpp @@ -533,6 +533,51 @@ 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 auto barrier = std::make_shared(2.0); + const BarrierPotential potential( + barrier, dhat, /*stiffness=*/1.0, /*use_physical_barrier=*/false, + /*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. + CHECK(potential(collision, x) == Catch::Approx((*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)); +} + // -- 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..c4bfab959 --- /dev/null +++ b/tests/src/tests/potential/test_esp_potential.cpp @@ -0,0 +1,1408 @@ +#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 + +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; + 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 / "../src/tests/potential/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 +/// "interior-degree-2" (three interior points, exact for degree 2). +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 == "interior-degree-2"); + return { + { { { 2. / 3, 1. / 6, 1. / 6 } }, 1. / 3 }, + { { { 1. / 6, 2. / 3, 1. / 6 } }, 1. / 3 }, + { { { 1. / 6, 1. / 6, 2. / 3 } }, 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; +} + +} // 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(1.0, 0.7, 0.3); + // Keep dbar = dhat * dbar_factor ≈ 0.15 so the active contact set is + // comparable across dbar_factor values. + const double dhat = 0.15 / dbar_factor; + 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 + fd::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 + fd::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( + fd::flatten(V), + [&](const Eigen::VectorXd& y) { + Eigen::MatrixXd V_ = fd::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( + fd::flatten(V), + [&](const Eigen::VectorXd& y) { + Eigen::MatrixXd V_ = fd::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 / "../src/tests/potential/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( + fd::flatten(V)(indices), + [&](const Eigen::VectorXd& y) { + Eigen::VectorXd y_ = fd::flatten(V); + y_(indices) = y; + Eigen::MatrixXd V_fd = fd::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_CASE( + "Convergent Quadrature Face Hessian", "[esp_potential], [esp_potential_3d]") +{ + const auto method = make_default_broad_phase(); + TriMeshData data = load_wrapped_sphere(); + Eigen::MatrixXd& V = data.V; + Eigen::MatrixXi& F = data.F; + 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 fid = 0; fid < F.rows(); ++fid) { + + size_t num_collision_pairs = 0; + const auto collisions = point_potential.build_collisions_at_face_center( + V, fid, num_collision_pairs); + + if (collisions->size() == 0) { + continue; + } + + Eigen::Vector3 vids; + vids << F(fid, 0), F(fid, 1), F(fid, 2); + + Eigen::RowVector3d face_center = + (V.row(vids[0]) + V.row(vids[1]) + V.row(vids[2])) / 3.; + + VertexMatrixView<3> V_extended(V, face_center); + + std::vector indices; + { + Eigen::VectorXd local_grad = PointPotentialHelper:: + evaluate_potential_gradient_at_face_center_with_cached_collisions( + V_extended, *collisions, params); + indices = collisions->dofs(); + + if (local_grad.norm() < 1e-10) { + continue; + } + } + + Eigen::MatrixXd h = PointPotentialHelper:: + evaluate_potential_hessian_at_face_center_with_cached_collisions( + V_extended, *collisions, params, PSDProjectionMethod::NONE); + + Eigen::MatrixXd fh; + fd::finite_jacobian( + fd::flatten(V)(indices), + [&](const Eigen::VectorXd& y) { + Eigen::VectorXd y_ = fd::flatten(V); + y_(indices) = y; + Eigen::MatrixXd V_fd = fd::unflatten(y_, 3); + Eigen::RowVector3d face_center_fd = + (V_fd.row(vids[0]) + V_fd.row(vids[1]) + V_fd.row(vids[2])) + / 3.; + VertexMatrixView<3> V_fd_extended(V_fd, face_center_fd); + + return PointPotentialHelper:: + evaluate_potential_gradient_at_face_center_with_cached_collisions( + V_fd_extended, *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( + fd::flatten(V), + [&](const Eigen::VectorXd& x) { + return potential(collisions, mesh, fd::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 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 face quadrature rule validation", "[esp_potential]") +{ + ESPParameters params(0.1); + CHECK(params.get_quad_rule().empty()); + + const FaceQuadRule centroid = { { { { 1. / 3, 1. / 3, 1. / 3 } }, 1.0 } }; + CHECK_NOTHROW(params.set_quad_rule(centroid)); + REQUIRE(params.get_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.get_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.get_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.get_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.get_quad_rule().size() == 3); // unchanged on failure + + CHECK_NOTHROW(params.set_quad_rule({})); // back to vertex integration + CHECK(params.get_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 19 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)); + } +} + +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( + fd::flatten(vertices), + [&](const Eigen::VectorXd& x) { + return potential( + collisions, mesh, fd::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( + fd::flatten(vertices), + [&](const Eigen::VectorXd& x) { + return potential.gradient( + collisions, mesh, fd::unflatten(x, vertices.cols())); + }, + fhess, fd::AccuracyOrder::SECOND, 1e-8); + + REQUIRE(hess.squaredNorm() > 1e-8); + CHECK((hess - fhess).norm() / hess.norm() < 1e-3); +} + +TEST_CASE("ESP potential 2D no forces", "[esp_potential], [esp_potential_2d]") +{ + const auto method = make_default_broad_phase(); + Eigen::MatrixXd V; + Eigen::MatrixXi E; + double dhat = 1.; + const int quadrature_order = GENERATE(1, 2, 7, 10, 14); + ESPParameters params(dhat, 1., quadrature_order); + + std::string name; + SECTION("square_1") + { + name = "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") + { + name = "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; + } + SECTION("circle") + { + const int n = GENERATE(5, 6, 7, 8, 9, 10, 50, 100, 200, 2000); + name = "circle" + std::to_string(n); + V.resize(n, 2); + E.resize(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); + } + for (int i = 0; i < n; i++) { + E(i, 0) = i; + E(i, 1) = (i + 1) % n; + } + } + + 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); + CAPTURE(name); + CAPTURE(quadrature_order); + 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); +} + +TEST_CASE( + "ESP potential 2D finite differences", + "[esp_potential], [esp_potential_2d]") +{ + const auto method = make_default_broad_phase(); + Eigen::MatrixXd V; + Eigen::MatrixXi E; + double dhat = 0.6; + constexpr double BA = 0; // a small constant to break perfect alignments + const int quadrature_order = GENERATE(1, 2, 7, 14); + ESPParameters params(dhat, 1., quadrature_order); + CAPTURE(quadrature_order); + + // The Hessian is checked 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. + auto run_checks = [&](const bool check_hessian) { + 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_ = fd::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( + fd::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(fd::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( + fd::flatten(V), + [&](const Eigen::VectorXd& x) { + return potential.gradient( + collisions, mesh, fd::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)); + }; + + SECTION("Corners") + { + const double P0x = GENERATE(.01, -.01, 0.0); + const double P1y = GENERATE(.49, .5, .51); + CAPTURE(P0x, P1y); + V.resize(8, 2); + E.resize(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; + run_checks(true); + } + + 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. + run_checks(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. + run_checks(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. + run_checks(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 orders 7 and 14. + run_checks(false); + } +} + +// 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()). +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", "interior-degree-2"); + 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 + fd::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 + fd::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; dbar_factor = 1 is a real near/far +// split. 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, which hid a gap at dbar_factor = 1. +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 = GENERATE(1.0, 0.7, 0.4, 0.1); + 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)); + + 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); + } + } + }; + + 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"); + } +} + +// 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 = + GENERATE(as(), "vertices+centroid", "interior-degree-2"); + const double dbar_factor = GENERATE(1.0, 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); +} 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/potential/wrapped_sphere.obj b/tests/src/tests/potential/wrapped_sphere.obj new file mode 100644 index 000000000..42a573e79 --- /dev/null +++ b/tests/src/tests/potential/wrapped_sphere.obj @@ -0,0 +1,2533 @@ +# Blender 4.2.0 +# www.blender.org +mtllib wrapped_sphere.mtl +o Sphere +v 0.000000 0.555570 0.145652 +v 0.000000 0.831470 0.538536 +v 0.000000 0.980785 0.195090 +v 0.000000 1.000000 0.000000 +v 0.000000 0.980785 -0.195090 +v 0.000000 0.831470 -0.555570 +v 0.038060 0.191342 -0.642302 +v 0.074658 0.375330 -0.270429 +v 0.108386 0.544895 0.145652 +v 0.137950 0.693520 0.455784 +v 0.162212 0.815493 0.538536 +v 0.180240 0.906127 0.382683 +v 0.191342 0.961940 0.195090 +v 0.195090 0.980785 0.000000 +v 0.191342 0.961940 -0.195090 +v 0.180240 0.906127 -0.382683 +v 0.162212 0.815493 -0.555570 +v 0.137950 0.693520 -0.707107 +v 0.108386 0.544895 -0.831470 +v 0.074658 0.375330 -0.923880 +v 0.038060 0.191342 -0.980785 +v 0.074658 0.180240 -0.642301 +v 0.146447 0.353553 -0.270429 +v 0.212608 0.513280 0.145651 +v 0.270598 0.653281 0.455784 +v 0.318190 0.768178 0.538536 +v 0.353553 0.853553 0.382683 +v 0.375330 0.906127 0.195090 +v 0.382683 0.923879 0.000000 +v 0.375330 0.906127 -0.195090 +v 0.353553 0.853553 -0.382683 +v 0.318190 0.768178 -0.555570 +v 0.270598 0.653281 -0.707107 +v 0.212608 0.513280 -0.831470 +v 0.146447 0.353553 -0.923880 +v 0.074658 0.180240 -0.980785 +v 0.108386 0.162212 -0.642301 +v 0.212608 0.318190 -0.270429 +v 0.308658 0.461940 0.145651 +v 0.392847 0.587938 0.455784 +v 0.461940 0.691342 0.538536 +v 0.513280 0.768178 0.382683 +v 0.544895 0.815493 0.195090 +v 0.555570 0.831469 0.000000 +v 0.544895 0.815493 -0.195090 +v 0.513280 0.768178 -0.382683 +v 0.461940 0.691342 -0.555570 +v 0.392847 0.587938 -0.707107 +v 0.308658 0.461940 -0.831470 +v 0.212608 0.318190 -0.923880 +v 0.108386 0.162212 -0.980785 +v 0.137950 0.137950 -0.642301 +v 0.270598 0.270598 -0.270429 +v 0.392847 0.392847 0.145651 +v 0.500000 0.500000 0.455784 +v 0.587938 0.587938 0.538536 +v 0.653281 0.653281 0.382683 +v 0.693520 0.693520 0.195090 +v 0.707107 0.707107 0.000000 +v 0.693520 0.693520 -0.195090 +v 0.653281 0.653281 -0.382683 +v 0.587938 0.587938 -0.555570 +v 0.500000 0.500000 -0.707107 +v 0.392847 0.392847 -0.831470 +v 0.270598 0.270598 -0.923880 +v 0.137950 0.137950 -0.980785 +v 0.162212 0.108386 -0.642301 +v 0.318190 0.212608 -0.270429 +v 0.461940 0.308658 0.145651 +v 0.587938 0.392847 0.455784 +v 0.691342 0.461940 0.538536 +v 0.768178 0.513280 0.382683 +v 0.815493 0.544895 0.195090 +v 0.831470 0.555570 0.000000 +v 0.815493 0.544895 -0.195090 +v 0.768178 0.513280 -0.382683 +v 0.691342 0.461940 -0.555570 +v 0.587938 0.392847 -0.707107 +v 0.461940 0.308658 -0.831470 +v 0.318190 0.212608 -0.923880 +v 0.162212 0.108386 -0.980785 +v 0.000000 0.000000 -0.803873 +v 0.180240 0.074658 -0.642301 +v 0.353553 0.146447 -0.270429 +v 0.513280 0.212607 0.145651 +v 0.653281 0.270598 0.455784 +v 0.768178 0.318190 0.538536 +v 0.853553 0.353553 0.382683 +v 0.906127 0.375330 0.195090 +v 0.923879 0.382683 0.000000 +v 0.906127 0.375330 -0.195090 +v 0.853553 0.353553 -0.382683 +v 0.768178 0.318190 -0.555570 +v 0.653281 0.270598 -0.707107 +v 0.513280 0.212607 -0.831470 +v 0.353553 0.146447 -0.923880 +v 0.180240 0.074658 -0.980785 +v 0.191342 0.038060 -0.642301 +v 0.375330 0.074658 -0.270429 +v 0.544895 0.108386 0.145651 +v 0.693520 0.137950 0.455784 +v 0.815493 0.162212 0.538536 +v 0.906127 0.180240 0.382683 +v 0.961940 0.191342 0.195090 +v 0.980785 0.195090 0.000000 +v 0.961940 0.191342 -0.195090 +v 0.906127 0.180240 -0.382683 +v 0.815493 0.162212 -0.555570 +v 0.693520 0.137950 -0.707107 +v 0.544895 0.108386 -0.831470 +v 0.375330 0.074658 -0.923880 +v 0.191342 0.038060 -0.980785 +v 0.195090 -0.000000 -0.642302 +v 0.382683 -0.000000 -0.270429 +v 0.555570 -0.000000 0.145651 +v 0.707107 0.000000 0.455784 +v 0.831469 -0.000000 0.538536 +v 0.923879 0.000000 0.382683 +v 0.980785 -0.000000 0.195090 +v 1.000000 -0.000000 0.000000 +v 0.980785 -0.000000 -0.195090 +v 0.923879 0.000000 -0.382683 +v 0.831469 -0.000000 -0.555570 +v 0.707107 0.000000 -0.707107 +v 0.555570 -0.000000 -0.831470 +v 0.382683 -0.000000 -0.923880 +v 0.195090 -0.000000 -0.980785 +v 0.191342 -0.038060 -0.642302 +v 0.375330 -0.074658 -0.270429 +v 0.544895 -0.108386 0.145651 +v 0.693520 -0.137950 0.455784 +v 0.815493 -0.162212 0.538536 +v 0.906127 -0.180240 0.382683 +v 0.961940 -0.191342 0.195090 +v 0.980785 -0.195090 0.000000 +v 0.961940 -0.191342 -0.195090 +v 0.906127 -0.180240 -0.382683 +v 0.815493 -0.162212 -0.555570 +v 0.693520 -0.137950 -0.707107 +v 0.544895 -0.108386 -0.831470 +v 0.375330 -0.074658 -0.923880 +v 0.191342 -0.038060 -0.980785 +v 0.180240 -0.074658 -0.642302 +v 0.353553 -0.146447 -0.270429 +v 0.513280 -0.212608 0.145651 +v 0.653281 -0.270598 0.455784 +v 0.768178 -0.318190 0.538536 +v 0.853553 -0.353553 0.382683 +v 0.906127 -0.375330 0.195090 +v 0.923879 -0.382683 0.000000 +v 0.906127 -0.375330 -0.195090 +v 0.853553 -0.353553 -0.382683 +v 0.768178 -0.318190 -0.555570 +v 0.653281 -0.270598 -0.707107 +v 0.513280 -0.212608 -0.831470 +v 0.353553 -0.146447 -0.923880 +v 0.180240 -0.074658 -0.980785 +v 0.162212 -0.108386 -0.642302 +v 0.318190 -0.212608 -0.270429 +v 0.461940 -0.308658 0.145651 +v 0.587938 -0.392847 0.455784 +v 0.691341 -0.461940 0.538536 +v 0.768178 -0.513280 0.382683 +v 0.815493 -0.544895 0.195090 +v 0.831469 -0.555570 0.000000 +v 0.815493 -0.544895 -0.195090 +v 0.768178 -0.513280 -0.382683 +v 0.691341 -0.461940 -0.555570 +v 0.587938 -0.392847 -0.707107 +v 0.461940 -0.308658 -0.831470 +v 0.318190 -0.212608 -0.923880 +v 0.162212 -0.108386 -0.980785 +v 0.137950 -0.137950 -0.642302 +v 0.270598 -0.270598 -0.270429 +v 0.392847 -0.392847 0.145651 +v 0.500000 -0.500000 0.455784 +v 0.587938 -0.587938 0.538535 +v 0.653281 -0.653281 0.382683 +v 0.693520 -0.693520 0.195090 +v 0.707106 -0.707107 0.000000 +v 0.693520 -0.693520 -0.195090 +v 0.653281 -0.653281 -0.382683 +v 0.587938 -0.587938 -0.555570 +v 0.500000 -0.500000 -0.707107 +v 0.392847 -0.392847 -0.831470 +v 0.270598 -0.270598 -0.923880 +v 0.137950 -0.137950 -0.980785 +v 0.108386 -0.162212 -0.642302 +v 0.212607 -0.318190 -0.270429 +v 0.308658 -0.461940 0.145651 +v 0.392847 -0.587938 0.455784 +v 0.461940 -0.691342 0.538536 +v 0.513280 -0.768178 0.382683 +v 0.544895 -0.815493 0.195090 +v 0.555570 -0.831469 0.000000 +v 0.544895 -0.815493 -0.195090 +v 0.513280 -0.768178 -0.382683 +v 0.461940 -0.691342 -0.555570 +v 0.392847 -0.587938 -0.707107 +v 0.308658 -0.461940 -0.831470 +v 0.212607 -0.318190 -0.923880 +v 0.108386 -0.162212 -0.980785 +v 0.074658 -0.180240 -0.642302 +v 0.146447 -0.353553 -0.270429 +v 0.212607 -0.513280 0.145651 +v 0.270598 -0.653281 0.455784 +v 0.318189 -0.768178 0.538535 +v 0.353553 -0.853553 0.382683 +v 0.375330 -0.906127 0.195090 +v 0.382683 -0.923879 0.000000 +v 0.375330 -0.906127 -0.195090 +v 0.353553 -0.853553 -0.382683 +v 0.318189 -0.768178 -0.555570 +v 0.270598 -0.653281 -0.707107 +v 0.212607 -0.513280 -0.831470 +v 0.146447 -0.353553 -0.923880 +v 0.074658 -0.180240 -0.980785 +v 0.038060 -0.191342 -0.642302 +v 0.074658 -0.375330 -0.270429 +v 0.108386 -0.544895 0.145651 +v 0.137950 -0.693520 0.455784 +v 0.162212 -0.815493 0.538535 +v 0.180240 -0.906127 0.382683 +v 0.191342 -0.961939 0.195090 +v 0.195090 -0.980785 0.000000 +v 0.191342 -0.961939 -0.195090 +v 0.180240 -0.906127 -0.382683 +v 0.162212 -0.815493 -0.555570 +v 0.137950 -0.693520 -0.707107 +v 0.108386 -0.544895 -0.831470 +v 0.074658 -0.375330 -0.923880 +v 0.038060 -0.191342 -0.980785 +v -0.000000 -0.195090 -0.642302 +v -0.000000 -0.382683 -0.270429 +v -0.000000 -0.555570 0.145651 +v -0.000000 -0.707107 0.455784 +v -0.000000 -0.831469 0.538535 +v 0.000000 -0.923879 0.382683 +v -0.000000 -0.980785 0.195090 +v -0.000000 -0.999999 0.000000 +v -0.000000 -0.980785 -0.195090 +v 0.000000 -0.923879 -0.382683 +v -0.000000 -0.831469 -0.555570 +v -0.000000 -0.707107 -0.707107 +v -0.000000 -0.555570 -0.831470 +v -0.000000 -0.382683 -0.923880 +v -0.000000 -0.195090 -0.980785 +v -0.038060 -0.191342 -0.642302 +v -0.074658 -0.375330 -0.270429 +v -0.108386 -0.544895 0.145651 +v -0.137950 -0.693520 0.455784 +v -0.162212 -0.815493 0.538535 +v -0.180240 -0.906127 0.382683 +v -0.191342 -0.961939 0.195090 +v -0.195091 -0.980785 0.000000 +v -0.191342 -0.961939 -0.195090 +v -0.180240 -0.906127 -0.382683 +v -0.162212 -0.815493 -0.555570 +v -0.137950 -0.693520 -0.707107 +v -0.108386 -0.544895 -0.831470 +v -0.074658 -0.375330 -0.923880 +v -0.038060 -0.191342 -0.980785 +v -0.074658 -0.180240 -0.642302 +v -0.146447 -0.353553 -0.270429 +v -0.212608 -0.513280 0.145651 +v -0.270598 -0.653281 0.455784 +v -0.318190 -0.768177 0.538535 +v -0.353553 -0.853553 0.382683 +v -0.375330 -0.906127 0.195090 +v -0.382683 -0.923879 0.000000 +v -0.375330 -0.906127 -0.195090 +v -0.353553 -0.853553 -0.382683 +v -0.318190 -0.768177 -0.555570 +v -0.270598 -0.653281 -0.707107 +v -0.212608 -0.513280 -0.831470 +v -0.146447 -0.353553 -0.923880 +v -0.074658 -0.180240 -0.980785 +v -0.108386 -0.162212 -0.642302 +v -0.212608 -0.318190 -0.270429 +v -0.308658 -0.461939 0.145651 +v -0.392847 -0.587938 0.455784 +v -0.461940 -0.691341 0.538535 +v -0.513280 -0.768178 0.382683 +v -0.544895 -0.815493 0.195090 +v -0.555570 -0.831469 0.000000 +v -0.544895 -0.815493 -0.195090 +v -0.513280 -0.768178 -0.382683 +v -0.461940 -0.691341 -0.555570 +v -0.392847 -0.587938 -0.707107 +v -0.308658 -0.461939 -0.831470 +v -0.212608 -0.318190 -0.923880 +v -0.108386 -0.162212 -0.980785 +v -0.137950 -0.137950 -0.642302 +v -0.270598 -0.270598 -0.270429 +v -0.392847 -0.392847 0.145650 +v -0.500000 -0.500000 0.455784 +v -0.587938 -0.587937 0.538535 +v -0.653281 -0.653281 0.382683 +v -0.693520 -0.693520 0.195090 +v -0.707106 -0.707106 0.000000 +v -0.693520 -0.693520 -0.195090 +v -0.653281 -0.653281 -0.382683 +v -0.587938 -0.587937 -0.555570 +v -0.500000 -0.500000 -0.707107 +v -0.392847 -0.392847 -0.831470 +v -0.270598 -0.270598 -0.923880 +v -0.137950 -0.137950 -0.980785 +v 0.000000 0.000000 -1.000000 +v -0.162212 -0.108386 -0.642302 +v -0.318190 -0.212607 -0.270430 +v -0.461940 -0.308658 0.145650 +v -0.587938 -0.392847 0.455784 +v -0.691341 -0.461939 0.538535 +v -0.768177 -0.513280 0.382683 +v -0.815493 -0.544895 0.195090 +v -0.831469 -0.555569 0.000000 +v -0.815493 -0.544895 -0.195090 +v -0.768177 -0.513280 -0.382683 +v -0.691341 -0.461939 -0.555570 +v -0.587938 -0.392847 -0.707107 +v -0.461940 -0.308658 -0.831470 +v -0.318190 -0.212607 -0.923880 +v -0.162212 -0.108386 -0.980785 +v -0.180240 -0.074658 -0.642302 +v -0.353553 -0.146447 -0.270430 +v -0.513280 -0.212607 0.145650 +v -0.653281 -0.270598 0.455784 +v -0.768177 -0.318189 0.538535 +v -0.853553 -0.353553 0.382683 +v -0.906127 -0.375330 0.195090 +v -0.923879 -0.382683 0.000000 +v -0.906127 -0.375330 -0.195090 +v -0.853553 -0.353553 -0.382683 +v -0.768177 -0.318189 -0.555570 +v -0.653281 -0.270598 -0.707107 +v -0.513280 -0.212607 -0.831470 +v -0.353553 -0.146447 -0.923880 +v -0.180240 -0.074658 -0.980785 +v -0.191342 -0.038060 -0.642302 +v -0.375330 -0.074658 -0.270430 +v -0.544895 -0.108386 0.145650 +v -0.693520 -0.137950 0.455784 +v -0.815493 -0.162211 0.538535 +v -0.906127 -0.180240 0.382683 +v -0.961939 -0.191341 0.195090 +v -0.980784 -0.195090 0.000000 +v -0.961939 -0.191341 -0.195090 +v -0.906127 -0.180240 -0.382683 +v -0.815493 -0.162211 -0.555570 +v -0.693520 -0.137950 -0.707107 +v -0.544895 -0.108386 -0.831470 +v -0.375330 -0.074658 -0.923880 +v -0.191342 -0.038060 -0.980785 +v -0.195090 0.000000 -0.642302 +v -0.382683 0.000000 -0.270430 +v -0.555570 0.000000 0.145650 +v -0.707107 0.000000 0.455784 +v -0.831469 0.000000 0.538535 +v -0.923879 0.000000 0.382683 +v -0.980785 0.000000 0.195090 +v -0.999999 0.000000 0.000000 +v -0.980785 0.000000 -0.195090 +v -0.923879 0.000000 -0.382683 +v -0.831469 0.000000 -0.555570 +v -0.707107 0.000000 -0.707107 +v -0.555570 0.000000 -0.831470 +v -0.382683 0.000000 -0.923880 +v -0.195090 0.000000 -0.980785 +v -0.191342 0.038060 -0.642302 +v -0.375330 0.074658 -0.270430 +v -0.544895 0.108386 0.145650 +v -0.693520 0.137950 0.455784 +v -0.815493 0.162212 0.538535 +v -0.906127 0.180240 0.382683 +v -0.961939 0.191342 0.195090 +v -0.980784 0.195091 0.000000 +v -0.961939 0.191342 -0.195090 +v -0.906127 0.180240 -0.382683 +v -0.815493 0.162212 -0.555570 +v -0.693520 0.137950 -0.707107 +v -0.544895 0.108386 -0.831470 +v -0.375330 0.074658 -0.923880 +v -0.191342 0.038060 -0.980785 +v -0.180240 0.074658 -0.642302 +v -0.353553 0.146447 -0.270430 +v -0.513279 0.212607 0.145650 +v -0.653281 0.270598 0.455784 +v -0.768177 0.318190 0.538535 +v -0.853553 0.353553 0.382683 +v -0.906127 0.375330 0.195090 +v -0.923878 0.382683 0.000000 +v -0.906127 0.375330 -0.195090 +v -0.853553 0.353553 -0.382683 +v -0.768177 0.318190 -0.555570 +v -0.653281 0.270598 -0.707107 +v -0.513279 0.212607 -0.831470 +v -0.353553 0.146447 -0.923880 +v -0.180240 0.074658 -0.980785 +v -0.162212 0.108386 -0.642302 +v -0.318189 0.212607 -0.270430 +v -0.461939 0.308658 0.145650 +v -0.587938 0.392847 0.455784 +v -0.691341 0.461940 0.538535 +v -0.768177 0.513280 0.382683 +v -0.815493 0.544895 0.195090 +v -0.831468 0.555570 0.000000 +v -0.815493 0.544895 -0.195090 +v -0.768177 0.513280 -0.382683 +v -0.691341 0.461940 -0.555570 +v -0.587938 0.392847 -0.707107 +v -0.461939 0.308658 -0.831470 +v -0.318189 0.212607 -0.923880 +v -0.162212 0.108386 -0.980785 +v -0.137950 0.137950 -0.642302 +v -0.270598 0.270598 -0.270430 +v -0.392847 0.392847 0.145650 +v -0.500000 0.500000 0.455784 +v -0.587937 0.587938 0.538535 +v -0.653281 0.653281 0.382683 +v -0.693519 0.693520 0.195090 +v -0.707106 0.707106 0.000000 +v -0.693519 0.693520 -0.195090 +v -0.653281 0.653281 -0.382683 +v -0.587937 0.587938 -0.555570 +v -0.500000 0.500000 -0.707107 +v -0.392847 0.392847 -0.831470 +v -0.270598 0.270598 -0.923880 +v -0.137950 0.137950 -0.980785 +v -0.108386 0.162212 -0.642302 +v -0.212607 0.318190 -0.270430 +v -0.308658 0.461939 0.145650 +v -0.392847 0.587938 0.455784 +v -0.461939 0.691341 0.538535 +v -0.513280 0.768177 0.382683 +v -0.544895 0.815493 0.195090 +v -0.555569 0.831469 0.000000 +v -0.544895 0.815493 -0.195090 +v -0.513280 0.768177 -0.382683 +v -0.461939 0.691341 -0.555570 +v -0.392847 0.587938 -0.707107 +v -0.308658 0.461939 -0.831470 +v -0.212607 0.318190 -0.923880 +v -0.108386 0.162212 -0.980785 +v -0.074658 0.180240 -0.642302 +v -0.146446 0.353553 -0.270430 +v -0.212607 0.513279 0.145650 +v -0.270598 0.653281 0.455784 +v -0.318189 0.768177 0.538535 +v -0.353553 0.853553 0.382683 +v -0.375330 0.906127 0.195090 +v -0.382683 0.923879 0.000000 +v -0.375330 0.906127 -0.195090 +v -0.353553 0.853553 -0.382683 +v -0.318189 0.768177 -0.555570 +v -0.270598 0.653281 -0.707107 +v -0.212607 0.513279 -0.831470 +v -0.146446 0.353553 -0.923880 +v -0.074658 0.180240 -0.980785 +v -0.038060 0.191342 -0.642302 +v -0.074658 0.375330 -0.270430 +v -0.108386 0.544895 0.145650 +v -0.137950 0.693520 0.455784 +v -0.162211 0.815493 0.538535 +v -0.180240 0.906127 0.382683 +v -0.191341 0.961939 0.195090 +v -0.195090 0.980784 0.000000 +v -0.191341 0.961939 -0.195090 +v -0.180240 0.906127 -0.382683 +v -0.162211 0.815493 -0.555570 +v -0.137950 0.693520 -0.707107 +v -0.108386 0.544895 -0.831470 +v -0.074658 0.375330 -0.923880 +v -0.038060 0.191342 -0.980785 +v 0.000000 0.195090 -0.642302 +v 0.000000 0.382683 -0.270430 +v 0.000000 0.707107 0.455784 +v 0.000000 0.923879 0.382683 +v 0.000000 0.923879 -0.382683 +v 0.000000 0.707107 -0.707107 +v 0.000000 0.555570 -0.831470 +v 0.000000 0.382683 -0.923880 +v 0.000000 0.195090 -0.980785 +vn -0.0876 -0.8894 0.4487 +vn 0.0938 0.9527 -0.2890 +vn -0.0906 -0.9197 0.3821 +vn 0.0865 0.8786 -0.4696 +vn -0.0881 -0.8950 0.4373 +vn 0.0759 0.7708 -0.6326 +vn -0.0545 -0.5531 0.8313 +vn 0.0624 0.6332 -0.7715 +vn 0.0844 0.8571 0.5082 +vn 0.0464 0.4709 -0.8810 +vn 0.0938 0.9527 0.2890 +vn 0.0286 0.2902 -0.9565 +vn 0.0975 0.9904 0.0975 +vn -0.0627 -0.6366 0.7687 +vn 0.0097 0.0980 -0.9951 +vn 0.0975 0.9904 -0.0976 +vn 0.2889 0.9524 0.0976 +vn -0.1857 -0.6121 0.7687 +vn 0.0286 0.0942 -0.9951 +vn 0.2889 0.9524 -0.0976 +vn -0.2594 -0.8552 0.4487 +vn 0.2779 0.9161 -0.2890 +vn -0.2683 -0.8843 0.3821 +vn 0.2563 0.8448 -0.4696 +vn -0.2611 -0.8606 0.4373 +vn 0.2248 0.7412 -0.6326 +vn -0.1613 -0.5319 0.8313 +vn 0.1847 0.6088 -0.7715 +vn 0.2500 0.8242 0.5082 +vn 0.1374 0.4528 -0.8810 +vn 0.2779 0.9161 0.2890 +vn 0.0846 0.2790 -0.9565 +vn 0.3651 0.6831 -0.6326 +vn -0.2620 -0.4902 0.8313 +vn 0.2999 0.5611 -0.7715 +vn 0.4060 0.7595 0.5082 +vn 0.2230 0.4173 -0.8810 +vn 0.4513 0.8443 0.2890 +vn 0.1374 0.2571 -0.9565 +vn 0.4691 0.8777 0.0975 +vn -0.3015 -0.5641 0.7687 +vn 0.0464 0.0869 -0.9951 +vn 0.4691 0.8777 -0.0975 +vn -0.4213 -0.7882 0.4487 +vn 0.4513 0.8443 -0.2890 +vn -0.4356 -0.8150 0.3821 +vn 0.4162 0.7786 -0.4696 +vn -0.4239 -0.7931 0.4373 +vn 0.0625 0.0761 -0.9951 +vn 0.6314 0.7693 -0.0975 +vn -0.5670 -0.6908 0.4487 +vn 0.6073 0.7400 -0.2890 +vn -0.5862 -0.7143 0.3821 +vn 0.5601 0.6825 -0.4696 +vn -0.5705 -0.6952 0.4373 +vn 0.4913 0.5987 -0.6326 +vn -0.3526 -0.4297 0.8313 +vn 0.4036 0.4918 -0.7715 +vn 0.5464 0.6657 0.5082 +vn 0.3002 0.3658 -0.8810 +vn 0.6073 0.7400 0.2890 +vn 0.1850 0.2254 -0.9565 +vn 0.6314 0.7693 0.0975 +vn -0.4058 -0.4945 0.7687 +vn 0.4918 0.4036 -0.7715 +vn 0.6657 0.5464 0.5082 +vn 0.3658 0.3002 -0.8810 +vn 0.7400 0.6073 0.2890 +vn 0.2254 0.1850 -0.9566 +vn 0.7693 0.6314 0.0975 +vn -0.4945 -0.4058 0.7687 +vn 0.0761 0.0625 -0.9951 +vn 0.7693 0.6314 -0.0975 +vn -0.6908 -0.5670 0.4487 +vn 0.7400 0.6073 -0.2890 +vn -0.7143 -0.5862 0.3821 +vn 0.6825 0.5601 -0.4696 +vn -0.6952 -0.5705 0.4373 +vn 0.5987 0.4913 -0.6326 +vn -0.4297 -0.3526 0.8313 +vn -0.7882 -0.4213 0.4487 +vn 0.8443 0.4513 -0.2890 +vn -0.8150 -0.4356 0.3821 +vn 0.7786 0.4162 -0.4696 +vn -0.7931 -0.4239 0.4373 +vn 0.6831 0.3651 -0.6326 +vn -0.4902 -0.2620 0.8313 +vn 0.5611 0.2999 -0.7715 +vn 0.7595 0.4060 0.5082 +vn 0.4173 0.2230 -0.8810 +vn 0.8443 0.4513 0.2890 +vn 0.2571 0.1374 -0.9565 +vn 0.8777 0.4691 0.0975 +vn -0.5641 -0.3015 0.7687 +vn 0.0869 0.0464 -0.9951 +vn 0.8777 0.4691 -0.0975 +vn 0.8242 0.2500 0.5082 +vn 0.4528 0.1374 -0.8810 +vn 0.9161 0.2779 0.2890 +vn 0.2790 0.0846 -0.9565 +vn 0.9524 0.2889 0.0975 +vn -0.6121 -0.1857 0.7687 +vn 0.0942 0.0286 -0.9951 +vn 0.9524 0.2889 -0.0975 +vn -0.8552 -0.2594 0.4487 +vn 0.9161 0.2779 -0.2890 +vn -0.8843 -0.2683 0.3821 +vn 0.8448 0.2563 -0.4696 +vn -0.8606 -0.2611 0.4373 +vn 0.7412 0.2248 -0.6326 +vn -0.5319 -0.1613 0.8313 +vn 0.6088 0.1847 -0.7715 +vn 0.9527 0.0938 -0.2890 +vn -0.9197 -0.0906 0.3821 +vn 0.8786 0.0865 -0.4696 +vn -0.8950 -0.0881 0.4373 +vn 0.7708 0.0759 -0.6326 +vn -0.5531 -0.0545 0.8313 +vn 0.6332 0.0624 -0.7715 +vn 0.8571 0.0844 0.5082 +vn 0.4709 0.0464 -0.8810 +vn 0.9527 0.0938 0.2890 +vn 0.2902 0.0286 -0.9565 +vn 0.9904 0.0975 0.0975 +vn -0.6366 -0.0627 0.7687 +vn 0.0980 0.0097 -0.9951 +vn 0.9904 0.0975 -0.0975 +vn -0.8894 -0.0876 0.4487 +vn 0.4709 -0.0464 -0.8810 +vn 0.9527 -0.0938 0.2890 +vn 0.2902 -0.0286 -0.9565 +vn 0.9904 -0.0976 0.0975 +vn -0.6366 0.0627 0.7687 +vn 0.0980 -0.0097 -0.9951 +vn 0.9904 -0.0976 -0.0975 +vn -0.8894 0.0876 0.4487 +vn 0.9527 -0.0938 -0.2890 +vn -0.9197 0.0906 0.3821 +vn 0.8786 -0.0865 -0.4696 +vn -0.8950 0.0881 0.4373 +vn 0.7708 -0.0759 -0.6326 +vn -0.5531 0.0545 0.8313 +vn 0.6332 -0.0624 -0.7715 +vn 0.8571 -0.0844 0.5082 +vn -0.8843 0.2683 0.3821 +vn 0.8448 -0.2563 -0.4696 +vn -0.8606 0.2611 0.4373 +vn 0.7412 -0.2248 -0.6326 +vn -0.5319 0.1613 0.8313 +vn 0.6088 -0.1847 -0.7715 +vn 0.8242 -0.2500 0.5082 +vn 0.4528 -0.1374 -0.8810 +vn 0.9161 -0.2779 0.2890 +vn 0.2790 -0.0846 -0.9565 +vn 0.9524 -0.2889 0.0975 +vn -0.6121 0.1857 0.7687 +vn 0.0942 -0.0286 -0.9951 +vn 0.9524 -0.2889 -0.0975 +vn -0.8552 0.2594 0.4487 +vn 0.9161 -0.2779 -0.2890 +vn 0.8443 -0.4513 0.2890 +vn 0.2571 -0.1374 -0.9565 +vn 0.8777 -0.4691 0.0975 +vn -0.5641 0.3015 0.7687 +vn 0.0869 -0.0464 -0.9951 +vn 0.8777 -0.4691 -0.0975 +vn -0.7882 0.4213 0.4487 +vn 0.8443 -0.4513 -0.2890 +vn -0.8150 0.4356 0.3821 +vn 0.7786 -0.4162 -0.4696 +vn -0.7931 0.4239 0.4373 +vn 0.6831 -0.3651 -0.6326 +vn -0.4902 0.2620 0.8313 +vn 0.5611 -0.2999 -0.7715 +vn 0.7595 -0.4060 0.5082 +vn 0.4173 -0.2230 -0.8810 +vn 0.6825 -0.5601 -0.4696 +vn -0.6952 0.5705 0.4373 +vn 0.5987 -0.4913 -0.6326 +vn -0.4297 0.3526 0.8313 +vn 0.4918 -0.4036 -0.7715 +vn 0.6657 -0.5464 0.5082 +vn 0.3658 -0.3002 -0.8810 +vn 0.7400 -0.6073 0.2890 +vn 0.2254 -0.1850 -0.9565 +vn 0.7693 -0.6314 0.0975 +vn -0.4945 0.4058 0.7687 +vn 0.0761 -0.0625 -0.9951 +vn 0.7693 -0.6314 -0.0975 +vn -0.6908 0.5670 0.4487 +vn 0.7400 -0.6073 -0.2890 +vn -0.7143 0.5862 0.3821 +vn 0.1850 -0.2254 -0.9565 +vn 0.6314 -0.7693 0.0975 +vn -0.4058 0.4945 0.7687 +vn 0.0625 -0.0761 -0.9951 +vn 0.6314 -0.7693 -0.0975 +vn -0.5670 0.6908 0.4487 +vn 0.6073 -0.7400 -0.2890 +vn -0.5862 0.7143 0.3821 +vn 0.5601 -0.6825 -0.4696 +vn -0.5705 0.6952 0.4373 +vn 0.4913 -0.5987 -0.6326 +vn -0.3526 0.4297 0.8313 +vn 0.4036 -0.4918 -0.7715 +vn 0.5464 -0.6657 0.5082 +vn 0.3002 -0.3658 -0.8810 +vn 0.6073 -0.7400 0.2890 +vn 0.3651 -0.6831 -0.6326 +vn -0.2620 0.4902 0.8313 +vn 0.2999 -0.5611 -0.7715 +vn 0.4060 -0.7595 0.5082 +vn 0.2230 -0.4173 -0.8810 +vn 0.4513 -0.8443 0.2890 +vn 0.1374 -0.2571 -0.9565 +vn 0.4691 -0.8777 0.0975 +vn -0.3015 0.5641 0.7687 +vn 0.0464 -0.0869 -0.9951 +vn 0.4691 -0.8777 -0.0975 +vn -0.4213 0.7882 0.4487 +vn 0.4513 -0.8443 -0.2890 +vn -0.4356 0.8150 0.3821 +vn 0.4162 -0.7786 -0.4696 +vn -0.4239 0.7931 0.4373 +vn -0.1857 0.6121 0.7687 +vn 0.0286 -0.0942 -0.9951 +vn 0.2889 -0.9524 -0.0975 +vn -0.2594 0.8552 0.4487 +vn 0.2779 -0.9161 -0.2890 +vn -0.2683 0.8843 0.3821 +vn 0.2563 -0.8448 -0.4696 +vn -0.2611 0.8606 0.4373 +vn 0.2248 -0.7412 -0.6326 +vn -0.1613 0.5319 0.8313 +vn 0.1847 -0.6088 -0.7715 +vn 0.2500 -0.8242 0.5082 +vn 0.1374 -0.4528 -0.8810 +vn 0.2779 -0.9161 0.2890 +vn 0.0846 -0.2790 -0.9565 +vn 0.2889 -0.9524 0.0975 +vn -0.0545 0.5531 0.8313 +vn 0.0624 -0.6332 -0.7715 +vn 0.0844 -0.8571 0.5082 +vn 0.0464 -0.4709 -0.8810 +vn 0.0938 -0.9527 0.2890 +vn 0.0286 -0.2902 -0.9565 +vn 0.0975 -0.9904 0.0975 +vn -0.0627 0.6366 0.7687 +vn 0.0097 -0.0980 -0.9951 +vn 0.0975 -0.9904 -0.0975 +vn -0.0876 0.8894 0.4487 +vn 0.0938 -0.9527 -0.2890 +vn -0.0906 0.9197 0.3821 +vn 0.0865 -0.8786 -0.4696 +vn -0.0881 0.8950 0.4373 +vn 0.0759 -0.7708 -0.6326 +vn -0.0975 -0.9904 -0.0975 +vn 0.0876 0.8894 0.4487 +vn -0.0938 -0.9527 -0.2890 +vn 0.0906 0.9197 0.3821 +vn -0.0865 -0.8786 -0.4696 +vn 0.0881 0.8950 0.4373 +vn -0.0759 -0.7708 -0.6326 +vn 0.0545 0.5531 0.8313 +vn -0.0624 -0.6332 -0.7715 +vn -0.0844 -0.8571 0.5082 +vn -0.0464 -0.4709 -0.8810 +vn -0.0938 -0.9527 0.2890 +vn -0.0286 -0.2902 -0.9565 +vn -0.0976 -0.9904 0.0975 +vn 0.0627 0.6366 0.7687 +vn -0.0097 -0.0980 -0.9951 +vn -0.1847 -0.6088 -0.7715 +vn -0.2500 -0.8242 0.5082 +vn -0.1374 -0.4528 -0.8810 +vn -0.2779 -0.9161 0.2890 +vn -0.0846 -0.2790 -0.9565 +vn -0.2889 -0.9524 0.0975 +vn 0.1857 0.6121 0.7687 +vn -0.0286 -0.0942 -0.9951 +vn -0.2889 -0.9524 -0.0975 +vn 0.2594 0.8552 0.4487 +vn -0.2779 -0.9161 -0.2890 +vn 0.2683 0.8843 0.3821 +vn -0.2563 -0.8448 -0.4696 +vn 0.2611 0.8606 0.4373 +vn -0.2248 -0.7412 -0.6326 +vn 0.1613 0.5319 0.8313 +vn 0.4213 0.7882 0.4487 +vn -0.4513 -0.8443 -0.2890 +vn 0.4356 0.8150 0.3821 +vn -0.4162 -0.7786 -0.4696 +vn 0.4239 0.7931 0.4373 +vn -0.3651 -0.6831 -0.6326 +vn 0.2620 0.4902 0.8313 +vn -0.2999 -0.5611 -0.7715 +vn -0.4060 -0.7595 0.5082 +vn -0.2230 -0.4173 -0.8810 +vn -0.4513 -0.8443 0.2890 +vn -0.1374 -0.2571 -0.9565 +vn -0.4691 -0.8777 0.0975 +vn 0.3015 0.5641 0.7687 +vn -0.0464 -0.0869 -0.9951 +vn -0.4691 -0.8777 -0.0975 +vn -0.5464 -0.6657 0.5082 +vn -0.3002 -0.3658 -0.8810 +vn -0.6073 -0.7400 0.2890 +vn -0.1850 -0.2254 -0.9565 +vn -0.6314 -0.7693 0.0975 +vn 0.4058 0.4945 0.7687 +vn -0.0625 -0.0761 -0.9951 +vn -0.6314 -0.7693 -0.0975 +vn 0.5670 0.6908 0.4487 +vn -0.6073 -0.7400 -0.2890 +vn 0.5862 0.7143 0.3821 +vn -0.5601 -0.6825 -0.4696 +vn 0.5705 0.6952 0.4373 +vn -0.4913 -0.5987 -0.6326 +vn 0.3526 0.4297 0.8313 +vn -0.4036 -0.4918 -0.7715 +vn -0.7400 -0.6073 -0.2890 +vn 0.7143 0.5862 0.3821 +vn -0.6825 -0.5601 -0.4696 +vn 0.6952 0.5705 0.4373 +vn -0.5987 -0.4913 -0.6326 +vn 0.4297 0.3526 0.8313 +vn -0.4918 -0.4036 -0.7715 +vn -0.6657 -0.5464 0.5082 +vn -0.3658 -0.3002 -0.8810 +vn -0.7400 -0.6073 0.2890 +vn -0.2254 -0.1850 -0.9566 +vn -0.7693 -0.6314 0.0975 +vn 0.4945 0.4058 0.7687 +vn -0.0761 -0.0625 -0.9951 +vn -0.7693 -0.6314 -0.0975 +vn 0.6908 0.5670 0.4487 +vn -0.4173 -0.2231 -0.8810 +vn -0.8443 -0.4513 0.2890 +vn -0.2571 -0.1374 -0.9565 +vn -0.8777 -0.4691 0.0975 +vn 0.5641 0.3015 0.7687 +vn -0.0869 -0.0464 -0.9951 +vn -0.8777 -0.4691 -0.0975 +vn 0.7882 0.4213 0.4487 +vn -0.8443 -0.4513 -0.2890 +vn 0.8150 0.4356 0.3821 +vn -0.7786 -0.4162 -0.4696 +vn 0.7931 0.4239 0.4373 +vn -0.6831 -0.3651 -0.6326 +vn 0.4902 0.2620 0.8313 +vn -0.5611 -0.2999 -0.7715 +vn -0.7595 -0.4060 0.5082 +vn -0.8448 -0.2563 -0.4696 +vn 0.8606 0.2611 0.4373 +vn -0.7412 -0.2248 -0.6326 +vn 0.5319 0.1613 0.8313 +vn -0.6088 -0.1847 -0.7715 +vn -0.8242 -0.2500 0.5082 +vn -0.4528 -0.1374 -0.8810 +vn -0.9161 -0.2779 0.2890 +vn -0.2790 -0.0846 -0.9565 +vn -0.9524 -0.2889 0.0975 +vn 0.6121 0.1857 0.7687 +vn -0.0942 -0.0286 -0.9951 +vn -0.9524 -0.2889 -0.0975 +vn 0.8552 0.2594 0.4487 +vn -0.9161 -0.2779 -0.2890 +vn 0.8843 0.2683 0.3821 +vn -0.2902 -0.0286 -0.9565 +vn -0.9904 -0.0975 0.0975 +vn 0.6366 0.0627 0.7687 +vn -0.0980 -0.0097 -0.9951 +vn -0.9904 -0.0976 -0.0975 +vn 0.8894 0.0876 0.4487 +vn -0.9527 -0.0938 -0.2890 +vn 0.9197 0.0906 0.3821 +vn -0.8786 -0.0865 -0.4696 +vn 0.8950 0.0881 0.4373 +vn -0.7708 -0.0759 -0.6326 +vn 0.5531 0.0545 0.8313 +vn -0.6332 -0.0624 -0.7715 +vn -0.8571 -0.0844 0.5082 +vn -0.4709 -0.0464 -0.8810 +vn -0.9527 -0.0938 0.2890 +vn 0.8950 -0.0881 0.4373 +vn -0.7708 0.0759 -0.6326 +vn 0.5531 -0.0545 0.8313 +vn -0.6332 0.0624 -0.7715 +vn -0.8571 0.0844 0.5082 +vn -0.4709 0.0464 -0.8810 +vn -0.9527 0.0938 0.2890 +vn -0.2902 0.0286 -0.9565 +vn -0.9904 0.0975 0.0975 +vn 0.6366 -0.0627 0.7687 +vn -0.0980 0.0097 -0.9951 +vn -0.9904 0.0976 -0.0975 +vn 0.8894 -0.0876 0.4487 +vn -0.9527 0.0938 -0.2890 +vn 0.9197 -0.0906 0.3821 +vn -0.8786 0.0865 -0.4696 +vn -0.9524 0.2889 0.0975 +vn 0.6121 -0.1857 0.7687 +vn -0.0942 0.0286 -0.9951 +vn -0.9524 0.2889 -0.0975 +vn 0.8552 -0.2594 0.4487 +vn -0.9161 0.2779 -0.2890 +vn 0.8843 -0.2683 0.3821 +vn -0.8448 0.2563 -0.4696 +vn 0.8606 -0.2611 0.4373 +vn -0.7412 0.2248 -0.6326 +vn 0.5319 -0.1613 0.8313 +vn -0.6088 0.1847 -0.7715 +vn -0.8242 0.2500 0.5082 +vn -0.4528 0.1374 -0.8810 +vn -0.9161 0.2779 0.2890 +vn -0.2790 0.0846 -0.9565 +vn -0.6831 0.3651 -0.6326 +vn 0.4902 -0.2620 0.8313 +vn -0.5611 0.2999 -0.7715 +vn -0.7595 0.4060 0.5082 +vn -0.4173 0.2231 -0.8810 +vn -0.8443 0.4513 0.2890 +vn -0.2571 0.1374 -0.9565 +vn -0.8777 0.4691 0.0975 +vn 0.5641 -0.3015 0.7687 +vn -0.0869 0.0464 -0.9951 +vn -0.8777 0.4691 -0.0975 +vn 0.7882 -0.4213 0.4487 +vn -0.8443 0.4513 -0.2890 +vn 0.8150 -0.4356 0.3821 +vn -0.7786 0.4162 -0.4696 +vn 0.7931 -0.4239 0.4373 +vn 0.4945 -0.4058 0.7687 +vn -0.0761 0.0625 -0.9951 +vn -0.7693 0.6314 -0.0975 +vn 0.6908 -0.5670 0.4487 +vn -0.7400 0.6073 -0.2890 +vn 0.7143 -0.5862 0.3821 +vn -0.6825 0.5601 -0.4696 +vn 0.6952 -0.5705 0.4373 +vn -0.5987 0.4913 -0.6326 +vn 0.4297 -0.3526 0.8313 +vn -0.4918 0.4036 -0.7715 +vn -0.6657 0.5464 0.5082 +vn -0.3658 0.3002 -0.8810 +vn -0.7400 0.6073 0.2890 +vn -0.2254 0.1850 -0.9565 +vn -0.7693 0.6314 0.0975 +vn 0.3526 -0.4297 0.8313 +vn -0.4036 0.4918 -0.7715 +vn -0.5464 0.6657 0.5082 +vn -0.3002 0.3658 -0.8810 +vn -0.6073 0.7400 0.2890 +vn -0.1850 0.2254 -0.9565 +vn -0.6314 0.7693 0.0975 +vn 0.4058 -0.4945 0.7687 +vn -0.0625 0.0761 -0.9951 +vn -0.6314 0.7693 -0.0975 +vn 0.5670 -0.6908 0.4487 +vn -0.6073 0.7400 -0.2890 +vn 0.5862 -0.7143 0.3821 +vn -0.5601 0.6825 -0.4696 +vn 0.5705 -0.6952 0.4373 +vn -0.4913 0.5987 -0.6326 +vn -0.4691 0.8777 -0.0975 +vn 0.4213 -0.7882 0.4487 +vn -0.4513 0.8443 -0.2890 +vn 0.4356 -0.8150 0.3821 +vn -0.4162 0.7786 -0.4696 +vn 0.4239 -0.7931 0.4373 +vn -0.3651 0.6831 -0.6326 +vn 0.2620 -0.4902 0.8313 +vn -0.2999 0.5611 -0.7715 +vn -0.4060 0.7595 0.5082 +vn -0.2230 0.4173 -0.8810 +vn -0.4513 0.8443 0.2890 +vn -0.1374 0.2571 -0.9565 +vn -0.4691 0.8777 0.0975 +vn 0.3015 -0.5641 0.7687 +vn -0.0464 0.0869 -0.9951 +vn -0.1847 0.6088 -0.7715 +vn -0.2500 0.8242 0.5082 +vn -0.1374 0.4528 -0.8810 +vn -0.2779 0.9161 0.2890 +vn -0.0846 0.2790 -0.9565 +vn -0.2889 0.9524 0.0975 +vn 0.1857 -0.6121 0.7687 +vn -0.0286 0.0942 -0.9951 +vn -0.2889 0.9524 -0.0975 +vn 0.2594 -0.8552 0.4487 +vn -0.2779 0.9161 -0.2890 +vn 0.2683 -0.8843 0.3821 +vn -0.2563 0.8448 -0.4696 +vn 0.2611 -0.8606 0.4373 +vn -0.2248 0.7412 -0.6326 +vn 0.1613 -0.5319 0.8313 +vn -0.0938 0.9527 -0.2890 +vn 0.0906 -0.9197 0.3821 +vn -0.0865 0.8786 -0.4696 +vn 0.0881 -0.8950 0.4373 +vn -0.0759 0.7708 -0.6326 +vn 0.0545 -0.5531 0.8313 +vn -0.0624 0.6332 -0.7715 +vn -0.0844 0.8571 0.5082 +vn -0.0464 0.4709 -0.8810 +vn -0.0938 0.9527 0.2890 +vn -0.0286 0.2902 -0.9565 +vn -0.0976 0.9904 0.0975 +vn 0.0627 -0.6366 0.7687 +vn -0.0097 0.0980 -0.9951 +vn -0.0976 0.9904 -0.0975 +vn 0.0876 -0.8894 0.4487 +vn 0.0975 0.9904 0.0976 +vn 0.0975 0.9904 -0.0975 +vn 0.2231 0.4173 -0.8810 +vn 0.2254 0.1850 -0.9565 +vn 0.4173 -0.2231 -0.8810 +vn 0.2231 -0.4173 -0.8810 +vn -0.0976 -0.9904 -0.0975 +vn -0.0975 -0.9904 0.0975 +vn -0.2231 -0.4173 -0.8810 +vn -0.2254 -0.1850 -0.9565 +vn -0.4173 -0.2230 -0.8810 +vn -0.9904 -0.0976 0.0975 +vn -0.9904 -0.0975 -0.0975 +vn -0.9904 0.0976 0.0975 +vn -0.9904 0.0975 -0.0975 +vt 0.750000 0.875000 +vt 0.718750 0.937500 +vt 0.718750 0.875000 +vt 0.750000 0.437500 +vt 0.718750 0.375000 +vt 0.750000 0.375000 +vt 0.750000 0.812500 +vt 0.718750 0.812500 +vt 0.718750 0.312500 +vt 0.750000 0.312500 +vt 0.750000 0.750000 +vt 0.718750 0.750000 +vt 0.718750 0.250000 +vt 0.750000 0.250000 +vt 0.750000 0.687500 +vt 0.718750 0.687500 +vt 0.718750 0.187500 +vt 0.750000 0.187500 +vt 0.750000 0.625000 +vt 0.718750 0.625000 +vt 0.718750 0.125000 +vt 0.750000 0.125000 +vt 0.750000 0.562500 +vt 0.718750 0.562500 +vt 0.718750 0.062500 +vt 0.750000 0.062500 +vt 0.718750 0.500000 +vt 0.750000 0.500000 +vt 0.750000 0.937500 +vt 0.734375 1.000000 +vt 0.734375 0.000000 +vt 0.718750 0.437500 +vt 0.687500 0.500000 +vt 0.703125 1.000000 +vt 0.687500 0.937500 +vt 0.703125 0.000000 +vt 0.687500 0.062500 +vt 0.687500 0.437500 +vt 0.687500 0.875000 +vt 0.687500 0.375000 +vt 0.687500 0.812500 +vt 0.687500 0.312500 +vt 0.687500 0.750000 +vt 0.687500 0.250000 +vt 0.687500 0.687500 +vt 0.687500 0.187500 +vt 0.687500 0.625000 +vt 0.687500 0.125000 +vt 0.687500 0.562500 +vt 0.656250 0.312500 +vt 0.656250 0.250000 +vt 0.656250 0.687500 +vt 0.656250 0.187500 +vt 0.656250 0.625000 +vt 0.656250 0.125000 +vt 0.656250 0.562500 +vt 0.656250 0.062500 +vt 0.656250 0.500000 +vt 0.671875 1.000000 +vt 0.656250 0.937500 +vt 0.671875 0.000000 +vt 0.656250 0.437500 +vt 0.656250 0.875000 +vt 0.656250 0.375000 +vt 0.656250 0.812500 +vt 0.656250 0.750000 +vt 0.640625 0.000000 +vt 0.625000 0.062500 +vt 0.625000 0.437500 +vt 0.625000 0.875000 +vt 0.625000 0.375000 +vt 0.625000 0.812500 +vt 0.625000 0.312500 +vt 0.625000 0.750000 +vt 0.625000 0.250000 +vt 0.625000 0.687500 +vt 0.625000 0.187500 +vt 0.625000 0.625000 +vt 0.625000 0.125000 +vt 0.625000 0.562500 +vt 0.625000 0.500000 +vt 0.640625 1.000000 +vt 0.625000 0.937500 +vt 0.593750 0.187500 +vt 0.593750 0.625000 +vt 0.593750 0.125000 +vt 0.593750 0.562500 +vt 0.593750 0.062500 +vt 0.593750 0.500000 +vt 0.609375 1.000000 +vt 0.593750 0.937500 +vt 0.609375 0.000000 +vt 0.593750 0.437500 +vt 0.593750 0.875000 +vt 0.593750 0.375000 +vt 0.593750 0.812500 +vt 0.593750 0.312500 +vt 0.593750 0.750000 +vt 0.593750 0.250000 +vt 0.593750 0.687500 +vt 0.562500 0.875000 +vt 0.562500 0.375000 +vt 0.562500 0.812500 +vt 0.562500 0.312500 +vt 0.562500 0.750000 +vt 0.562500 0.250000 +vt 0.562500 0.687500 +vt 0.562500 0.187500 +vt 0.562500 0.625000 +vt 0.562500 0.125000 +vt 0.562500 0.562500 +vt 0.562500 0.062500 +vt 0.562500 0.500000 +vt 0.578125 1.000000 +vt 0.562500 0.937500 +vt 0.578125 0.000000 +vt 0.562500 0.437500 +vt 0.531250 0.625000 +vt 0.531250 0.187500 +vt 0.531250 0.125000 +vt 0.531250 0.562500 +vt 0.531250 0.062500 +vt 0.531250 0.500000 +vt 0.546875 1.000000 +vt 0.531250 0.937500 +vt 0.546875 0.000000 +vt 0.531250 0.437500 +vt 0.531250 0.875000 +vt 0.531250 0.375000 +vt 0.531250 0.812500 +vt 0.531250 0.312500 +vt 0.531250 0.750000 +vt 0.531250 0.250000 +vt 0.531250 0.687500 +vt 0.500000 0.375000 +vt 0.500000 0.812500 +vt 0.500000 0.312500 +vt 0.500000 0.750000 +vt 0.500000 0.250000 +vt 0.500000 0.687500 +vt 0.500000 0.187500 +vt 0.500000 0.625000 +vt 0.500000 0.125000 +vt 0.500000 0.562500 +vt 0.500000 0.062500 +vt 0.500000 0.500000 +vt 0.515625 1.000000 +vt 0.500000 0.937500 +vt 0.515625 0.000000 +vt 0.500000 0.437500 +vt 0.500000 0.875000 +vt 0.468750 0.125000 +vt 0.468750 0.625000 +vt 0.468750 0.562500 +vt 0.468750 0.062500 +vt 0.468750 0.500000 +vt 0.484375 1.000000 +vt 0.468750 0.937500 +vt 0.484375 0.000000 +vt 0.468750 0.437500 +vt 0.468750 0.875000 +vt 0.468750 0.375000 +vt 0.468750 0.812500 +vt 0.468750 0.312500 +vt 0.468750 0.750000 +vt 0.468750 0.250000 +vt 0.468750 0.687500 +vt 0.468750 0.187500 +vt 0.437500 0.875000 +vt 0.437500 0.812500 +vt 0.437500 0.375000 +vt 0.437500 0.312500 +vt 0.437500 0.750000 +vt 0.437500 0.250000 +vt 0.437500 0.687500 +vt 0.437500 0.187500 +vt 0.437500 0.625000 +vt 0.437500 0.125000 +vt 0.437500 0.562500 +vt 0.437500 0.062500 +vt 0.437500 0.500000 +vt 0.453125 1.000000 +vt 0.437500 0.937500 +vt 0.453125 0.000000 +vt 0.437500 0.437500 +vt 0.406250 0.625000 +vt 0.406250 0.562500 +vt 0.406250 0.062500 +vt 0.406250 0.500000 +vt 0.421875 1.000000 +vt 0.406250 0.937500 +vt 0.421875 0.000000 +vt 0.406250 0.437500 +vt 0.406250 0.875000 +vt 0.406250 0.375000 +vt 0.406250 0.812500 +vt 0.406250 0.312500 +vt 0.406250 0.750000 +vt 0.406250 0.250000 +vt 0.406250 0.687500 +vt 0.406250 0.187500 +vt 0.406250 0.125000 +vt 0.375000 0.375000 +vt 0.375000 0.312500 +vt 0.375000 0.750000 +vt 0.375000 0.250000 +vt 0.375000 0.687500 +vt 0.375000 0.187500 +vt 0.375000 0.625000 +vt 0.375000 0.125000 +vt 0.375000 0.562500 +vt 0.375000 0.062500 +vt 0.375000 0.500000 +vt 0.390625 1.000000 +vt 0.375000 0.937500 +vt 0.390625 0.000000 +vt 0.375000 0.437500 +vt 0.375000 0.875000 +vt 0.375000 0.812500 +vt 0.343750 0.062500 +vt 0.343750 0.562500 +vt 0.343750 0.500000 +vt 0.359375 1.000000 +vt 0.343750 0.937500 +vt 0.359375 0.000000 +vt 0.343750 0.437500 +vt 0.343750 0.875000 +vt 0.343750 0.375000 +vt 0.343750 0.812500 +vt 0.343750 0.312500 +vt 0.343750 0.750000 +vt 0.343750 0.250000 +vt 0.343750 0.687500 +vt 0.343750 0.187500 +vt 0.343750 0.625000 +vt 0.343750 0.125000 +vt 0.312500 0.250000 +vt 0.312500 0.750000 +vt 0.312500 0.687500 +vt 0.312500 0.187500 +vt 0.312500 0.625000 +vt 0.312500 0.125000 +vt 0.312500 0.562500 +vt 0.312500 0.062500 +vt 0.312500 0.500000 +vt 0.328125 1.000000 +vt 0.312500 0.937500 +vt 0.328125 0.000000 +vt 0.312500 0.437500 +vt 0.312500 0.875000 +vt 0.312500 0.375000 +vt 0.312500 0.812500 +vt 0.312500 0.312500 +vt 0.296875 1.000000 +vt 0.281250 0.937500 +vt 0.296875 0.000000 +vt 0.281250 0.062500 +vt 0.281250 0.437500 +vt 0.281250 0.875000 +vt 0.281250 0.375000 +vt 0.281250 0.812500 +vt 0.281250 0.312500 +vt 0.281250 0.750000 +vt 0.281250 0.250000 +vt 0.281250 0.687500 +vt 0.281250 0.187500 +vt 0.281250 0.625000 +vt 0.281250 0.125000 +vt 0.281250 0.562500 +vt 0.281250 0.500000 +vt 0.250000 0.750000 +vt 0.250000 0.687500 +vt 0.250000 0.250000 +vt 0.250000 0.187500 +vt 0.250000 0.625000 +vt 0.250000 0.125000 +vt 0.250000 0.562500 +vt 0.250000 0.062500 +vt 0.250000 0.500000 +vt 0.265625 1.000000 +vt 0.250000 0.937500 +vt 0.265625 0.000000 +vt 0.250000 0.437500 +vt 0.250000 0.875000 +vt 0.250000 0.375000 +vt 0.250000 0.812500 +vt 0.250000 0.312500 +vt 0.218750 0.437500 +vt 0.218750 0.875000 +vt 0.218750 0.375000 +vt 0.218750 0.812500 +vt 0.218750 0.312500 +vt 0.218750 0.750000 +vt 0.218750 0.250000 +vt 0.218750 0.687500 +vt 0.218750 0.187500 +vt 0.218750 0.625000 +vt 0.218750 0.125000 +vt 0.218750 0.562500 +vt 0.218750 0.062500 +vt 0.218750 0.500000 +vt 0.234375 1.000000 +vt 0.218750 0.937500 +vt 0.234375 0.000000 +vt 0.187500 0.250000 +vt 0.187500 0.187500 +vt 0.187500 0.625000 +vt 0.187500 0.125000 +vt 0.187500 0.562500 +vt 0.187500 0.062500 +vt 0.187500 0.500000 +vt 0.203125 1.000000 +vt 0.187500 0.937500 +vt 0.203125 0.000000 +vt 0.187500 0.437500 +vt 0.187500 0.875000 +vt 0.187500 0.375000 +vt 0.187500 0.812500 +vt 0.187500 0.312500 +vt 0.187500 0.750000 +vt 0.187500 0.687500 +vt 0.156250 0.937500 +vt 0.156250 0.875000 +vt 0.156250 0.375000 +vt 0.156250 0.812500 +vt 0.156250 0.312500 +vt 0.156250 0.750000 +vt 0.156250 0.250000 +vt 0.156250 0.687500 +vt 0.156250 0.187500 +vt 0.156250 0.625000 +vt 0.156250 0.125000 +vt 0.156250 0.562500 +vt 0.156250 0.062500 +vt 0.156250 0.500000 +vt 0.171875 1.000000 +vt 0.171875 0.000000 +vt 0.156250 0.437500 +vt 0.125000 0.625000 +vt 0.125000 0.125000 +vt 0.125000 0.562500 +vt 0.125000 0.062500 +vt 0.125000 0.500000 +vt 0.140625 1.000000 +vt 0.125000 0.937500 +vt 0.140625 0.000000 +vt 0.125000 0.437500 +vt 0.125000 0.875000 +vt 0.125000 0.375000 +vt 0.125000 0.812500 +vt 0.125000 0.312500 +vt 0.125000 0.750000 +vt 0.125000 0.250000 +vt 0.125000 0.687500 +vt 0.125000 0.187500 +vt 0.093750 0.375000 +vt 0.093750 0.812500 +vt 0.093750 0.312500 +vt 0.093750 0.750000 +vt 0.093750 0.250000 +vt 0.093750 0.687500 +vt 0.093750 0.187500 +vt 0.093750 0.625000 +vt 0.093750 0.125000 +vt 0.093750 0.562500 +vt 0.093750 0.062500 +vt 0.093750 0.500000 +vt 0.109375 1.000000 +vt 0.093750 0.937500 +vt 0.109375 0.000000 +vt 0.093750 0.437500 +vt 0.093750 0.875000 +vt 0.062500 0.125000 +vt 0.062500 0.625000 +vt 0.062500 0.562500 +vt 0.062500 0.062500 +vt 0.062500 0.500000 +vt 0.078125 1.000000 +vt 0.062500 0.937500 +vt 0.078125 0.000000 +vt 0.062500 0.437500 +vt 0.062500 0.875000 +vt 0.062500 0.375000 +vt 0.062500 0.812500 +vt 0.062500 0.312500 +vt 0.062500 0.750000 +vt 0.062500 0.250000 +vt 0.062500 0.687500 +vt 0.062500 0.187500 +vt 0.031250 0.375000 +vt 0.031250 0.312500 +vt 0.031250 0.750000 +vt 0.031250 0.250000 +vt 0.031250 0.687500 +vt 0.031250 0.187500 +vt 0.031250 0.625000 +vt 0.031250 0.125000 +vt 0.031250 0.562500 +vt 0.031250 0.062500 +vt 0.031250 0.500000 +vt 0.046875 1.000000 +vt 0.031250 0.937500 +vt 0.046875 0.000000 +vt 0.031250 0.437500 +vt 0.031250 0.875000 +vt 0.031250 0.812500 +vt 0.000000 0.062500 +vt 0.000000 0.562500 +vt 0.000000 0.500000 +vt 0.015625 1.000000 +vt 0.000000 0.937500 +vt 0.015625 0.000000 +vt 0.000000 0.437500 +vt 0.000000 0.875000 +vt 0.000000 0.375000 +vt 0.000000 0.812500 +vt 0.000000 0.312500 +vt 0.000000 0.750000 +vt 0.000000 0.250000 +vt 0.000000 0.687500 +vt 0.000000 0.187500 +vt 0.000000 0.625000 +vt 0.000000 0.125000 +vt 1.000000 0.812500 +vt 0.968750 0.750000 +vt 1.000000 0.750000 +vt 1.000000 0.312500 +vt 0.968750 0.250000 +vt 1.000000 0.250000 +vt 1.000000 0.687500 +vt 0.968750 0.687500 +vt 1.000000 0.187500 +vt 0.968750 0.187500 +vt 0.968750 0.625000 +vt 1.000000 0.625000 +vt 0.968750 0.125000 +vt 1.000000 0.125000 +vt 1.000000 0.562500 +vt 0.968750 0.562500 +vt 0.968750 0.062500 +vt 1.000000 0.062500 +vt 1.000000 0.500000 +vt 0.968750 0.500000 +vt 1.000000 0.937500 +vt 0.984375 1.000000 +vt 0.968750 0.937500 +vt 0.984375 0.000000 +vt 0.968750 0.437500 +vt 1.000000 0.437500 +vt 0.968750 0.875000 +vt 1.000000 0.875000 +vt 0.968750 0.375000 +vt 1.000000 0.375000 +vt 0.968750 0.812500 +vt 0.968750 0.312500 +vt 0.937500 0.562500 +vt 0.937500 0.500000 +vt 0.953125 1.000000 +vt 0.937500 0.937500 +vt 0.953125 0.000000 +vt 0.937500 0.062500 +vt 0.937500 0.437500 +vt 0.937500 0.875000 +vt 0.937500 0.375000 +vt 0.937500 0.812500 +vt 0.937500 0.312500 +vt 0.937500 0.750000 +vt 0.937500 0.250000 +vt 0.937500 0.687500 +vt 0.937500 0.187500 +vt 0.937500 0.625000 +vt 0.937500 0.125000 +vt 0.906250 0.250000 +vt 0.906250 0.750000 +vt 0.906250 0.687500 +vt 0.906250 0.187500 +vt 0.906250 0.625000 +vt 0.906250 0.125000 +vt 0.906250 0.562500 +vt 0.906250 0.062500 +vt 0.906250 0.500000 +vt 0.921875 1.000000 +vt 0.906250 0.937500 +vt 0.921875 0.000000 +vt 0.906250 0.437500 +vt 0.906250 0.875000 +vt 0.906250 0.375000 +vt 0.906250 0.812500 +vt 0.906250 0.312500 +vt 0.890625 1.000000 +vt 0.875000 0.937500 +vt 0.890625 0.000000 +vt 0.875000 0.062500 +vt 0.875000 0.437500 +vt 0.875000 0.875000 +vt 0.875000 0.375000 +vt 0.875000 0.812500 +vt 0.875000 0.312500 +vt 0.875000 0.750000 +vt 0.875000 0.250000 +vt 0.875000 0.687500 +vt 0.875000 0.187500 +vt 0.875000 0.625000 +vt 0.875000 0.125000 +vt 0.875000 0.562500 +vt 0.875000 0.500000 +vt 0.843750 0.750000 +vt 0.843750 0.687500 +vt 0.843750 0.250000 +vt 0.843750 0.187500 +vt 0.843750 0.625000 +vt 0.843750 0.125000 +vt 0.843750 0.562500 +vt 0.843750 0.062500 +vt 0.843750 0.500000 +vt 0.859375 1.000000 +vt 0.843750 0.937500 +vt 0.859375 0.000000 +vt 0.843750 0.437500 +vt 0.843750 0.875000 +vt 0.843750 0.375000 +vt 0.843750 0.812500 +vt 0.843750 0.312500 +vt 0.812500 0.437500 +vt 0.812500 0.937500 +vt 0.812500 0.875000 +vt 0.812500 0.375000 +vt 0.812500 0.812500 +vt 0.812500 0.312500 +vt 0.812500 0.750000 +vt 0.812500 0.250000 +vt 0.812500 0.687500 +vt 0.812500 0.187500 +vt 0.812500 0.625000 +vt 0.812500 0.125000 +vt 0.812500 0.562500 +vt 0.812500 0.062500 +vt 0.812500 0.500000 +vt 0.828125 1.000000 +vt 0.828125 0.000000 +vt 0.781250 0.250000 +vt 0.781250 0.187500 +vt 0.781250 0.625000 +vt 0.781250 0.125000 +vt 0.781250 0.562500 +vt 0.781250 0.062500 +vt 0.781250 0.500000 +vt 0.796875 1.000000 +vt 0.781250 0.937500 +vt 0.796875 0.000000 +vt 0.781250 0.437500 +vt 0.781250 0.875000 +vt 0.781250 0.375000 +vt 0.781250 0.812500 +vt 0.781250 0.312500 +vt 0.781250 0.750000 +vt 0.781250 0.687500 +vt 0.765625 1.000000 +vt 0.765625 0.000000 +s 0 +f 475/1/1 7/2/1 8/3/1 +f 5/4/2 16/5/2 478/6/2 +f 1/7/3 8/3/3 9/8/3 +f 478/6/4 17/9/4 6/10/4 +f 476/11/5 9/8/5 10/12/5 +f 6/10/6 18/13/6 479/14/6 +f 2/15/7 10/12/7 11/16/7 +f 479/14/8 19/17/8 480/18/8 +f 477/19/9 11/16/9 12/20/9 +f 480/18/10 20/21/10 481/22/10 +f 3/23/11 12/20/11 13/24/11 +f 481/22/12 21/25/12 482/26/12 +f 3/23/13 14/27/13 4/28/13 +f 474/29/14 82/30/14 7/2/14 +f 308/31/15 482/26/15 21/25/15 +f 4/28/16 15/32/16 5/4/16 +f 13/24/17 29/33/17 14/27/17 +f 7/2/18 82/34/18 22/35/18 +f 308/36/19 21/25/19 36/37/19 +f 14/27/20 30/38/20 15/32/20 +f 7/2/21 23/39/21 8/3/21 +f 15/32/22 31/40/22 16/5/22 +f 8/3/23 24/41/23 9/8/23 +f 16/5/24 32/42/24 17/9/24 +f 9/8/25 25/43/25 10/12/25 +f 17/9/26 33/44/26 18/13/26 +f 10/12/27 26/45/27 11/16/27 +f 19/17/28 33/44/28 34/46/28 +f 11/16/29 27/47/29 12/20/29 +f 20/21/30 34/46/30 35/48/30 +f 13/24/31 27/47/31 28/49/31 +f 20/21/32 36/37/32 21/25/32 +f 33/44/33 47/50/33 48/51/33 +f 25/43/34 41/52/34 26/45/34 +f 34/46/35 48/51/35 49/53/35 +f 26/45/36 42/54/36 27/47/36 +f 34/46/37 50/55/37 35/48/37 +f 27/47/38 43/56/38 28/49/38 +f 35/48/39 51/57/39 36/37/39 +f 28/49/40 44/58/40 29/33/40 +f 22/35/41 82/59/41 37/60/41 +f 308/61/42 36/37/42 51/57/42 +f 30/38/43 44/58/43 45/62/43 +f 22/35/44 38/63/44 23/39/44 +f 30/38/45 46/64/45 31/40/45 +f 23/39/46 39/65/46 24/41/46 +f 32/42/47 46/64/47 47/50/47 +f 24/41/48 40/66/48 25/43/48 +f 308/67/49 51/57/49 66/68/49 +f 44/58/50 60/69/50 45/62/50 +f 37/60/51 53/70/51 38/63/51 +f 45/62/52 61/71/52 46/64/52 +f 38/63/53 54/72/53 39/65/53 +f 47/50/54 61/71/54 62/73/54 +f 39/65/55 55/74/55 40/66/55 +f 47/50/56 63/75/56 48/51/56 +f 40/66/57 56/76/57 41/52/57 +f 48/51/58 64/77/58 49/53/58 +f 41/52/59 57/78/59 42/54/59 +f 49/53/60 65/79/60 50/55/60 +f 43/56/61 57/78/61 58/80/61 +f 50/55/62 66/68/62 51/57/62 +f 44/58/63 58/80/63 59/81/63 +f 37/60/64 82/82/64 52/83/64 +f 63/75/65 79/84/65 64/77/65 +f 56/76/66 72/85/66 57/78/66 +f 65/79/67 79/84/67 80/86/67 +f 58/80/68 72/85/68 73/87/68 +f 65/79/69 81/88/69 66/68/69 +f 59/81/70 73/87/70 74/89/70 +f 52/83/71 82/90/71 67/91/71 +f 308/92/72 66/68/72 81/88/72 +f 59/81/73 75/93/73 60/69/73 +f 52/83/74 68/94/74 53/70/74 +f 60/69/75 76/95/75 61/71/75 +f 53/70/76 69/96/76 54/72/76 +f 62/73/77 76/95/77 77/97/77 +f 54/72/78 70/98/78 55/74/78 +f 62/73/79 78/99/79 63/75/79 +f 56/76/80 70/98/80 71/100/80 +f 67/91/81 84/101/81 68/94/81 +f 75/93/82 92/102/82 76/95/82 +f 68/94/83 85/103/83 69/96/83 +f 77/97/84 92/102/84 93/104/84 +f 69/96/85 86/105/85 70/98/85 +f 77/97/86 94/106/86 78/99/86 +f 71/100/87 86/105/87 87/107/87 +f 78/99/88 95/108/88 79/84/88 +f 71/100/89 88/109/89 72/85/89 +f 79/84/90 96/110/90 80/86/90 +f 73/87/91 88/109/91 89/111/91 +f 81/88/92 96/110/92 97/112/92 +f 74/89/93 89/111/93 90/113/93 +f 67/91/94 82/114/94 83/115/94 +f 308/116/95 81/88/95 97/112/95 +f 74/89/96 91/117/96 75/93/96 +f 87/107/97 103/118/97 88/109/97 +f 96/110/98 110/119/98 111/120/98 +f 89/111/99 103/118/99 104/121/99 +f 96/110/100 112/122/100 97/112/100 +f 90/113/101 104/121/101 105/123/101 +f 83/115/102 82/124/102 98/125/102 +f 308/126/103 97/112/103 112/122/103 +f 90/113/104 106/127/104 91/117/104 +f 84/101/105 98/125/105 99/128/105 +f 91/117/106 107/129/106 92/102/106 +f 84/101/107 100/130/107 85/103/107 +f 93/104/108 107/129/108 108/131/108 +f 85/103/109 101/132/109 86/105/109 +f 93/104/110 109/133/110 94/106/110 +f 87/107/111 101/132/111 102/134/111 +f 95/108/112 109/133/112 110/119/112 +f 106/127/113 122/135/113 107/129/113 +f 99/128/114 115/136/114 100/130/114 +f 108/131/115 122/135/115 123/137/115 +f 100/130/116 116/138/116 101/132/116 +f 108/131/117 124/139/117 109/133/117 +f 102/134/118 116/138/118 117/140/118 +f 110/119/119 124/139/119 125/141/119 +f 102/134/120 118/142/120 103/118/120 +f 110/119/121 126/143/121 111/120/121 +f 104/121/122 118/142/122 119/144/122 +f 111/120/123 127/145/123 112/122/123 +f 105/123/124 119/144/124 120/146/124 +f 98/125/125 82/147/125 113/148/125 +f 308/149/126 112/122/126 127/145/126 +f 105/123/127 121/150/127 106/127/127 +f 99/128/128 113/148/128 114/151/128 +f 125/141/129 141/152/129 126/143/129 +f 119/144/130 133/153/130 134/154/130 +f 126/143/131 142/155/131 127/145/131 +f 120/146/132 134/154/132 135/156/132 +f 113/148/133 82/157/133 128/158/133 +f 308/159/134 127/145/134 142/155/134 +f 120/146/135 136/160/135 121/150/135 +f 113/148/136 129/161/136 114/151/136 +f 121/150/137 137/162/137 122/135/137 +f 114/151/138 130/163/138 115/136/138 +f 123/137/139 137/162/139 138/164/139 +f 115/136/140 131/165/140 116/138/140 +f 123/137/141 139/166/141 124/139/141 +f 117/140/142 131/165/142 132/167/142 +f 125/141/143 139/166/143 140/168/143 +f 117/140/144 133/153/144 118/142/144 +f 130/163/145 144/169/145 145/170/145 +f 138/164/146 152/171/146 153/172/146 +f 130/163/147 146/173/147 131/165/147 +f 138/164/148 154/174/148 139/166/148 +f 132/167/149 146/173/149 147/175/149 +f 140/168/150 154/174/150 155/176/150 +f 132/167/151 148/177/151 133/153/151 +f 140/168/152 156/178/152 141/152/152 +f 134/154/153 148/177/153 149/179/153 +f 142/155/154 156/178/154 157/180/154 +f 135/156/155 149/179/155 150/181/155 +f 128/158/156 82/182/156 143/183/156 +f 308/184/157 142/155/157 157/180/157 +f 135/156/158 151/185/158 136/160/158 +f 128/158/159 144/169/159 129/161/159 +f 136/160/160 152/171/160 137/162/160 +f 149/179/161 163/186/161 164/187/161 +f 156/178/162 172/188/162 157/180/162 +f 150/181/163 164/187/163 165/189/163 +f 143/183/164 82/190/164 158/191/164 +f 308/192/165 157/180/165 172/188/165 +f 150/181/166 166/193/166 151/185/166 +f 143/183/167 159/194/167 144/169/167 +f 151/185/168 167/195/168 152/171/168 +f 145/170/169 159/194/169 160/196/169 +f 153/172/170 167/195/170 168/197/170 +f 145/170/171 161/198/171 146/173/171 +f 153/172/172 169/199/172 154/174/172 +f 147/175/173 161/198/173 162/200/173 +f 155/176/174 169/199/174 170/201/174 +f 147/175/175 163/186/175 148/177/175 +f 155/176/176 171/202/176 156/178/176 +f 168/197/177 182/203/177 183/204/177 +f 160/196/178 176/205/178 161/198/178 +f 168/197/179 184/206/179 169/199/179 +f 162/200/180 176/205/180 177/207/180 +f 169/199/181 185/208/181 170/201/181 +f 162/200/182 178/209/182 163/186/182 +f 171/202/183 185/208/183 186/210/183 +f 164/187/184 178/209/184 179/211/184 +f 172/188/185 186/210/185 187/212/185 +f 165/189/186 179/211/186 180/213/186 +f 158/191/187 82/214/187 173/215/187 +f 308/216/188 172/188/188 187/212/188 +f 165/189/189 181/217/189 166/193/189 +f 158/191/190 174/218/190 159/194/190 +f 166/193/191 182/203/191 167/195/191 +f 159/194/192 175/219/192 160/196/192 +f 186/210/193 202/220/193 187/212/193 +f 180/213/194 194/221/194 195/222/194 +f 173/215/195 82/223/195 188/224/195 +f 308/225/196 187/212/196 202/220/196 +f 180/213/197 196/226/197 181/217/197 +f 174/218/198 188/224/198 189/227/198 +f 181/217/199 197/228/199 182/203/199 +f 175/219/200 189/227/200 190/229/200 +f 183/204/201 197/228/201 198/230/201 +f 175/219/202 191/231/202 176/205/202 +f 183/204/203 199/232/203 184/206/203 +f 177/207/204 191/231/204 192/233/204 +f 185/208/205 199/232/205 200/234/205 +f 177/207/206 193/235/206 178/209/206 +f 185/208/207 201/236/207 186/210/207 +f 179/211/208 193/235/208 194/221/208 +f 198/230/209 214/237/209 199/232/209 +f 192/233/210 206/238/210 207/239/210 +f 200/234/211 214/237/211 215/240/211 +f 192/233/212 208/241/212 193/235/212 +f 200/234/213 216/242/213 201/236/213 +f 194/221/214 208/241/214 209/243/214 +f 201/236/215 217/244/215 202/220/215 +f 195/222/216 209/243/216 210/245/216 +f 188/224/217 82/246/217 203/247/217 +f 308/248/218 202/220/218 217/244/218 +f 195/222/219 211/249/219 196/226/219 +f 188/224/220 204/250/220 189/227/220 +f 196/226/221 212/251/221 197/228/221 +f 189/227/222 205/252/222 190/229/222 +f 198/230/223 212/251/223 213/253/223 +f 190/229/224 206/238/224 191/231/224 +f 203/247/225 82/254/225 218/255/225 +f 308/256/226 217/244/226 232/257/226 +f 210/245/227 226/258/227 211/249/227 +f 203/247/228 219/259/228 204/250/228 +f 211/249/229 227/260/229 212/251/229 +f 204/250/230 220/261/230 205/252/230 +f 213/253/231 227/260/231 228/262/231 +f 205/252/232 221/263/232 206/238/232 +f 213/253/233 229/264/233 214/237/233 +f 207/239/234 221/263/234 222/265/234 +f 215/240/235 229/264/235 230/266/235 +f 207/239/236 223/267/236 208/241/236 +f 215/240/237 231/268/237 216/242/237 +f 209/243/238 223/267/238 224/269/238 +f 216/242/239 232/257/239 217/244/239 +f 210/245/240 224/269/240 225/270/240 +f 222/265/241 236/271/241 237/272/241 +f 230/266/242 244/273/242 245/274/242 +f 222/265/243 238/275/243 223/267/243 +f 230/266/244 246/276/244 231/268/244 +f 224/269/245 238/275/245 239/277/245 +f 232/257/246 246/276/246 247/278/246 +f 225/270/247 239/277/247 240/279/247 +f 218/255/248 82/280/248 233/281/248 +f 308/282/249 232/257/249 247/278/249 +f 225/270/250 241/283/250 226/258/250 +f 218/255/251 234/284/251 219/259/251 +f 226/258/252 242/285/252 227/260/252 +f 220/261/253 234/284/253 235/286/253 +f 228/262/254 242/285/254 243/287/254 +f 220/261/255 236/271/255 221/263/255 +f 228/262/256 244/273/256 229/264/256 +f 240/279/257 256/288/257 241/283/257 +f 233/281/258 249/289/258 234/284/258 +f 241/283/259 257/290/259 242/285/259 +f 234/284/260 250/291/260 235/286/260 +f 243/287/261 257/290/261 258/292/261 +f 235/286/262 251/293/262 236/271/262 +f 243/287/263 259/294/263 244/273/263 +f 237/272/264 251/293/264 252/295/264 +f 245/274/265 259/294/265 260/296/265 +f 237/272/266 253/297/266 238/275/266 +f 245/274/267 261/298/267 246/276/267 +f 239/277/268 253/297/268 254/299/268 +f 246/276/269 262/300/269 247/278/269 +f 240/279/270 254/299/270 255/301/270 +f 233/281/271 82/302/271 248/303/271 +f 308/304/272 247/278/272 262/300/272 +f 260/296/273 274/305/273 275/306/273 +f 252/295/274 268/307/274 253/297/274 +f 260/296/275 276/308/275 261/298/275 +f 254/299/276 268/307/276 269/309/276 +f 261/298/277 277/310/277 262/300/277 +f 255/301/278 269/309/278 270/311/278 +f 248/303/279 82/312/279 263/313/279 +f 308/314/280 262/300/280 277/310/280 +f 255/301/281 271/315/281 256/288/281 +f 249/289/282 263/313/282 264/316/282 +f 256/288/283 272/317/283 257/290/283 +f 249/289/284 265/318/284 250/291/284 +f 258/292/285 272/317/285 273/319/285 +f 250/291/286 266/320/286 251/293/286 +f 258/292/287 274/305/287 259/294/287 +f 252/295/288 266/320/288 267/321/288 +f 264/316/289 278/322/289 279/323/289 +f 271/315/290 287/324/290 272/317/290 +f 264/316/291 280/325/291 265/318/291 +f 273/319/292 287/324/292 288/326/292 +f 265/318/293 281/327/293 266/320/293 +f 273/319/294 289/328/294 274/305/294 +f 267/321/295 281/327/295 282/329/295 +f 275/306/296 289/328/296 290/330/296 +f 267/321/297 283/331/297 268/307/297 +f 275/306/298 291/332/298 276/308/298 +f 269/309/299 283/331/299 284/333/299 +f 277/310/300 291/332/300 292/334/300 +f 270/311/301 284/333/301 285/335/301 +f 263/313/302 82/336/302 278/322/302 +f 308/337/303 277/310/303 292/334/303 +f 270/311/304 286/338/304 271/315/304 +f 282/329/305 298/339/305 283/331/305 +f 290/330/306 306/340/306 291/332/306 +f 284/333/307 298/339/307 299/341/307 +f 292/334/308 306/340/308 307/342/308 +f 285/335/309 299/341/309 300/343/309 +f 278/322/310 82/344/310 293/345/310 +f 308/346/311 292/334/311 307/342/311 +f 285/335/312 301/347/312 286/338/312 +f 278/322/313 294/348/313 279/323/313 +f 286/338/314 302/349/314 287/324/314 +f 279/323/315 295/350/315 280/325/315 +f 288/326/316 302/349/316 303/351/316 +f 280/325/317 296/352/317 281/327/317 +f 288/326/318 304/353/318 289/328/318 +f 282/329/319 296/352/319 297/354/319 +f 290/330/320 304/353/320 305/355/320 +f 301/347/321 318/356/321 302/349/321 +f 294/348/322 311/357/322 295/350/322 +f 303/351/323 318/356/323 319/358/323 +f 295/350/324 312/359/324 296/352/324 +f 303/351/325 320/360/325 304/353/325 +f 297/354/326 312/359/326 313/361/326 +f 305/355/327 320/360/327 321/362/327 +f 297/354/328 314/363/328 298/339/328 +f 305/355/329 322/364/329 306/340/329 +f 299/341/330 314/363/330 315/365/330 +f 306/340/331 323/366/331 307/342/331 +f 300/343/332 315/365/332 316/367/332 +f 293/345/333 82/368/333 309/369/333 +f 308/370/334 307/342/334 323/366/334 +f 300/343/335 317/371/335 301/347/335 +f 293/345/336 310/372/336 294/348/336 +f 321/362/337 337/373/337 322/364/337 +f 315/365/338 329/374/338 330/375/338 +f 322/364/339 338/376/339 323/366/339 +f 316/367/340 330/375/340 331/377/340 +f 309/369/341 82/378/341 324/379/341 +f 308/380/342 323/366/342 338/376/342 +f 316/367/343 332/381/343 317/371/343 +f 309/369/344 325/382/344 310/372/344 +f 317/371/345 333/383/345 318/356/345 +f 310/372/346 326/384/346 311/357/346 +f 319/358/347 333/383/347 334/385/347 +f 311/357/348 327/386/348 312/359/348 +f 319/358/349 335/387/349 320/360/349 +f 312/359/350 328/388/350 313/361/350 +f 321/362/351 335/387/351 336/389/351 +f 313/361/352 329/374/352 314/363/352 +f 334/385/353 348/390/353 349/391/353 +f 326/384/354 342/392/354 327/386/354 +f 334/385/355 350/393/355 335/387/355 +f 328/388/356 342/392/356 343/394/356 +f 336/389/357 350/393/357 351/395/357 +f 328/388/358 344/396/358 329/374/358 +f 336/389/359 352/397/359 337/373/359 +f 330/375/360 344/396/360 345/398/360 +f 337/373/361 353/399/361 338/376/361 +f 331/377/362 345/398/362 346/400/362 +f 324/379/363 82/401/363 339/402/363 +f 308/403/364 338/376/364 353/399/364 +f 331/377/365 347/404/365 332/381/365 +f 324/379/366 340/405/366 325/382/366 +f 332/381/367 348/390/367 333/383/367 +f 325/382/368 341/406/368 326/384/368 +f 352/397/369 368/407/369 353/399/369 +f 346/400/370 360/408/370 361/409/370 +f 339/402/371 82/410/371 354/411/371 +f 308/412/372 353/399/372 368/407/372 +f 346/400/373 362/413/373 347/404/373 +f 339/402/374 355/414/374 340/405/374 +f 347/404/375 363/415/375 348/390/375 +f 341/406/376 355/414/376 356/416/376 +f 349/391/377 363/415/377 364/417/377 +f 341/406/378 357/418/378 342/392/378 +f 349/391/379 365/419/379 350/393/379 +f 343/394/380 357/418/380 358/420/380 +f 351/395/381 365/419/381 366/421/381 +f 343/394/382 359/422/382 344/396/382 +f 351/395/383 367/423/383 352/397/383 +f 345/398/384 359/422/384 360/408/384 +f 356/424/385 372/425/385 357/426/385 +f 364/427/386 380/428/386 365/429/386 +f 358/430/387 372/425/387 373/431/387 +f 366/432/388 380/428/388 381/433/388 +f 358/430/389 374/434/389 359/435/389 +f 366/432/390 382/436/390 367/437/390 +f 360/438/391 374/434/391 375/439/391 +f 367/437/392 383/440/392 368/441/392 +f 361/442/393 375/439/393 376/443/393 +f 354/444/394 82/445/394 369/446/394 +f 308/447/395 368/441/395 383/440/395 +f 361/442/396 377/448/396 362/449/396 +f 354/444/397 370/450/397 355/451/397 +f 362/449/398 378/452/398 363/453/398 +f 356/424/399 370/450/399 371/454/399 +f 364/427/400 378/452/400 379/455/400 +f 376/443/401 390/456/401 391/457/401 +f 369/446/402 82/458/402 384/459/402 +f 308/460/403 383/440/403 398/461/403 +f 376/443/404 392/462/404 377/448/404 +f 369/446/405 385/463/405 370/450/405 +f 377/448/406 393/464/406 378/452/406 +f 371/454/407 385/463/407 386/465/407 +f 379/455/408 393/464/408 394/466/408 +f 371/454/409 387/467/409 372/425/409 +f 379/455/410 395/468/410 380/428/410 +f 373/431/411 387/467/411 388/469/411 +f 381/433/412 395/468/412 396/470/412 +f 373/431/413 389/471/413 374/434/413 +f 381/433/414 397/472/414 382/436/414 +f 375/439/415 389/471/415 390/456/415 +f 382/436/416 398/461/416 383/440/416 +f 394/466/417 410/473/417 395/468/417 +f 388/469/418 402/474/418 403/475/418 +f 396/470/419 410/473/419 411/476/419 +f 388/469/420 404/477/420 389/471/420 +f 397/472/421 411/476/421 412/478/421 +f 390/456/422 404/477/422 405/479/422 +f 397/472/423 413/480/423 398/461/423 +f 391/457/424 405/479/424 406/481/424 +f 384/459/425 82/482/425 399/483/425 +f 308/484/426 398/461/426 413/480/426 +f 391/457/427 407/485/427 392/462/427 +f 385/463/428 399/483/428 400/486/428 +f 392/462/429 408/487/429 393/464/429 +f 385/463/430 401/488/430 386/465/430 +f 394/466/431 408/487/431 409/489/431 +f 386/465/432 402/474/432 387/467/432 +f 399/483/433 82/490/433 414/491/433 +f 308/492/434 413/480/434 428/493/434 +f 406/481/435 422/494/435 407/485/435 +f 399/483/436 415/495/436 400/486/436 +f 407/485/437 423/496/437 408/487/437 +f 401/488/438 415/495/438 416/497/438 +f 409/489/439 423/496/439 424/498/439 +f 401/488/440 417/499/440 402/474/440 +f 409/489/441 425/500/441 410/473/441 +f 403/475/442 417/499/442 418/501/442 +f 411/476/443 425/500/443 426/502/443 +f 403/475/444 419/503/444 404/477/444 +f 411/476/445 427/504/445 412/478/445 +f 405/479/446 419/503/446 420/505/446 +f 412/478/447 428/493/447 413/480/447 +f 406/481/448 420/505/448 421/506/448 +f 418/501/449 432/507/449 433/508/449 +f 426/502/450 440/509/450 441/510/450 +f 418/501/451 434/511/451 419/503/451 +f 426/502/452 442/512/452 427/504/452 +f 420/505/453 434/511/453 435/513/453 +f 427/504/454 443/514/454 428/493/454 +f 421/506/455 435/513/455 436/515/455 +f 414/491/456 82/516/456 429/517/456 +f 308/518/457 428/493/457 443/514/457 +f 421/506/458 437/519/458 422/494/458 +f 414/491/459 430/520/459 415/495/459 +f 422/494/460 438/521/460 423/496/460 +f 416/497/461 430/520/461 431/522/461 +f 424/498/462 438/521/462 439/523/462 +f 416/497/463 432/507/463 417/499/463 +f 424/498/464 440/509/464 425/500/464 +f 436/515/465 452/524/465 437/519/465 +f 430/520/466 444/525/466 445/526/466 +f 437/519/467 453/527/467 438/521/467 +f 430/520/468 446/528/468 431/522/468 +f 439/523/469 453/527/469 454/529/469 +f 431/522/470 447/530/470 432/507/470 +f 439/523/471 455/531/471 440/509/471 +f 433/508/472 447/530/472 448/532/472 +f 441/510/473 455/531/473 456/533/473 +f 433/508/474 449/534/474 434/511/474 +f 441/510/475 457/535/475 442/512/475 +f 435/513/476 449/534/476 450/536/476 +f 442/512/477 458/537/477 443/514/477 +f 436/515/478 450/536/478 451/538/478 +f 429/517/479 82/539/479 444/525/479 +f 308/540/480 443/514/480 458/537/480 +f 456/533/481 470/541/481 471/542/481 +f 448/532/482 464/543/482 449/534/482 +f 456/533/483 472/544/483 457/535/483 +f 450/536/484 464/543/484 465/545/484 +f 458/537/485 472/544/485 473/546/485 +f 451/538/486 465/545/486 466/547/486 +f 444/525/487 82/548/487 459/549/487 +f 308/550/488 458/537/488 473/546/488 +f 451/538/489 467/551/489 452/524/489 +f 444/525/490 460/552/490 445/526/490 +f 452/524/491 468/553/491 453/527/491 +f 446/528/492 460/552/492 461/554/492 +f 454/529/493 468/553/493 469/555/493 +f 446/528/494 462/556/494 447/530/494 +f 454/529/495 470/541/495 455/531/495 +f 448/532/496 462/556/496 463/557/496 +f 467/551/497 478/6/497 468/553/497 +f 461/554/498 475/1/498 1/7/498 +f 468/553/499 6/10/499 469/555/499 +f 462/556/500 1/7/500 476/11/500 +f 469/555/501 479/14/501 470/541/501 +f 463/557/502 476/11/502 2/15/502 +f 471/542/503 479/14/503 480/18/503 +f 464/543/504 2/15/504 477/19/504 +f 471/542/505 481/22/505 472/544/505 +f 465/545/506 477/19/506 3/23/506 +f 472/544/507 482/26/507 473/546/507 +f 466/547/508 3/23/508 4/28/508 +f 459/549/509 82/558/509 474/29/509 +f 308/559/510 473/546/510 482/26/510 +f 466/547/511 5/4/511 467/551/511 +f 460/552/512 474/29/512 475/1/512 +f 475/1/1 474/29/1 7/2/1 +f 5/4/2 15/32/2 16/5/2 +f 1/7/3 475/1/3 8/3/3 +f 478/6/4 16/5/4 17/9/4 +f 476/11/5 1/7/5 9/8/5 +f 6/10/6 17/9/6 18/13/6 +f 2/15/7 476/11/7 10/12/7 +f 479/14/8 18/13/8 19/17/8 +f 477/19/9 2/15/9 11/16/9 +f 480/18/10 19/17/10 20/21/10 +f 3/23/11 477/19/11 12/20/11 +f 481/22/12 20/21/12 21/25/12 +f 3/23/513 13/24/513 14/27/513 +f 4/28/514 14/27/514 15/32/514 +f 13/24/17 28/49/17 29/33/17 +f 14/27/20 29/33/20 30/38/20 +f 7/2/21 22/35/21 23/39/21 +f 15/32/22 30/38/22 31/40/22 +f 8/3/23 23/39/23 24/41/23 +f 16/5/24 31/40/24 32/42/24 +f 9/8/25 24/41/25 25/43/25 +f 17/9/26 32/42/26 33/44/26 +f 10/12/27 25/43/27 26/45/27 +f 19/17/28 18/13/28 33/44/28 +f 11/16/29 26/45/29 27/47/29 +f 20/21/30 19/17/30 34/46/30 +f 13/24/31 12/20/31 27/47/31 +f 20/21/32 35/48/32 36/37/32 +f 33/44/33 32/42/33 47/50/33 +f 25/43/34 40/66/34 41/52/34 +f 34/46/35 33/44/35 48/51/35 +f 26/45/36 41/52/36 42/54/36 +f 34/46/515 49/53/515 50/55/515 +f 27/47/38 42/54/38 43/56/38 +f 35/48/39 50/55/39 51/57/39 +f 28/49/40 43/56/40 44/58/40 +f 30/38/43 29/33/43 44/58/43 +f 22/35/44 37/60/44 38/63/44 +f 30/38/45 45/62/45 46/64/45 +f 23/39/46 38/63/46 39/65/46 +f 32/42/47 31/40/47 46/64/47 +f 24/41/48 39/65/48 40/66/48 +f 44/58/50 59/81/50 60/69/50 +f 37/60/51 52/83/51 53/70/51 +f 45/62/52 60/69/52 61/71/52 +f 38/63/53 53/70/53 54/72/53 +f 47/50/54 46/64/54 61/71/54 +f 39/65/55 54/72/55 55/74/55 +f 47/50/56 62/73/56 63/75/56 +f 40/66/57 55/74/57 56/76/57 +f 48/51/58 63/75/58 64/77/58 +f 41/52/59 56/76/59 57/78/59 +f 49/53/60 64/77/60 65/79/60 +f 43/56/61 42/54/61 57/78/61 +f 50/55/62 65/79/62 66/68/62 +f 44/58/63 43/56/63 58/80/63 +f 63/75/65 78/99/65 79/84/65 +f 56/76/66 71/100/66 72/85/66 +f 65/79/67 64/77/67 79/84/67 +f 58/80/68 57/78/68 72/85/68 +f 65/79/516 80/86/516 81/88/516 +f 59/81/70 58/80/70 73/87/70 +f 59/81/73 74/89/73 75/93/73 +f 52/83/74 67/91/74 68/94/74 +f 60/69/75 75/93/75 76/95/75 +f 53/70/76 68/94/76 69/96/76 +f 62/73/77 61/71/77 76/95/77 +f 54/72/78 69/96/78 70/98/78 +f 62/73/79 77/97/79 78/99/79 +f 56/76/80 55/74/80 70/98/80 +f 67/91/81 83/115/81 84/101/81 +f 75/93/82 91/117/82 92/102/82 +f 68/94/83 84/101/83 85/103/83 +f 77/97/84 76/95/84 92/102/84 +f 69/96/85 85/103/85 86/105/85 +f 77/97/86 93/104/86 94/106/86 +f 71/100/87 70/98/87 86/105/87 +f 78/99/88 94/106/88 95/108/88 +f 71/100/89 87/107/89 88/109/89 +f 79/84/90 95/108/90 96/110/90 +f 73/87/91 72/85/91 88/109/91 +f 81/88/92 80/86/92 96/110/92 +f 74/89/93 73/87/93 89/111/93 +f 74/89/96 90/113/96 91/117/96 +f 87/107/97 102/134/97 103/118/97 +f 96/110/98 95/108/98 110/119/98 +f 89/111/99 88/109/99 103/118/99 +f 96/110/100 111/120/100 112/122/100 +f 90/113/101 89/111/101 104/121/101 +f 90/113/104 105/123/104 106/127/104 +f 84/101/105 83/115/105 98/125/105 +f 91/117/106 106/127/106 107/129/106 +f 84/101/107 99/128/107 100/130/107 +f 93/104/108 92/102/108 107/129/108 +f 85/103/109 100/130/109 101/132/109 +f 93/104/110 108/131/110 109/133/110 +f 87/107/111 86/105/111 101/132/111 +f 95/108/112 94/106/112 109/133/112 +f 106/127/113 121/150/113 122/135/113 +f 99/128/114 114/151/114 115/136/114 +f 108/131/115 107/129/115 122/135/115 +f 100/130/116 115/136/116 116/138/116 +f 108/131/117 123/137/117 124/139/117 +f 102/134/118 101/132/118 116/138/118 +f 110/119/119 109/133/119 124/139/119 +f 102/134/120 117/140/120 118/142/120 +f 110/119/121 125/141/121 126/143/121 +f 104/121/122 103/118/122 118/142/122 +f 111/120/123 126/143/123 127/145/123 +f 105/123/124 104/121/124 119/144/124 +f 105/123/127 120/146/127 121/150/127 +f 99/128/128 98/125/128 113/148/128 +f 125/141/129 140/168/129 141/152/129 +f 119/144/130 118/142/130 133/153/130 +f 126/143/131 141/152/131 142/155/131 +f 120/146/132 119/144/132 134/154/132 +f 120/146/135 135/156/135 136/160/135 +f 113/148/136 128/158/136 129/161/136 +f 121/150/137 136/160/137 137/162/137 +f 114/151/138 129/161/138 130/163/138 +f 123/137/139 122/135/139 137/162/139 +f 115/136/140 130/163/140 131/165/140 +f 123/137/141 138/164/141 139/166/141 +f 117/140/142 116/138/142 131/165/142 +f 125/141/143 124/139/143 139/166/143 +f 117/140/144 132/167/144 133/153/144 +f 130/163/145 129/161/145 144/169/145 +f 138/164/146 137/162/146 152/171/146 +f 130/163/147 145/170/147 146/173/147 +f 138/164/148 153/172/148 154/174/148 +f 132/167/149 131/165/149 146/173/149 +f 140/168/150 139/166/150 154/174/150 +f 132/167/151 147/175/151 148/177/151 +f 140/168/152 155/176/152 156/178/152 +f 134/154/153 133/153/153 148/177/153 +f 142/155/154 141/152/154 156/178/154 +f 135/156/155 134/154/155 149/179/155 +f 135/156/158 150/181/158 151/185/158 +f 128/158/159 143/183/159 144/169/159 +f 136/160/160 151/185/160 152/171/160 +f 149/179/161 148/177/161 163/186/161 +f 156/178/162 171/202/162 172/188/162 +f 150/181/163 149/179/163 164/187/163 +f 150/181/166 165/189/166 166/193/166 +f 143/183/167 158/191/167 159/194/167 +f 151/185/168 166/193/168 167/195/168 +f 145/170/169 144/169/169 159/194/169 +f 153/172/170 152/171/170 167/195/170 +f 145/170/171 160/196/171 161/198/171 +f 153/172/172 168/197/172 169/199/172 +f 147/175/173 146/173/173 161/198/173 +f 155/176/174 154/174/174 169/199/174 +f 147/175/175 162/200/175 163/186/175 +f 155/176/517 170/201/517 171/202/517 +f 168/197/177 167/195/177 182/203/177 +f 160/196/178 175/219/178 176/205/178 +f 168/197/179 183/204/179 184/206/179 +f 162/200/180 161/198/180 176/205/180 +f 169/199/181 184/206/181 185/208/181 +f 162/200/182 177/207/182 178/209/182 +f 171/202/183 170/201/183 185/208/183 +f 164/187/184 163/186/184 178/209/184 +f 172/188/185 171/202/185 186/210/185 +f 165/189/186 164/187/186 179/211/186 +f 165/189/189 180/213/189 181/217/189 +f 158/191/190 173/215/190 174/218/190 +f 166/193/191 181/217/191 182/203/191 +f 159/194/192 174/218/192 175/219/192 +f 186/210/193 201/236/193 202/220/193 +f 180/213/194 179/211/194 194/221/194 +f 180/213/197 195/222/197 196/226/197 +f 174/218/198 173/215/198 188/224/198 +f 181/217/199 196/226/199 197/228/199 +f 175/219/200 174/218/200 189/227/200 +f 183/204/201 182/203/201 197/228/201 +f 175/219/202 190/229/202 191/231/202 +f 183/204/203 198/230/203 199/232/203 +f 177/207/204 176/205/204 191/231/204 +f 185/208/205 184/206/205 199/232/205 +f 177/207/206 192/233/206 193/235/206 +f 185/208/207 200/234/207 201/236/207 +f 179/211/208 178/209/208 193/235/208 +f 198/230/209 213/253/209 214/237/209 +f 192/233/210 191/231/210 206/238/210 +f 200/234/211 199/232/211 214/237/211 +f 192/233/212 207/239/212 208/241/212 +f 200/234/518 215/240/518 216/242/518 +f 194/221/214 193/235/214 208/241/214 +f 201/236/215 216/242/215 217/244/215 +f 195/222/216 194/221/216 209/243/216 +f 195/222/219 210/245/219 211/249/219 +f 188/224/220 203/247/220 204/250/220 +f 196/226/221 211/249/221 212/251/221 +f 189/227/222 204/250/222 205/252/222 +f 198/230/223 197/228/223 212/251/223 +f 190/229/224 205/252/224 206/238/224 +f 210/245/227 225/270/227 226/258/227 +f 203/247/228 218/255/228 219/259/228 +f 211/249/229 226/258/229 227/260/229 +f 204/250/230 219/259/230 220/261/230 +f 213/253/231 212/251/231 227/260/231 +f 205/252/232 220/261/232 221/263/232 +f 213/253/233 228/262/233 229/264/233 +f 207/239/234 206/238/234 221/263/234 +f 215/240/235 214/237/235 229/264/235 +f 207/239/236 222/265/236 223/267/236 +f 215/240/237 230/266/237 231/268/237 +f 209/243/238 208/241/238 223/267/238 +f 216/242/239 231/268/239 232/257/239 +f 210/245/240 209/243/240 224/269/240 +f 222/265/241 221/263/241 236/271/241 +f 230/266/242 229/264/242 244/273/242 +f 222/265/243 237/272/243 238/275/243 +f 230/266/244 245/274/244 246/276/244 +f 224/269/245 223/267/245 238/275/245 +f 232/257/246 231/268/246 246/276/246 +f 225/270/247 224/269/247 239/277/247 +f 225/270/250 240/279/250 241/283/250 +f 218/255/251 233/281/251 234/284/251 +f 226/258/252 241/283/252 242/285/252 +f 220/261/253 219/259/253 234/284/253 +f 228/262/254 227/260/254 242/285/254 +f 220/261/255 235/286/255 236/271/255 +f 228/262/256 243/287/256 244/273/256 +f 240/279/519 255/301/519 256/288/519 +f 233/281/258 248/303/258 249/289/258 +f 241/283/259 256/288/259 257/290/259 +f 234/284/260 249/289/260 250/291/260 +f 243/287/261 242/285/261 257/290/261 +f 235/286/262 250/291/262 251/293/262 +f 243/287/263 258/292/263 259/294/263 +f 237/272/264 236/271/264 251/293/264 +f 245/274/265 244/273/265 259/294/265 +f 237/272/266 252/295/266 253/297/266 +f 245/274/267 260/296/267 261/298/267 +f 239/277/268 238/275/268 253/297/268 +f 246/276/269 261/298/269 262/300/269 +f 240/279/520 239/277/520 254/299/520 +f 260/296/273 259/294/273 274/305/273 +f 252/295/274 267/321/274 268/307/274 +f 260/296/275 275/306/275 276/308/275 +f 254/299/276 253/297/276 268/307/276 +f 261/298/277 276/308/277 277/310/277 +f 255/301/278 254/299/278 269/309/278 +f 255/301/281 270/311/281 271/315/281 +f 249/289/282 248/303/282 263/313/282 +f 256/288/283 271/315/283 272/317/283 +f 249/289/284 264/316/284 265/318/284 +f 258/292/285 257/290/285 272/317/285 +f 250/291/286 265/318/286 266/320/286 +f 258/292/287 273/319/287 274/305/287 +f 252/295/288 251/293/288 266/320/288 +f 264/316/289 263/313/289 278/322/289 +f 271/315/290 286/338/290 287/324/290 +f 264/316/291 279/323/291 280/325/291 +f 273/319/292 272/317/292 287/324/292 +f 265/318/293 280/325/293 281/327/293 +f 273/319/294 288/326/294 289/328/294 +f 267/321/295 266/320/295 281/327/295 +f 275/306/296 274/305/296 289/328/296 +f 267/321/297 282/329/297 283/331/297 +f 275/306/521 290/330/521 291/332/521 +f 269/309/299 268/307/299 283/331/299 +f 277/310/300 276/308/300 291/332/300 +f 270/311/301 269/309/301 284/333/301 +f 270/311/304 285/335/304 286/338/304 +f 282/329/305 297/354/305 298/339/305 +f 290/330/306 305/355/306 306/340/306 +f 284/333/307 283/331/307 298/339/307 +f 292/334/308 291/332/308 306/340/308 +f 285/335/309 284/333/309 299/341/309 +f 285/335/312 300/343/312 301/347/312 +f 278/322/313 293/345/313 294/348/313 +f 286/338/314 301/347/314 302/349/314 +f 279/323/315 294/348/315 295/350/315 +f 288/326/316 287/324/316 302/349/316 +f 280/325/317 295/350/317 296/352/317 +f 288/326/318 303/351/318 304/353/318 +f 282/329/319 281/327/319 296/352/319 +f 290/330/320 289/328/320 304/353/320 +f 301/347/321 317/371/321 318/356/321 +f 294/348/322 310/372/322 311/357/322 +f 303/351/323 302/349/323 318/356/323 +f 295/350/324 311/357/324 312/359/324 +f 303/351/325 319/358/325 320/360/325 +f 297/354/326 296/352/326 312/359/326 +f 305/355/327 304/353/327 320/360/327 +f 297/354/328 313/361/328 314/363/328 +f 305/355/329 321/362/329 322/364/329 +f 299/341/330 298/339/330 314/363/330 +f 306/340/522 322/364/522 323/366/522 +f 300/343/332 299/341/332 315/365/332 +f 300/343/335 316/367/335 317/371/335 +f 293/345/336 309/369/336 310/372/336 +f 321/362/523 336/389/523 337/373/523 +f 315/365/338 314/363/338 329/374/338 +f 322/364/339 337/373/339 338/376/339 +f 316/367/340 315/365/340 330/375/340 +f 316/367/343 331/377/343 332/381/343 +f 309/369/344 324/379/344 325/382/344 +f 317/371/345 332/381/345 333/383/345 +f 310/372/346 325/382/346 326/384/346 +f 319/358/347 318/356/347 333/383/347 +f 311/357/348 326/384/348 327/386/348 +f 319/358/349 334/385/349 335/387/349 +f 312/359/350 327/386/350 328/388/350 +f 321/362/351 320/360/351 335/387/351 +f 313/361/352 328/388/352 329/374/352 +f 334/385/353 333/383/353 348/390/353 +f 326/384/354 341/406/354 342/392/354 +f 334/385/355 349/391/355 350/393/355 +f 328/388/356 327/386/356 342/392/356 +f 336/389/357 335/387/357 350/393/357 +f 328/388/358 343/394/358 344/396/358 +f 336/389/359 351/395/359 352/397/359 +f 330/375/360 329/374/360 344/396/360 +f 337/373/361 352/397/361 353/399/361 +f 331/377/362 330/375/362 345/398/362 +f 331/377/365 346/400/365 347/404/365 +f 324/379/366 339/402/366 340/405/366 +f 332/381/367 347/404/367 348/390/367 +f 325/382/368 340/405/368 341/406/368 +f 352/397/369 367/423/369 368/407/369 +f 346/400/524 345/398/524 360/408/524 +f 346/400/525 361/409/525 362/413/525 +f 339/402/374 354/411/374 355/414/374 +f 347/404/375 362/413/375 363/415/375 +f 341/406/376 340/405/376 355/414/376 +f 349/391/377 348/390/377 363/415/377 +f 341/406/378 356/416/378 357/418/378 +f 349/391/379 364/417/379 365/419/379 +f 343/394/380 342/392/380 357/418/380 +f 351/395/381 350/393/381 365/419/381 +f 343/394/382 358/420/382 359/422/382 +f 351/395/383 366/421/383 367/423/383 +f 345/398/384 344/396/384 359/422/384 +f 356/424/385 371/454/385 372/425/385 +f 364/427/386 379/455/386 380/428/386 +f 358/430/387 357/426/387 372/425/387 +f 366/432/388 365/429/388 380/428/388 +f 358/430/389 373/431/389 374/434/389 +f 366/432/390 381/433/390 382/436/390 +f 360/438/391 359/435/391 374/434/391 +f 367/437/392 382/436/392 383/440/392 +f 361/442/526 360/438/526 375/439/526 +f 361/442/527 376/443/527 377/448/527 +f 354/444/397 369/446/397 370/450/397 +f 362/449/398 377/448/398 378/452/398 +f 356/424/399 355/451/399 370/450/399 +f 364/427/400 363/453/400 378/452/400 +f 376/443/401 375/439/401 390/456/401 +f 376/443/404 391/457/404 392/462/404 +f 369/446/405 384/459/405 385/463/405 +f 377/448/406 392/462/406 393/464/406 +f 371/454/407 370/450/407 385/463/407 +f 379/455/408 378/452/408 393/464/408 +f 371/454/409 386/465/409 387/467/409 +f 379/455/410 394/466/410 395/468/410 +f 373/431/411 372/425/411 387/467/411 +f 381/433/412 380/428/412 395/468/412 +f 373/431/413 388/469/413 389/471/413 +f 381/433/414 396/470/414 397/472/414 +f 375/439/415 374/434/415 389/471/415 +f 382/436/416 397/472/416 398/461/416 +f 394/466/417 409/489/417 410/473/417 +f 388/469/418 387/467/418 402/474/418 +f 396/470/419 395/468/419 410/473/419 +f 388/469/420 403/475/420 404/477/420 +f 397/472/421 396/470/421 411/476/421 +f 390/456/422 389/471/422 404/477/422 +f 397/472/423 412/478/423 413/480/423 +f 391/457/424 390/456/424 405/479/424 +f 391/457/427 406/481/427 407/485/427 +f 385/463/428 384/459/428 399/483/428 +f 392/462/429 407/485/429 408/487/429 +f 385/463/430 400/486/430 401/488/430 +f 394/466/431 393/464/431 408/487/431 +f 386/465/432 401/488/432 402/474/432 +f 406/481/435 421/506/435 422/494/435 +f 399/483/436 414/491/436 415/495/436 +f 407/485/437 422/494/437 423/496/437 +f 401/488/438 400/486/438 415/495/438 +f 409/489/439 408/487/439 423/496/439 +f 401/488/440 416/497/440 417/499/440 +f 409/489/441 424/498/441 425/500/441 +f 403/475/442 402/474/442 417/499/442 +f 411/476/443 410/473/443 425/500/443 +f 403/475/444 418/501/444 419/503/444 +f 411/476/445 426/502/445 427/504/445 +f 405/479/446 404/477/446 419/503/446 +f 412/478/447 427/504/447 428/493/447 +f 406/481/448 405/479/448 420/505/448 +f 418/501/449 417/499/449 432/507/449 +f 426/502/450 425/500/450 440/509/450 +f 418/501/451 433/508/451 434/511/451 +f 426/502/452 441/510/452 442/512/452 +f 420/505/453 419/503/453 434/511/453 +f 427/504/454 442/512/454 443/514/454 +f 421/506/455 420/505/455 435/513/455 +f 421/506/458 436/515/458 437/519/458 +f 414/491/459 429/517/459 430/520/459 +f 422/494/460 437/519/460 438/521/460 +f 416/497/461 415/495/461 430/520/461 +f 424/498/462 423/496/462 438/521/462 +f 416/497/463 431/522/463 432/507/463 +f 424/498/464 439/523/464 440/509/464 +f 436/515/465 451/538/465 452/524/465 +f 430/520/466 429/517/466 444/525/466 +f 437/519/467 452/524/467 453/527/467 +f 430/520/468 445/526/468 446/528/468 +f 439/523/469 438/521/469 453/527/469 +f 431/522/470 446/528/470 447/530/470 +f 439/523/471 454/529/471 455/531/471 +f 433/508/472 432/507/472 447/530/472 +f 441/510/473 440/509/473 455/531/473 +f 433/508/474 448/532/474 449/534/474 +f 441/510/475 456/533/475 457/535/475 +f 435/513/476 434/511/476 449/534/476 +f 442/512/477 457/535/477 458/537/477 +f 436/515/478 435/513/478 450/536/478 +f 456/533/481 455/531/481 470/541/481 +f 448/532/482 463/557/482 464/543/482 +f 456/533/483 471/542/483 472/544/483 +f 450/536/484 449/534/484 464/543/484 +f 458/537/485 457/535/485 472/544/485 +f 451/538/486 450/536/486 465/545/486 +f 451/538/489 466/547/489 467/551/489 +f 444/525/490 459/549/490 460/552/490 +f 452/524/491 467/551/491 468/553/491 +f 446/528/492 445/526/492 460/552/492 +f 454/529/493 453/527/493 468/553/493 +f 446/528/494 461/554/494 462/556/494 +f 454/529/495 469/555/495 470/541/495 +f 448/532/496 447/530/496 462/556/496 +f 467/551/497 5/4/497 478/6/497 +f 461/554/498 460/552/498 475/1/498 +f 468/553/499 478/6/499 6/10/499 +f 462/556/500 461/554/500 1/7/500 +f 469/555/501 6/10/501 479/14/501 +f 463/557/502 462/556/502 476/11/502 +f 471/542/503 470/541/503 479/14/503 +f 464/543/504 463/557/504 2/15/504 +f 471/542/505 480/18/505 481/22/505 +f 465/545/506 464/543/506 477/19/506 +f 472/544/507 481/22/507 482/26/507 +f 466/547/508 465/545/508 3/23/508 +f 466/547/511 4/28/511 5/4/511 +f 460/552/512 459/549/512 474/29/512 diff --git a/tests/src/tests/utils/CMakeLists.txt b/tests/src/tests/utils/CMakeLists.txt index c386e8915..34f57b7e0 100644 --- a/tests/src/tests/utils/CMakeLists.txt +++ b/tests/src/tests/utils/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests 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..be014463b --- /dev/null +++ b/tests/src/tests/utils/test_vertex_matrix_view.cpp @@ -0,0 +1,129 @@ +#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))); + } + } +}