diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index f4239d455..b71a818e5 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -68,7 +68,7 @@ jobs: run: | cd build ctest --verbose -j ${{ steps.cpu-cores.outputs.count }} - lcov --directory . --capture --output-file coverage.info --ignore-errors inconsistent,format,gcov + lcov --directory . --capture --output-file coverage.info --ignore-errors inconsistent,format,gcov,mismatch lcov --remove coverage.info --ignore-errors unused '/usr/*' "$HOME/.cache/*" "*tests/*" --output-file coverage.info - name: Upload coverage reports to Codecov diff --git a/.gitignore b/.gitignore index 0b599431c..149693837 100644 --- a/.gitignore +++ b/.gitignore @@ -675,4 +675,5 @@ CMakeGraphVizOptions.cmake .zed/* .claude -graphify-out \ No newline at end of file +graphify-out +__cmake_systeminformation \ No newline at end of file diff --git a/CLAUDE.md b/CLAUDE.md index 615fe7c87..ee7ef7500 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -40,6 +40,27 @@ cd build/test && ctest --verbose -R "test_name_pattern" ./build/test/ipc_toolkit_tests "[tag]" ``` +### Benchmarks + +Benchmarks are hidden Catch2 cases tagged `[!benchmark]`, so they don't run by +default — but a tag filter like `"[simd]"` *will* pull them in alongside the +unit tests. Exclude them explicitly when you only want correctness: + +```bash +./build/test/ipc_toolkit_tests "[simd] ~[!benchmark]" +``` + +**Never take benchmark numbers from a `test`/`debug` build.** Those presets are +`CMAKE_BUILD_TYPE=Debug`, where the templated kernels aren't inlined and the +asserts are live, so the results are meaningless — and misleading, since the +SIMD paths lean hardest on inlining and lose the most. Build a Release +configuration with tests enabled and benchmark that instead: + +```bash +cmake -S . -B build/benchmark -DCMAKE_BUILD_TYPE=Release -DIPC_TOOLKIT_BUILD_TESTS=ON +cmake --build build/benchmark -j 8 +``` + ## Code Style - **Formatter:** clang-format, WebKit-based style, **80-character column limit**. Pre-commit hooks enforce this — always run before committing: diff --git a/CMakeLists.txt b/CMakeLists.txt index 177954f9f..ee04d3984 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -302,12 +302,16 @@ target_link_libraries(ipc_toolkit PRIVATE ipc::toolkit::warnings) # SIMD support if(IPC_TOOLKIT_WITH_SIMD) - # Add SIMD flags to compiler flags - target_compile_options(ipc_toolkit PRIVATE ${SIMD_CXX_FLAGS}) + # Add SIMD flags to compiler flags. + # NOTE: PUBLIC because xsimd::default_arch is selected from each translation + # unit's own flags. ipc/utils/simd.hpp exposes batch types in the public API, + # and a consumer compiled without these flags would name a different type + # than the one instantiated in the library, failing to link. + target_compile_options(ipc_toolkit PUBLIC ${SIMD_CXX_FLAGS}) # Link against cross-platform xsimd library include(xsimd) - target_link_libraries(ipc_toolkit PRIVATE xsimd::xsimd) + target_link_libraries(ipc_toolkit PUBLIC xsimd::xsimd) # Disable vectorization in Eigen since I've found it to have alignment issues. # NOTE: I don't know why this needs to be public, but it crashes if I make it private. @@ -339,6 +343,7 @@ endif() if(IPC_TOOLKIT_WITH_CODE_COVERAGE AND CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") # Add required flags (GCC & LLVM/Clang) + # NOTE: tests/CMakeLists.txt repeats these for the test target; keep in sync. target_compile_options(ipc_toolkit PRIVATE -g # generate debug info --coverage # sets all required flags diff --git a/cmake/find/FindSIMD.cmake b/cmake/find/FindSIMD.cmake index c3e8bf645..11db9a522 100644 --- a/cmake/find/FindSIMD.cmake +++ b/cmake/find/FindSIMD.cmake @@ -180,8 +180,14 @@ function (test_sse_availability) endfunction() -# This script checks for the highest level of FMA support on the host -# by compiling and running small C++ programs that uses FMA intrinsics. +# This script checks for x86 FMA3 support on the host by compiling and running +# a small C++ program that uses the AVX2 FMA intrinsics. +# +# NOTE: This probe is x86-only -- it needs `-mavx2 -mfma` and , so +# it cannot succeed on AArch64 and reports DETECTED_FMA3_X86 as failed there. +# That is not a missing capability: AArch64 has no opt-in FMA flag because +# fmadd/fmsub and the NEON vfmaq_* family are mandatory in the base ISA, so an +# empty FMA_FLAGS is the correct answer and NEON implies a fused multiply-add. # If any FMA support is detected, the following variables are set: # @@ -194,7 +200,7 @@ endfunction() function (test_fma_availability) set(FMA_FLAGS) set(FMA_FOUND) - set(DETECTED_FMA) + set(DETECTED_FMA3_X86) include(CheckCXXSourceRuns) set(CMAKE_REQUIRED_FLAGS) @@ -219,12 +225,12 @@ function (test_fma_availability) __m256d result = _mm256_fmsub_pd (a, b, c); return 0; - }" DETECTED_FMA) + }" DETECTED_FMA3_X86) endif() set(CMAKE_REQUIRED_FLAGS) - if(DETECTED_FMA) + if(DETECTED_FMA3_X86) SET(FMA_FOUND 1) if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") SET(FMA_FLAGS "${FMA_FLAGS} -mfma") diff --git a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake index 2cbe5ea5a..edd7c2401 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake @@ -47,6 +47,7 @@ else() -Wpointer-arith -Wformat=2 -Wuninitialized + -Wno-maybe-uninitialized -Wcast-qual -Wmissing-noreturn -Wmissing-format-attribute @@ -179,6 +180,13 @@ else() if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "GNU") list(APPEND IPC_TOOLKIT_WARNING_FLAGS -Wnull-dereference) endif() + + # GCC 16 mis-analyzes TBB's enumerable_thread_specific. GCC <= 15 and Clang + # are clean, so only suppress it where it fires. + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" + AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 16) + list(APPEND IPC_TOOLKIT_WARNING_FLAGS -Wno-array-bounds) + endif() endif() add_library(ipc_toolkit_warnings INTERFACE) diff --git a/cmake/recipes/meshfem_sparse.cmake b/cmake/recipes/meshfem_sparse.cmake index 0ede646ac..966ce085e 100644 --- a/cmake/recipes/meshfem_sparse.cmake +++ b/cmake/recipes/meshfem_sparse.cmake @@ -58,7 +58,9 @@ target_include_directories(MeshFEMSparse SYSTEM PUBLIC # MeshFEMCore's headers include the CMake-generated . We # build a static library, so the export macros are empty. -file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/meshfem/exports/MeshFEM_export.h" [[ +file(CONFIGURE + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/meshfem/exports/MeshFEM_export.h" + CONTENT [[ #pragma once #define MESHFEM_EXPORT #define MESHFEM_NO_EXPORT diff --git a/codecov.yml b/codecov.yml index b28f2ce69..d83565144 100644 --- a/codecov.yml +++ b/codecov.yml @@ -10,4 +10,4 @@ coverage: threshold: 5% only_pulls: true ignore: - - "tests/*" + - "tests/**" diff --git a/docs/source/_static/graphviz/dependencies.dot b/docs/source/_static/graphviz/dependencies.dot index 50643f46d..f76aba605 100644 --- a/docs/source/_static/graphviz/dependencies.dot +++ b/docs/source/_static/graphviz/dependencies.dot @@ -2,7 +2,10 @@ digraph "IPC Toolkit Dependencies" { bgcolor = "transparent"; splines = ortho; layout = dot; - nodesep = 0.2; + // 0.28 rather than 0.2: with less separation the ortho router has no room + // to turn igl_predicates -> igl_core and hairpins it into igl_core's west + // side, putting the arrowhead on top of the predicates node. + nodesep = 0.28; ranksep = 0.5; node [fontname = "Menlo"; style = filled; penwidth = 2;]; edge [penwidth = 2; fontname = "Menlo";]; @@ -17,9 +20,13 @@ digraph "IPC Toolkit Dependencies" { legendNode0 [label = "Static Library";shape = box;style = "rounded,filled";fillcolor = "#D5E8D4";color = "#8FB976";]; legendNode1 [label = "Shared Library";shape = box;style = "rounded,filled";fillcolor = "#CCE7F8";color = "#6596B2";]; legendNode2 [label = "Interface Library";shape = box;style = "rounded,filled";fillcolor = "#FFE6CC";color = "#DAA52D";]; + legendNode3 [label = "Optional Dependency";shape = box;style = "rounded,filled,dashed";fillcolor = "#F5F5F5";color = "#999999";fontcolor = "#555555";]; legendNode0 -> legendNode1 [label = "Public"; color = "#8FB976"; fontcolor = "#8FB976";]; legendNode2 -> legendNode0 [label = "Interface"; color = "#DAA52D"; fontcolor = "#DAA52D";]; legendNode1 -> legendNode2 [label = "Private"; color = "#BE6562"; fontcolor = "#BE6562";]; + // Dashed marks a dependency behind an IPC_TOOLKIT_WITH_* CMake option, + // independent of the link scope the edge colour encodes. + legendNode2 -> legendNode3 [label = "Optional"; style = "dashed"; color = "#999999"; fontcolor = "#555555";]; } // Force ipc_toolkit to top subgraph { @@ -40,18 +47,18 @@ digraph "IPC Toolkit Dependencies" { "node5" [label = "ipc_toolkit\n(ipc::toolkit)";shape = box;style = "rounded,filled";fillcolor = "#D5E8D4";color = "#8FB976";]; "node5" -> "node0" [color = "#8FB976";]; // ipc_toolkit -> Eigen3_Eigen - "node5" -> "node1" [color = "#8FB976";]; - // ipc_toolkit -> filib + "node5" -> "node1" [color = "#8FB976"; style = "dashed";]; + // ipc_toolkit -> filib (IPC_TOOLKIT_WITH_FILIB) "node5" -> "node2" [color = "#BE6562";]; // ipc_toolkit -> igl_core "node5" -> "node3" [color = "#BE6562";]; // ipc_toolkit -> igl_predicates "node15" [label = "xsimd\n(xsimd::xsimd)";shape = box;style = "rounded,filled";fillcolor = "#FFE6CC";color = "#DAA52D";]; - "node5" -> "node15" [color = "#BE6562";]; - // ipc_toolkit -> xsimd + "node5" -> "node15" [color = "#8FB976"; style = "dashed";]; + // ipc_toolkit -> xsimd (IPC_TOOLKIT_WITH_SIMD) "node6" [label = "robin_map\n(tsl::robin_map)";shape = box;style = "rounded,filled";fillcolor = "#FFE6CC";color = "#DAA52D";]; - "node5" -> "node6" [color = "#BE6562";]; - // ipc_toolkit -> robin_map + "node5" -> "node6" [color = "#BE6562"; style = "dashed";]; + // ipc_toolkit -> robin_map (IPC_TOOLKIT_WITH_ROBIN_MAP) "node7" [label = "scalable_ccd\n(scalable_ccd::scalable_ccd)";shape = box;style = "rounded,filled";fillcolor = "#D5E8D4";color = "#8FB976";]; "node7" -> "node0" [color = "#8FB976";]; // scalable_ccd -> Eigen3_Eigen @@ -75,16 +82,20 @@ digraph "IPC Toolkit Dependencies" { "node5" -> "node11" [color = "#BE6562";]; // ipc_toolkit -> tight_inclusion "node12" [label = "absl_hash\n(absl::hash)";shape = box;style = "rounded,filled";fillcolor = "#D5E8D4";color = "#8FB976";]; - "node5" -> "node12" [color = "#BE6562";]; - // ipc_toolkit -> TinyAD + "node5" -> "node12" [color = "#BE6562"; style = "dashed";]; + // ipc_toolkit -> absl_hash (IPC_TOOLKIT_WITH_ABSEIL) "node13" [label = "TinyAD\n(TinyAD::TinyAD)";shape = box;style = "rounded,filled";fillcolor = "#D5E8D4";color = "#8FB976";]; "node5" -> "node13" [color = "#8FB976";]; + // ipc_toolkit -> TinyAD "node13" -> "node0" [color = "#BE6562";]; + // TinyAD -> Eigen3_Eigen "node13" -> "node9" [color = "#BE6562";]; - // ipc_toolkit -> nlohmann_json - "node14" [label = "nlohmann_json\n(nlohmann_json::nlohmann_json)";shape = box;style = "rounded,filled";fillcolor = "#FFE6CC";color = "#DAA52D";]; - "node5" -> "node14" [color = "#8FB976";]; - // ipc_toolkit -> tracy - "node16" [label = "TracyClient\n(Tracy::TracyClient)";shape = box;style = "rounded,filled";fillcolor = "#D5E8D4";color = "#8FB976";]; - "node5" -> "node16" [color = "#8FB976";]; + // TinyAD -> tbb + "node17" [label = "MeshFEMSparse\n(MeshFEM::Sparse)";shape = box;style = "rounded,filled";fillcolor = "#D5E8D4";color = "#8FB976";]; + "node5" -> "node17" [color = "#BE6562"; style = "dashed";]; + // ipc_toolkit -> MeshFEMSparse (IPC_TOOLKIT_WITH_MESHFEM_SPARSE) + "node17" -> "node0" [color = "#8FB976";]; + // MeshFEMSparse -> Eigen3_Eigen + "node17" -> "node9" [color = "#8FB976";]; + // MeshFEMSparse -> tbb } \ No newline at end of file diff --git a/docs/source/_static/graphviz/dependencies.svg b/docs/source/_static/graphviz/dependencies.svg index d502307ca..6e9d89f80 100644 --- a/docs/source/_static/graphviz/dependencies.svg +++ b/docs/source/_static/graphviz/dependencies.svg @@ -4,309 +4,321 @@ - - + + IPC Toolkit Dependencies clusterLegend - -Legend + +Legend legendNode0 - -Static Library + +Static Library legendNode1 - -Shared Library + +Shared Library legendNode0->legendNode1 - - -Public + + +Public legendNode2 - -Interface Library + +Interface Library legendNode1->legendNode2 - - -Private + + +Private legendNode2->legendNode0 - - -Interface + + +Interface - + +legendNode3 + +Optional Dependency + + + +legendNode2->legendNode3 + + +Optional + + + node5 - -ipc_toolkit -(ipc::toolkit) + +ipc_toolkit +(ipc::toolkit) - + node0 - -Eigen3_Eigen -(Eigen3::Eigen) + +Eigen3_Eigen +(Eigen3::Eigen) - + node5->node0 - - + + - + node1 - -filib -(filib::filib) + +filib +(filib::filib) - + node5->node1 - - + + - + node2 - -igl_core -(igl::core) + +igl_core +(igl::core) - + node5->node2 - - + + - + node3 - -igl_predicates -(igl::predicates) + +igl_predicates +(igl::predicates) - + node5->node3 - - + + - + node15 - -xsimd -(xsimd::xsimd) + +xsimd +(xsimd::xsimd) - + node5->node15 - - + + - + node6 - -robin_map -(tsl::robin_map) + +robin_map +(tsl::robin_map) - + node5->node6 - - + + - + node7 - -scalable_ccd -(scalable_ccd::scalable_ccd) + +scalable_ccd +(scalable_ccd::scalable_ccd) - + node5->node7 - - + + - + node8 - -spdlog -(spdlog::spdlog) + +spdlog +(spdlog::spdlog) - + node5->node8 - - + + - + node9 - -tbb -(TBB::tbb) + +tbb +(TBB::tbb) - + node5->node9 - - + + - + node11 - -tight_inclusion -(tight_inclusion::tight_inclusion) + +tight_inclusion +(tight_inclusion::tight_inclusion) - + node5->node11 - - + + - + node12 - -absl_hash -(absl::hash) + +absl_hash +(absl::hash) - + node5->node12 - - + + - + node13 - -TinyAD -(TinyAD::TinyAD) + +TinyAD +(TinyAD::TinyAD) - + node5->node13 - - - - - -node14 - -nlohmann_json -(nlohmann_json::nlohmann_json) - - - -node5->node14 - - + + - + -node16 - -TracyClient -(Tracy::TracyClient) +node17 + +MeshFEMSparse +(MeshFEM::Sparse) - + -node5->node16 - - +node5->node17 + + - + node2->node0 - - + + - + node3->node2 - - + + - + node4 - -predicates -(predicates::predicates) + +predicates +(predicates::predicates) - + node3->node4 - - + + - + node7->node0 - - + + - + node7->node8 - - + + - + node7->node9 - - + + - + node11->node0 - - + + - + node11->node8 - - + + - + node13->node0 - - + + - + node13->node9 - - + + + + + +node17->node0 + + + + + +node17->node9 + + diff --git a/docs/source/about/dependencies.rst b/docs/source/about/dependencies.rst index 82b6162d2..8205991ce 100644 --- a/docs/source/about/dependencies.rst +++ b/docs/source/about/dependencies.rst @@ -4,7 +4,7 @@ Dependencies .. figure:: /_static/graphviz/dependencies.svg :align: center - Default dependencies of the ``ipc::toolkit`` library. Excludes CUDA and Python bindings. + Default dependencies of the ``ipc::toolkit`` library. Edge colour is the link scope; a dashed edge marks an optional dependency, enabled by default but controlled by an ``IPC_TOOLKIT_WITH_*`` CMake option. Dependencies that are off by default are omitted. Excludes CUDA and Python bindings. The IPC Toolkit depends on a handful of third-party libraries, which are used to provide various functionality. @@ -120,6 +120,11 @@ Additionally, IPC Toolkit may optionally use the following libraries: Some of these libraries are enabled by default, and some are not. You can enable or disable them by passing the appropriate CMake option when you configure the IPC Toolkit build. +.. warning:: + ``xsimd`` is linked **publicly**, and the detected SIMD flags (``SIMD_CXX_FLAGS``, typically ``-march=native``) are applied publicly to anything linking ``ipc::toolkit``. This is a requirement rather than a convenience: ``ipc/utils/simd.hpp`` exposes ``ipc::SimdBatch`` in the public API, and ``xsimd::default_arch`` is resolved from each translation unit's *own* compiler flags. A consumer compiled without these flags therefore names a different batch type than the one instantiated inside the library, and the link fails. + + The practical consequence is that your binaries are compiled for the machine that built them and will not run on hardware lacking those instructions. If you need portable binaries, set ``IPC_TOOLKIT_WITH_SIMD`` to ``OFF``; the library then builds without ``xsimd`` and without the architecture flags. + .. 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). diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 062018a69..08793fa8b 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -42,28 +42,34 @@ API Changes |:wrench:| - Move gradient assembly into a shared ``ipc::assemble_gradient`` (``ipc/utils/gradient_assembler.hpp``) that all five gradient-producing potentials route through (`#246 `_). -- Templatize the distance functions on the scalar type. +- Templatize the distance, tangent, friction, adhesion, and geometry functions on the scalar type. - - The free functions now take a scalar template parameter ``T`` and are instantiated for both ``float`` and ``double``. - - The distance functions are now a two-layer API. + - Functions now take a scalar template parameter ``T`` and are instantiated for ``float``, ``double``, ``xsimd::batch``, and ``xsimd::batch`` types. + - 💥 **[Breaking]** Mixed-precision calls no longer deduce: ``barrier(float_d, 0.001)`` must become ``barrier(float_d, 0.001f)``. + - 💥 **[Breaking]** The gradients/Hessians of the point-point, point-line, and point-edge distances and the ``normalization_*`` functions now return **fixed-size** Eigen types (e.g. ``Eigen::Vector``) when the argument type knows its dimension at compile time, and the previous ``VectorMax``/``MatrixMax`` types otherwise. + - The functions are now a two-layer API. - The concrete kernels moved into ``ipc::detail`` and are templated on the scalar and the dimension (``template ``, or just ```` for the 3D-only). - The public names in ``ipc`` are thin front ends templated on the argument expression types. They deduce ``T``, dispatch on the compile-time dimension when the arguments know it, and fall back to a single runtime branch on ``size()`` otherwise. Existing calls are unaffected. - Autodiff scalars are supported for the value functions only; passing one to a ``*_gradient``/``*_hessian`` or to a ``*_distance_type`` predicate is a compile-time or ``AUTO``-dispatch error rather than silently wrong output. - - 💥 **[Breaking]** Mixed-precision calls no longer deduce: ``barrier(float_d, 0.001)`` must become ``barrier(float_d, 0.001f)``. - - 💥 **[Breaking]** The gradients/Hessians of the point-point, point-line, and point-edge distances and the ``normalization_*`` functions now return **fixed-size** Eigen types (e.g. ``Eigen::Vector``) when the argument type knows its dimension at compile time, and the previous ``VectorMax``/``MatrixMax`` types otherwise. + - ``AUTO`` and the ``*_distance_type`` predicates are **not** available for batch scalars and throw ``std::invalid_argument``. The distance type is a per-lane property but the predicates return a single enum, so two lanes cannot report different closest features. Resolve the distance types scalar-side and group problems by type before batching. + - ``edge_edge_closest_point`` and ``point_triangle_closest_point`` solve their 2×2 symmetric positive-definite system in closed form instead of with ``A.ldlt().solve()``, whose pivot is scalar control flow a batch cannot take per-lane. + + - Solve using Cramer's rule, which is a closed form for 2×2 systems. The determinant is computed in Kahan's fused multiply-add form to recover the rounding error of one product, so the solution stays accurate even when the two products cancel badly. + - The pivot in ``LDLT`` loses the same digits as the naive determinant, so it is no more accurate than the closed form. + - Measured against the exact solution of the same double inputs, the closed form and ``LDLT`` stay within about 2× of each other on well-conditioned systems, but at 1e-4 rad the closed form is accurate to 4e-17 relative where ``LDLT`` reaches only 2e-9. - Templatize the barrier classes on the scalar type. - ``ipc::Barrier`` is now an alias for the class template ``ipc::BarrierBase`` (defaulting to ``double``). - 💥 **[Breaking]** The concrete barriers are now class templates and must be spelled with explicit template arguments (e.g., ``ipc::ClampedLogBarrier<>``). - The free functions ``ipc::barrier``, ``ipc::barrier_first_derivative``, and ``ipc::barrier_second_derivative`` are now templates defaulting to ``double``. + - Replace ``if``/``else``` branches in the barrier functions with ``select_lazy`` cascades, so a batch may carry lanes on either side of ``dhat``. Single scalar inputs still only evaluate the active branch. -- Templatize the tangent functions +- Add ``ipc::numext`` namespace containing an override for ``sqrt``, ``abs``, ``log``, ``fma``, and ``atan2``. For ``float`` and ``double`` they call ``std::``; for ``xsimd::batch`` they call the corresponding ``xsimd`` function. This allows the templated distance and barrier functions to call ``numext::sqrt`` and friends without knowing whether they are operating on scalars or batches. +- 💥 **[Breaking]** ``xsimd`` and ``SIMD_CXX_FLAGS`` are now linked/applied ``PUBLIC`` rather than ``PRIVATE``. ``xsimd::default_arch`` is selected from each translation unit's own compiler flags, so a consumer built without the library's SIMD flags would name a *different* batch type than the one instantiated and fail to link. This means consumers are now compiled with the detected SIMD flags (typically ``-march=native``); disable ``IPC_TOOLKIT_WITH_SIMD`` if that is not wanted. - - Follow the distance family: fixed-size kernels in ``ipc::detail`` templated on ```` (or ```` where the function is 3D-only), behind front ends that deduce both from the argument expressions. - - Return types are fixed-size when the argument type knows its dimension and the previous ``VectorMax``/``MatrixMax`` types otherwise. Performance |:zap:| ~~~~~~~~~~~~~~~~~~~ diff --git a/python/src/adhesion/adhesion.cpp b/python/src/adhesion/adhesion.cpp index aeb319269..5d8f14af0 100644 --- a/python/src/adhesion/adhesion.cpp +++ b/python/src/adhesion/adhesion.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_adhesion(py::module_& m) { m.def( - "normal_adhesion_potential", &normal_adhesion_potential, + "normal_adhesion_potential", &normal_adhesion_potential, R"ipc_Qu8mg5v7( The normal adhesion potential. @@ -24,7 +24,7 @@ void define_adhesion(py::module_& m) m.def( "normal_adhesion_potential_first_derivative", - &normal_adhesion_potential_first_derivative, + &normal_adhesion_potential_first_derivative, R"ipc_Qu8mg5v7( The first derivative of the normal adhesion potential wrt d. @@ -41,7 +41,7 @@ void define_adhesion(py::module_& m) m.def( "normal_adhesion_potential_second_derivative", - &normal_adhesion_potential_second_derivative, + &normal_adhesion_potential_second_derivative, R"ipc_Qu8mg5v7( The second derivative of the normal adhesion potential wrt d. @@ -58,7 +58,7 @@ void define_adhesion(py::module_& m) m.def( "max_normal_adhesion_force_magnitude", - &max_normal_adhesion_force_magnitude, + &max_normal_adhesion_force_magnitude, R"ipc_Qu8mg5v7( The maximum normal adhesion force magnitude. @@ -73,7 +73,7 @@ void define_adhesion(py::module_& m) "dhat_p"_a, "dhat_a"_a, "a2"_a); m.def( - "tangential_adhesion_f0", &tangential_adhesion_f0, + "tangential_adhesion_f0", &tangential_adhesion_f0, R"ipc_Qu8mg5v7( The tangential adhesion mollifier function. @@ -87,7 +87,7 @@ void define_adhesion(py::module_& m) "y"_a, "eps_a"_a); m.def( - "tangential_adhesion_f1", &tangential_adhesion_f1, + "tangential_adhesion_f1", &tangential_adhesion_f1, R"ipc_Qu8mg5v7( The first derivative of the tangential adhesion mollifier function. @@ -101,7 +101,7 @@ void define_adhesion(py::module_& m) "y"_a, "eps_a"_a); m.def( - "tangential_adhesion_f2", &tangential_adhesion_f2, + "tangential_adhesion_f2", &tangential_adhesion_f2, R"ipc_Qu8mg5v7( The second derivative of the tangential adhesion mollifier function. @@ -115,7 +115,7 @@ void define_adhesion(py::module_& m) "y"_a, "eps_a"_a); m.def( - "tangential_adhesion_f1_over_x", &tangential_adhesion_f1_over_x, + "tangential_adhesion_f1_over_x", &tangential_adhesion_f1_over_x, R"ipc_Qu8mg5v7( The first derivative of the tangential adhesion mollifier function divided by y. @@ -130,7 +130,7 @@ void define_adhesion(py::module_& m) m.def( "tangential_adhesion_f2_x_minus_f1_over_x3", - &tangential_adhesion_f2_x_minus_f1_over_x3, + &tangential_adhesion_f2_x_minus_f1_over_x3, R"ipc_Qu8mg5v7( The second derivative of the tangential adhesion mollifier function times y minus the first derivative all divided by y cubed. @@ -144,7 +144,7 @@ void define_adhesion(py::module_& m) "y"_a, "eps_a"_a); m.def( - "smooth_mu_a0", &smooth_mu_a0, + "smooth_mu_a0", &smooth_mu_a0, R"ipc_Qu8mg5v7( Compute the value of the ∫ μ(y) a₁(y) dy, where a₁ is the first derivative of the smooth tangential adhesion mollifier. @@ -163,7 +163,7 @@ void define_adhesion(py::module_& m) "y"_a, "mu_s"_a, "mu_k"_a, "eps_a"_a); m.def( - "smooth_mu_a1", &smooth_mu_a1, + "smooth_mu_a1", &smooth_mu_a1, R"ipc_Qu8mg5v7( Compute the value of the μ(y) a₁(y), where a₁ is the first derivative of the smooth tangential adhesion mollifier. @@ -182,7 +182,7 @@ void define_adhesion(py::module_& m) "y"_a, "mu_s"_a, "mu_k"_a, "eps_a"_a); m.def( - "smooth_mu_a2", &smooth_mu_a2, + "smooth_mu_a2", &smooth_mu_a2, R"ipc_Qu8mg5v7( Compute the value of d/dy (μ(y) a₁(y)), where a₁ is the first derivative of the smooth tangential adhesion mollifier. @@ -201,7 +201,7 @@ void define_adhesion(py::module_& m) "y"_a, "mu_s"_a, "mu_k"_a, "eps_a"_a); m.def( - "smooth_mu_a1_over_x", &smooth_mu_a1_over_x, + "smooth_mu_a1_over_x", &smooth_mu_a1_over_x, R"ipc_Qu8mg5v7( Compute the value of the μ(y) a₁(y) / y, where a₁ is the first derivative of the smooth tangential adhesion mollifier. @@ -222,7 +222,7 @@ void define_adhesion(py::module_& m) m.def( "smooth_mu_a2_x_minus_mu_a1_over_x3", - &smooth_mu_a2_x_minus_mu_a1_over_x3, + &smooth_mu_a2_x_minus_mu_a1_over_x3, R"ipc_Qu8mg5v7( Compute the value of the [(d/dy μ(y) a₁(y)) ⋅ y - μ(y) a₁(y)] / y³, where a₁ and a₂ are the first and second derivatives of the smooth tangential adhesion mollifier. diff --git a/python/src/friction/smooth_friction_mollifier.cpp b/python/src/friction/smooth_friction_mollifier.cpp index ba9fe0d99..74f9e593b 100644 --- a/python/src/friction/smooth_friction_mollifier.cpp +++ b/python/src/friction/smooth_friction_mollifier.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_smooth_friction_mollifier(py::module_& m) { m.def( - "smooth_friction_f0", &smooth_friction_f0, + "smooth_friction_f0", &smooth_friction_f0, R"ipc_Qu8mg5v7( Smooth friction mollifier function. @@ -30,7 +30,7 @@ void define_smooth_friction_mollifier(py::module_& m) "y"_a, "eps_v"_a); m.def( - "smooth_friction_f1", &smooth_friction_f1, + "smooth_friction_f1", &smooth_friction_f1, R"ipc_Qu8mg5v7( The first derivative of the smooth friction mollifier. @@ -51,7 +51,7 @@ void define_smooth_friction_mollifier(py::module_& m) "y"_a, "eps_v"_a); m.def( - "smooth_friction_f2", &smooth_friction_f2, + "smooth_friction_f2", &smooth_friction_f2, R"ipc_Qu8mg5v7( The second derivative of the smooth friction mollifier. @@ -72,7 +72,7 @@ void define_smooth_friction_mollifier(py::module_& m) "y"_a, "eps_v"_a); m.def( - "smooth_friction_f1_over_x", &smooth_friction_f1_over_x, + "smooth_friction_f1_over_x", &smooth_friction_f1_over_x, R"ipc_Qu8mg5v7( Compute the derivative of the smooth friction mollifier divided by y (:math:`\frac{f_0'(y)}{y}`). @@ -94,7 +94,7 @@ void define_smooth_friction_mollifier(py::module_& m) m.def( "smooth_friction_f2_x_minus_f1_over_x3", - &smooth_friction_f2_x_minus_f1_over_x3, + &smooth_friction_f2_x_minus_f1_over_x3, R"ipc_Qu8mg5v7( The derivative of f1 times y minus f1 all divided by y cubed. diff --git a/python/src/friction/smooth_mu.cpp b/python/src/friction/smooth_mu.cpp index adc5f5241..5b1f64066 100644 --- a/python/src/friction/smooth_mu.cpp +++ b/python/src/friction/smooth_mu.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_smooth_mu(py::module_& m) { m.def( - "smooth_mu", &smooth_mu, + "smooth_mu", &smooth_mu, R"ipc_Qu8mg5v7( Smooth coefficient from static to kinetic friction. @@ -23,7 +23,7 @@ void define_smooth_mu(py::module_& m) "y"_a, "mu_s"_a, "mu_k"_a, "eps_v"_a); m.def( - "smooth_mu_derivative", &smooth_mu_derivative, + "smooth_mu_derivative", &smooth_mu_derivative, R"ipc_Qu8mg5v7( Compute the derivative of the smooth coefficient from static to kinetic friction. @@ -39,7 +39,7 @@ void define_smooth_mu(py::module_& m) "y"_a, "mu_s"_a, "mu_k"_a, "eps_v"_a); m.def( - "smooth_mu_f0", &smooth_mu_f0, + "smooth_mu_f0", &smooth_mu_f0, R"ipc_Qu8mg5v7( Compute the value of the ∫ μ(y) f₁(y) dy, where f₁ is the first derivative of the smooth friction mollifier. @@ -55,7 +55,7 @@ void define_smooth_mu(py::module_& m) "y"_a, "mu_s"_a, "mu_k"_a, "eps_v"_a); m.def( - "smooth_mu_f1", &smooth_mu_f1, + "smooth_mu_f1", &smooth_mu_f1, R"ipc_Qu8mg5v7( Compute the value of the μ(y) f₁(y), where f₁ is the first derivative of the smooth friction mollifier. @@ -71,7 +71,7 @@ void define_smooth_mu(py::module_& m) "y"_a, "mu_s"_a, "mu_k"_a, "eps_v"_a); m.def( - "smooth_mu_f2", &smooth_mu_f2, + "smooth_mu_f2", &smooth_mu_f2, R"ipc_Qu8mg5v7( Compute the value of d/dy (μ(y) f₁(y)), where f₁ is the first derivative of the smooth friction mollifier. @@ -87,7 +87,7 @@ void define_smooth_mu(py::module_& m) "y"_a, "mu_s"_a, "mu_k"_a, "eps_v"_a); m.def( - "smooth_mu_f1_over_x", &smooth_mu_f1_over_x, + "smooth_mu_f1_over_x", &smooth_mu_f1_over_x, R"ipc_Qu8mg5v7( Compute the value of the μ(y) f₁(y) / y, where f₁ is the first derivative of the smooth friction mollifier. @@ -107,7 +107,7 @@ void define_smooth_mu(py::module_& m) m.def( "smooth_mu_f2_x_minus_mu_f1_over_x3", - &smooth_mu_f2_x_minus_mu_f1_over_x3, + &smooth_mu_f2_x_minus_mu_f1_over_x3, R"ipc_Qu8mg5v7( Compute the value of the [(d/dy μ(y) f₁(y)) ⋅ y - μ(y) f₁(y)] / y³, where f₁ and f₂ are the first and second derivatives of the smooth friction mollifier. diff --git a/python/src/geometry/angle.cpp b/python/src/geometry/angle.cpp index 3d22665e6..9d1c32c1d 100644 --- a/python/src/geometry/angle.cpp +++ b/python/src/geometry/angle.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_angle(py::module_& m) { m.def( - "dihedral_angle", &dihedral_angle, + "dihedral_angle", &detail::dihedral_angle, R"ipc_Qu8mg5v7( Compute the bending angle between two triangles sharing an edge. x0---x2 @@ -33,7 +33,7 @@ void define_angle(py::module_& m) "x0"_a, "x1"_a, "x2"_a, "x3"_a); m.def( - "dihedral_angle_gradient", &dihedral_angle_gradient, + "dihedral_angle_gradient", &detail::dihedral_angle_gradient, R"ipc_Qu8mg5v7( Compute the Jacobian of the bending angle between two triangles sharing an edge. x0---x2 diff --git a/src/ipc/adhesion/CMakeLists.txt b/src/ipc/adhesion/CMakeLists.txt index c361c68b6..aa77f3ac0 100644 --- a/src/ipc/adhesion/CMakeLists.txt +++ b/src/ipc/adhesion/CMakeLists.txt @@ -1,6 +1,5 @@ set(SOURCES - adhesion.cpp adhesion.hpp ) -target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file +target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/adhesion/adhesion.cpp b/src/ipc/adhesion/adhesion.cpp deleted file mode 100644 index 17dcf161b..000000000 --- a/src/ipc/adhesion/adhesion.cpp +++ /dev/null @@ -1,204 +0,0 @@ -// Adhesion model of Fang and Li et al. [2023]. - -#include "adhesion.hpp" - -#include - -#include - -namespace ipc { - -// -- Normal Adhesion ---------------------------------------------------------- - -double normal_adhesion_potential( - const double d, const double dhat_p, const double dhat_a, const double a2) -{ - assert(d >= 0); - assert(dhat_p < dhat_a); - assert(a2 < 0); - if (d < dhat_p) { - const double a1 = a2 * (1 - dhat_a / dhat_p); - const double c1 = - a2 * (dhat_a - dhat_p) * (dhat_a - dhat_p) - dhat_p * dhat_p * a1; - return a1 * d * d + c1; - } else if (d < dhat_a) { - const double b2 = -2 * a2 * dhat_a; - const double c2 = a2 * dhat_a * dhat_a; - return (a2 * d + b2) * d + c2; - } else { - return 0; - } -} - -double normal_adhesion_potential_first_derivative( - const double d, const double dhat_p, const double dhat_a, const double a2) -{ - assert(d >= 0); - assert(dhat_p < dhat_a); - assert(a2 < 0); - if (d < dhat_p) { - const double a1 = a2 * (1 - dhat_a / dhat_p); - return 2 * a1 * d; - } else if (d < dhat_a) { - // const double b2 = -2 * a2 * dhat_a; - return 2 * a2 * (d - dhat_a); - } else { - return 0; - } -} - -double normal_adhesion_potential_second_derivative( - const double d, const double dhat_p, const double dhat_a, const double a2) -{ - assert(d >= 0); - assert(dhat_p < dhat_a); - assert(a2 < 0); - if (d < dhat_p) { - const double a1 = a2 * (1 - dhat_a / dhat_p); - return 2 * a1; - } else if (d < dhat_a) { - return 2 * a2; - } else { - return 0; - } -} - -double max_normal_adhesion_force_magnitude( - const double dhat_p, const double dhat_a, const double a2) -{ - assert(dhat_p < dhat_a); - assert(a2 < 0); - // max_d a' = a'(d̂ₚ) = 2a₂ (d̂ₚ - d̂ₐ) - return 2 * a2 * (dhat_p - dhat_a); -} - -// -- Tangential Adhesion ------------------------------------------------------ - -double tangential_adhesion_f0(const double y, const double eps_a) -{ - assert(eps_a > 0); - if (y <= 0) { - return 0; - } else if (y >= 2 * eps_a) { - return 4 * eps_a / 3; - } - return y * y / eps_a * (1 - y / (3 * eps_a)); // -y³/(3ϵ²) + y²/ϵ -} - -double tangential_adhesion_f1(const double y, const double eps_a) -{ - assert(eps_a > 0); - if (y >= 2 * eps_a || y <= 0) { - return 0; - } - - const double y_over_eps_a = y / eps_a; - return y_over_eps_a * (2 - y_over_eps_a); // -y²/ϵ² + 2y/ϵ -} - -double tangential_adhesion_f2(const double y, const double eps_a) -{ - assert(eps_a > 0); - if (y >= 2 * eps_a || y <= 0) { - return 0; - } - - return (2 - 2 * y / eps_a) / eps_a; // -2y/ϵ² + 2/ϵ -} - -double tangential_adhesion_f1_over_x(const double y, const double eps_a) -{ - assert(eps_a > 0); - if (y >= 2 * eps_a || y <= 0) { - return 0; - } - - return (2 - y / eps_a) / eps_a; // -y/ϵ² + 2/ϵ -} - -double -tangential_adhesion_f2_x_minus_f1_over_x3(const double y, const double eps_a) -{ - assert(eps_a > 0); - assert(y >= 0); - if (y >= 2 * eps_a) { - return 0; - } - return -1 / (y * eps_a * eps_a); -} - -// ~~ Smooth μ variants ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -// Here a0, a1, and a2 refer to the mollifier functions above. - -double smooth_mu_a0( - const double y, const double mu_s, const double mu_k, const double eps_a) -{ - assert(eps_a > 0); - const double delta_mu = mu_k - mu_s; - if (y <= 0) { - return 0; - } else if (mu_s == mu_k || y >= eps_a) { - // If the static and kinetic friction coefficients are equal, simplify. - const double c = (11 / 48.) * eps_a * delta_mu; - return mu_k * tangential_adhesion_f0(y, eps_a) - c; - } else { - const double z = y / eps_a; - if (y < 0.5 * eps_a) { - return y * z - * (z * (z * (1 - 0.4 * z) * delta_mu - mu_s / 3.0) + mu_s); - } else { - return y * z - * (z - * (z * (0.4 * z - 2) * delta_mu + 3 * mu_k - - (10.0 / 3.0) * mu_s) - - mu_k + 2 * mu_s) - + (3.0 / 80.0) * eps_a * delta_mu; - } - } -} - -double smooth_mu_a1( - const double y, const double mu_s, const double mu_k, const double eps_a) -{ - return smooth_mu(y, mu_s, mu_k, eps_a) * tangential_adhesion_f1(y, eps_a); -} - -double smooth_mu_a2( - const double y, const double mu_s, const double mu_k, const double eps_a) -{ - return smooth_mu_derivative(y, mu_s, mu_k, eps_a) - * tangential_adhesion_f1(y, eps_a) - + smooth_mu(y, mu_s, mu_k, eps_a) * tangential_adhesion_f2(y, eps_a); -} - -double smooth_mu_a1_over_x( - const double y, const double mu_s, const double mu_k, const double eps_a) -{ - // This is a known formulation: μ(y) f₁(y) / y - // where we use the robust division by y to avoid division by zero. - return smooth_mu(y, mu_s, mu_k, eps_a) - * tangential_adhesion_f1_over_x(y, eps_a); -} - -double smooth_mu_a2_x_minus_mu_a1_over_x3( - const double y, const double mu_s, const double mu_k, const double eps_a) -{ - assert(eps_a > 0); - assert(y >= 0); - if (mu_s == mu_k || y >= eps_a) { - // If the static and kinetic friction coefficients are equal, simplify. - return mu_k * tangential_adhesion_f2_x_minus_f1_over_x3(y, eps_a); - } else { - const double delta_mu = mu_k - mu_s; - const double z = 1 / eps_a; - if (y < 0.5 * eps_a) { - return z * z * (z * (8 - 6 * z * y) * delta_mu - mu_s / y); - } else { - return z * z - * (z * (6 * z * y - 16) * delta_mu - + (9 * mu_k - 10 * mu_s) / y); - } - } -} - -} // namespace ipc diff --git a/src/ipc/adhesion/adhesion.hpp b/src/ipc/adhesion/adhesion.hpp index 8f0b8269b..50823f966 100644 --- a/src/ipc/adhesion/adhesion.hpp +++ b/src/ipc/adhesion/adhesion.hpp @@ -1,7 +1,18 @@ #pragma once +// Adhesion model of Fang and Li et al. [2023]. + +#include +#include +#include + +#include + namespace ipc { +// See the note atop smooth_friction_mollifier.hpp for why the branches here are +// written as `select_lazy` and what that means for a batch scalar. + // Fang and Li et al. [2023]: // -- Normal Adhesion ---------------------------------------------------------- @@ -9,39 +20,94 @@ namespace ipc { /// @{ /// @brief The normal adhesion potential. +/// @tparam T The scalar type. /// @param d distance /// @param dhat_p distance of largest adhesion force (\f$\hat{d}_p\f$) where \f$0 < \hat{d}_p < \hat{d}_a\f$ /// @param dhat_a adhesion activation distance (\f$\hat{d}_a\f$) /// @param a2 adjustable parameter relating to the maximum derivative of a (\f$a_2\f$) /// @return The normal adhesion potential. -double normal_adhesion_potential( - const double d, const double dhat_p, const double dhat_a, const double a2); +template +inline T +normal_adhesion_potential(const T d, const T dhat_p, const T dhat_a, const T a2) +{ + assert(all_of(d >= T(0))); + assert(all_of(dhat_p < dhat_a)); + assert(all_of(a2 < T(0))); + return select_lazy( + d < dhat_p, + [&] { + const T a1 = a2 * (T(1) - dhat_a / dhat_p); + const T c1 = a2 * (dhat_a - dhat_p) * (dhat_a - dhat_p) + - dhat_p * dhat_p * a1; + return a1 * d * d + c1; + }, + d < dhat_a, + [&] { + const T b2 = T(-2) * a2 * dhat_a; + const T c2 = a2 * dhat_a * dhat_a; + return (a2 * d + b2) * d + c2; + }, + [&] { return T(0); }); +} /// @brief The first derivative of the normal adhesion potential wrt d. +/// @tparam T The scalar type. /// @param d distance /// @param dhat_p distance of largest adhesion force (\f$\hat{d}_p\f$) where \f$0 < \hat{d}_p < \hat{d}_a\f$ /// @param dhat_a adhesion activation distance (\f$\hat{d}_a\f$) /// @param a2 adjustable parameter relating to the maximum derivative of a (\f$a_2\f$) /// @return The first derivative of the normal adhesion potential wrt d. -double normal_adhesion_potential_first_derivative( - const double d, const double dhat_p, const double dhat_a, const double a2); +template +inline T normal_adhesion_potential_first_derivative( + const T d, const T dhat_p, const T dhat_a, const T a2) +{ + assert(all_of(d >= T(0))); + assert(all_of(dhat_p < dhat_a)); + assert(all_of(a2 < T(0))); + return select_lazy( + d < dhat_p, + [&] { + const T a1 = a2 * (T(1) - dhat_a / dhat_p); + return T(2) * a1 * d; + }, + d < dhat_a, [&] { return T(2) * a2 * (d - dhat_a); }, + [&] { return T(0); }); +} /// @brief The second derivative of the normal adhesion potential wrt d. +/// @tparam T The scalar type. /// @param d distance /// @param dhat_p distance of largest adhesion force (\f$\hat{d}_p\f$) where \f$0 < \hat{d}_p < \hat{d}_a\f$ /// @param dhat_a adhesion activation distance (\f$\hat{d}_a\f$) /// @param a2 adjustable parameter relating to the maximum derivative of a (\f$a_2\f$) /// @return The second derivative of the normal adhesion potential wrt d. -double normal_adhesion_potential_second_derivative( - const double d, const double dhat_p, const double dhat_a, const double a2); +template +inline T normal_adhesion_potential_second_derivative( + const T d, const T dhat_p, const T dhat_a, const T a2) +{ + assert(all_of(d >= T(0))); + assert(all_of(dhat_p < dhat_a)); + assert(all_of(a2 < T(0))); + return select_lazy( + d < dhat_p, [&] { return T(2) * a2 * (T(1) - dhat_a / dhat_p); }, + d < dhat_a, [&] { return T(2) * a2; }, [&] { return T(0); }); +} /// @brief The maximum normal adhesion force magnitude. +/// @tparam T The scalar type. /// @param dhat_p distance of largest adhesion force (\f$\hat{d}_p\f$) where \f$0 < \hat{d}_p < \hat{d}_a\f$ /// @param dhat_a adhesion activation distance (\f$\hat{d}_a\f$) /// @param a2 adjustable parameter relating to the maximum derivative of a (\f$a_2\f$) /// @return The maximum normal adhesion force magnitude. -double max_normal_adhesion_force_magnitude( - const double dhat_p, const double dhat_a, const double a2); +template +inline T +max_normal_adhesion_force_magnitude(const T dhat_p, const T dhat_a, const T a2) +{ + assert(all_of(dhat_p < dhat_a)); + assert(all_of(a2 < T(0))); + // max_d a' = a'(d̂ₚ) = 2a₂ (d̂ₚ - d̂ₐ) + return T(2) * a2 * (dhat_p - dhat_a); +} /// @} @@ -50,90 +116,201 @@ double max_normal_adhesion_force_magnitude( /// @{ /// @brief The tangential adhesion mollifier function. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The tangential adhesion mollifier function at y. -double tangential_adhesion_f0(const double y, const double eps_a); +template inline T tangential_adhesion_f0(const T y, const T eps_a) +{ + assert(all_of(eps_a > T(0))); + return select_lazy( + y <= T(0), [&] { return T(0); }, y >= T(2) * eps_a, + [&] { return T(4) * eps_a / T(3); }, + // -y³/(3ϵ²) + y²/ϵ + [&] { return y * y / eps_a * (T(1) - y / (T(3) * eps_a)); }); +} /// @brief The first derivative of the tangential adhesion mollifier function. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The first derivative of the tangential adhesion mollifier function at y. -double tangential_adhesion_f1(const double y, const double eps_a); +template inline T tangential_adhesion_f1(const T y, const T eps_a) +{ + assert(all_of(eps_a > T(0))); + return select_lazy( + y >= T(2) * eps_a || y <= T(0), [&] { return T(0); }, + [&] { + const T y_over_eps_a = y / eps_a; + return y_over_eps_a * (T(2) - y_over_eps_a); // -y²/ϵ² + 2y/ϵ + }); +} /// @brief The second derivative of the tangential adhesion mollifier function. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The second derivative of the tangential adhesion mollifier function at y. -double tangential_adhesion_f2(const double y, const double eps_a); +template inline T tangential_adhesion_f2(const T y, const T eps_a) +{ + assert(all_of(eps_a > T(0))); + return select_lazy( + y >= T(2) * eps_a || y <= T(0), [&] { return T(0); }, + [&] { return (T(2) - T(2) * y / eps_a) / eps_a; }); // -2y/ϵ² + 2/ϵ +} /// @brief The first derivative of the tangential adhesion mollifier function divided by y. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The first derivative of the tangential adhesion mollifier function divided by y. -double tangential_adhesion_f1_over_x(const double y, const double eps_a); +template +inline T tangential_adhesion_f1_over_x(const T y, const T eps_a) +{ + assert(all_of(eps_a > T(0))); + return select_lazy( + y >= T(2) * eps_a || y <= T(0), [&] { return T(0); }, + [&] { return (T(2) - y / eps_a) / eps_a; }); // -y/ϵ² + 2/ϵ +} /// @brief The second derivative of the tangential adhesion mollifier function times y minus the first derivative all divided by y cubed. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The second derivative of the tangential adhesion mollifier function times y minus the first derivative all divided by y cubed. -double -tangential_adhesion_f2_x_minus_f1_over_x3(const double y, const double eps_a); +template +inline T tangential_adhesion_f2_x_minus_f1_over_x3(const T y, const T eps_a) +{ + assert(all_of(eps_a > T(0))); + assert(all_of(y >= T(0))); + return select_lazy( + y >= T(2) * eps_a, [&] { return T(0); }, + [&] { return T(-1) / (y * eps_a * eps_a); }); +} // ~~ Smooth μ variants ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // NOTE: Here a0, a1, and a2 refer to the mollifier functions above. /// @brief Compute the value of the ∫ μ(y) a₁(y) dy, where a₁ is the first derivative of the smooth tangential adhesion mollifier. /// @note The `a0`/`a1` are unrelated to the `a0`/`a1` in the normal adhesion. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static adhesion. /// @param mu_k Coefficient of kinetic adhesion. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The value of the integral at y. -double smooth_mu_a0( - const double y, const double mu_s, const double mu_k, const double eps_a); +template +inline T smooth_mu_a0(const T y, const T mu_s, const T mu_k, const T eps_a) +{ + assert(all_of(eps_a > T(0))); + const T delta_mu = mu_k - mu_s; + const T z = y / eps_a; + return select_lazy( + y <= T(0), [&] { return T(0); }, // + mu_s == mu_k || y >= eps_a, + [&] { + const T c = literal(11 / 48.) * eps_a * delta_mu; + return mu_k * tangential_adhesion_f0(y, eps_a) - c; + }, + y < T(0.5) * eps_a, + [&] { + return y * z + * (z + * (z * (T(1) - literal(0.4) * z) * delta_mu + - mu_s / T(3)) + + mu_s); + }, + [&] { + return y * z + * (z + * (z * (literal(0.4) * z - T(2)) * delta_mu + + T(3) * mu_k - literal(10.0 / 3.0) * mu_s) + - mu_k + T(2) * mu_s) + + literal(3.0 / 80.0) * eps_a * delta_mu; + }); +} /// @brief Compute the value of the μ(y) a₁(y), where a₁ is the first derivative of the smooth tangential adhesion mollifier. /// @note The `a1` is unrelated to the `a1` in the normal adhesion. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static adhesion. /// @param mu_k Coefficient of kinetic adhesion. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The value of the product at y. -double smooth_mu_a1( - const double y, const double mu_s, const double mu_k, const double eps_a); +template +inline T smooth_mu_a1(const T y, const T mu_s, const T mu_k, const T eps_a) +{ + return smooth_mu(y, mu_s, mu_k, eps_a) * tangential_adhesion_f1(y, eps_a); +} /// @brief Compute the value of d/dy (μ(y) a₁(y)), where a₁ is the first derivative of the smooth tangential adhesion mollifier. /// @note The `a1`/`a2` are unrelated to the `a1`/`a2` in the normal adhesion. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static adhesion. /// @param mu_k Coefficient of kinetic adhesion. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The value of the derivative at y. -double smooth_mu_a2( - const double y, const double mu_s, const double mu_k, const double eps_a); +template +inline T smooth_mu_a2(const T y, const T mu_s, const T mu_k, const T eps_a) +{ + return smooth_mu_derivative(y, mu_s, mu_k, eps_a) + * tangential_adhesion_f1(y, eps_a) + + smooth_mu(y, mu_s, mu_k, eps_a) * tangential_adhesion_f2(y, eps_a); +} /// @brief Compute the value of the μ(y) a₁(y) / y, where a₁ is the first derivative of the smooth tangential adhesion mollifier. /// @note The `x` in the function name refers to the parameter `y`. /// @note The `a1` is unrelated to the `a1` in the normal adhesion. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static adhesion. /// @param mu_k Coefficient of kinetic adhesion. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The value of the product at y. -double smooth_mu_a1_over_x( - const double y, const double mu_s, const double mu_k, const double eps_a); +template +inline T +smooth_mu_a1_over_x(const T y, const T mu_s, const T mu_k, const T eps_a) +{ + // This is a known formulation: μ(y) f₁(y) / y + // where we use the robust division by y to avoid division by zero. + return smooth_mu(y, mu_s, mu_k, eps_a) + * tangential_adhesion_f1_over_x(y, eps_a); +} /// @brief Compute the value of the [(d/dy μ(y) a₁(y)) ⋅ y - μ(y) a₁(y)] / y³, where a₁ and a₂ are the first and second derivatives of the smooth tangential adhesion mollifier. /// @note The `x` in the function name refers to the parameter `y`. /// @note The `a1`/`a2` are unrelated to the `a1`/`a2` in the normal adhesion. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static adhesion. /// @param mu_k Coefficient of kinetic adhesion. /// @param eps_a Velocity threshold below which static adhesion force is applied. /// @return The value of the expression at y. -double smooth_mu_a2_x_minus_mu_a1_over_x3( - const double y, const double mu_s, const double mu_k, const double eps_a); +template +inline T smooth_mu_a2_x_minus_mu_a1_over_x3( + const T y, const T mu_s, const T mu_k, const T eps_a) +{ + assert(all_of(eps_a > T(0))); + assert(all_of(y >= T(0))); + const T delta_mu = mu_k - mu_s; + const T z = T(1) / eps_a; + return select_lazy( + mu_s == mu_k || y >= eps_a, + [&] { + return mu_k * tangential_adhesion_f2_x_minus_f1_over_x3(y, eps_a); + }, + y < T(0.5) * eps_a, + [&] { + return z * z * (z * (T(8) - T(6) * z * y) * delta_mu - mu_s / y); + }, + [&] { + return z * z + * (z * (T(6) * z * y - T(16)) * delta_mu + + (T(9) * mu_k - T(10) * mu_s) / y); + }); +} /// @} diff --git a/src/ipc/barrier/barrier.cpp b/src/ipc/barrier/barrier.cpp index 947b1ae3f..49ad52121 100644 --- a/src/ipc/barrier/barrier.cpp +++ b/src/ipc/barrier/barrier.cpp @@ -5,43 +5,48 @@ // inequality constraints on a function. #include "barrier.hpp" -#include -#include +#include +#include namespace ipc { +// Each barrier is one select_lazy cascade, ordered by increasing d so it +// reads like the piecewise definition in the header. A scalar evaluates only +// the case it lands in -- so the log below is never reached for d <= 0 -- while +// a batch evaluates every case and blends per-lane, earlier cases winning. + template T barrier(const T d, const T dhat) { - if (d <= T(0)) { - return std::numeric_limits::infinity(); - } - if (d >= dhat) { - return T(0); - } // b(d) = -(d-d̂)²ln(d / d̂) - const T d_minus_dhat = (d - dhat); - return -d_minus_dhat * d_minus_dhat * std::log(d / dhat); + return select_lazy( + d <= T(0), [&] { return infinity(); }, // + d < dhat, [&] { return -sqr(d - dhat) * log(d / dhat); }, + [&] { return T(0); }); } template T barrier_first_derivative(const T d, const T dhat) { - if (d <= T(0) || d >= dhat) { - return T(0); - } // b(d) = -(d - d̂)²ln(d / d̂) // b'(d) = -2(d - d̂)ln(d / d̂) - (d-d̂)²(1 / d) // = (d - d̂) * (-2ln(d/d̂) - (d - d̂) / d) // = (d̂ - d) * (2ln(d/d̂) - d̂/d + 1) - return (dhat - d) * (2 * std::log(d / dhat) - dhat / d + 1); + return select_lazy( + d <= T(0), [&] { return T(0); }, // + d < dhat, + [&] { return (dhat - d) * (2 * log(d / dhat) - dhat / d + 1); }, + [&] { return T(0); }); } template T barrier_second_derivative(const T d, const T dhat) { - if (d <= T(0) || d >= dhat) { - return T(0); - } - const T dhat_d = dhat / d; - return (dhat_d + 2) * dhat_d - 2 * std::log(d / dhat) - 3; + return select_lazy( + d <= T(0), [&] { return T(0); }, // + d < dhat, + [&] { + const T dhat_d = dhat / d; + return (dhat_d + 2) * dhat_d - 2 * log(d / dhat) - 3; + }, + [&] { return T(0); }); } // ============================================================================ @@ -49,42 +54,48 @@ template T barrier_second_derivative(const T d, const T dhat) template T ClampedLogSqBarrier::operator()(const T d, const T dhat) const { - if (d <= T(0)) { - return std::numeric_limits::infinity(); - } - if (d >= dhat) { - return T(0); - } // b(d) = (d-d̂)²ln²(d / d̂) - const T d_minus_dhat = (d - dhat); - const T log_d_dhat = std::log(d / dhat); - return d_minus_dhat * d_minus_dhat * log_d_dhat * log_d_dhat; + return select_lazy( + d <= T(0), [&] { return infinity(); }, // + d < dhat, + [&] { + const T log_d_dhat = log(d / dhat); + return sqr(d - dhat) * sqr(log_d_dhat); + }, + [&] { return T(0); }); } template T ClampedLogSqBarrier::first_derivative(const T d, const T dhat) const { - if (d <= T(0) || d >= dhat) { - return T(0); - } // b(d) = (d - d̂)²ln²(d / d̂) // b'(d) = 2 (d - d̂) ln²(d / d̂) + 2 (d - d̂)² ln(d / d̂) / d // = 2 (d - d̂) ln(d / d̂) [ln(d / d̂) + (d - d̂) / d] - const T d_minus_dhat = (d - dhat); - const T log_d_dhat = std::log(d / dhat); - return T(2) * d_minus_dhat * log_d_dhat * (log_d_dhat + d_minus_dhat / d); + return select_lazy( + d <= T(0), [&] { return T(0); }, // + d < dhat, + [&] { + const T d_minus_dhat = (d - dhat); + const T log_d_dhat = log(d / dhat); + return T(2) * d_minus_dhat * log_d_dhat + * (log_d_dhat + d_minus_dhat / d); + }, + [&] { return T(0); }); } template T ClampedLogSqBarrier::second_derivative(const T d, const T dhat) const { - if (d <= T(0) || d >= dhat) { - return T(0); - } - const T t0 = dhat - d; - const T t1 = std::log(d / dhat); - const T t2 = (t0 * t0) / (d * d); - return T(2) * ((t1 * t1) - (t1 - T(1)) * t2 - T(4) * t1 * t0 / d); + return select_lazy( + d <= T(0), [&] { return T(0); }, // + d < dhat, + [&] { + const T t0 = dhat - d; + const T t1 = log(d / dhat); + const T t2 = sqr(t0) / sqr(d); + return T(2) * (sqr(t1) - (t1 - T(1)) * t2 - T(4) * t1 * t0 / d); + }, + [&] { return T(0); }); } // ============================================================================ @@ -92,34 +103,29 @@ T ClampedLogSqBarrier::second_derivative(const T d, const T dhat) const template T CubicBarrier::operator()(const T d, const T dhat) const { - if (d < dhat) { - // b(d) = (d - d̂)³ - const T d_minus_dhat = (d - dhat); - return -T(2) / T(3) / dhat * d_minus_dhat * d_minus_dhat * d_minus_dhat; - } else { - return T(0); - } + // b(d) = (d - d̂)³ + // + // A polynomial: finite at d <= 0, so unlike the log barriers it needs no + // penetration guard. + return select_lazy( + d < dhat, [&] { return -T(2) / T(3) / dhat * cubic(d - dhat); }, + [&] { return T(0); }); } template T CubicBarrier::first_derivative(const T d, const T dhat) const { - if (d < dhat) { - const T d_minus_dhat = (d - dhat); - return T(-2) / dhat * d_minus_dhat * d_minus_dhat; - } else { - return T(0); - } + return select_lazy( + d < dhat, [&] { return T(-2) / dhat * sqr(d - dhat); }, + [&] { return T(0); }); } template T CubicBarrier::second_derivative(const T d, const T dhat) const { - if (d < dhat) { - return T(4) * (T(1) - d / dhat); - } else { - return T(0); - } + return select_lazy( + d < dhat, [&] { return T(4) * (T(1) - d / dhat); }, + [&] { return T(0); }); } // ============================================================================ @@ -127,37 +133,32 @@ T CubicBarrier::second_derivative(const T d, const T dhat) const template T TwoStageBarrier::operator()(const T d, const T dhat) const { - if (d >= dhat) { - return T(0); - } else if (d >= T(0.5) * dhat) { - return T(0.5) * (dhat - d) * (dhat - d); - } else { - return T(-0.25) * dhat * dhat * (std::log(T(2) * d / dhat) - T(0.5)); - } + return select_lazy( + d <= T(0), [&] { return infinity(); }, // + d < T(0.5) * dhat, + [&] { return T(-0.25) * sqr(dhat) * (log(T(2) * d / dhat) - T(0.5)); }, + d < dhat, [&] { return T(0.5) * sqr(dhat - d); }, // + [&] { return T(0); }); } template T TwoStageBarrier::first_derivative(const T d, const T dhat) const { - if (d >= dhat) { - return T(0); - } else if (d >= T(0.5) * dhat) { - return d - dhat; - } else { - return T(-0.25) * dhat * dhat / d; - } + return select_lazy( + d <= T(0), [&] { return T(0); }, // + d < T(0.5) * dhat, [&] { return T(-0.25) * sqr(dhat) / d; }, // + d < dhat, [&] { return d - dhat; }, // + [&] { return T(0); }); } template T TwoStageBarrier::second_derivative(const T d, const T dhat) const { - if (d >= dhat) { - return T(0); - } else if (d >= T(0.5) * dhat) { - return T(1); - } else { - return (T(0.25) * dhat * dhat) / (d * d); - } + return select_lazy( + d <= T(0), [&] { return T(0); }, // + d < T(0.5) * dhat, [&] { return (T(0.25) * sqr(dhat)) / sqr(d); }, // + d < dhat, [&] { return T(1); }, // + [&] { return T(0); }); } // ============================================================================ @@ -179,6 +180,30 @@ template float barrier_first_derivative(const float d, const float dhat); template double barrier_first_derivative(const double d, const double dhat); template float barrier_second_derivative(const float d, const float dhat); template double barrier_second_derivative(const double d, const double dhat); +#ifdef IPC_TOOLKIT_WITH_SIMD +template class BarrierBase>; +template class BarrierBase>; +template class ClampedLogBarrier>; +template class ClampedLogBarrier>; +template class ClampedLogSqBarrier>; +template class ClampedLogSqBarrier>; +template class CubicBarrier>; +template class CubicBarrier>; +template class TwoStageBarrier>; +template class TwoStageBarrier>; +template SimdBatch +barrier(const SimdBatch d, const SimdBatch dhat); +template SimdBatch +barrier(const SimdBatch d, const SimdBatch dhat); +template SimdBatch +barrier_first_derivative(const SimdBatch d, const SimdBatch dhat); +template SimdBatch barrier_first_derivative( + const SimdBatch d, const SimdBatch dhat); +template SimdBatch barrier_second_derivative( + const SimdBatch d, const SimdBatch dhat); +template SimdBatch barrier_second_derivative( + const SimdBatch d, const SimdBatch dhat); +#endif /// @endcond // ============================================================================ diff --git a/src/ipc/barrier/barrier.hpp b/src/ipc/barrier/barrier.hpp index 105705140..58c869eff 100644 --- a/src/ipc/barrier/barrier.hpp +++ b/src/ipc/barrier/barrier.hpp @@ -338,6 +338,7 @@ template class TwoStageBarrier : public BarrierBase { * * \f\[ * b(d) = \begin{cases} + * \infty & d \le 0\\ * -\frac{\hat{d}^2}{4} \left(\ln\left(\frac{2d}{\hat{d}}\right) - * \tfrac{1}{2}\right) & d < \frac{\hat{d}}{2}\\ * \tfrac{1}{2} (\hat{d} - d)^2 & d < \hat{d}\\ @@ -356,7 +357,8 @@ template class TwoStageBarrier : public BarrierBase { * * \f\[ * b'(d) = \begin{cases} - * -\frac{\hat{d}}{4d} & d < \frac{\hat{d}}{2}\\ + * 0 & d \le 0\\ + * -\frac{\hat{d}^2}{4d} & d < \frac{\hat{d}}{2}\\ * d - \hat{d} & d < \hat{d}\\ * 0 & d \ge \hat{d} * \end{cases} @@ -373,7 +375,8 @@ template class TwoStageBarrier : public BarrierBase { * * \f\[ * b''(d) = \begin{cases} - * \frac{\hat{d}}{4d^2} & d < \frac{\hat{d}}{2}\\ + * 0 & d \le 0\\ + * \frac{\hat{d}^2}{4d^2} & d < \frac{\hat{d}}{2}\\ * 1 & d < \hat{d}\\ * 0 & d \ge \hat{d} * \end{cases} diff --git a/src/ipc/broad_phase/voxel_size_heuristic.cpp b/src/ipc/broad_phase/voxel_size_heuristic.cpp index f7f01e3c1..9919b7f7e 100644 --- a/src/ipc/broad_phase/voxel_size_heuristic.cpp +++ b/src/ipc/broad_phase/voxel_size_heuristic.cpp @@ -1,5 +1,6 @@ #include "voxel_size_heuristic.hpp" +#include #include #include @@ -9,9 +10,6 @@ namespace ipc { namespace { // Avoid unused variable warnings inline void check_success(bool success) { assert(success); } - - // Faster than std::pow(x, 2) - inline double sqr(double x) { return x * x; } } // namespace double suggest_good_voxel_size( diff --git a/src/ipc/collisions/normal/normal_collisions.cpp b/src/ipc/collisions/normal/normal_collisions.cpp index d24630e06..607657c68 100644 --- a/src/ipc/collisions/normal/normal_collisions.cpp +++ b/src/ipc/collisions/normal/normal_collisions.cpp @@ -1,6 +1,7 @@ #include "normal_collisions.hpp" #include +#include #include #include @@ -13,10 +14,6 @@ namespace ipc { -namespace { - inline double sqr(double x) { return x * x; } -} // namespace - void NormalCollisions::build( const CollisionMesh& mesh, Eigen::ConstRef vertices, diff --git a/src/ipc/config.hpp.in b/src/ipc/config.hpp.in index 2107213b3..581b1abb6 100644 --- a/src/ipc/config.hpp.in +++ b/src/ipc/config.hpp.in @@ -1,7 +1,5 @@ #pragma once -#include - #include // WARNING: Do not modify config.hpp directly. Instead, modify config.hpp.in. @@ -41,9 +39,16 @@ using index_t = int32_t; // using scalar_t = float; // #endif +// Mirror the Eigen::StorageOptions enum to avoid including Eigen headers in +// this file. +namespace { + constexpr int ColMajor = 0x0; // NOLINT + constexpr int RowMajor = 0x1; // NOLINT +} // namespace + // clang-format off /// @brief The layout of derivatives of vertex positions. The default is `Eigen::RowMajor`. -inline constexpr int VERTEX_DERIVATIVE_LAYOUT = Eigen::@IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT@; +inline constexpr int VERTEX_DERIVATIVE_LAYOUT = @IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT@; // clang-format on } // namespace ipc \ No newline at end of file diff --git a/src/ipc/distance/edge_edge.cpp b/src/ipc/distance/edge_edge.cpp index fd2df2aa9..1d308ca76 100644 --- a/src/ipc/distance/edge_edge.cpp +++ b/src/ipc/distance/edge_edge.cpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace ipc::detail { @@ -297,6 +298,11 @@ IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADGrad<12>); IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADHessian<12>); IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADGrad<13>); IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADHessian<13>); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_EDGE_EDGE(SimdBatch); +IPC_INSTANTIATE_EDGE_EDGE(SimdBatch); +#endif + #undef IPC_INSTANTIATE_EDGE_EDGE #undef IPC_INSTANTIATE_EDGE_EDGE_VALUE diff --git a/src/ipc/distance/edge_edge_mollifier.cpp b/src/ipc/distance/edge_edge_mollifier.cpp index 88c632ca8..44bf82e45 100644 --- a/src/ipc/distance/edge_edge_mollifier.cpp +++ b/src/ipc/distance/edge_edge_mollifier.cpp @@ -1,7 +1,5 @@ #include "edge_edge_mollifier.hpp" -#include - namespace ipc { namespace detail { @@ -20,17 +18,21 @@ namespace detail { ea0_rest, ea1_rest, eb0_rest, eb1_rest); const T ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - // ∇ₓ m = ∂m/∂ε · ∇ₓε - // (m depends on rest positions only through eps_x, since the - // cross-squarednorm s is a function of POSITIONS only) - return edge_edge_mollifier_derivative_wrt_eps_x( - ee_cross_norm_sqr, eps_x) - * edge_edge_mollifier_threshold_gradient( - ea0_rest, ea1_rest, eb0_rest, eb1_rest); - } else { - return Eigen::Vector::Zero(); + + if constexpr (!is_simd_batch_v) { + // Shortcut for the common case of the mollifier being inactive. + if (ee_cross_norm_sqr >= eps_x) { + return Eigen::Vector::Zero(); + } } + + // ∇ₓ m = ∂m/∂ε · ∇ₓε + // (m depends on rest positions only through eps_x, since the + // cross-squarednorm s is a function of POSITIONS only) + return edge_edge_mollifier_derivative_wrt_eps_x( + ee_cross_norm_sqr, eps_x) + * edge_edge_mollifier_threshold_gradient( + ea0_rest, ea1_rest, eb0_rest, eb1_rest); } template @@ -48,19 +50,23 @@ namespace detail { ea0_rest, ea1_rest, eb0_rest, eb1_rest); const T ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - // ∂²m/∂ε∂s (∇ₓε)(∇ᵤs(x+u))ᵀ + ∂m/∂s ∇ᵤ²s(x+u) - return edge_edge_mollifier_gradient_derivative_wrt_eps_x( - ee_cross_norm_sqr, eps_x) - * edge_edge_mollifier_threshold_gradient( - ea0_rest, ea1_rest, eb0_rest, eb1_rest) - * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1) - .transpose() - + ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) - * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); - } else { - return Eigen::Matrix::Zero(); + + if constexpr (!is_simd_batch_v) { + // Shortcut for the common case of the mollifier being inactive. + if (ee_cross_norm_sqr >= eps_x) { + return Eigen::Matrix::Zero(); + } } + + // ∂²m/∂ε∂s (∇ₓε)(∇ᵤs(x+u))ᵀ + ∂m/∂s ∇ᵤ²s(x+u) + return edge_edge_mollifier_gradient_derivative_wrt_eps_x( + ee_cross_norm_sqr, eps_x) + * edge_edge_mollifier_threshold_gradient( + ea0_rest, ea1_rest, eb0_rest, eb1_rest) + * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1) + .transpose() + + ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) + * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); } #define IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(T) \ @@ -86,6 +92,10 @@ namespace detail { IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(float); IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(double); +#ifdef IPC_TOOLKIT_WITH_SIMD + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(SimdBatch); + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(SimdBatch); +#endif #undef IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER } // namespace detail @@ -406,7 +416,7 @@ namespace autogen { const auto t1 = eb0x - eb1x; const auto t2 = eb0y - eb1y; const auto t3 = eb0z - eb1z; - const auto t4 = 2 * scale; + const auto t4 = T(2) * scale; const auto t5 = t4 * ((t1 * t1) + (t2 * t2) + (t3 * t3)); const auto t6 = t0 * t5; const auto t7 = ea0y - ea1y; @@ -431,14 +441,21 @@ namespace autogen { grad[11] = -t14; } - // clang-format off - template void edge_edge_cross_squarednorm_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12]); - template void edge_edge_cross_squarednorm_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12]); - template void edge_edge_cross_squarednorm_hessian(float, float, float, float, float, float, float, float, float, float, float, float, float[144]); - template void edge_edge_cross_squarednorm_hessian(double, double, double, double, double, double, double, double, double, double, double, double, double[144]); - template void edge_edge_mollifier_threshold_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12], float); - template void edge_edge_mollifier_threshold_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12], double); - // clang-format on +#define IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER_AUTOGEN(T) \ + template void edge_edge_cross_squarednorm_gradient( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[12]); \ + template void edge_edge_cross_squarednorm_hessian( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[144]); \ + template void edge_edge_mollifier_threshold_gradient( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[12], T) + + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER_AUTOGEN(float); + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER_AUTOGEN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER_AUTOGEN(SimdBatch); + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER_AUTOGEN(SimdBatch); +#endif +#undef IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER_AUTOGEN } // namespace autogen } // namespace ipc \ No newline at end of file diff --git a/src/ipc/distance/edge_edge_mollifier.hpp b/src/ipc/distance/edge_edge_mollifier.hpp index 63902c794..d3a20cddc 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -1,8 +1,8 @@ #pragma once +#include #include - -#include +#include namespace ipc { @@ -17,7 +17,7 @@ namespace autogen { T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T H[144]); template void edge_edge_mollifier_threshold_gradient( - T ea0x, T ea0y, T ea0z, T ea1x, T ea1y, T ea1z, T eb0x, T eb0y, T eb0z, T eb1x, T eb1y, T eb1z, T grad[12], T scale = T(1e-3)); + T ea0x, T ea0y, T ea0z, T ea1x, T ea1y, T ea1z, T eb0x, T eb0y, T eb0z, T eb1x, T eb1y, T eb1z, T grad[12], T scale = literal(1e-3)); // clang-format on } // namespace autogen @@ -29,12 +29,13 @@ namespace autogen { /// @return The mollifier coefficient to premultiply the edge-edge distance. template inline T edge_edge_mollifier(const T x, const T eps_x) { - if (x < eps_x) { - const T x_div_eps_x = x / eps_x; - return (-x_div_eps_x + T(2)) * x_div_eps_x; - } else { - return T(1); - } + return select_lazy( + x < eps_x, + [&] { + const T x_div_eps_x = x / eps_x; + return (-x_div_eps_x + T(2)) * x_div_eps_x; + }, + [&] { return T(1); }); } /// @brief The gradient of the mollifier function for edge-edge distance. @@ -44,12 +45,13 @@ template inline T edge_edge_mollifier(const T x, const T eps_x) template inline T edge_edge_mollifier_gradient(const T x, const T eps_x) { - if (x < eps_x) { - const T one_div_eps_x = T(1) / eps_x; - return T(2) * one_div_eps_x * std::fma(-one_div_eps_x, x, T(1)); - } else { - return T(0); - } + return select_lazy( + x < eps_x, + [&] { + const T one_div_eps_x = T(1) / eps_x; + return T(2) * one_div_eps_x * fma(-one_div_eps_x, x, T(1)); + }, + [&] { return T(0); }); } /// @brief The derivative of the mollifier function for edge-edge distance wrt @@ -61,8 +63,10 @@ inline T edge_edge_mollifier_gradient(const T x, const T eps_x) template inline T edge_edge_mollifier_derivative_wrt_eps_x(const T x, const T eps_x) { - return x < eps_x ? (T(2) * x * (-eps_x + x) / (eps_x * eps_x * eps_x)) - : T(0); + return select_lazy( + x < eps_x, + [&] { return T(2) * x * (-eps_x + x) / (eps_x * eps_x * eps_x); }, + [&] { return T(0); }); } /// @brief The hessian of the mollifier function for edge-edge distance. @@ -72,11 +76,9 @@ inline T edge_edge_mollifier_derivative_wrt_eps_x(const T x, const T eps_x) template inline T edge_edge_mollifier_hessian(const T x, const T eps_x) { - if (x < eps_x) { - return T(-2) / (eps_x * eps_x); - } else { - return T(0); - } + return select_lazy( + x < eps_x, [&] { return T(-2) / (eps_x * eps_x); }, + [&] { return T(0); }); } /// @brief The derivative of the gradient of the mollifier function for @@ -89,8 +91,10 @@ template inline T edge_edge_mollifier_gradient_derivative_wrt_eps_x(const T x, const T eps_x) { - return x < eps_x ? (T(2) * (-eps_x + T(2) * x) / (eps_x * eps_x * eps_x)) - : T(0); + return select_lazy( + x < eps_x, + [&] { return T(2) * (-eps_x + T(2) * x) / (eps_x * eps_x * eps_x); }, + [&] { return T(0); }); } // --- Fixed-size kernels --------------------------------------------------- @@ -146,15 +150,8 @@ namespace detail { Eigen::ConstRef> eb1, const T eps_x) { - const T ee_cross_norm_sqr = - edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - // NOTE: ipc:: qualification: the scalar overload is hidden by the - // same-named kernel in this namespace. - return ipc::edge_edge_mollifier(ee_cross_norm_sqr, eps_x); - } else { - return T(1); - } + return ipc::edge_edge_mollifier( + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1), eps_x); } /// @note Prefer the ipc::edge_edge_mollifier_gradient front end. @@ -168,12 +165,14 @@ namespace detail { { const T ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - return ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) - * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); - } else { - return Eigen::Vector::Zero(); + if constexpr (!is_simd_batch_v) { + // Shortcut for the common case of the mollifier being inactive. + if (ee_cross_norm_sqr >= eps_x) { + return Eigen::Vector::Zero(); + } } + return ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) + * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); } /// @note Prefer the ipc::edge_edge_mollifier_hessian front end. @@ -187,18 +186,20 @@ namespace detail { { const T ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - const Eigen::Vector grad = - edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); - - return (ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) - * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1)) - + ((ipc::edge_edge_mollifier_hessian(ee_cross_norm_sqr, eps_x) - * grad) - * grad.transpose()); - } else { - return Eigen::Matrix::Zero(); + if constexpr (!is_simd_batch_v) { + // Shortcut for the common case of the mollifier being inactive. + if (ee_cross_norm_sqr >= eps_x) { + return Eigen::Matrix::Zero(); + } } + const Eigen::Vector grad = + edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); + + return (ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) + * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1)) + + ((ipc::edge_edge_mollifier_hessian(ee_cross_norm_sqr, eps_x) + * grad) + * grad.transpose()); } /// @note Prefer the ipc::edge_edge_mollifier_gradient_wrt_x front end. @@ -234,7 +235,7 @@ namespace detail { Eigen::ConstRef> eb0_rest, Eigen::ConstRef> eb1_rest) { - return T(1e-3) * (ea0_rest - ea1_rest).squaredNorm() + return literal(1e-3) * (ea0_rest - ea1_rest).squaredNorm() * (eb0_rest - eb1_rest).squaredNorm(); } @@ -250,7 +251,7 @@ namespace detail { autogen::edge_edge_mollifier_threshold_gradient( ea0_rest[0], ea0_rest[1], ea0_rest[2], ea1_rest[0], ea1_rest[1], ea1_rest[2], eb0_rest[0], eb0_rest[1], eb0_rest[2], eb1_rest[0], - eb1_rest[1], eb1_rest[2], grad.data(), /*scale=*/T(1e-3)); + eb1_rest[1], eb1_rest[2], grad.data(), /*scale=*/literal(1e-3)); return grad; } } // namespace detail diff --git a/src/ipc/distance/line_line.cpp b/src/ipc/distance/line_line.cpp index 4af226bb4..12d7301a3 100644 --- a/src/ipc/distance/line_line.cpp +++ b/src/ipc/distance/line_line.cpp @@ -1,6 +1,7 @@ #include "line_line.hpp" #include +#include namespace ipc::autogen { @@ -566,6 +567,12 @@ void line_line_distance_hessian( IPC_INSTANTIATE_LINE_LINE_AUTOGEN(float); IPC_INSTANTIATE_LINE_LINE_AUTOGEN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +// SIMD batches, so that the batched distance gradients/Hessians link. +IPC_INSTANTIATE_LINE_LINE_AUTOGEN(SimdBatch); +IPC_INSTANTIATE_LINE_LINE_AUTOGEN(SimdBatch); +#endif + #undef IPC_INSTANTIATE_LINE_LINE_AUTOGEN } // namespace ipc::autogen diff --git a/src/ipc/distance/point_edge.cpp b/src/ipc/distance/point_edge.cpp index 8fbce7b1c..31fefdbcf 100644 --- a/src/ipc/distance/point_edge.cpp +++ b/src/ipc/distance/point_edge.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include // std::invalid_argument @@ -71,6 +72,12 @@ namespace detail { IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(float, 3); IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(double, 2); IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(double, 3); +#ifdef IPC_TOOLKIT_WITH_SIMD + IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(SimdBatch, 2); + IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(SimdBatch, 3); + IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(SimdBatch, 2); + IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(SimdBatch, 3); +#endif #undef IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN diff --git a/src/ipc/distance/point_line.cpp b/src/ipc/distance/point_line.cpp index 2956b55a0..be2c4b04f 100644 --- a/src/ipc/distance/point_line.cpp +++ b/src/ipc/distance/point_line.cpp @@ -1,6 +1,7 @@ #include "point_line.hpp" #include +#include namespace ipc::autogen { @@ -408,6 +409,11 @@ void point_line_distance_hessian_3D( IPC_INSTANTIATE_POINT_LINE_AUTOGEN(float); IPC_INSTANTIATE_POINT_LINE_AUTOGEN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_POINT_LINE_AUTOGEN(SimdBatch); +IPC_INSTANTIATE_POINT_LINE_AUTOGEN(SimdBatch); +#endif + #undef IPC_INSTANTIATE_POINT_LINE_AUTOGEN } // namespace ipc::autogen diff --git a/src/ipc/distance/point_plane.cpp b/src/ipc/distance/point_plane.cpp index fb250630c..70374eaa6 100644 --- a/src/ipc/distance/point_plane.cpp +++ b/src/ipc/distance/point_plane.cpp @@ -1,5 +1,7 @@ #include "point_plane.hpp" +#include + namespace ipc::autogen { // This function was generated by the Symbolic Math Toolbox version 8.3. @@ -566,6 +568,11 @@ void point_plane_distance_hessian( IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(float); IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(SimdBatch); +IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(SimdBatch); +#endif + #undef IPC_INSTANTIATE_POINT_PLANE_AUTOGEN } // namespace ipc::autogen diff --git a/src/ipc/distance/point_triangle.cpp b/src/ipc/distance/point_triangle.cpp index 27d5863ba..484d8ca2f 100644 --- a/src/ipc/distance/point_triangle.cpp +++ b/src/ipc/distance/point_triangle.cpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace ipc::detail { @@ -259,6 +260,14 @@ IPC_INSTANTIATE_POINT_TRIANGLE_VALUE(ADHessian<12>); IPC_INSTANTIATE_POINT_TRIANGLE_VALUE(ADGrad<13>); IPC_INSTANTIATE_POINT_TRIANGLE_VALUE(ADHessian<13>); +#ifdef IPC_TOOLKIT_WITH_SIMD +// SIMD batches. Only an explicit distance type is supported. +// See ipc/utils/simd.hpp for why AUTO cannot work lane-wise, and for +// the requirement that callers match the library's SIMD build flags. +IPC_INSTANTIATE_POINT_TRIANGLE(SimdBatch); +IPC_INSTANTIATE_POINT_TRIANGLE(SimdBatch); +#endif + #undef IPC_INSTANTIATE_POINT_TRIANGLE #undef IPC_INSTANTIATE_POINT_TRIANGLE_VALUE diff --git a/src/ipc/distance/signed/line_line.cpp b/src/ipc/distance/signed/line_line.cpp index b010cad45..c82e8a7aa 100644 --- a/src/ipc/distance/signed/line_line.cpp +++ b/src/ipc/distance/signed/line_line.cpp @@ -1,5 +1,7 @@ #include "line_line.hpp" +#include + namespace ipc::detail { template @@ -26,7 +28,8 @@ Eigen::Matrix line_line_signed_distance_hessian( // Contract the normal Hessian (3x12x12) with vector v (3x1). // This computes (v ⋅ d²n/dx²). // The result is a 1x12x12 vector, which maps to the 12x12 Hessian matrix. - hess = (hess_n.reshaped(3, 144).transpose() * v).reshaped(12, 12); + hess = (hess_n.reshaped(Eigen::fix<3>, Eigen::fix<144>).transpose() * v) + .reshaped(Eigen::fix<12>, Eigen::fix<12>); // --------------------------------------------------------- // 2. Add Jacobian Terms (Product Rule Corrections) @@ -70,9 +73,20 @@ Eigen::Matrix line_line_signed_distance_hessian( return hess; } -// clang-format off -template Matrix12f line_line_signed_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Matrix12d line_line_signed_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -// clang-format on +#define IPC_INSTANTIATE_LINE_LINE_SIGNED_DISTANCE_HESSIAN(T) \ + template Eigen::Matrix line_line_signed_distance_hessian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>) + +IPC_INSTANTIATE_LINE_LINE_SIGNED_DISTANCE_HESSIAN(float); +IPC_INSTANTIATE_LINE_LINE_SIGNED_DISTANCE_HESSIAN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_LINE_LINE_SIGNED_DISTANCE_HESSIAN(SimdBatch); +IPC_INSTANTIATE_LINE_LINE_SIGNED_DISTANCE_HESSIAN(SimdBatch); +#endif + +#undef IPC_INSTANTIATE_LINE_LINE_SIGNED_DISTANCE_HESSIAN } // namespace ipc::detail diff --git a/src/ipc/distance/signed/point_line.cpp b/src/ipc/distance/signed/point_line.cpp index 7ad051ca4..6000aaf20 100644 --- a/src/ipc/distance/signed/point_line.cpp +++ b/src/ipc/distance/signed/point_line.cpp @@ -1,5 +1,7 @@ #include "point_line.hpp" +#include + namespace ipc::detail { template @@ -23,7 +25,8 @@ Eigen::Matrix point_line_signed_distance_hessian( // --------------------------------------------------------- // Contract the normal Hessian (2x36) with vector v (2x1). // Result is 1x36, mapped to 6x6. - hess = (hess_n.reshaped(2, 36).transpose() * v).reshaped(6, 6); + hess = (hess_n.reshaped(Eigen::fix<2>, Eigen::fix<36>).transpose() * v) + .reshaped(Eigen::fix<6>, Eigen::fix<6>); // --------------------------------------------------------- // 2. Add Jacobian Terms (Product Rule Corrections) @@ -34,7 +37,9 @@ Eigen::Matrix point_line_signed_distance_hessian( // Extract 2x2 Jacobian blocks for e₀ and e₁ // Note: ∇ n has columns 0-1 (p), 2-3 (e₀), 4-5 (e₁). // Normal usually doesn't depend on p, so block(0,0) is zero. - assert(bool(jac_n.template leftCols<2>().isZero())); + if constexpr (std::is_floating_point_v) { + assert(bool(jac_n.template leftCols<2>().isZero())); + } const auto J_e0 = jac_n.template middleCols<2>(2); const auto J_e1 = jac_n.template rightCols<2>(); @@ -69,9 +74,19 @@ Eigen::Matrix point_line_signed_distance_hessian( return hess; } -// clang-format off -template Eigen::Matrix point_line_signed_distance_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); -template Eigen::Matrix point_line_signed_distance_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); -// clang-format on +#define IPC_INSTANTIATE_POINT_LINE_SIGNED_DISTANCE_HESSIAN(T) \ + template Eigen::Matrix point_line_signed_distance_hessian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>) + +IPC_INSTANTIATE_POINT_LINE_SIGNED_DISTANCE_HESSIAN(float); +IPC_INSTANTIATE_POINT_LINE_SIGNED_DISTANCE_HESSIAN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_POINT_LINE_SIGNED_DISTANCE_HESSIAN(SimdBatch); +IPC_INSTANTIATE_POINT_LINE_SIGNED_DISTANCE_HESSIAN(SimdBatch); +#endif + +#undef IPC_INSTANTIATE_POINT_LINE_SIGNED_DISTANCE_HESSIAN } // namespace ipc::detail diff --git a/src/ipc/distance/signed/point_plane.cpp b/src/ipc/distance/signed/point_plane.cpp index 268b5355a..3b5b41c3a 100644 --- a/src/ipc/distance/signed/point_plane.cpp +++ b/src/ipc/distance/signed/point_plane.cpp @@ -1,5 +1,7 @@ #include "point_plane.hpp" +#include + namespace ipc::detail { template @@ -41,7 +43,8 @@ Eigen::Matrix point_plane_signed_distance_hessian( // A. Contraction of the normal Hessian tensor with vector v // hess_n is 3x81. v is 3x1. Result is 1x81, which maps to 9x9. hess.template block<9, 9>(3, 3) = - (hess_n.reshaped(3, 81).transpose() * v).reshaped(9, 9); + (hess_n.reshaped(Eigen::fix<3>, Eigen::fix<81>).transpose() * v) + .reshaped(Eigen::fix<9>, Eigen::fix<9>); // B. Subtract first derivative terms (Product Rule corrections) // Extract 3x3 Jacobian blocks for t0, t1, t2 @@ -69,9 +72,20 @@ Eigen::Matrix point_plane_signed_distance_hessian( return hess; } -// clang-format off -template Matrix12f point_plane_signed_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Matrix12d point_plane_signed_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -// clang-format on +#define IPC_INSTANTIATE_POINT_PLANE_SIGNED_DISTANCE_HESSIAN(T) \ + template Eigen::Matrix point_plane_signed_distance_hessian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>) + +IPC_INSTANTIATE_POINT_PLANE_SIGNED_DISTANCE_HESSIAN(float); +IPC_INSTANTIATE_POINT_PLANE_SIGNED_DISTANCE_HESSIAN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_POINT_PLANE_SIGNED_DISTANCE_HESSIAN(SimdBatch); +IPC_INSTANTIATE_POINT_PLANE_SIGNED_DISTANCE_HESSIAN(SimdBatch); +#endif + +#undef IPC_INSTANTIATE_POINT_PLANE_SIGNED_DISTANCE_HESSIAN } // namespace ipc::detail diff --git a/src/ipc/friction/CMakeLists.txt b/src/ipc/friction/CMakeLists.txt index 07cfd2205..29a67eb10 100644 --- a/src/ipc/friction/CMakeLists.txt +++ b/src/ipc/friction/CMakeLists.txt @@ -1,5 +1,4 @@ set(SOURCES - smooth_friction_mollifier.cpp smooth_friction_mollifier.hpp smooth_mu.cpp smooth_mu.hpp diff --git a/src/ipc/friction/smooth_friction_mollifier.cpp b/src/ipc/friction/smooth_friction_mollifier.cpp deleted file mode 100644 index cc534d63b..000000000 --- a/src/ipc/friction/smooth_friction_mollifier.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "smooth_friction_mollifier.hpp" - -#include -#include - -namespace ipc { - -double smooth_friction_f0(const double y, const double eps_v) -{ - assert(eps_v > 0); - if (std::abs(y) >= eps_v) { - return y; - } - return y * y * (1 - y / (3 * eps_v)) / eps_v + eps_v / 3; -} - -double smooth_friction_f1(const double y, const double eps_v) -{ - assert(eps_v > 0); - if (std::abs(y) >= eps_v) { - return 1; - } - - const double y_over_eps_v = y / eps_v; - return y_over_eps_v * (2 - y_over_eps_v); -} - -double smooth_friction_f2(const double y, const double eps_v) -{ - assert(eps_v > 0); - if (std::abs(y) >= eps_v) { - return 0; - } - return (2 - 2 * y / eps_v) / eps_v; -} - -double smooth_friction_f1_over_x(const double y, const double eps_v) -{ - assert(eps_v > 0); - if (std::abs(y) >= eps_v) { - return 1 / y; - } - return (2 - y / eps_v) / eps_v; -} - -double smooth_friction_f2_x_minus_f1_over_x3(const double y, const double eps_v) -{ - assert(eps_v > 0); - if (std::abs(y) >= eps_v) { - return -1 / (y * y * y); - } - return -1 / (y * eps_v * eps_v); -} - -} // namespace ipc diff --git a/src/ipc/friction/smooth_friction_mollifier.hpp b/src/ipc/friction/smooth_friction_mollifier.hpp index a37104dae..99d97447a 100644 --- a/src/ipc/friction/smooth_friction_mollifier.hpp +++ b/src/ipc/friction/smooth_friction_mollifier.hpp @@ -1,7 +1,24 @@ #pragma once +#include +#include + +#include + namespace ipc { +// Each function below is piecewise around |y| = eps_v. We write the branch as +// `select_lazy` rather than an `if`, which is what lets one definition serve a +// plain `double`, an autodiff scalar, and a SIMD batch. +// +// The two paths differ in a way worth knowing before editing these: for a +// scalar, `select_lazy` is an ordinary `if`/`else` and only the winning branch +// runs. For a batch, lanes may straddle the threshold, so *both* branches run +// and the results are blended per-lane. Several of the inactive branches below +// divide by `y`, so on a lane where `y == 0` that branch produces an infinity — +// which is harmless, because the blend is a bitwise per-lane select that +// discards it rather than arithmetic that would propagate it into a NaN. + /// @brief Smooth friction mollifier function. /// /// \f\[ @@ -13,10 +30,19 @@ namespace ipc { /// \end{cases} /// \f\] /// +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the mollifier function at y. -double smooth_friction_f0(const double y, const double eps_v); +template inline T smooth_friction_f0(const T y, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + return select_lazy( + ipc::numext::abs(y) >= eps_v, [&] { return y; }, + [&] { + return y * y * (T(1) - y / (T(3) * eps_v)) / eps_v + eps_v / T(3); + }); +} /// @brief The first derivative of the smooth friction mollifier. /// @@ -27,10 +53,20 @@ double smooth_friction_f0(const double y, const double eps_v); /// \end{cases} /// \f\] /// +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the derivative of the smooth friction mollifier at y. -double smooth_friction_f1(const double y, const double eps_v); +template inline T smooth_friction_f1(const T y, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + return select_lazy( + ipc::numext::abs(y) >= eps_v, [&] { return T(1); }, + [&] { + const T y_over_eps_v = y / eps_v; + return y_over_eps_v * (T(2) - y_over_eps_v); + }); +} /// @brief The second derivative of the smooth friction mollifier. /// @@ -41,10 +77,17 @@ double smooth_friction_f1(const double y, const double eps_v); /// \end{cases} /// \f\] /// +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the second derivative of the smooth friction mollifier at y. -double smooth_friction_f2(const double y, const double eps_v); +template inline T smooth_friction_f2(const T y, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + return select_lazy( + ipc::numext::abs(y) >= eps_v, [&] { return T(0); }, + [&] { return (T(2) - T(2) * y / eps_v) / eps_v; }); +} /// @brief Compute the derivative of the smooth friction mollifier divided by y (\f$\frac{f_0'(y)}{y}\f$). /// @@ -56,10 +99,18 @@ double smooth_friction_f2(const double y, const double eps_v); /// \f\] /// /// @note The `x` in the function name refers to the parameter `y`. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the derivative of smooth_friction_f0 divided by y. -double smooth_friction_f1_over_x(const double y, const double eps_v); +template +inline T smooth_friction_f1_over_x(const T y, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + return select_lazy( + ipc::numext::abs(y) >= eps_v, [&] { return T(1) / y; }, + [&] { return (T(2) - y / eps_v) / eps_v; }); +} /// @brief The derivative of f1 times y minus f1 all divided by y cubed. /// @@ -71,10 +122,17 @@ double smooth_friction_f1_over_x(const double y, const double eps_v); /// \f\] /// /// @note The `x` in the function name refers to the parameter `y`. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The derivative of f1 times y minus f1 all divided by y cubed. -double -smooth_friction_f2_x_minus_f1_over_x3(const double y, const double eps_v); +template +inline T smooth_friction_f2_x_minus_f1_over_x3(const T y, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + return select_lazy( + ipc::numext::abs(y) >= eps_v, [&] { return T(-1) / (y * y * y); }, + [&] { return T(-1) / (y * eps_v * eps_v); }); +} } // namespace ipc diff --git a/src/ipc/friction/smooth_mu.cpp b/src/ipc/friction/smooth_mu.cpp index c7df9b33e..d1ed795df 100644 --- a/src/ipc/friction/smooth_mu.cpp +++ b/src/ipc/friction/smooth_mu.cpp @@ -1,116 +1,15 @@ #include "smooth_mu.hpp" -#include - -#include -#include - namespace ipc { -double smooth_mu( - const double y, const double mu_s, const double mu_k, const double eps_v) -{ - assert(eps_v > 0); - if (mu_s == mu_k || std::abs(y) >= eps_v) { - // If the static and kinetic friction coefficients are equal, simplify. - return mu_k; - } else { - const double z = std::abs(y) / eps_v; - if (std::abs(y) < 0.5 * eps_v) { - return 2 * (mu_k - mu_s) * z * z + mu_s; - } else { - return -2 * (mu_k - mu_s) * (z * (z - 2) + 1) + mu_k; - } - } -} - -double smooth_mu_derivative( - const double y, const double mu_s, const double mu_k, const double eps_v) -{ - assert(eps_v > 0); - if (mu_s == mu_k || std::abs(y) >= eps_v) { - // If the static and kinetic friction coefficients are equal, simplify. - return 0; - } else { - const double z = std::abs(y) / eps_v; - if (std::abs(y) < 0.5 * eps_v) { - return 4 * (mu_k - mu_s) * z / eps_v; - } else { - return -4 * (mu_k - mu_s) * (z - 1) / eps_v; - } - } -} - -double smooth_mu_f0( - const double y, const double mu_s, const double mu_k, const double eps_v) -{ - assert(eps_v > 0); - if (mu_s == mu_k || std::abs(y) >= eps_v) { - // If the static and kinetic friction coefficients are equal, simplify. - return mu_k * smooth_friction_f0(y, eps_v); - } else { - const double delta_mu = mu_k - mu_s; - const double z = std::abs(y) / eps_v; - if (std::abs(y) < 0.5 * eps_v) { - return y * z - * (z * (z * (1 - 0.4 * z) * delta_mu - mu_s / 3.0) + mu_s) - + (9.0 / 16.0) * eps_v * mu_k - (11.0 / 48.0) * eps_v * mu_s; - } else { - return y * z - * (z - * (z * (0.4 * z - 2) * delta_mu - + (3 * mu_k - (10.0 / 3.0) * mu_s)) - + (2 * mu_s - mu_k)) - + 0.6 * eps_v * mu_k - (4.0 / 15.0) * eps_v * mu_s; - } - } -} - -double smooth_mu_f1( - const double y, const double mu_s, const double mu_k, const double eps_v) -{ - // This is a known formulation: μ(y) f₁(y) - return smooth_mu(y, mu_s, mu_k, eps_v) * smooth_friction_f1(y, eps_v); -} - -double smooth_mu_f2( - const double y, const double mu_s, const double mu_k, const double eps_v) -{ - // Apply the chain rule: - return smooth_mu_derivative(y, mu_s, mu_k, eps_v) - * smooth_friction_f1(y, eps_v) - + smooth_mu(y, mu_s, mu_k, eps_v) * smooth_friction_f2(y, eps_v); -} - -double smooth_mu_f1_over_x( - const double y, const double mu_s, const double mu_k, const double eps_v) -{ - // This is a known formulation: μ(y) f₁(y) / y - // where we use the robust division by y to avoid division by zero. - return smooth_mu(y, mu_s, mu_k, eps_v) - * smooth_friction_f1_over_x(y, eps_v); -} - -double smooth_mu_f2_x_minus_mu_f1_over_x3( - const double y, const double mu_s, const double mu_k, const double eps_v) -{ - assert(eps_v > 0); - if (mu_s == mu_k || std::abs(y) >= eps_v) { - // If the static and kinetic friction coefficients are equal, - // simplify. - return mu_k * smooth_friction_f2_x_minus_f1_over_x3(y, eps_v); - } else { - const double delta_mu = mu_k - mu_s; - const double z = 1 / eps_v; - if (std::abs(y) < 0.5 * eps_v) { - return z * z * (z * (8 - 6 * y * z) * delta_mu - mu_s / y); - } else { - return z * z - * (z * (6 * y * z - 16) * delta_mu - + (9 * mu_k - 10 * mu_s) / y); - } - } -} +// The anisotropic helpers stay `double`-only for now, unlike the scalar μ +// functions in the header, because no batch caller exists for them yet. Two of +// them do branch per collision: `anisotropic_x_from_tau_aniso` guards a +// vanishing tangential velocity and `anisotropic_mu_eff_f` is a function of +// its direction. A batch friction path with anisotropic μ would therefore need +// to template those two as well. Only `anisotropic_mu_eff_from_tau_aniso` +// tests the material alone -- whether an ellipse axis is set, whether the +// caller disabled μ -- and can stay scalar regardless. std::pair anisotropic_mu_eff_f( Eigen::ConstRef tau_dir, @@ -169,4 +68,4 @@ std::pair anisotropic_mu_eff_from_tau_aniso( return mu_eff; } -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/friction/smooth_mu.hpp b/src/ipc/friction/smooth_mu.hpp index e16246262..ae2a9d79a 100644 --- a/src/ipc/friction/smooth_mu.hpp +++ b/src/ipc/friction/smooth_mu.hpp @@ -1,75 +1,181 @@ #pragma once +#include +#include #include +#include +#include #include // for std::pair namespace ipc { +// The `mu_s == mu_k` test that opens most of these functions is a fast path, +// not a special case: when the two coefficients are equal the general formulas +// below reduce to exactly the same value, so the blend a batch performs is +// correct whichever side a lane lands on. What the test buys a *scalar* caller +// is skipping the general formula entirely, which is the common setting where +// a material has one friction coefficient. +// +// See the note atop smooth_friction_mollifier.hpp for why these branches are +// written as `select_lazy` and what that means for a batch scalar. + /// @brief Smooth coefficient from static to kinetic friction. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static friction. /// @param mu_k Coefficient of kinetic friction. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the μ at y. -double smooth_mu( - const double y, const double mu_s, const double mu_k, const double eps_v); +template +inline T smooth_mu(const T y, const T mu_s, const T mu_k, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + const T abs_y = ipc::numext::abs(y); + const T z = abs_y / eps_v; + return select_lazy( + mu_s == mu_k || abs_y >= eps_v, [&] { return mu_k; }, + abs_y < T(0.5) * eps_v, + [&] { return T(2) * (mu_k - mu_s) * z * z + mu_s; }, + [&] { return T(-2) * (mu_k - mu_s) * (z * (z - T(2)) + T(1)) + mu_k; }); +} /// @brief Compute the derivative of the smooth coefficient from static to kinetic friction. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static friction. /// @param mu_k Coefficient of kinetic friction. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the derivative at y. -double smooth_mu_derivative( - const double y, const double mu_s, const double mu_k, const double eps_v); +template +inline T +smooth_mu_derivative(const T y, const T mu_s, const T mu_k, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + const T abs_y = ipc::numext::abs(y); + const T z = abs_y / eps_v; + return select_lazy( + mu_s == mu_k || abs_y >= eps_v, [&] { return T(0); }, + abs_y < T(0.5) * eps_v, + [&] { return T(4) * (mu_k - mu_s) * z / eps_v; }, + [&] { return T(-4) * (mu_k - mu_s) * (z - T(1)) / eps_v; }); +} /// @brief Compute the value of the ∫ μ(y) f₁(y) dy, where f₁ is the first derivative of the smooth friction mollifier. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static friction. /// @param mu_k Coefficient of kinetic friction. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the integral at y. -double smooth_mu_f0( - const double y, const double mu_s, const double mu_k, const double eps_v); +template +inline T smooth_mu_f0(const T y, const T mu_s, const T mu_k, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + const T abs_y = ipc::numext::abs(y); + const T delta_mu = mu_k - mu_s; + const T z = abs_y / eps_v; + return select_lazy( + mu_s == mu_k || abs_y >= eps_v, + [&] { return mu_k * smooth_friction_f0(y, eps_v); }, + abs_y < T(0.5) * eps_v, + [&] { + return y * z + * (z + * (z * (T(1) - literal(0.4) * z) * delta_mu + - mu_s / T(3)) + + mu_s) + + literal(9.0 / 16.0) * eps_v * mu_k + - literal(11.0 / 48.0) * eps_v * mu_s; + }, + [&] { + return y * z + * (z + * (z * (literal(0.4) * z - T(2)) * delta_mu + + (T(3) * mu_k - literal(10.0 / 3.0) * mu_s)) + + (T(2) * mu_s - mu_k)) + + literal(0.6) * eps_v * mu_k + - literal(4.0 / 15.0) * eps_v * mu_s; + }); +} /// @brief Compute the value of the μ(y) f₁(y), where f₁ is the first derivative of the smooth friction mollifier. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static friction. /// @param mu_k Coefficient of kinetic friction. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the product at y. -double smooth_mu_f1( - const double y, const double mu_s, const double mu_k, const double eps_v); +template +inline T smooth_mu_f1(const T y, const T mu_s, const T mu_k, const T eps_v) +{ + // This is a known formulation: μ(y) f₁(y) + return smooth_mu(y, mu_s, mu_k, eps_v) * smooth_friction_f1(y, eps_v); +} /// @brief Compute the value of d/dy (μ(y) f₁(y)), where f₁ is the first derivative of the smooth friction mollifier. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static friction. /// @param mu_k Coefficient of kinetic friction. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the derivative at y. -double smooth_mu_f2( - const double y, const double mu_s, const double mu_k, const double eps_v); +template +inline T smooth_mu_f2(const T y, const T mu_s, const T mu_k, const T eps_v) +{ + // Apply the chain rule: + return smooth_mu_derivative(y, mu_s, mu_k, eps_v) + * smooth_friction_f1(y, eps_v) + + smooth_mu(y, mu_s, mu_k, eps_v) * smooth_friction_f2(y, eps_v); +} /// @brief Compute the value of the μ(y) f₁(y) / y, where f₁ is the first derivative of the smooth friction mollifier. /// @note The `x` in the function name refers to the parameter `y`. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static friction. /// @param mu_k Coefficient of kinetic friction. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the product at y. -double smooth_mu_f1_over_x( - const double y, const double mu_s, const double mu_k, const double eps_v); +template +inline T +smooth_mu_f1_over_x(const T y, const T mu_s, const T mu_k, const T eps_v) +{ + // This is a known formulation: μ(y) f₁(y) / y + // where we use the robust division by y to avoid division by zero. + return smooth_mu(y, mu_s, mu_k, eps_v) + * smooth_friction_f1_over_x(y, eps_v); +} /// @brief Compute the value of the [(d/dy μ(y) f₁(y)) ⋅ y - μ(y) f₁(y)] / y³, where f₁ and f₂ are the first and second derivatives of the smooth friction mollifier. /// @note The `x` in the function name refers to the parameter `y`. +/// @tparam T The scalar type. /// @param y The tangential relative speed. /// @param mu_s Coefficient of static friction. /// @param mu_k Coefficient of kinetic friction. /// @param eps_v Velocity threshold below which static friction force is applied. /// @return The value of the expression at y. -double smooth_mu_f2_x_minus_mu_f1_over_x3( - const double y, const double mu_s, const double mu_k, const double eps_v); +template +inline T smooth_mu_f2_x_minus_mu_f1_over_x3( + const T y, const T mu_s, const T mu_k, const T eps_v) +{ + assert(all_of(eps_v > T(0))); + const T abs_y = ipc::numext::abs(y); + const T delta_mu = mu_k - mu_s; + const T z = T(1) / eps_v; + return select_lazy( + mu_s == mu_k || abs_y >= eps_v, + [&] { return mu_k * smooth_friction_f2_x_minus_f1_over_x3(y, eps_v); }, + abs_y < T(0.5) * eps_v, + [&] { + return z * z * (z * (T(8) - T(6) * y * z) * delta_mu - mu_s / y); + }, + [&] { + return z * z + * (z * (T(6) * y * z - T(16)) * delta_mu + + (T(9) * mu_k - T(10) * mu_s) / y); + }); +} /// Elliptical L2 (matchstick cone) anisotropic friction. Call /// anisotropic_x_from_tau_aniso, then anisotropic_mu_eff_f. @@ -139,4 +245,4 @@ anisotropic_x_from_tau_aniso(Eigen::ConstRef tau_aniso); const double mu_k_isotropic, const bool no_mu = false); -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/geometry/angle.cpp b/src/ipc/geometry/angle.cpp index 80a0a9b5f..0014899aa 100644 --- a/src/ipc/geometry/angle.cpp +++ b/src/ipc/geometry/angle.cpp @@ -1,96 +1,111 @@ #include "angle.hpp" +#include #include +#include +#include -namespace ipc { +#include +#include -double dihedral_angle( - Eigen::ConstRef x0, - Eigen::ConstRef x1, - Eigen::ConstRef x2, - Eigen::ConstRef x3) +namespace ipc::detail { + +namespace { + /// @brief Jacobians of the two triangle normals with respect to all 12 DOFs + /// (x0, x1, x2, x3). + /// + /// n0 = normal(x0, x1, x2) does not depend on x3, so its last block is + /// zero. n1 = normal(x1, x0, x3) comes back in (x1, x0, x3) order, so we + /// permute its columns into place and zero the x2 block. + template + inline std::pair, Eigen::Matrix> + dihedral_normal_jacobians( + Eigen::ConstRef> x0, + Eigen::ConstRef> x1, + Eigen::ConstRef> x2, + Eigen::ConstRef> x3) + { + Eigen::Matrix dn0_dx; + dn0_dx.template leftCols<9>() = triangle_normal_jacobian(x0, x1, x2); + dn0_dx.template rightCols<3>().setZero(); + + Eigen::Matrix dn1_dx; + const std::array idx = { { 3, 4, 5, 0, 1, 2, 9, 10, 11 } }; + dn1_dx(Eigen::all, idx) = triangle_normal_jacobian(x1, x0, x3); + dn1_dx.template middleCols<3>(6).setZero(); + + return { dn0_dx, dn1_dx }; + } +} // namespace + +template +T dihedral_angle( + Eigen::ConstRef> x0, + Eigen::ConstRef> x1, + Eigen::ConstRef> x2, + Eigen::ConstRef> x3) { - const Eigen::Vector3d n0 = triangle_normal(x0, x1, x2); - const Eigen::Vector3d n1 = triangle_normal(x1, x0, x3); - const Eigen::Vector3d e = (x1 - x0).normalized(); + const Eigen::Vector3 n0 = triangle_normal(x0, x1, x2); + const Eigen::Vector3 n1 = triangle_normal(x1, x0, x3); + const Eigen::Vector3 e = normalized(x1 - x0); - const double sin_theta = n0.cross(n1).dot(e); - const double cos_theta = n0.dot(n1); + const T sin_theta = n0.cross(n1).dot(e); + const T cos_theta = n0.dot(n1); - return std::atan2(sin_theta, cos_theta); + return ipc::numext::atan2(sin_theta, cos_theta); } -Eigen::Vector dihedral_angle_gradient( - Eigen::ConstRef x0, - Eigen::ConstRef x1, - Eigen::ConstRef x2, - Eigen::ConstRef x3) +template +Eigen::Vector dihedral_angle_gradient( + Eigen::ConstRef> x0, + Eigen::ConstRef> x1, + Eigen::ConstRef> x2, + Eigen::ConstRef> x3) { - const Eigen::Vector3d n0 = triangle_normal(x0, x1, x2); - const Eigen::Vector3d n1 = triangle_normal(x1, x0, x3); - const Eigen::Vector3d e = (x1 - x0).normalized(); + const Eigen::Vector3 n0 = triangle_normal(x0, x1, x2); + const Eigen::Vector3 n1 = triangle_normal(x1, x0, x3); + const Eigen::Vector3 e = normalized(x1 - x0); // --- Normal gradients --- - Eigen::Matrix dn0_dx; - dn0_dx.leftCols<9>() = triangle_normal_jacobian(x0, x1, x2); - dn0_dx.rightCols<3>().setZero(); - - Eigen::Matrix dn1_dx; - const std::array idx = { { 3, 4, 5, 0, 1, 2, 9, 10, 11 } }; - dn1_dx(Eigen::all, idx) = triangle_normal_jacobian(x1, x0, x3); - dn1_dx.middleCols<3>(6).setZero(); + const auto [dn0_dx, dn1_dx] = dihedral_normal_jacobians(x0, x1, x2, x3); // --- Angle gradient --- - const Eigen::Vector dcos_dx = + const Eigen::Vector dcos_dx = dn0_dx.transpose() * n1 + dn1_dx.transpose() * n0; - const Eigen::Vector dsin_dx = - (cross_product_matrix(n0) * dn1_dx - cross_product_matrix(n1) * dn0_dx) + const Eigen::Vector dsin_dx = + (cross_product_matrix(n0) * dn1_dx + - cross_product_matrix(n1) * dn0_dx) .transpose() * e; // --- Product rule --- - const double sin_theta = n0.cross(n1).dot(e); - const double cos_theta = n0.dot(n1); + const T sin_theta = n0.cross(n1).dot(e); + const T cos_theta = n0.dot(n1); return dsin_dx * cos_theta - dcos_dx * sin_theta; } -namespace { - inline Eigen::Vector3d cross( - Eigen::ConstRef a, Eigen::ConstRef b) - { - return a.cross(b); - } -} // namespace - -Matrix12d dihedral_angle_hessian( - Eigen::ConstRef x0, - Eigen::ConstRef x1, - Eigen::ConstRef x2, - Eigen::ConstRef x3) +template +Eigen::Matrix dihedral_angle_hessian( + Eigen::ConstRef> x0, + Eigen::ConstRef> x1, + Eigen::ConstRef> x2, + Eigen::ConstRef> x3) { - const Eigen::Vector3d n0 = triangle_normal(x0, x1, x2); - const Eigen::Vector3d n1 = triangle_normal(x1, x0, x3); + using Matrix12 = Eigen::Matrix; + + const Eigen::Vector3 n0 = triangle_normal(x0, x1, x2); + const Eigen::Vector3 n1 = triangle_normal(x1, x0, x3); // ------------------------------------------------------------------------- // Jacobian of n0 and n1 w.r.t. all 12 DOFs - // dn0_dx: n0 depends on (x0, x1, x2), not x3 → zero last 3 cols - // dn1_dx: n1 = triangle_normal(x1, x0, x3), permute cols (x1→0..2, x0→3..5, - // x3→6..8) → x2 block zero // ------------------------------------------------------------------------- - Eigen::Matrix dn0_dx; - dn0_dx.leftCols<9>() = triangle_normal_jacobian(x0, x1, x2); - dn0_dx.rightCols<3>().setZero(); - - Eigen::Matrix dn1_dx; - const std::array idx = { { 3, 4, 5, 0, 1, 2, 9, 10, 11 } }; - dn1_dx(Eigen::all, idx) = triangle_normal_jacobian(x1, x0, x3); - dn1_dx.middleCols<3>(6).setZero(); + const auto [dn0_dx, dn1_dx] = dihedral_normal_jacobians(x0, x1, x2, x3); // ------------------------------------------------------------------------- // Hessians of n0 and n1 — shape (27 × 9) each, then embedded in 12-DOF @@ -104,9 +119,9 @@ Matrix12d dihedral_angle_hessian( // ------------------------------------------------------------------------- // Raw hessians in local (9-DOF) coordinate systems - const Eigen::Matrix d2n0_local = + const Eigen::Matrix d2n0_local = triangle_normal_hessian(x0, x1, x2); - const Eigen::Matrix d2n1_local = + const Eigen::Matrix d2n1_local = triangle_normal_hessian(x1, x0, x3); // We will access them on-the-fly via the index maps rather than building @@ -146,9 +161,10 @@ Matrix12d dihedral_angle_hessian( // ∂²e_k/(∂x1_p ∂x0_q) = -He[k][p,q] // ------------------------------------------------------------------------- - const auto [e, Je, He] = normalization_and_jacobian_and_hessian(x1 - x0); + const auto [e, Je, He] = + normalization_and_jacobian_and_hessian(x1 - x0); - // He is std::array where He[k] is the 3×3 Hessian for + // He is std::array, 3> where He[k] is the 3×3 Hessian for // component k of the normalized vector. // ------------------------------------------------------------------------- @@ -157,33 +173,33 @@ Matrix12d dihedral_angle_hessian( // ∂e/∂x0 = -J_norm, ∂e/∂x1 = +J_norm // ------------------------------------------------------------------------- - Eigen::Matrix de_dx = Eigen::Matrix::Zero(); - de_dx.leftCols<3>() = -Je; // ∂e/∂x0 - de_dx.middleCols<3>(3) = Je; // ∂e/∂x1 + Eigen::Matrix de_dx = Eigen::Matrix::Zero(); + de_dx.template leftCols<3>() = -Je; // ∂e/∂x0 + de_dx.template middleCols<3>(3) = Je; // ∂e/∂x1 // ------------------------------------------------------------------------- // Scalars s = (n0 × n1) · e and c = n0 · n1 // ------------------------------------------------------------------------- - const Eigen::Vector3d m = n0.cross(n1); // n0 × n1 - const double sin_theta = m.dot(e); - const double cos_theta = n0.dot(n1); + const Eigen::Vector3 m = n0.cross(n1); // n0 × n1 + const T sin_theta = m.dot(e); + const T cos_theta = n0.dot(n1); // ------------------------------------------------------------------------- // Jacobian of s and c (12-vectors) — re-derived consistently with gradient // ------------------------------------------------------------------------- // dm_dx = ∂(n0×n1)/∂x (3×12) - const Eigen::Matrix dm_dx = - cross_product_matrix(n0) * dn1_dx - cross_product_matrix(n1) * dn0_dx; + const Eigen::Matrix dm_dx = cross_product_matrix(n0) * dn1_dx + - cross_product_matrix(n1) * dn0_dx; - const Eigen::Vector dcos_dx = + const Eigen::Vector dcos_dx = dn0_dx.transpose() * n1 + dn1_dx.transpose() * n0; - const Eigen::Vector dsin_dx = + const Eigen::Vector dsin_dx = dm_dx.transpose() * e + de_dx.transpose() * m; - const Eigen::Vector dtheta_dx = + const Eigen::Vector dtheta_dx = dsin_dx * cos_theta - dcos_dx * sin_theta; // ------------------------------------------------------------------------- @@ -195,7 +211,7 @@ Matrix12d dihedral_angle_hessian( // + n0 · (∂²n1/(∂xp ∂xq)) // ------------------------------------------------------------------------- - Matrix12d H_cos; + Matrix12 H_cos; // Cross-Jacobian terms (symmetric) H_cos = dn0_dx.transpose() * dn1_dx; @@ -232,7 +248,7 @@ Matrix12d dihedral_angle_hessian( // + m · [∂²e/(∂xp ∂xq)] // ------------------------------------------------------------------------- - Matrix12d H_sin; + Matrix12 H_sin; // Cross-Jacobian terms (m–e interaction) H_sin = dm_dx.transpose() * de_dx; @@ -243,16 +259,16 @@ Matrix12d dihedral_angle_hessian( // Block (x0,x0): +He[k], Block (x1,x1): +He[k], // Block (x0,x1): -He[k], Block (x1,x0): -He[k] for (int k = 0; k < 3; k++) { - const Eigen::Matrix3d& Hek = He[k]; // 3×3 - const double mk = m(k); + const Eigen::Matrix3& Hek = He[k]; // 3×3 + const T mk = m(k); // (x0, x0) - H_sin.block<3, 3>(0, 0) += mk * Hek; + H_sin.template block<3, 3>(0, 0) += mk * Hek; // (x1, x1) - H_sin.block<3, 3>(3, 3) += mk * Hek; + H_sin.template block<3, 3>(3, 3) += mk * Hek; // (x0, x1) - H_sin.block<3, 3>(0, 3) -= mk * Hek; + H_sin.template block<3, 3>(0, 3) -= mk * Hek; // (x1, x0) - H_sin.block<3, 3>(3, 0) -= mk * Hek; + H_sin.template block<3, 3>(3, 0) -= mk * Hek; } // Second derivative of m = n0 × n1 contracted with e: @@ -270,7 +286,7 @@ Matrix12d dihedral_angle_hessian( // so eᵀ [v×] = (-v × e)ᵀ = (e × v)ᵀ (as a row vector) // Pre-compute (e × n0) for efficiency - const Eigen::Vector3d e_cross_n0 = cross(e, n0); + const Eigen::Vector3 e_cross_n0 = e.cross(n0); // --- Terms involving first × first derivatives (cross of Jacobian cols) // --- -eᵀ [dn1/dxq ×] dn0/dxp = (e × dn1/dxq) · dn0/dxp @@ -280,15 +296,15 @@ Matrix12d dihedral_angle_hessian( // => e · (dn0/dxq × dn1/dxp) // Also: e · (v × w) = -(e × v) · w... let's just use dot directly. for (int q = 0; q < 12; q++) { - const Eigen::Vector3d dn1_q = dn1_dx.col(q); - const Eigen::Vector3d dn0_q = dn0_dx.col(q); + const Eigen::Vector3 dn1_q = dn1_dx.col(q); + const Eigen::Vector3 dn0_q = dn0_dx.col(q); // e · (dn1_q × dn0_p) for all p → negate the cross and dot with e // Term: -eᵀ [dn1_q×] dn0_dxp = e · (dn1_q × dn0_dxp) ... but // [v×]w = v×w, so eᵀ [v×] w = e·(v×w) = (e×v)·w // -eᵀ [dn1_q ×] dn0_dxp = -(e × dn1_q) · dn0_dxp - const Eigen::Vector3d neg_e_cross_dn1_q = -(cross(e, dn1_q)); + const Eigen::Vector3 neg_e_cross_dn1_q = -(e.cross(dn1_q)); // +eᵀ [dn0_q ×] dn1_dxp = (e × dn0_q) · dn1_dxp - const Eigen::Vector3d e_cross_dn0_q = cross(e, dn0_q); + const Eigen::Vector3 e_cross_dn0_q = e.cross(dn0_q); for (int p = 0; p < 12; p++) { H_sin(p, q) += neg_e_cross_dn1_q.dot(dn0_dx.col(p)); H_sin(p, q) += e_cross_dn0_q.dot(dn1_dx.col(p)); @@ -301,7 +317,7 @@ Matrix12d dihedral_angle_hessian( // +eᵀ [n0×] ∂²n1/(∂xp ∂xq) = (e × n0) · ∂²n1/(∂xp ∂xq) // where eᵀ [n1×] w = (e×n1)·w // eᵀ [n0×] w = (e×n0)·w - const Eigen::Vector3d neg_e_cross_n1 = -cross(e, n1); // -(e × n1) + const Eigen::Vector3 neg_e_cross_n1 = -e.cross(n1); for (int p = 0; p < 12; p++) { const int lp0 = g2l_n0(p); @@ -337,13 +353,29 @@ Matrix12d dihedral_angle_hessian( // - 2·∇θ·(s·∇s + c·∇c)ᵀ (denominator derivative) // ------------------------------------------------------------------------- - Matrix12d H_theta = cos_theta * H_sin - sin_theta * H_cos + Matrix12 H_theta = cos_theta * H_sin - sin_theta * H_cos + dcos_dx * dsin_dx.transpose() - dsin_dx * dcos_dx.transpose() - dtheta_dx - * ((2 * sin_theta) * dsin_dx + (2 * cos_theta) * dcos_dx) + * ((T(2) * sin_theta) * dsin_dx + (T(2) * cos_theta) * dcos_dx) .transpose(); return H_theta; } -} // namespace ipc \ No newline at end of file +// clang-format off +#define IPC_INSTANTIATE_ANGLE(T) \ + template T dihedral_angle(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template Eigen::Vector dihedral_angle_gradient(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template Eigen::Matrix dihedral_angle_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>) + +IPC_INSTANTIATE_ANGLE(float); +IPC_INSTANTIATE_ANGLE(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_ANGLE(SimdBatch); +IPC_INSTANTIATE_ANGLE(SimdBatch); +#endif + +#undef IPC_INSTANTIATE_ANGLE +// clang-format on + +} // namespace ipc::detail diff --git a/src/ipc/geometry/angle.hpp b/src/ipc/geometry/angle.hpp index d4a861225..6ae1ce2a5 100644 --- a/src/ipc/geometry/angle.hpp +++ b/src/ipc/geometry/angle.hpp @@ -4,6 +4,32 @@ namespace ipc { +namespace detail { + /// @note Prefer the ipc::dihedral_angle front end. + template + T dihedral_angle( + Eigen::ConstRef> x0, + Eigen::ConstRef> x1, + Eigen::ConstRef> x2, + Eigen::ConstRef> x3); + + /// @note Prefer the ipc::dihedral_angle_gradient front end. + template + Eigen::Vector dihedral_angle_gradient( + Eigen::ConstRef> x0, + Eigen::ConstRef> x1, + Eigen::ConstRef> x2, + Eigen::ConstRef> x3); + + /// @note Prefer the ipc::dihedral_angle_hessian front end. + template + Eigen::Matrix dihedral_angle_hessian( + Eigen::ConstRef> x0, + Eigen::ConstRef> x1, + Eigen::ConstRef> x2, + Eigen::ConstRef> x3); +} // namespace detail + /// @brief Compute the bending angle between two triangles sharing an edge. /// x0---x3 /// | \ | @@ -13,11 +39,20 @@ namespace ipc { /// @param x2 The opposite vertex of the first triangle. /// @param x3 The opposite vertex of the second triangle. /// @return The bending angle between the two triangles. -double dihedral_angle( - Eigen::ConstRef x0, - Eigen::ConstRef x1, - Eigen::ConstRef x2, - Eigen::ConstRef x3); +template < + typename DerivedX0, + typename DerivedX1, + typename DerivedX2, + typename DerivedX3> +inline auto dihedral_angle( + const Eigen::MatrixBase& x0, + const Eigen::MatrixBase& x1, + const Eigen::MatrixBase& x2, + const Eigen::MatrixBase& x3) +{ + using T = typename DerivedX0::Scalar; + return detail::dihedral_angle(x0, x1, x2, x3); +} /// @brief Compute the Jacobian of the bending angle between two triangles sharing an edge. /// x0---x3 @@ -28,11 +63,20 @@ double dihedral_angle( /// @param x2 The opposite vertex of the first triangle. /// @param x3 The opposite vertex of the second triangle. /// @return The Jacobian matrix of the bending angle with respect to the input vertices. -Eigen::Vector dihedral_angle_gradient( - Eigen::ConstRef x0, - Eigen::ConstRef x1, - Eigen::ConstRef x2, - Eigen::ConstRef x3); +template < + typename DerivedX0, + typename DerivedX1, + typename DerivedX2, + typename DerivedX3> +inline auto dihedral_angle_gradient( + const Eigen::MatrixBase& x0, + const Eigen::MatrixBase& x1, + const Eigen::MatrixBase& x2, + const Eigen::MatrixBase& x3) +{ + using T = typename DerivedX0::Scalar; + return detail::dihedral_angle_gradient(x0, x1, x2, x3); +} /// @brief Compute the Hessian of the bending angle between two triangles sharing an edge. /// x0---x3 @@ -43,10 +87,19 @@ Eigen::Vector dihedral_angle_gradient( /// @param x2 The opposite vertex of the first triangle. /// @param x3 The opposite vertex of the second triangle. /// @return The 12x12 Hessian matrix of the bending angle with respect to the input vertices. -Eigen::Matrix dihedral_angle_hessian( - Eigen::ConstRef x0, - Eigen::ConstRef x1, - Eigen::ConstRef x2, - Eigen::ConstRef x3); +template < + typename DerivedX0, + typename DerivedX1, + typename DerivedX2, + typename DerivedX3> +inline auto dihedral_angle_hessian( + const Eigen::MatrixBase& x0, + const Eigen::MatrixBase& x1, + const Eigen::MatrixBase& x2, + const Eigen::MatrixBase& x3) +{ + using T = typename DerivedX0::Scalar; + return detail::dihedral_angle_hessian(x0, x1, x2, x3); +} -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/geometry/area.cpp b/src/ipc/geometry/area.cpp index 99a54fcbe..150ac7245 100644 --- a/src/ipc/geometry/area.cpp +++ b/src/ipc/geometry/area.cpp @@ -1,6 +1,6 @@ #include "area.hpp" -#include +#include namespace ipc::autogen { @@ -32,7 +32,7 @@ void triangle_area_gradient( const T t11 = t0_z - t1_z; const T t12 = t10 * t2 - t11 * t5; const T t13 = t10 * t6 - t11 * t3; - const T t14 = T(0.5) / std::sqrt(t12 * t12 + t13 * t13 + t7 * t7); + const T t14 = T(0.5) / ipc::numext::sqrt(t12 * t12 + t13 * t13 + t7 * t7); const T t15 = t1_x + t4; dA[0] = t14 * (t1 * t7 + t12 * t9); dA[1] = -t14 * (-t13 * t9 + t15 * t7); @@ -50,6 +50,10 @@ void triangle_area_gradient( IPC_INSTANTIATE_AREA_AUTOGEN(float); IPC_INSTANTIATE_AREA_AUTOGEN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_AREA_AUTOGEN(SimdBatch); +IPC_INSTANTIATE_AREA_AUTOGEN(SimdBatch); +#endif #undef IPC_INSTANTIATE_AREA_AUTOGEN } // namespace ipc::autogen diff --git a/src/ipc/geometry/area.hpp b/src/ipc/geometry/area.hpp index 24fc22d35..b57bb8d85 100644 --- a/src/ipc/geometry/area.hpp +++ b/src/ipc/geometry/area.hpp @@ -5,6 +5,7 @@ #include #include +#include namespace ipc { @@ -48,7 +49,11 @@ namespace detail { Eigen::ConstRef> e1) { static_assert(dim == 2 || dim == 3, "edges are only 2D or 3D"); - assert((e1 - e0).norm() != 0); + // A degenerate edge divides by zero below. A batch cannot answer that + // with one bool, so it is only asserted for a plain scalar. + if constexpr (std::is_floating_point_v) { + assert((e1 - e0).norm() != 0); + } // ∇ ‖e₁ - e₀‖ Eigen::Vector grad; diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp index 6d9e88448..ebba14be2 100644 --- a/src/ipc/geometry/normal.cpp +++ b/src/ipc/geometry/normal.cpp @@ -1,8 +1,7 @@ #include "ipc/geometry/normal.hpp" #include - -#include +#include namespace ipc::detail { @@ -184,7 +183,7 @@ MatrixMax point_line_normal_hessian( const VectorMax3 z = point_line_unnormalized_normal(p, e0, e1); const T z_norm2 = z.squaredNorm(); - const T z_norm = std::sqrt(z_norm2); + const T z_norm = ipc::numext::sqrt(z_norm2); const T z_norm3 = z_norm2 * z_norm; const int DIM = z.size(); // dimension (2 or 3) @@ -271,7 +270,7 @@ Eigen::Matrix triangle_normal_hessian( { const Eigen::Vector3 z = triangle_unnormalized_normal(a, b, c); const T z_norm2 = z.squaredNorm(); - const T z_norm = std::sqrt(z_norm2); + const T z_norm = ipc::numext::sqrt(z_norm2); const T z_norm3 = z_norm2 * z_norm; const auto dz_dx = triangle_unnormalized_normal_jacobian(a, b, c); @@ -350,7 +349,7 @@ Eigen::Matrix line_line_normal_hessian( const Eigen::Vector3 z = line_line_unnormalized_normal(ea0, ea1, eb0, eb1); const T z_norm2 = z.squaredNorm(); - const T z_norm = std::sqrt(z_norm2); + const T z_norm = ipc::numext::sqrt(z_norm2); const T z_norm3 = z_norm2 * z_norm; const Eigen::Matrix dz_dx = @@ -380,24 +379,25 @@ Eigen::Matrix line_line_normal_hessian( } // clang-format off -template VectorMax3f point_line_unnormalized_normal(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template VectorMax3d point_line_unnormalized_normal(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template MatrixMax point_line_unnormalized_normal_jacobian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template MatrixMax point_line_unnormalized_normal_jacobian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template MatrixMax point_line_unnormalized_normal_hessian(Eigen::ConstRef,Eigen::ConstRef,Eigen::ConstRef); -template MatrixMax point_line_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template MatrixMax point_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template MatrixMax point_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Eigen::Matrix cross_product_matrix_jacobian(); -template Eigen::Matrix cross_product_matrix_jacobian(); -template Eigen::Matrix triangle_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Eigen::Matrix triangle_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Eigen::Matrix triangle_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Eigen::Matrix triangle_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Eigen::Matrix line_line_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Eigen::Matrix line_line_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Eigen::Matrix line_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Eigen::Matrix line_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +#define IPC_INSTANTIATE_NORMAL(T) \ + template VectorMax3 point_line_unnormalized_normal(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template MatrixMax point_line_unnormalized_normal_jacobian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template MatrixMax point_line_unnormalized_normal_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template MatrixMax point_line_normal_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template Eigen::Matrix cross_product_matrix_jacobian(); \ + template Eigen::Matrix triangle_unnormalized_normal_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template Eigen::Matrix triangle_normal_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template Eigen::Matrix line_line_unnormalized_normal_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); \ + template Eigen::Matrix line_line_normal_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>) + +IPC_INSTANTIATE_NORMAL(float); +IPC_INSTANTIATE_NORMAL(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_NORMAL(SimdBatch); +IPC_INSTANTIATE_NORMAL(SimdBatch); +#endif + +#undef IPC_INSTANTIATE_NORMAL // clang-format on } // namespace ipc::detail diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index df4da5755..de821f927 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -214,7 +215,7 @@ namespace detail { Eigen::ConstRef> e0, Eigen::ConstRef> e1) { - return point_line_unnormalized_normal(p, e0, e1).normalized(); + return normalized(point_line_unnormalized_normal(p, e0, e1)); } /// @brief Computes the Jacobian of the unnormalized normal vector of a @@ -303,7 +304,7 @@ namespace detail { Eigen::ConstRef> b, Eigen::ConstRef> c) { - return triangle_unnormalized_normal(a, b, c).normalized(); + return normalized(triangle_unnormalized_normal(a, b, c)); } /// @brief Computes the Jacobian of the unnormalized normal vector of a @@ -403,7 +404,7 @@ namespace detail { Eigen::ConstRef> eb0, Eigen::ConstRef> eb1) { - return line_line_unnormalized_normal(ea0, ea1, eb0, eb1).normalized(); + return normalized(line_line_unnormalized_normal(ea0, ea1, eb0, eb1)); } /// @brief Computes the Jacobian of the unnormalized normal vector of two diff --git a/src/ipc/math/CMakeLists.txt b/src/ipc/math/CMakeLists.txt index b08bec9c8..3715b35bb 100644 --- a/src/ipc/math/CMakeLists.txt +++ b/src/ipc/math/CMakeLists.txt @@ -1,10 +1,12 @@ set(SOURCES + heaviside_type.hpp interval.cpp interval.hpp math.cpp math.hpp math.tpp morton.hpp + scalar_math.hpp ) target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/src/ipc/math/heaviside_type.hpp b/src/ipc/math/heaviside_type.hpp new file mode 100644 index 000000000..b15728872 --- /dev/null +++ b/src/ipc/math/heaviside_type.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +namespace ipc { + +enum class HeavisideType : uint8_t { ZERO = 0, ONE = 1, VARIANT = 2 }; + +struct OrientationTypes { + + static HeavisideType + compute_type(const double val, const double alpha, const double beta); + + int size() const { return m_size; } + void set_size(const int size); + const HeavisideType& tangent_type(const int i) const + { + return tangent_types[i]; + } + const HeavisideType& normal_type(const int i) const + { + return normal_types[i]; + } + HeavisideType& tangent_type(const int i) { return tangent_types[i]; } + HeavisideType& normal_type(const int i) { return normal_types[i]; } + + int m_size = 0; + std::vector tangent_types, normal_types; +}; + +} // namespace ipc diff --git a/src/ipc/math/math.hpp b/src/ipc/math/math.hpp index 64ded490f..18a98bf3b 100644 --- a/src/ipc/math/math.hpp +++ b/src/ipc/math/math.hpp @@ -1,37 +1,15 @@ #pragma once +// Everything here needs Eigen (via eigen_ext.hpp), and math.tpp below pulls in +// the autodiff scalars. Consumers that only want the scalar helpers or the +// Heaviside types should include those headers directly and skip both costs. #include -#include +#include +#include #include namespace ipc { -enum class HeavisideType : uint8_t { ZERO = 0, ONE = 1, VARIANT = 2 }; - -struct OrientationTypes { - - static HeavisideType - compute_type(const double val, const double alpha, const double beta); - - int size() const { return m_size; } - void set_size(const int size); - const HeavisideType& tangent_type(const int i) const - { - return tangent_types[i]; - } - const HeavisideType& normal_type(const int i) const - { - return normal_types[i]; - } - HeavisideType& tangent_type(const int i) { return tangent_types[i]; } - HeavisideType& normal_type(const int i) { return normal_types[i]; } - - int m_size = 0; - std::vector tangent_types, normal_types; -}; - -constexpr double MOLLIFIER_THRESHOLD_EPS = 1e-2; - template struct Math { Math() = delete; Math(const Math&) = delete; @@ -39,9 +17,9 @@ template struct Math { // NOTE: Define these in the class definition to allow inlining static double sign(const double x) { return x >= 0 ? 1.0 : -1.0; } - static T abs(const T& x) { return x >= 0 ? x : -x; } - static T sqr(const T& x) { return x * x; } - static T cubic(const T& x) { return x * x * x; } + static T abs(const T& x) { return ipc::numext::abs(x); } + static T sqr(const T& x) { return ipc::sqr(x); } + static T cubic(const T& x) { return ipc::cubic(x); } static T cubic_spline(const T& x); static double cubic_spline_grad(const double x); diff --git a/src/ipc/math/math.tpp b/src/ipc/math/math.tpp index 71a130b9b..074438833 100644 --- a/src/ipc/math/math.tpp +++ b/src/ipc/math/math.tpp @@ -78,14 +78,14 @@ namespace { template T Math::cubic_spline(const T& x) { - if (abs(x) >= 1) { + if (Math::abs(x) >= 1) { return T(0.); } - if (abs(x) >= 0.5) { - return cubic(1 - abs(x)) * (4. / 3.); + if (Math::abs(x) >= 0.5) { + return cubic(1 - Math::abs(x)) * (4. / 3.); } - return 2. / 3. - 4. * (x * x) * (1 - abs(x)); + return 2. / 3. - 4. * (x * x) * (1 - Math::abs(x)); } template double Math::cubic_spline_grad(const double x) { diff --git a/src/ipc/math/scalar_math.hpp b/src/ipc/math/scalar_math.hpp new file mode 100644 index 000000000..b8cf484ae --- /dev/null +++ b/src/ipc/math/scalar_math.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include + +#include +#include + +// When compiling CUDA device code with NVCC pull in math functions from the +// global namespace. In host mode, and when device code is compiled with clang, +// use the std versions. +#if defined(IPC_TOOLKIT_WITH_CUDA) && defined(__NVCC__) +#define IPC_TOOLKIT_USING_STD(FUNC) using ::FUNC; +#else +#define IPC_TOOLKIT_USING_STD(FUNC) using std::FUNC; +#endif + +namespace ipc { + +/// @brief Portable forwarders to the `std` math functions. +/// +/// These live in their own namespace, following `Eigen::numext`, because they +/// carry the most collision-prone names in C++: at `ipc` scope they join the +/// overload set of anyone writing `using namespace ipc;`, where `ipc::abs` was +/// found to hijack Eigen's array `abs`. They are mechanism rather than API -- +/// each exists only so a template can reach `xsimd::`/autodiff overloads by +/// ADL -- so callers should say `numext::sqrt` explicitly. +namespace numext { + +/// @brief Define `ipc::numext::FUNC` forwarding to the `std` counterpart. +#define IPC_TOOLKIT_DEFINE_STD(FUNC) \ + template < \ + typename T, typename... Ts, \ + typename = std::enable_if_t<(std::is_same_v && ...)>> \ + inline auto FUNC(const T& x, const Ts&... rest) \ + { \ + IPC_TOOLKIT_USING_STD(FUNC) \ + return FUNC(x, rest...); \ + } + + IPC_TOOLKIT_DEFINE_STD(abs) + IPC_TOOLKIT_DEFINE_STD(atan2) + IPC_TOOLKIT_DEFINE_STD(fma) + IPC_TOOLKIT_DEFINE_STD(log) + IPC_TOOLKIT_DEFINE_STD(sqrt) + +// This is a public header, so the macro does not outlive its use here. +#undef IPC_TOOLKIT_DEFINE_STD + +} // namespace numext + +constexpr double MOLLIFIER_THRESHOLD_EPS = 1e-2; + +/// @brief Square of `x`, for any scalar the library templates on. +/// @note Faster than `std::pow(x, 2)`. +template inline T sqr(const T& x) { return x * x; } + +/// @brief Cube of `x`, for any scalar the library templates on. +template inline T cubic(const T& x) { return x * x * x; } + +} // namespace ipc diff --git a/src/ipc/smooth_contact/common.hpp b/src/ipc/smooth_contact/common.hpp index f7390f512..e504caa89 100644 --- a/src/ipc/smooth_contact/common.hpp +++ b/src/ipc/smooth_contact/common.hpp @@ -57,19 +57,19 @@ struct SmoothContactParameters { logger().error( "Parameter 'dhat' must be greater than 0! dhat: {}", dhat); } - if (abs(alpha_t) > 1) { + if (std::abs(alpha_t) > 1) { logger().error( "Parameter 'alpha_t' must be in [-1, 1]! alpha_t: {}", alpha_t); } - if (abs(alpha_n) > 1) { + if (std::abs(alpha_n) > 1) { logger().error( "Parameter 'alpha_n' must be in [-1, 1]! alpha_n: {}", alpha_n); } - if (abs(beta_t) > 1) { + if (std::abs(beta_t) > 1) { logger().error( "Parameter 'beta_t' must be in [-1, 1]! beta_t: {}", beta_t); } - if (abs(beta_n) > 1) { + if (std::abs(beta_n) > 1) { logger().error( "Parameter 'beta_n' must be in [-1, 1]! beta_n: {}", beta_n); } diff --git a/src/ipc/tangent/closest_point.cpp b/src/ipc/tangent/closest_point.cpp index 7919d9b79..a7002076a 100644 --- a/src/ipc/tangent/closest_point.cpp +++ b/src/ipc/tangent/closest_point.cpp @@ -1,5 +1,7 @@ #include "closest_point.hpp" +#include + namespace ipc::autogen { // hess is (6×6) flattened in column-major order template @@ -4387,6 +4389,10 @@ void point_triangle_closest_point_hessian_1( IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN(float); IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN(SimdBatch); +IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN(SimdBatch); +#endif #undef IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN } // namespace ipc::autogen diff --git a/src/ipc/tangent/closest_point.hpp b/src/ipc/tangent/closest_point.hpp index 2fa7570d6..9f5c4603d 100644 --- a/src/ipc/tangent/closest_point.hpp +++ b/src/ipc/tangent/closest_point.hpp @@ -1,8 +1,7 @@ #pragma once #include - -#include +#include #include #include @@ -140,11 +139,71 @@ namespace detail { /// The 1e-10 bound is tuned for double, so scale it by the relative /// precision of T; otherwise the float instantiation would be held to a /// double-precision bound. + /// We go through `scalar_of_t` because a batch has no + /// `std::numeric_limits` specialization, so the bound has to come from its + /// lane type. template inline constexpr double CLOSEST_POINT_RESIDUAL_TOL = 1e-10 - * (static_cast(std::numeric_limits::epsilon()) + * (static_cast(std::numeric_limits>::epsilon()) / std::numeric_limits::epsilon()); + /// @brief Solves `Ax = b` for a 2x2 symmetric positive-definite matrix `A`. + /// + /// We implement this manually using Cramer's rule instead of + /// `A.ldlt().solve(b)` because Eigen's LDLT uses pivoting, which introduces + /// branching and breaks vectorization. Testing shows Cramer's rule provides + /// comparable accuracy to both Eigen's and manual LDLT implementations, as + /// accuracy is limited by `A`'s conditioning rather than the algorithm. + /// Thus, Cramer's rule wins by being completely branchless. + /// + /// `A` is a Gram matrix (`basis · basisᵀ`), so it is positive semidefinite. + /// A computed `det(A) <= 0` indicates rounding error on a singular matrix + /// (e.g., degenerate geometry). To avoid NaNs, we treat this as singular + /// and branchlessly return 0 (matching Eigen's LDLT pseudo-inverse + /// behavior). Internal callers exclude degenerate cases and will never hit + /// this path. + /// + /// Debug builds check the solve residual using a relative bound (since + /// Cramer's rule is not backward stable), exempting these purposely zeroed + /// singular systems. + /// + /// @tparam T The scalar type. + /// @param A The 2x2 SPD matrix. + /// @param b The right-hand side vector. + /// @return The solution vector `x`. + template + inline Eigen::Vector2 solve_spd_2x2( + Eigen::ConstRef> A, + Eigen::ConstRef> b) + { + // Kahan's 2x2 determinant. The naive `a00*a11 - a01*a10` cancels + // catastrophically as the edges approach parallel, which is exactly + // where we care: both products round to nearly the same number and the + // subtraction keeps only the low bits, so the relative error in `det` + // grows with the conditioning. Here `bc` is the rounded product and + // `bc_err` the error that rounding dropped, which `fma` recovers + // because it rounds once instead of twice. Adding it back gives a + // `det` good to a couple of ulp however badly the two products cancel. + // + // On an architecture with no hardware FMA, xsimd falls back to a plain + // `x * y + z`; `bc_err` is then zero and this degrades to the naive + // expression rather than misbehaving. + const T bc = A(0, 1) * A(1, 0); + const T bc_err = ipc::numext::fma(A(0, 1), A(1, 0), -bc); + const T det = ipc::numext::fma(A(0, 0), A(1, 1), -bc) - bc_err; + const auto is_nonsingular = det > T(0); + const T inv_det = select(is_nonsingular, T(1) / det, T(0)); + const Eigen::Vector2 x( + (A(1, 1) * b[0] - A(0, 1) * b[1]) * inv_det, + (A(0, 0) * b[1] - A(1, 0) * b[0]) * inv_det); +#ifndef NDEBUG + const T scale = A.norm() * x.norm() + b.norm(); + const T tol = literal(CLOSEST_POINT_RESIDUAL_TOL); + assert(all_of(det <= T(0) || (A * x - b).norm() <= tol * scale)); +#endif + return x; + } + // ======================================================================== // Point - Edge @@ -251,9 +310,7 @@ namespace detail { rhs[0] = -eb_to_ea.dot(ea); rhs[1] = eb_to_ea.dot(eb); - const Eigen::Vector2 x = A.ldlt().solve(rhs); - assert((A * x - rhs).norm() < CLOSEST_POINT_RESIDUAL_TOL); - return x; + return solve_spd_2x2(A, rhs); } /// @brief Compute the Jacobian of the closest points between two edges. @@ -324,9 +381,7 @@ namespace detail { basis.row(1) = Eigen::RowVector3(t2 - t0); // edge 1 const Eigen::Matrix2 A = basis * basis.transpose(); const Eigen::Vector2 b = basis * (p - t0); - const Eigen::Vector2 x = A.ldlt().solve(b); - assert((A * x - b).norm() < CLOSEST_POINT_RESIDUAL_TOL); - return x; + return solve_spd_2x2(A, b); } /// @brief Compute the Jacobian of the closest point on the triangle. diff --git a/src/ipc/tangent/tangent_basis.cpp b/src/ipc/tangent/tangent_basis.cpp index 762117cd5..182bba771 100644 --- a/src/ipc/tangent/tangent_basis.cpp +++ b/src/ipc/tangent/tangent_basis.cpp @@ -1,8 +1,8 @@ #include "tangent_basis.hpp" -#include +#include -#include +#include namespace ipc::detail { @@ -111,6 +111,12 @@ IPC_INSTANTIATE_TANGENT_BASIS_ND(float, 2); IPC_INSTANTIATE_TANGENT_BASIS_ND(float, 3); IPC_INSTANTIATE_TANGENT_BASIS_ND(double, 2); IPC_INSTANTIATE_TANGENT_BASIS_ND(double, 3); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_TANGENT_BASIS_ND(SimdBatch, 2); +IPC_INSTANTIATE_TANGENT_BASIS_ND(SimdBatch, 3); +IPC_INSTANTIATE_TANGENT_BASIS_ND(SimdBatch, 2); +IPC_INSTANTIATE_TANGENT_BASIS_ND(SimdBatch, 3); +#endif #undef IPC_INSTANTIATE_TANGENT_BASIS_ND @@ -138,6 +144,10 @@ IPC_INSTANTIATE_TANGENT_BASIS_ND(double, 3); IPC_INSTANTIATE_TANGENT_BASIS_3D(float); IPC_INSTANTIATE_TANGENT_BASIS_3D(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_TANGENT_BASIS_3D(SimdBatch); +IPC_INSTANTIATE_TANGENT_BASIS_3D(SimdBatch); +#endif #undef IPC_INSTANTIATE_TANGENT_BASIS_3D @@ -149,7 +159,10 @@ namespace { /// @brief Compute the power of 1.5 of a number. /// @param x Number to compute the power of 1.5 /// @return x^(1.5) - template inline T pow_1_5(T x) { return x * std::sqrt(x); } + template inline T pow_1_5(T x) + { + return x * ipc::numext::sqrt(x); + } } // namespace // J is (2×4) flattened in column-major order @@ -164,7 +177,7 @@ void point_point_tangent_basis_2D_jacobian( const T t4 = t2 + t3; const T t5 = t0 * t1 / pow_1_5(t4); const T t6 = -t5; - const T t7 = T(1) / std::sqrt(t4); + const T t7 = T(1) / ipc::numext::sqrt(t4); const T t8 = T(1) / t4; const T t9 = t2 * t8 - 1; const T t10 = t3 * t8 - 1; @@ -189,65 +202,66 @@ void point_point_tangent_basis_3D_jacobian( const T t3 = t2 * t2; const T t4 = p0_y - p1_y; const T t5 = t4 * t4; - const bool t6 = (t1 + t3) < (t3 + t5); + const auto t6 = (t1 + t3) < (t3 + t5); const T t7 = -t2; - const T t8 = t6 ? 0 : t7; - const T t9 = (t6 ? 0 : t3) + (t6 ? t3 : 0) + (t6 ? t5 : t1); + const T t8 = select(t6, T(0), t7); + const T t9 = + select(t6, T(0), t3) + select(t6, t3, T(0)) + select(t6, t5, t1); const T t10 = T(1) / pow_1_5(t9); - const T t11 = T(0.5) * (t6 ? 0 : (2 * t0)); + const T t11 = T(0.5) * select(t6, T(0), T(2) * t0); const T t12 = t10 * t11; - const T t13 = t6 ? t2 : 0; - const T t14 = T(1) / std::sqrt(t9); - const T t15 = t6 ? 0 : 1; + const T t13 = select(t6, t2, T(0)); + const T t14 = T(1) / ipc::numext::sqrt(t9); + const T t15 = select(t6, T(0), T(1)); const T t16 = -t4; - const T t17 = t6 ? t16 : t0; + const T t17 = select(t6, t16, t0); const T t18 = T(1) / t9; const T t19 = t17 * t18; const T t20 = t0 * t13 - t4 * t8; const T t21 = -t20; - const bool t22 = t1 + (t7 * t7) < (t16 * t16) + t3; - const T t23 = t22 ? 0 : t7; + const auto t22 = t1 + (t7 * t7) < (t16 * t16) + t3; + const T t23 = select(t22, T(0), t7); const T t24 = -t0; - const T t25 = t22 ? t16 : t0; + const T t25 = select(t22, t16, t0); const T t26 = -t23 * t7 + t24 * t25; const T t27 = -t13 * t2 + t17 * t4; const T t28 = -t27; const T t29 = t21 * t21 + t26 * t26 + t28 * t28; - const T t30 = T(1) / std::sqrt(t29); + const T t30 = T(1) / ipc::numext::sqrt(t29); const T t31 = t15 * t4; const T t32 = T(1) / t29; - const T t33 = t22 ? t2 : 0; + const T t33 = select(t22, t2, T(0)); const T t34 = -t16 * t23 + t24 * t33; const T t35 = t33 * t34; const T t36 = t16 * t25 - t33 * t7; - const T t37 = t22 ? 0 : 1; + const T t37 = select(t22, T(0), T(1)); const T t38 = t16 * t37; const T t39 = t24 * t37; const T t40 = -t25; const T t41 = -t26; const T t42 = t32 * (-t35 + t36 * t38 + t41 * (-t39 - t40)); - const T t43 = T(0.5) * (t6 ? (2 * t4) : 0); + const T t43 = T(0.5) * select(t6, T(2) * t4, T(0)); const T t44 = t10 * t43; - const T t45 = t6 ? -1 : 0; + const T t45 = select(t6, T(-1), T(0)); const T t46 = -t0 * t17 + t2 * t8; const T t47 = t20 * t20 + t27 * t27 + t46 * t46; - const T t48 = T(1) / std::sqrt(t47); + const T t48 = T(1) / ipc::numext::sqrt(t47); const T t49 = T(1) / t47; const T t50 = t0 * t45; const T t51 = t17 + t4 * t45; - const T t52 = t22 ? (-1) : 0; + const T t52 = select(t22, T(-1), T(0)); const T t53 = t24 * t52; const T t54 = t32 * (t23 * t34 + t36 * (t16 * t52 + t40) - t41 * t53); const T t55 = -t8; - const T t56 = t6 ? 0 : -1; + const T t56 = select(t6, T(0), T(-1)); const T t57 = T(0.5) * t8; const T t58 = 2 * t2; - const T t59 = t18 * ((t22 ? 0 : t58) + (t22 ? t58 : 0)); - const T t60 = t6 ? 1 : 0; + const T t59 = t18 * (select(t22, T(0), t58) + select(t22, t58, T(0))); + const T t60 = select(t6, T(1), T(0)); const T t61 = T(0.5) * t13; const T t62 = T(0.5) * t10 * t17; - const T t63 = t22 ? 1 : 0; - const T t64 = t22 ? 0 : -1; + const T t63 = select(t22, T(1), T(0)); + const T t64 = select(t22, T(0), T(-1)); const T t65 = t16 * t64; const T t66 = -t33 + t63 * t7; const T t67 = t28 * t32; @@ -255,18 +269,18 @@ void point_point_tangent_basis_3D_jacobian( const T t69 = t2 * t56 + t8; const T t70 = t21 * (t4 * t56 - t68) + t26 * t69 - t28 * t66; const T t71 = t4 * t56; - const T t72 = t6 ? 0 : (2 * t24); + const T t72 = select(t6, T(0), T(2) * t24); const T t73 = t10 * t72; const T t74 = T(0.5) * t19; const T t75 = t24 * t64 + t25; const T t76 = t35 + t36 * t65 - t41 * t75; - const T t77 = t6 ? (2 * t16) : 0; + const T t77 = select(t6, T(2) * t16, T(0)); const T t78 = t10 * t77; const T t79 = -t17 + t4 * t60; const T t80 = t0 * t46 * t60 - t20 * t8 - t27 * t79; const T t81 = t20 * t49; const T t82 = 2 * t7; - const T t83 = t18 * ((t22 ? 0 : t82) + (t22 ? t82 : 0)); + const T t83 = t18 * (select(t22, T(0), t82) + select(t22, t82, T(0))); const T t84 = t33 + t52 * t7; const T t85 = t15 * t2; const T t86 = t21 * (t15 * t4 - t50) - t26 * (-t55 - t85) - t28 * t84; @@ -284,7 +298,7 @@ void point_point_tangent_basis_3D_jacobian( J[11] = t30 * (-t21 * t54 - t55); J[12] = t14 * (t56 - t57 * t59); J[13] = t14 * (-t59 * t61 + t60); - J[14] = -t62 * ((t6 ? 0 : t58) + (t6 ? t58 : 0)); + J[14] = -t62 * (select(t6, T(0), t58) + select(t6, t58, T(0))); J[15] = -t30 * (t66 + t67 @@ -306,7 +320,7 @@ void point_point_tangent_basis_3D_jacobian( J[29] = -t48 * (t8 + t80 * t81); J[30] = t14 * (t15 - t57 * t83); J[31] = t14 * (t45 - t61 * t83); - J[32] = -t62 * ((t6 ? 0 : t82) + (t6 ? t82 : 0)); + J[32] = -t62 * (select(t6, T(0), t82) + select(t6, t82, T(0))); J[33] = -t30 * (t67 * (t34 * (-t38 + t53) - t36 * t84 + t41 * (t23 + t37 * t7)) + t84); @@ -324,10 +338,10 @@ void point_edge_tangent_basis_2D_jacobian( const T t2 = e0_y - e1_y; const T t3 = t2 * t2; const T t4 = t1 + t3; - const T t5 = T(1) / std::sqrt(t4); + const T t5 = T(1) / ipc::numext::sqrt(t4); const T t6 = T(1) / t4; const T t7 = t1 * t6 - 1; - const T t8 = t0 * t2 / (t4 * std::sqrt(t4)); + const T t8 = t0 * t2 / (t4 * ipc::numext::sqrt(t4)); const T t9 = t3 * t6 - 1; const T t10 = -t8; J[0] = 0; @@ -384,7 +398,7 @@ void point_edge_tangent_basis_3D_jacobian( const T t23 = T(1) / pow_1_5(t22); const T t24 = t19 * t23; const T t25 = t17 * t17 + t21; - const T t26 = T(1) / std::sqrt(t25); + const T t26 = T(1) / ipc::numext::sqrt(t25); const T t27 = -t5; const T t28 = -t1; const T t29 = t11 * t27 - t15 * t28; @@ -393,7 +407,7 @@ void point_edge_tangent_basis_3D_jacobian( const T t32 = t17 * t31; const T t33 = -e0_z; const T t34 = e1_z + t33; - const T t35 = T(1) / std::sqrt(t22); + const T t35 = T(1) / ipc::numext::sqrt(t22); const T t36 = -e0_y; const T t37 = T(1) / t22; const T t38 = t37 * t8; @@ -411,7 +425,7 @@ void point_edge_tangent_basis_3D_jacobian( const T t50 = t1 * t1; const T t51 = t10 * t10; const T t52 = t49 + t50 + t51; - const T t53 = T(1) / std::sqrt(t52); + const T t53 = T(1) / ipc::numext::sqrt(t52); const T t54 = T(1) / t52; const T t55 = t49 * t54 - 1; const T t56 = T(1) / pow_1_5(t52); @@ -516,7 +530,7 @@ void edge_edge_tangent_basis_jacobian( const T t5 = t4 * t4; const T t6 = t3 + t5; const T t7 = t1 + t6; - const T t8 = T(1) / std::sqrt(t7); + const T t8 = T(1) / ipc::numext::sqrt(t7); const T t9 = T(1) / t7; const T t10 = t3 * t9 - 1; const T t11 = T(1) / pow_1_5(t7); @@ -558,7 +572,7 @@ void edge_edge_tangent_basis_jacobian( const T t47 = t44 + t46; const T t48 = -t20 * t26 + t27 * t47; const T t49 = t33 * t33 + t43 * t43 + t48 * t48; - const T t50 = T(1) / std::sqrt(t49); + const T t50 = T(1) / ipc::numext::sqrt(t49); const T t51 = t27 * t29; const T t52 = -t51; const T t53 = T(1) / t49; @@ -573,7 +587,7 @@ void edge_edge_tangent_basis_jacobian( const T t61 = t0 * t41 + t2 * t25; const T t62 = t0 * t37 + t25 * t4; const T t63 = t42 * t42 + t61 * t61 + t62 * t62; - const T t64 = T(1) / std::sqrt(t63); + const T t64 = T(1) / ipc::numext::sqrt(t63); const T t65 = 2 * t34; const T t66 = t36 + t65; const T t67 = T(1) / t63; @@ -743,7 +757,7 @@ void point_triangle_tangent_basis_jacobian( const T t5 = t4 * t4; const T t6 = t3 + t5; const T t7 = t1 + t6; - const T t8 = (T(1) / std::sqrt(t7)); + const T t8 = (T(1) / ipc::numext::sqrt(t7)); const T t9 = T(1) / t7; const T t10 = t3 * t9 - 1; const T t11 = T(1) / pow_1_5(t7); @@ -786,7 +800,7 @@ void point_triangle_tangent_basis_jacobian( const T t48 = t45 + t47; const T t49 = -t24 * t27 + t28 * t48; const T t50 = t35 * t35 + t44 * t44 + t49 * t49; - const T t51 = T(1) / std::sqrt(t50); + const T t51 = T(1) / ipc::numext::sqrt(t50); const T t52 = t17 + t1_z; const T t53 = -t52; const T t54 = t1_y + t30; @@ -803,7 +817,7 @@ void point_triangle_tangent_basis_jacobian( const T t65 = t0 * t42 + t2 * t26; const T t66 = t0 * t39 + t26 * t4; const T t67 = t43 * t43 + t65 * t65 + t66 * t66; - const T t68 = T(1) / std::sqrt(t67); + const T t68 = T(1) / ipc::numext::sqrt(t67); const T t69 = t18 * t2; const T t70 = t22 * t4; const T t71 = -t70; @@ -946,6 +960,10 @@ void point_triangle_tangent_basis_jacobian( IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN(float); IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN(double); +#ifdef IPC_TOOLKIT_WITH_SIMD +IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN(SimdBatch); +IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN(SimdBatch); +#endif #undef IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN diff --git a/src/ipc/tangent/tangent_basis.hpp b/src/ipc/tangent/tangent_basis.hpp index 8454bb4f4..14ba361ac 100644 --- a/src/ipc/tangent/tangent_basis.hpp +++ b/src/ipc/tangent/tangent_basis.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include @@ -25,7 +26,7 @@ namespace detail { dim == 2 || dim == 3, "point-point tangent basis is only 2D or 3D"); if constexpr (dim == 2) { - const Eigen::Vector2 p0_to_p1 = (p1 - p0).normalized(); + const Eigen::Vector2 p0_to_p1 = normalized(p1 - p0); return Eigen::Vector2(-p0_to_p1.y(), p0_to_p1.x()); } else { const Eigen::Vector3 p0_to_p1 = p1 - p0; @@ -35,8 +36,23 @@ namespace detail { const Eigen::Vector3 cross_y = Eigen::Vector3::UnitY().cross(p0_to_p1); + // Prefer whichever reference axis is least parallel to the pair. + // A batch cannot answer that with one bool, so it builds both + // bases and blends them per-lane. Eigen::Matrix basis; - if (cross_x.squaredNorm() > cross_y.squaredNorm()) { + if constexpr (is_simd_batch_v) { + Eigen::Matrix basis_x, basis_y; + basis_x.col(0) = normalized(cross_x); + basis_x.col(1) = normalized(p0_to_p1.cross(cross_x)); + basis_y.col(0) = normalized(cross_y); + basis_y.col(1) = normalized(p0_to_p1.cross(cross_y)); + + const auto prefer_x = + cross_x.squaredNorm() > cross_y.squaredNorm(); + for (Eigen::Index i = 0; i < basis.size(); ++i) { + basis(i) = select(prefer_x, basis_x(i), basis_y(i)); + } + } else if (cross_x.squaredNorm() > cross_y.squaredNorm()) { basis.col(0) = cross_x.normalized(); basis.col(1) = p0_to_p1.cross(cross_x).normalized(); } else { @@ -80,13 +96,13 @@ namespace detail { dim == 2 || dim == 3, "point-edge tangent basis is only 2D or 3D"); if constexpr (dim == 2) { - return (e1 - e0).normalized(); + return normalized(e1 - e0); } else { const Eigen::Vector3 e = e1 - e0; Eigen::Matrix basis; - basis.col(0) = e.normalized(); - basis.col(1) = e.cross(Eigen::Vector3(p - e0)).normalized(); + basis.col(0) = normalized(e); + basis.col(1) = normalized(e.cross(p - e0)); return basis; } } @@ -124,14 +140,16 @@ namespace detail { const Eigen::Vector3 ea = ea1 - ea0; // Edge A direction const Eigen::Vector3 normal = ea.cross(eb1 - eb0); // The normal will be zero if the edges are parallel (i.e. coplanar). - assert(normal.norm() != 0); + if constexpr (std::is_floating_point_v) { + assert(normal.norm() != 0); + } Eigen::Matrix basis; // The first basis vector is along edge A. - basis.col(0) = ea.normalized(); + basis.col(0) = normalized(ea); // The second basis vector is orthogonal to the first and the edge-edge // normal. - basis.col(1) = normal.cross(ea).normalized(); + basis.col(1) = normalized(normal.cross(ea)); return basis; } @@ -168,15 +186,17 @@ namespace detail { { const Eigen::Vector3 e0 = t1 - t0; const Eigen::Vector3 normal = e0.cross(t2 - t0); - assert(normal.norm() != 0); + if constexpr (std::is_floating_point_v) { + assert(normal.norm() != 0); + } Eigen::Matrix basis; // The first basis vector is along first edge of the triangle. - basis.col(0) = e0.normalized(); + basis.col(0) = normalized(e0); // The second basis vector is orthogonal to the first and the triangle // normal. - basis.col(1) = normal.cross(e0).normalized(); + basis.col(1) = normalized(normal.cross(e0)); return basis; } diff --git a/src/ipc/utils/CMakeLists.txt b/src/ipc/utils/CMakeLists.txt index cb6cd1c4a..ed1a23faf 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -16,6 +16,7 @@ set(SOURCES merge_thread_local.hpp profiler.cpp profiler.hpp + simd.hpp save_obj.cpp save_obj.hpp unordered_map_and_set.cpp diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 5f6a194b6..8b00f0e6e 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -4,14 +4,6 @@ #include #include -#include - -#ifdef EIGEN_DONT_VECTORIZE -// NOTE: Avoid error about abs casting double to int. Eigen does this -// internally but seemingly only if EIGEN_DONT_VECTORIZE is not defined. -// TODO: We should always use std::abs to avoid this issue. -EIGEN_USING_STD(abs) // using std::abs; -#endif namespace Eigen { template using RowRef = Ref>; diff --git a/src/ipc/utils/simd.hpp b/src/ipc/utils/simd.hpp new file mode 100644 index 000000000..a731f960a --- /dev/null +++ b/src/ipc/utils/simd.hpp @@ -0,0 +1,232 @@ +#pragma once + +#include +#include + +#include + +#include +#include +#include + +namespace ipc { + +/// @brief Whether `T` is an `xsimd::batch`, i.e. a per-lane scalar with no +/// single `bool` value: comparisons and reductions like `isZero()` cannot be +/// used in scalar control flow (`if`, `assert`) for such a `T`. +/// +/// Specialized below, where the batch type itself is available. +template inline constexpr bool is_simd_batch_v = false; + +/// @brief The scalar behind `T`: `T` itself, or a batch's lane type. +/// +/// We need this because a batch has no `std::numeric_limits` specialization. +/// Whenever we want a scalar's limits (an epsilon, an infinity) for a type we +/// template on, we have to ask the lane type instead of `T` itself. +template > struct ScalarOf { + using type = T; +}; +template struct ScalarOf { + using type = typename T::value_type; +}; +template using scalar_of_t = typename ScalarOf::type; + +/// @brief `T(c)` for a constant written as a `double` literal. +/// +/// We round `c` to the lane type first. For a `float` batch, `T(0.4)` is a +/// constructor call receiving a `double`, an implicit narrowing that +/// `-Wfloat-conversion` reports at every instantiation. For a plain scalar this +/// is the explicit cast the code would have written anyway. +template inline T literal(const double c) +{ + return T(static_cast>(c)); +} + +/// @brief Whether `mask` holds for every lane. +/// +/// This is the scalar counterpart of `xsimd::all`. A batch answers a +/// comparison per-lane, but an `assert` needs a single `bool`, so `all_of` +/// collapses the two cases and lets one check compile for both. +/// +/// We overload on the two argument types rather than relying on ADL to find +/// `xsimd::all`. The tradeoff is a little duplication in exchange for keeping +/// a name this generic from matching arbitrary types elsewhere in `ipc`. +inline bool all_of(const bool mask) { return mask; } + +/// @brief Pick between `a` and `b`. +/// +/// The scalar counterpart of `xsimd::select`, which ADL finds for a batch +/// `mask`, so one `select(cond, a, b)` compiles for both. +template inline T select(const bool mask, const T& a, const T& b) +{ + return mask ? a : b; +} + +/// @brief `+infinity` for any scalar the library templates on. +template inline T infinity() +{ + return T(std::numeric_limits>::infinity()); +} + +/// @brief A first-match-wins cascade of cases: +/// `select_lazy(mask1, value1, mask2, value2, ..., else_value)`. +/// +/// Each value is a callable, which is what lets the two scalar/batch +/// semantics share one definition: +/// +/// - For a scalar `T`, this is an `if`/`else if` chain: only the first +/// matching value is evaluated. A case guarded by `d > 0` can therefore +/// hold a `log(d)` safely. +/// - For a batch, no mask has one answer, so *every* value is evaluated and +/// blended per-lane. One batch may carry lanes in different cases. A value +/// may be evaluated on lanes its mask excludes -- producing a NaN that is +/// then blended away -- so it must not have side effects. +/// +/// The masks are ordinary arguments, so unlike the values they are all +/// evaluated before the call, whichever case wins. Keep them to comparisons. +/// +/// @warning Earlier cases take precedence in both paths, which for the batch +/// comes from the order the blend is folded. Masks may overlap, and only that +/// order decides the winner, so a test must cover lanes that fall in +/// overlapping cases. +template inline auto select_lazy(F&& else_value) +{ + return else_value(); +} + +template +inline auto select_lazy(const Mask& mask, F&& value, Rest&&... rest) +{ + if constexpr (std::is_same_v, bool>) { + return mask ? value() : select_lazy(std::forward(rest)...); + } else { + return select(mask, value(), select_lazy(std::forward(rest)...)); + } +} + +} // namespace ipc + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +namespace Eigen { + +/// @brief Let Eigen treat an xsimd batch as a scalar type. +/// +/// This makes ``Eigen::Vector3>`` a valid argument to +/// the distance functions, evaluating one independent problem per SIMD lane. +template +struct NumTraits> : GenericNumTraits> { + using Real = xsimd::batch; + using NonInteger = xsimd::batch; + using Nested = xsimd::batch; + using Literal = xsimd::batch; + + // NOLINTBEGIN(readability-identifier-naming,performance-enum-size) + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned = 1, + // A batch is not trivially default-constructible in the sense Eigen + // means; make it initialize its coefficients. + RequireInitialization = 1, + ReadCost = 1, + AddCost = 1, + MulCost = 1 + }; + // NOLINTEND(readability-identifier-naming,performance-enum-size) + + static Real epsilon() { return Real(NumTraits::epsilon()); } + static Real dummy_precision() + { + return Real(NumTraits::dummy_precision()); + } + static int digits10() { return NumTraits::digits10(); } + static Real highest() { return Real(NumTraits::highest()); } + static Real lowest() { return Real(NumTraits::lowest()); } +}; + +} // namespace Eigen + +namespace ipc { + +/// @brief A SIMD batch of ``T``, usable as the scalar type of a distance query. +/// +/// Packing one independent problem per lane (a structure-of-arrays layout) +/// lets a single call evaluate ``SimdBatch::size`` problems at once: +/// +/// @code +/// Eigen::Vector3> p, e0, e1; +/// for (int k = 0; k < 3; ++k) { +/// p[k] = ipc::SimdBatch::load_unaligned(&p_soa[k * n + i]); +/// // ... likewise for e0, e1 +/// } +/// const auto d = ipc::point_edge_distance(p, e0, e1, +/// PointEdgeDistanceType::P_E); +/// @endcode +/// +/// @warning **An explicit distance type is required.** ``AUTO`` and the +/// ``*_distance_type`` predicates are not available for batch scalars, and +/// throw ``std::invalid_argument`` if reached: the distance type is a per-lane +/// property, but the predicates return a single enum, so two lanes of the same +/// batch cannot report different closest features. Resolve the distance types +/// scalar-side and group the problems by type before batching, which is the +/// natural structure-of-arrays layout anyway. +/// +/// @note Functions that select a case from the values themselves — clamping a +/// barrier at ``d̂``, choosing a tangent-basis reference axis — evaluate every +/// case for a batch and blend the results per-lane, so a batch can carry lanes +/// that fall in different cases. The ``*_distance_type`` predicates are the +/// exception above: their result is an enum, not a number to blend. +/// +/// @warning ``xsimd::default_arch`` is chosen from the compiler flags of each +/// translation unit, so a caller compiled without the SIMD flags the library +/// was built with names a *different* type than the one instantiated here, and +/// will fail to link. Build such callers with the same flags — CMake reports +/// them as ``SIMD_CXX_FLAGS``. +template using SimdBatch = xsimd::batch; + +template +inline constexpr bool is_simd_batch_v> = true; + +/// @brief Whether `mask` holds for every lane of a batch. +template +inline bool all_of(const xsimd::batch_bool& mask) +{ + return xsimd::all(mask); +} + +} // namespace ipc + +#endif + +namespace ipc { + +/// @brief `v.normalized()`, but also defined for a batch scalar. +/// +/// Eigen's own `normalized()` leaves a zero-length vector unscaled behind an +/// `if (squaredNorm() > 0)`, which a batch cannot answer with one bool. This +/// applies that same rule per-lane and otherwise defers to Eigen. +template +inline typename Derived::PlainObject +normalized(const Eigen::MatrixBase& v) +{ + using T = typename Derived::Scalar; + if constexpr (is_simd_batch_v) { + const typename Derived::PlainObject n = v.derived(); + const T z = n.squaredNorm(); + const typename Derived::PlainObject scaled = n / ipc::numext::sqrt(z); + const auto is_nonzero = z > T(0); + + typename Derived::PlainObject out = n; + for (Eigen::Index i = 0; i < out.size(); ++i) { + out(i) = select(is_nonzero, scaled(i), n(i)); + } + return out; + } else { + return v.normalized(); + } +} + +} // namespace ipc diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 886476759..716cb3c87 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -102,6 +102,23 @@ target_link_libraries(ipc_toolkit_tests PRIVATE ipc::toolkit::warnings) target_compile_definitions(ipc_toolkit_tests PRIVATE CATCH_CONFIG_ENABLE_BENCHMARKING) +# Code coverage +# +# The library sets these on itself privately; we repeat them here rather than +# inherit them, so instrumentation reaches the tests without every consumer of +# ipc::toolkit picking it up too. This is needed because much of the library is +# header-only templates: a template is compiled in whichever translation unit +# instantiates it, so a header only ever called from a test would otherwise +# have no counters anywhere and read as untested. The linker flags do come from +# ipc::toolkit, which applies them PUBLIC. +if(IPC_TOOLKIT_WITH_CODE_COVERAGE AND CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") + target_compile_options(ipc_toolkit_tests PRIVATE + -g # generate debug info + --coverage # sets all required flags + -fprofile-update=atomic + ) +endif() + if (FILIB_BUILD_SHARED_LIB AND WIN32) # Copy DLLs to the output directory add_custom_command( diff --git a/tests/src/tests/CMakeLists.txt b/tests/src/tests/CMakeLists.txt index 6ac22663d..cca3ed913 100644 --- a/tests/src/tests/CMakeLists.txt +++ b/tests/src/tests/CMakeLists.txt @@ -13,6 +13,7 @@ set(SOURCES benchmark_eigen.cpp # Utilities + simd_utils.hpp utils.cpp utils.hpp ) diff --git a/tests/src/tests/adhesion/CMakeLists.txt b/tests/src/tests/adhesion/CMakeLists.txt index 8a4d34f38..ee9fb88b8 100644 --- a/tests/src/tests/adhesion/CMakeLists.txt +++ b/tests/src/tests/adhesion/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES # Tests test_adhesion.cpp + test_simd_adhesion.cpp # Benchmarks diff --git a/tests/src/tests/adhesion/test_simd_adhesion.cpp b/tests/src/tests/adhesion/test_simd_adhesion.cpp new file mode 100644 index 000000000..b6e1d72e0 --- /dev/null +++ b/tests/src/tests/adhesion/test_simd_adhesion.cpp @@ -0,0 +1,131 @@ +#include +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +#include +#include + +using namespace ipc; +using namespace ipc::tests; + +namespace { + +/// @brief Speeds as multiples of eps_a, landing a lane in each piece: at rest, +/// either side of the half-threshold the smooth-μ formulas split on, just +/// inside, exactly at, and past the threshold. +constexpr std::array NONNEGATIVE_MULTIPLES = { + { 0.0, 0.25, 0.49, 0.5, 0.75, 1.0, 2.5 } +}; + +/// @brief The same speeds plus a negative one. The tangential adhesion +/// functions clamp at y <= 0 rather than mirroring on |y|, so the negative +/// entry checks that clamp. +constexpr std::array MULTIPLES = { { -1.0, 0.0, 0.25, 0.49, 0.5, + 0.75, 1.0, 2.5 } }; + +} // namespace + +TEST_CASE( + "SIMD batch normal adhesion matches the scalar one lane-wise", + "[adhesion][normal_adhesion][simd]") +{ + constexpr double DHAT_P = 1e-3; + constexpr double DHAT_A = 2e-3; + const double max_slope = GENERATE(-1.0, -1e3); + + // Distances landing in each piece: the quadratic below d̂ₚ, the second + // quadratic between d̂ₚ and d̂ₐ, and the inactive region past d̂ₐ -- plus the + // two breakpoints themselves, where the pieces must agree. + constexpr std::array DS = { { 0.0, 0.5e-3, DHAT_P, 1.5e-3, + DHAT_A, 3e-3, 1.0 } }; + + auto check = [&](const std::string& name, auto&& f) { + check_swept_lanes_with(name, DS, f, DHAT_P, DHAT_A, max_slope); + }; + + check("potential", [](auto d, auto dhat_p, auto dhat_a, auto a2) { + return normal_adhesion_potential(d, dhat_p, dhat_a, a2); + }); + check("first_derivative", [](auto d, auto dhat_p, auto dhat_a, auto a2) { + return normal_adhesion_potential_first_derivative( + d, dhat_p, dhat_a, a2); + }); + check("second_derivative", [](auto d, auto dhat_p, auto dhat_a, auto a2) { + return normal_adhesion_potential_second_derivative( + d, dhat_p, dhat_a, a2); + }); +} + +TEST_CASE( + "SIMD batch tangential adhesion matches the scalar one lane-wise", + "[adhesion][tangential_adhesion][simd]") +{ + const double eps_a = GENERATE(1e-3, 0.1, 1.0); + + auto check = [&](const std::string& name, const auto& ys, auto&& f) { + check_swept_lanes_with(name, ys, f, eps_a); + }; + + const auto ys = scaled(MULTIPLES, eps_a); + check( + "f0", ys, [](auto y, auto e) { return tangential_adhesion_f0(y, e); }); + check( + "f1", ys, [](auto y, auto e) { return tangential_adhesion_f1(y, e); }); + check( + "f2", ys, [](auto y, auto e) { return tangential_adhesion_f2(y, e); }); + check("f1_over_x", ys, [](auto y, auto e) { + return tangential_adhesion_f1_over_x(y, e); + }); + + // This one asserts y >= 0, so it gets the non-negative speeds only. Zero is + // where its `1/y` branch is singular: the scalar path returns -inf there, + // and the batch, which evaluates that branch on every lane, has to agree. + check( + "f2_x_minus_f1_over_x3", scaled(NONNEGATIVE_MULTIPLES, eps_a), + [](auto y, auto e) { + return tangential_adhesion_f2_x_minus_f1_over_x3(y, e); + }); +} + +TEST_CASE( + "SIMD batch smooth mu adhesion variants match the scalar ones lane-wise", + "[adhesion][smooth_mu][simd]") +{ + const double eps_a = GENERATE(1e-3, 0.1, 1.0); + + // The equal-coefficient pair is the fast path these functions short-circuit + // on, and it has to agree with the general formulas it skips. + const auto mus = GENERATE( + std::pair { 0.5, 0.5 }, std::pair { 0.5, 0.1 }, std::pair { 0.1, 0.5 }); + const double mu_s = mus.first, mu_k = mus.second; + + // Non-negative only: smooth_mu_a2_x_minus_mu_a1_over_x3 asserts y >= 0. + const auto ys = scaled(NONNEGATIVE_MULTIPLES, eps_a); + + auto check = [&](const std::string& name, auto&& f) { + check_swept_lanes_with(name, ys, f, mu_s, mu_k, eps_a); + }; + + check("a0", [](auto y, auto s, auto k, auto e) { + return smooth_mu_a0(y, s, k, e); + }); + check("a1", [](auto y, auto s, auto k, auto e) { + return smooth_mu_a1(y, s, k, e); + }); + check("a2", [](auto y, auto s, auto k, auto e) { + return smooth_mu_a2(y, s, k, e); + }); + check("a1_over_x", [](auto y, auto s, auto k, auto e) { + return smooth_mu_a1_over_x(y, s, k, e); + }); + check("a2_x_minus_mu_a1_over_x3", [](auto y, auto s, auto k, auto e) { + return smooth_mu_a2_x_minus_mu_a1_over_x3(y, s, k, e); + }); +} + +#endif diff --git a/tests/src/tests/barrier/CMakeLists.txt b/tests/src/tests/barrier/CMakeLists.txt index 002031f61..0a9268f2a 100644 --- a/tests/src/tests/barrier/CMakeLists.txt +++ b/tests/src/tests/barrier/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests test_adaptive_stiffness.cpp test_barrier.cpp + test_simd_barrier.cpp # Benchmarks diff --git a/tests/src/tests/barrier/test_barrier.cpp b/tests/src/tests/barrier/test_barrier.cpp index 01e155d68..d50c67f32 100644 --- a/tests/src/tests/barrier/test_barrier.cpp +++ b/tests/src/tests/barrier/test_barrier.cpp @@ -13,7 +13,9 @@ #include #include +#include #include +#include using namespace ipc; @@ -59,8 +61,9 @@ TEST_CASE("Spline derivatives", "[deriv]") double deriv_ad = y_ad.grad(0); double hess_ad = y_ad.Hess(0); - CHECK(abs(deriv_ad - deriv) < 1e-14 * std::max(1., abs(deriv))); - CHECK(abs(hess_ad - hess) < 1e-12 * std::max(1., abs(hess))); + CHECK( + std::abs(deriv_ad - deriv) < 1e-14 * std::max(1., std::abs(deriv))); + CHECK(std::abs(hess_ad - hess) < 1e-12 * std::max(1., std::abs(hess))); } } @@ -78,8 +81,9 @@ TEST_CASE("Heaviside derivatives", "[deriv]") double deriv_ad = y_ad.grad(0); double hess_ad = y_ad.Hess(0); - CHECK(abs(deriv_ad - deriv) < 1e-14 * std::max(1., abs(deriv))); - CHECK(abs(hess_ad - hess) < 1e-12 * std::max(1., abs(hess))); + CHECK( + std::abs(deriv_ad - deriv) < 1e-14 * std::max(1., std::abs(deriv))); + CHECK(std::abs(hess_ad - hess) < 1e-12 * std::max(1., std::abs(hess))); } } @@ -100,8 +104,9 @@ TEST_CASE("Inv barrier derivatives", "[deriv]") double deriv_ad = y_ad.grad(0); double hess_ad = y_ad.Hess(0); - CHECK(abs(deriv_ad - deriv) < 1e-14 * std::max(1., abs(deriv))); - CHECK(abs(hess_ad - hess) < 1e-12 * std::max(1., abs(hess))); + CHECK( + std::abs(deriv_ad - deriv) < 1e-14 * std::max(1., std::abs(deriv))); + CHECK(std::abs(hess_ad - hess) < 1e-12 * std::max(1., std::abs(hess))); } ScalarBase::setVariableCount(3); @@ -236,7 +241,7 @@ TEST_CASE("negative_orientation_penalty derivatives", "[deriv]") // hess_ad.topLeftCorner(6, 6).setZero(); // y_ad = T(y_ad.val, y_ad.grad, hess_ad); - CHECK(abs(y - y_ad.val) <= 1e-12); + CHECK(std::abs(y - y_ad.val) <= 1e-12); CHECK((grad - y_ad.grad).norm() <= 1e-12); CHECK((hess - y_ad.Hess).norm() <= 1e-10); } @@ -502,4 +507,75 @@ TEST_CASE("Physical barrier", "[barrier]") CHECK( b_original_second_derivative == Catch::Approx(b_new_second_derivative)); -} \ No newline at end of file +} +TEST_CASE("Barrier penetration convention", "[barrier]") +{ + // Every log-based barrier must resolve d <= 0 (penetration) to +inf, and + // its derivatives to 0, rather than producing NaN or a finite value with + // the wrong sign. TwoStageBarrier regressed on this: with no d <= 0 guard + // its log term returned NaN for d < 0, and its first derivative flipped + // sign, reporting an attractive force at penetration. + const double dhat = 1e-2; + const double d = GENERATE(-1e-1, -1e-2, -1e-4, 0.0); + CAPTURE(d, dhat); + + std::vector> barriers = { + std::make_shared>(), + std::make_shared>(), + std::make_shared>(), + }; + + for (const auto& barrier : barriers) { + const double b = (*barrier)(d, dhat); + const double db = barrier->first_derivative(d, dhat); + const double d2b = barrier->second_derivative(d, dhat); + CAPTURE(b, db, d2b); + + CHECK(!std::isnan(b)); + CHECK(!std::isnan(db)); + CHECK(!std::isnan(d2b)); + + CHECK(std::isinf(b)); + CHECK(b > 0); + CHECK(db == 0.0); + CHECK(d2b == 0.0); + } +} + +TEST_CASE("Barrier stage boundaries", "[barrier]") +{ + // The select_lazy cascades are ordered by increasing d, so each case is + // bounded below by the previous one. That makes the choice of < vs <= the + // only thing deciding which case an exact boundary lands in -- and + // TwoStageBarrier's second derivative is discontinuous at dhat, so a + // boundary that slipped one case over would change the value there. + const double dhat = 1e-2; + + SECTION("d == dhat is inactive for every barrier") + { + const ipc::ClampedLogBarrier<> clamped_log; + const ipc::ClampedLogSqBarrier<> clamped_log_sq; + const ipc::CubicBarrier<> cubic; + const ipc::TwoStageBarrier<> two_stage; + + for (const ipc::Barrier* barrier : + { static_cast(&clamped_log), + static_cast(&clamped_log_sq), + static_cast(&cubic), + static_cast(&two_stage) }) { + CHECK((*barrier)(dhat, dhat) == 0.0); + CHECK(barrier->first_derivative(dhat, dhat) == 0.0); + CHECK(barrier->second_derivative(dhat, dhat) == 0.0); + } + } + + SECTION("d == dhat/2 is the quadratic stage of TwoStageBarrier") + { + const ipc::TwoStageBarrier<> two_stage; + const double d = 0.5 * dhat; + CHECK( + two_stage(d, dhat) == Catch::Approx(0.5 * (dhat - d) * (dhat - d))); + CHECK(two_stage.first_derivative(d, dhat) == Catch::Approx(d - dhat)); + CHECK(two_stage.second_derivative(d, dhat) == 1.0); + } +} diff --git a/tests/src/tests/barrier/test_simd_barrier.cpp b/tests/src/tests/barrier/test_simd_barrier.cpp new file mode 100644 index 000000000..e6c2d13da --- /dev/null +++ b/tests/src/tests/barrier/test_simd_barrier.cpp @@ -0,0 +1,113 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +#include + +using namespace ipc; +using namespace ipc::tests; + +namespace { + +/// @brief d <= 0 (penetration), d in (0, dhat/2), d in (dhat/2, dhat), and +/// d >= dhat (inactive) -- every branch of every barrier. +std::array regions(const double dhat) +{ + return { { -0.1 * dhat, 0.1 * dhat, 0.7 * dhat, 1.5 * dhat } }; +} + +/// @brief The barriers take two arguments, so bind dhat and sweep d. +/// +/// The tolerance is looser than a value's: a barrier is a log of a ratio, so +/// the two paths' multiply-add contraction shows up amplified. +template +void check_matches_scalar( + const std::string& name, + const double dhat, + BarrierValue&& value, + BarrierBatch&& batch_value) +{ + check_swept_lanes( + name, regions(dhat), [&](double d) { return value(d, dhat); }, + [&](Batch d) { return batch_value(d, Batch(dhat)); }, 1e-12); +} + +} // namespace + +TEST_CASE( + "SIMD batch barriers match the scalar ones lane-wise", "[barrier][simd]") +{ + const double dhat = 1e-2; + + check_matches_scalar( + "barrier", dhat, [](double d, double dh) { return barrier(d, dh); }, + [](Batch d, Batch dh) { return barrier(d, dh); }); + check_matches_scalar( + "barrier_first_derivative", dhat, + [](double d, double dh) { return barrier_first_derivative(d, dh); }, + [](Batch d, Batch dh) { return barrier_first_derivative(d, dh); }); + check_matches_scalar( + "barrier_second_derivative", dhat, + [](double d, double dh) { return barrier_second_derivative(d, dh); }, + [](Batch d, Batch dh) { return barrier_second_derivative(d, dh); }); + + const ClampedLogSqBarrier log_sq; + const ClampedLogSqBarrier log_sq_batch; + check_matches_scalar( + "ClampedLogSqBarrier", dhat, + [&](double d, double dh) { return log_sq(d, dh); }, + [&](Batch d, Batch dh) { return log_sq_batch(d, dh); }); + check_matches_scalar( + "ClampedLogSqBarrier::first_derivative", dhat, + [&](double d, double dh) { return log_sq.first_derivative(d, dh); }, + [&](Batch d, Batch dh) { + return log_sq_batch.first_derivative(d, dh); + }); + check_matches_scalar( + "ClampedLogSqBarrier::second_derivative", dhat, + [&](double d, double dh) { return log_sq.second_derivative(d, dh); }, + [&](Batch d, Batch dh) { + return log_sq_batch.second_derivative(d, dh); + }); + + const CubicBarrier cubic; + const CubicBarrier cubic_batch; + check_matches_scalar( + "CubicBarrier", dhat, [&](double d, double dh) { return cubic(d, dh); }, + [&](Batch d, Batch dh) { return cubic_batch(d, dh); }); + check_matches_scalar( + "CubicBarrier::first_derivative", dhat, + [&](double d, double dh) { return cubic.first_derivative(d, dh); }, + [&](Batch d, Batch dh) { return cubic_batch.first_derivative(d, dh); }); + check_matches_scalar( + "CubicBarrier::second_derivative", dhat, + [&](double d, double dh) { return cubic.second_derivative(d, dh); }, + [&](Batch d, Batch dh) { + return cubic_batch.second_derivative(d, dh); + }); + + const TwoStageBarrier two_stage; + const TwoStageBarrier two_stage_batch; + check_matches_scalar( + "TwoStageBarrier", dhat, + [&](double d, double dh) { return two_stage(d, dh); }, + [&](Batch d, Batch dh) { return two_stage_batch(d, dh); }); + check_matches_scalar( + "TwoStageBarrier::first_derivative", dhat, + [&](double d, double dh) { return two_stage.first_derivative(d, dh); }, + [&](Batch d, Batch dh) { + return two_stage_batch.first_derivative(d, dh); + }); + check_matches_scalar( + "TwoStageBarrier::second_derivative", dhat, + [&](double d, double dh) { return two_stage.second_derivative(d, dh); }, + [&](Batch d, Batch dh) { + return two_stage_batch.second_derivative(d, dh); + }); +} + +#endif diff --git a/tests/src/tests/broad_phase/test_lbvh.cpp b/tests/src/tests/broad_phase/test_lbvh.cpp index 80c204f58..08d4b39eb 100644 --- a/tests/src/tests/broad_phase/test_lbvh.cpp +++ b/tests/src/tests/broad_phase/test_lbvh.cpp @@ -12,8 +12,6 @@ #include #include -#include - using namespace ipc; namespace { diff --git a/tests/src/tests/distance/CMakeLists.txt b/tests/src/tests/distance/CMakeLists.txt index 84e05e6d5..829389de9 100644 --- a/tests/src/tests/distance/CMakeLists.txt +++ b/tests/src/tests/distance/CMakeLists.txt @@ -9,6 +9,9 @@ set(SOURCES test_point_plane.cpp test_point_point.cpp test_point_triangle.cpp + test_simd_distance.cpp + test_simd_edge_edge_mollifier.cpp + test_simd_signed_distance.cpp test_signed_distance.cpp # Benchmarks diff --git a/tests/src/tests/distance/test_point_edge.cpp b/tests/src/tests/distance/test_point_edge.cpp index 45ff8947a..f13d75474 100644 --- a/tests/src/tests/distance/test_point_edge.cpp +++ b/tests/src/tests/distance/test_point_edge.cpp @@ -93,6 +93,16 @@ TEMPLATE_TEST_CASE_SIG( const VectorMax3d p = ((e1 - e0) * alpha + e0) + d * n; + // Two random points occasionally land almost on top of each other. The + // derivatives are still correct there, but comparing them against + // finite differences is not: with the point hundreds of edge-lengths + // away, the difference quotients lose all their precision. Measured + // against the analytic Hessian, every edge longer than this agrees to + // 2e-4, while shorter ones disagree by O(1) -- and disagree by + // *different* amounts at second and eighth order, which is how we know + // it is the finite differences drifting rather than the Hessian. + const bool edge_is_degenerate = (e1 - e0).norm() <= 0.05; + CAPTURE(alpha, dim); { // Distance @@ -104,7 +114,8 @@ TEMPLATE_TEST_CASE_SIG( } // Gradient (skip C1 transition points) - if (abs(alpha) < 1e-5 && abs(alpha - 1.0) < 1e-5) { + if (std::abs(alpha) > 1e-5 && std::abs(alpha - 1.0) > 1e-5 + && !edge_is_degenerate) { const VectorMax9d grad = point_edge_distance_gradient(p, e0, e1); // Compute the gradient using finite differences @@ -118,7 +129,8 @@ TEMPLATE_TEST_CASE_SIG( } // Hessian (skip C1 transition points) - if (abs(alpha) < 1e-5 && abs(alpha - 1.0) < 1e-5) { + if (std::abs(alpha) > 1e-5 && std::abs(alpha - 1.0) > 1e-5 + && !edge_is_degenerate) { const MatrixMax9d hess = point_edge_distance_hessian(p, e0, e1); // Compute the gradient using finite differences VectorMax9d x(3 * dim); @@ -171,7 +183,7 @@ TEMPLATE_TEST_CASE_SIG( } // Gradient (skip C1 transition points) - // if (abs(alpha) < 1e-5 && abs(alpha - 1.0) < 1e-5) { + // if (std::abs(alpha) > 1e-5 && std::abs(alpha - 1.0) > 1e-5) { { const VectorMax9d grad = point_edge_distance_gradient(p, e0, e1); @@ -185,7 +197,7 @@ TEMPLATE_TEST_CASE_SIG( } // Hessian (skip C1 transition points) - if (abs(alpha) < 1e-5 && abs(alpha - 1.0) < 1e-5) { + if (std::abs(alpha) > 1e-5 && std::abs(alpha - 1.0) > 1e-5) { const MatrixMax9d hess = point_edge_distance_hessian(p, e0, e1); // Compute the gradient using finite differences VectorMax9d x(3 * dim); @@ -242,7 +254,7 @@ TEMPLATE_TEST_CASE_SIG( } // Gradient (skip C1 transition points) - if (abs(alpha) > 1e-5 && abs(alpha - 1.0) > 1e-5) { + if (std::abs(alpha) > 1e-5 && std::abs(alpha - 1.0) > 1e-5) { const auto [vec, grad] = PointEdgeDistanceDerivatives< dim>::point_edge_closest_point_direction_grad(p, e0, e1, dtype); @@ -265,7 +277,7 @@ TEMPLATE_TEST_CASE_SIG( } // Gradient (skip C1 transition points) - if (abs(alpha) > 1e-5 && abs(alpha - 1.0) > 1e-5) { + if (std::abs(alpha) > 1e-5 && std::abs(alpha - 1.0) > 1e-5) { VectorMax9d x(3 * dim); x << e0, e1, p; @@ -297,7 +309,7 @@ TEMPLATE_TEST_CASE_SIG( } // Hessian (skip C1 transition points) - if (abs(alpha) > 1e-5 && abs(alpha - 1.0) > 1e-5) { + if (std::abs(alpha) > 1e-5 && std::abs(alpha - 1.0) > 1e-5) { const auto [vec, grad, hess] = PointEdgeDistanceDerivatives< dim>::point_edge_closest_point_direction_hessian(p, e0, e1, dtype); // Compute the gradient using finite differences diff --git a/tests/src/tests/distance/test_simd_distance.cpp b/tests/src/tests/distance/test_simd_distance.cpp new file mode 100644 index 000000000..8fb15c46a --- /dev/null +++ b/tests/src/tests/distance/test_simd_distance.cpp @@ -0,0 +1,194 @@ +#include +#include +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include +#include +#include +#include +#include +#include + +using namespace ipc; +using namespace ipc::tests; + +TEST_CASE( + "SIMD batch distances match the scalar ones lane-wise", "[distance][simd]") +{ + const Points<3> A = random_points<3>(1), B = random_points<3>(2), + C = random_points<3>(3), D = random_points<3>(4); + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + // A batch lane must agree with the scalar answer to within rounding; the + // two differ only in how the compiler contracts multiply-adds. + check_scalar_lanes( + "point_point_distance", point_point_distance(a, b), + [&](int l) { return point_point_distance(A[l], B[l]); }); + check_scalar_lanes( + "point_line_distance", point_line_distance(a, b, c), + [&](int l) { return point_line_distance(A[l], B[l], C[l]); }); + check_scalar_lanes( + "line_line_distance", line_line_distance(a, b, c, d), + [&](int l) { return line_line_distance(A[l], B[l], C[l], D[l]); }); + check_scalar_lanes( + "point_edge_distance", + point_edge_distance(a, b, c, PointEdgeDistanceType::P_E), [&](int l) { + return point_edge_distance( + A[l], B[l], C[l], PointEdgeDistanceType::P_E); + }); + check_scalar_lanes( + "edge_edge_distance", + edge_edge_distance(a, b, c, d, EdgeEdgeDistanceType::EA_EB), + [&](int l) { + return edge_edge_distance( + A[l], B[l], C[l], D[l], EdgeEdgeDistanceType::EA_EB); + }); + check_scalar_lanes( + "point_triangle_distance", + point_triangle_distance(a, b, c, d, PointTriangleDistanceType::P_T), + [&](int l) { + return point_triangle_distance( + A[l], B[l], C[l], D[l], PointTriangleDistanceType::P_T); + }); +} + +TEST_CASE( + "SIMD batch distance gradients and Hessians match lane-wise", + "[distance][simd]") +{ + const Points<3> A = random_points<3>(11), B = random_points<3>(12), + C = random_points<3>(13), D = random_points<3>(14); + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + check_lanes( + "point_point_distance_gradient", point_point_distance_gradient(a, b), + [&](int l) { + return point_point_distance_gradient(A[l], B[l]).eval(); + }); + check_lanes( + "point_point_distance_hessian", point_point_distance_hessian(a, b), + [&](int l) { return point_point_distance_hessian(A[l], B[l]).eval(); }); + + check_lanes( + "point_line_distance_gradient", point_line_distance_gradient(a, b, c), + [&](int l) { + return point_line_distance_gradient(A[l], B[l], C[l]).eval(); + }); + check_lanes( + "point_line_distance_hessian", point_line_distance_hessian(a, b, c), + [&](int l) { + return point_line_distance_hessian(A[l], B[l], C[l]).eval(); + }); + + check_lanes( + "line_line_distance_gradient", line_line_distance_gradient(a, b, c, d), + [&](int l) { + return line_line_distance_gradient(A[l], B[l], C[l], D[l]).eval(); + }); + check_lanes( + "line_line_distance_hessian", line_line_distance_hessian(a, b, c, d), + [&](int l) { + return line_line_distance_hessian(A[l], B[l], C[l], D[l]).eval(); + }); + + check_lanes( + "point_edge_distance_gradient", + point_edge_distance_gradient(a, b, c, PointEdgeDistanceType::P_E), + [&](int l) { + return point_edge_distance_gradient( + A[l], B[l], C[l], PointEdgeDistanceType::P_E) + .eval(); + }); + check_lanes( + "point_edge_distance_hessian", + point_edge_distance_hessian(a, b, c, PointEdgeDistanceType::P_E), + [&](int l) { + return point_edge_distance_hessian( + A[l], B[l], C[l], PointEdgeDistanceType::P_E) + .eval(); + }); + + check_lanes( + "edge_edge_distance_gradient", + edge_edge_distance_gradient(a, b, c, d, EdgeEdgeDistanceType::EA_EB), + [&](int l) { + return edge_edge_distance_gradient( + A[l], B[l], C[l], D[l], EdgeEdgeDistanceType::EA_EB) + .eval(); + }); + check_lanes( + "edge_edge_distance_hessian", + edge_edge_distance_hessian(a, b, c, d, EdgeEdgeDistanceType::EA_EB), + [&](int l) { + return edge_edge_distance_hessian( + A[l], B[l], C[l], D[l], EdgeEdgeDistanceType::EA_EB) + .eval(); + }); + + check_lanes( + "point_triangle_distance_gradient", + point_triangle_distance_gradient( + a, b, c, d, PointTriangleDistanceType::P_T), + [&](int l) { + return point_triangle_distance_gradient( + A[l], B[l], C[l], D[l], PointTriangleDistanceType::P_T) + .eval(); + }); + check_lanes( + "point_triangle_distance_hessian", + point_triangle_distance_hessian( + a, b, c, d, PointTriangleDistanceType::P_T), + [&](int l) { + return point_triangle_distance_hessian( + A[l], B[l], C[l], D[l], PointTriangleDistanceType::P_T) + .eval(); + }); +} + +TEST_CASE("SIMD batch distances reject AUTO", "[distance][simd]") +{ + // The distance type is a per-lane property, but the predicates return a + // single enum, so AUTO cannot be resolved for a batch. It must throw + // rather than silently apply one lane's classification to all of them. + const Points<3> A = random_points<3>(5), B = random_points<3>(6), + C = random_points<3>(7), D = random_points<3>(8); + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + // Not just any exception: the specific diagnostic naming the function, + // rather than the generic "invalid distance type" from the switch default. + const auto explicit_required = Catch::Matchers::MessageMatches( + Catch::Matchers::ContainsSubstring("explicit distance type")); + + CHECK_THROWS_MATCHES( + point_edge_distance(a, b, c, PointEdgeDistanceType::AUTO), + std::invalid_argument, explicit_required); + CHECK_THROWS_MATCHES( + edge_edge_distance(a, b, c, d, EdgeEdgeDistanceType::AUTO), + std::invalid_argument, explicit_required); + CHECK_THROWS_MATCHES( + point_triangle_distance(a, b, c, d, PointTriangleDistanceType::AUTO), + std::invalid_argument, explicit_required); + CHECK_THROWS_MATCHES( + edge_edge_distance_gradient(a, b, c, d, EdgeEdgeDistanceType::AUTO), + std::invalid_argument, explicit_required); + CHECK_THROWS_MATCHES( + edge_edge_distance_hessian(a, b, c, d, EdgeEdgeDistanceType::AUTO), + std::invalid_argument, explicit_required); + CHECK_THROWS_MATCHES( + point_triangle_distance_gradient( + a, b, c, d, PointTriangleDistanceType::AUTO), + std::invalid_argument, explicit_required); + CHECK_THROWS_MATCHES( + point_triangle_distance_hessian( + a, b, c, d, PointTriangleDistanceType::AUTO), + std::invalid_argument, explicit_required); +} + +#endif diff --git a/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp b/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp new file mode 100644 index 000000000..a34079725 --- /dev/null +++ b/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp @@ -0,0 +1,211 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +#include + +#include +#include +#include + +using namespace ipc; +using namespace ipc::tests; + +namespace { + +/// @brief The threshold of two unit-length rest edges. +constexpr double EPS_X = 1e-3; + +/// @brief Angles between the two edges placing a lane on either side of the +/// mollifier threshold: exactly parallel, mollified, just inside the +/// threshold, and inactive. The mollifier activates at sin²θ < eps_x, i.e. +/// θ < 0.0316 rad for unit rest edges. +constexpr std::array THETAS = { { 0.0, 0.02, 0.0316, 0.4 } }; + +/// @brief A lane-dependent rotation, so the gradients are not mostly zeros the +/// way an axis-aligned configuration would make them. +Eigen::Matrix3d rotation(const int l) +{ + return (Eigen::AngleAxisd( + 0.7 + 0.31 * l, Eigen::Vector3d(1, 2, 3).normalized()) + * Eigen::AngleAxisd( + 0.4 * l - 0.2, Eigen::Vector3d(-2, 1, 0.5).normalized())) + .toRotationMatrix(); +} + +struct Config { + Eigen::Vector3d ea0, ea1, eb0, eb1; +}; + +/// @brief Two unit-length edges meeting at an angle `theta`, rigidly placed by +/// lane. Unit lengths keep the rest threshold at exactly `EPS_X`, so `theta` +/// alone decides which side of the branch the lane lands on. +Config config(const double theta, const int l) +{ + const Eigen::Matrix3d R = rotation(l); + const Eigen::Vector3d t(0.1 * l, -0.2, 0.3); + + const Eigen::Vector3d ea0(0, 0, 0); + const Eigen::Vector3d ea1(1, 0, 0); + const Eigen::Vector3d eb0(0, 1, 0.3); + const Eigen::Vector3d eb1 = + eb0 + Eigen::Vector3d(std::cos(theta), std::sin(theta), 0); + + return { R * ea0 + t, R * ea1 + t, R * eb0 + t, R * eb1 + t }; +} + +} // namespace + +TEST_CASE( + "SIMD batch edge-edge mollifier matches the scalar one lane-wise", + "[distance][mollifier][simd]") +{ + // x straddling eps_x, so one batch carries mollified and inactive lanes. + constexpr std::array REGION_XS = { + { 0.0, 0.1 * EPS_X, 0.9 * EPS_X, 2.0 * EPS_X } + }; + + const auto check = [&](const std::string& name, auto&& scalar_fn, + auto&& batch_fn) { + check_swept_lanes( + name, REGION_XS, [&](double x) { return scalar_fn(x, EPS_X); }, + [&](Batch x) { return batch_fn(x, Batch(EPS_X)); }); + }; + + check( + "mollifier", + [](double x, double e) { return edge_edge_mollifier(x, e); }, + [](Batch x, Batch e) { return edge_edge_mollifier(x, e); }); + check( + "gradient", + [](double x, double e) { return edge_edge_mollifier_gradient(x, e); }, + [](Batch x, Batch e) { return edge_edge_mollifier_gradient(x, e); }); + check( + "hessian", + [](double x, double e) { return edge_edge_mollifier_hessian(x, e); }, + [](Batch x, Batch e) { return edge_edge_mollifier_hessian(x, e); }); + check( + "derivative_wrt_eps_x", + [](double x, double e) { + return edge_edge_mollifier_derivative_wrt_eps_x(x, e); + }, + [](Batch x, Batch e) { + return edge_edge_mollifier_derivative_wrt_eps_x(x, e); + }); + check( + "gradient_derivative_wrt_eps_x", + [](double x, double e) { + return edge_edge_mollifier_gradient_derivative_wrt_eps_x(x, e); + }, + [](Batch x, Batch e) { + return edge_edge_mollifier_gradient_derivative_wrt_eps_x(x, e); + }); +} + +TEST_CASE( + "SIMD batch edge-edge mollifier blends the threshold branch per lane", + "[distance][mollifier][simd]") +{ + for (int offset = 0; offset < int(THETAS.size()); ++offset) { + const Lanes thetas = lane_cases(THETAS, offset); + const std::string at = " (offset " + std::to_string(offset) + ")"; + + Points<3> EA0, EA1, EB0, EB1; + // The rest configuration is the same geometry at theta = 0: both edges + // are unit length, so the threshold is exactly EPS_X on every lane. + Points<3> RA0, RA1, RB0, RB1; + for (int l = 0; l < L; ++l) { + const Config c = config(thetas[l], l); + EA0[l] = c.ea0, EA1[l] = c.ea1, EB0[l] = c.eb0, EB1[l] = c.eb1; + const Config r = config(0.0, l + 1); + RA0[l] = r.ea0, RA1[l] = r.ea1, RB0[l] = r.eb0, RB1[l] = r.eb1; + } + + const Eigen::Vector3 ea0 = pack(EA0), ea1 = pack(EA1), + eb0 = pack(EB0), eb1 = pack(EB1); + const Eigen::Vector3 ra0 = pack(RA0), ra1 = pack(RA1), + rb0 = pack(RB0), rb1 = pack(RB1); + const Batch eps_x(EPS_X); + + // The threshold itself must agree before anything built on it does. + check_scalar_lanes( + "threshold" + at, edge_edge_mollifier_threshold(ra0, ra1, rb0, rb1), + [&](int l) { + return edge_edge_mollifier_threshold( + RA0[l], RA1[l], RB0[l], RB1[l]); + }); + check_scalar_lanes( + "cross_squarednorm" + at, + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1), [&](int l) { + return edge_edge_cross_squarednorm( + EA0[l], EA1[l], EB0[l], EB1[l]); + }); + check_scalar_lanes( + "mollifier" + at, edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x), + [&](int l) { + return edge_edge_mollifier( + EA0[l], EA1[l], EB0[l], EB1[l], EPS_X); + }); + + check_lanes( + "cross_squarednorm_gradient" + at, + edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1), + [&](int l) { + return edge_edge_cross_squarednorm_gradient( + EA0[l], EA1[l], EB0[l], EB1[l]); + }); + check_lanes( + "cross_squarednorm_hessian" + at, + edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1), + [&](int l) { + return edge_edge_cross_squarednorm_hessian( + EA0[l], EA1[l], EB0[l], EB1[l]); + }); + + check_lanes( + "mollifier_gradient" + at, + edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x), + [&](int l) { + return edge_edge_mollifier_gradient( + EA0[l], EA1[l], EB0[l], EB1[l], EPS_X); + }); + check_lanes( + "mollifier_hessian" + at, + edge_edge_mollifier_hessian(ea0, ea1, eb0, eb1, eps_x), [&](int l) { + return edge_edge_mollifier_hessian( + EA0[l], EA1[l], EB0[l], EB1[l], EPS_X); + }); + + check_lanes( + "threshold_gradient" + at, + edge_edge_mollifier_threshold_gradient(ra0, ra1, rb0, rb1), + [&](int l) { + return edge_edge_mollifier_threshold_gradient( + RA0[l], RA1[l], RB0[l], RB1[l]); + }); + check_lanes( + "mollifier_gradient_wrt_x" + at, + edge_edge_mollifier_gradient_wrt_x( + ra0, ra1, rb0, rb1, ea0, ea1, eb0, eb1), + [&](int l) { + return edge_edge_mollifier_gradient_wrt_x( + RA0[l], RA1[l], RB0[l], RB1[l], EA0[l], EA1[l], EB0[l], + EB1[l]); + }); + check_lanes( + "mollifier_gradient_jacobian_wrt_x" + at, + edge_edge_mollifier_gradient_jacobian_wrt_x( + ra0, ra1, rb0, rb1, ea0, ea1, eb0, eb1), + [&](int l) { + return edge_edge_mollifier_gradient_jacobian_wrt_x( + RA0[l], RA1[l], RB0[l], RB1[l], EA0[l], EA1[l], EB0[l], + EB1[l]); + }); + } +} + +#endif diff --git a/tests/src/tests/distance/test_simd_signed_distance.cpp b/tests/src/tests/distance/test_simd_signed_distance.cpp new file mode 100644 index 000000000..afd90ff38 --- /dev/null +++ b/tests/src/tests/distance/test_simd_signed_distance.cpp @@ -0,0 +1,122 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include +#include +#include + +using namespace ipc; +using namespace ipc::tests; + +namespace { + +/// @brief Agreement bound for a signed distance *value*. +/// +/// Looser than the shared `VALUE_TOL`, and structurally so: an unsigned +/// distance is a squared or absolute length, but a signed distance is +/// `normal.dot(p - x0)` with the normal a *normalized* cross product. The +/// cross product cancels, and the `sqrt` and division that follow amplify the +/// batch and scalar paths' differing multiply-add contraction rather than +/// costing one rounding step. Measured agreement is ~1e-10 relative. +constexpr double SIGNED_VALUE_TOL = DERIVATIVE_TOL; + +} // namespace + +TEST_CASE( + "SIMD batch signed distances match the scalar ones lane-wise", + "[distance][signed_distance][simd]") +{ + SECTION("point-line (2D)") + { + const auto P = random_points<2>(11); + const auto E0 = random_points<2>(12); + const auto E1 = random_points<2>(13); + + const auto p = pack(P), e0 = pack(E0), e1 = pack(E1); + + check_scalar_lanes( + "value", point_line_signed_distance(p, e0, e1), + [&](int l) { + return point_line_signed_distance(P[l], E0[l], E1[l]); + }, + SIGNED_VALUE_TOL); + check_lanes( + "gradient", point_line_signed_distance_gradient(p, e0, e1), + [&](int l) { + return point_line_signed_distance_gradient(P[l], E0[l], E1[l]) + .eval(); + }); + check_lanes( + "hessian", point_line_signed_distance_hessian(p, e0, e1), + [&](int l) { + return point_line_signed_distance_hessian(P[l], E0[l], E1[l]) + .eval(); + }); + } + + SECTION("line-line") + { + const auto A = random_points<3>(21); + const auto B = random_points<3>(22); + const auto C = random_points<3>(23); + const auto D = random_points<3>(24); + + const auto a = pack(A), b = pack(B), c = pack(C), d = pack(D); + + check_scalar_lanes( + "value", line_line_signed_distance(a, b, c, d), + [&](int l) { + return line_line_signed_distance(A[l], B[l], C[l], D[l]); + }, + SIGNED_VALUE_TOL); + check_lanes( + "gradient", line_line_signed_distance_gradient(a, b, c, d), + [&](int l) { + return line_line_signed_distance_gradient( + A[l], B[l], C[l], D[l]) + .eval(); + }); + check_lanes( + "hessian", line_line_signed_distance_hessian(a, b, c, d), + [&](int l) { + return line_line_signed_distance_hessian(A[l], B[l], C[l], D[l]) + .eval(); + }); + } + + SECTION("point-plane") + { + const auto P = random_points<3>(31); + const auto T0 = random_points<3>(32); + const auto T1 = random_points<3>(33); + const auto T2 = random_points<3>(34); + + const auto p = pack(P), t0 = pack(T0), t1 = pack(T1), t2 = pack(T2); + + check_scalar_lanes( + "value", point_plane_signed_distance(p, t0, t1, t2), + [&](int l) { + return point_plane_signed_distance(P[l], T0[l], T1[l], T2[l]); + }, + SIGNED_VALUE_TOL); + check_lanes( + "gradient", point_plane_signed_distance_gradient(p, t0, t1, t2), + [&](int l) { + return point_plane_signed_distance_gradient( + P[l], T0[l], T1[l], T2[l]) + .eval(); + }); + check_lanes( + "hessian", point_plane_signed_distance_hessian(p, t0, t1, t2), + [&](int l) { + return point_plane_signed_distance_hessian( + P[l], T0[l], T1[l], T2[l]) + .eval(); + }); + } +} + +#endif diff --git a/tests/src/tests/friction/CMakeLists.txt b/tests/src/tests/friction/CMakeLists.txt index a51df5617..ec317dae0 100644 --- a/tests/src/tests/friction/CMakeLists.txt +++ b/tests/src/tests/friction/CMakeLists.txt @@ -3,6 +3,7 @@ set(SOURCES test_anisotropic_friction.cpp test_force_jacobian.cpp test_friction.cpp + test_simd_friction.cpp test_smooth_friction_mollifier.cpp test_smooth_mu.cpp diff --git a/tests/src/tests/friction/test_simd_friction.cpp b/tests/src/tests/friction/test_simd_friction.cpp new file mode 100644 index 000000000..d6894301b --- /dev/null +++ b/tests/src/tests/friction/test_simd_friction.cpp @@ -0,0 +1,95 @@ +#include +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include +#include + +#include +#include + +using namespace ipc; +using namespace ipc::tests; + +namespace { + +/// @brief Speeds placing a lane in each piece of the mollifier, as multiples of +/// eps_v: well inside, either side of the half-threshold the smooth-μ formulas +/// split on, just inside, exactly at, and past the threshold -- plus the +/// negative mirror, since these functions branch on |y| but the value itself +/// carries the sign. Zero is in the list because that is where the `1/y` +/// branches are singular: the scalar path never takes them there, while a batch +/// evaluates them anyway and must blend the infinity away. +constexpr std::array Y_MULTIPLES = { + { -2.0, -1.0, -0.75, -0.25, 0.0, 0.25, 0.49, 0.5, 0.75, 1.0, 2.0 } +}; + +} // namespace + +TEST_CASE( + "SIMD batch smooth friction mollifier matches the scalar one lane-wise", + "[friction][mollifier][simd]") +{ + const double eps_v = GENERATE(1e-3, 0.1, 1.0); + const auto ys = scaled(Y_MULTIPLES, eps_v); + + auto check = [&](const std::string& name, auto&& f) { + check_swept_lanes_with(name, ys, f, eps_v); + }; + + check("f0", [](auto y, auto e) { return smooth_friction_f0(y, e); }); + check("f1", [](auto y, auto e) { return smooth_friction_f1(y, e); }); + check("f2", [](auto y, auto e) { return smooth_friction_f2(y, e); }); + check("f1_over_x", [](auto y, auto e) { + return smooth_friction_f1_over_x(y, e); + }); + check("f2_x_minus_f1_over_x3", [](auto y, auto e) { + return smooth_friction_f2_x_minus_f1_over_x3(y, e); + }); +} + +TEST_CASE( + "SIMD batch smooth mu matches the scalar one lane-wise", + "[friction][smooth_mu][simd]") +{ + const double eps_v = GENERATE(1e-3, 0.1, 1.0); + + // The equal-coefficient pair is the fast path these functions short-circuit + // on, and it has to agree with the general formulas it skips. + const auto mus = GENERATE( + std::pair { 0.5, 0.5 }, std::pair { 0.5, 0.1 }, std::pair { 0.1, 0.5 }); + const double mu_s = mus.first, mu_k = mus.second; + + const auto ys = scaled(Y_MULTIPLES, eps_v); + + auto check = [&](const std::string& name, auto&& f) { + check_swept_lanes_with(name, ys, f, mu_s, mu_k, eps_v); + }; + + check("mu", [](auto y, auto s, auto k, auto e) { + return smooth_mu(y, s, k, e); + }); + check("mu_derivative", [](auto y, auto s, auto k, auto e) { + return smooth_mu_derivative(y, s, k, e); + }); + check("mu_f0", [](auto y, auto s, auto k, auto e) { + return smooth_mu_f0(y, s, k, e); + }); + check("mu_f1", [](auto y, auto s, auto k, auto e) { + return smooth_mu_f1(y, s, k, e); + }); + check("mu_f2", [](auto y, auto s, auto k, auto e) { + return smooth_mu_f2(y, s, k, e); + }); + check("mu_f1_over_x", [](auto y, auto s, auto k, auto e) { + return smooth_mu_f1_over_x(y, s, k, e); + }); + check("mu_f2_x_minus_mu_f1_over_x3", [](auto y, auto s, auto k, auto e) { + return smooth_mu_f2_x_minus_mu_f1_over_x3(y, s, k, e); + }); +} + +#endif diff --git a/tests/src/tests/geometry/CMakeLists.txt b/tests/src/tests/geometry/CMakeLists.txt index 77c567134..8e8c0b4dc 100644 --- a/tests/src/tests/geometry/CMakeLists.txt +++ b/tests/src/tests/geometry/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES test_angle.cpp test_intersection.cpp + test_simd_geometry.cpp ) target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) diff --git a/tests/src/tests/geometry/test_simd_geometry.cpp b/tests/src/tests/geometry/test_simd_geometry.cpp new file mode 100644 index 000000000..e788c8185 --- /dev/null +++ b/tests/src/tests/geometry/test_simd_geometry.cpp @@ -0,0 +1,283 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include +#include +#include + +#include +#include + +using namespace ipc; +using namespace ipc::tests; + +TEST_CASE( + "SIMD batch normals match the scalar ones lane-wise", "[normal][simd]") +{ + const Points<3> A = random_points<3>(1), B = random_points<3>(2), + C = random_points<3>(3), D = random_points<3>(4); + + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + SECTION("normalization") + { + check_lanes( + "normalization_jacobian", normalization_jacobian(a), + [&](int l) { return normalization_jacobian(A[l]).eval(); }); + // normalization_hessian returns one 3x3 slice per coordinate. + const auto H = normalization_hessian(a); + for (int k = 0; k < 3; ++k) { + check_lanes( + "normalization_hessian[" + std::to_string(k) + "]", H[k], + [&](int l) { return normalization_hessian(A[l])[k]; }); + } + } + + SECTION("point-line") + { + check_lanes( + "unnormalized", point_line_unnormalized_normal(a, b, c), + [&](int l) { + return point_line_unnormalized_normal(A[l], B[l], C[l]).eval(); + }); + check_lanes("normal", point_line_normal(a, b, c), [&](int l) { + return point_line_normal(A[l], B[l], C[l]).eval(); + }); + check_lanes( + "unnormalized_jacobian", + point_line_unnormalized_normal_jacobian(a, b, c), [&](int l) { + return point_line_unnormalized_normal_jacobian(A[l], B[l], C[l]) + .eval(); + }); + check_lanes( + "normal_jacobian", point_line_normal_jacobian(a, b, c), [&](int l) { + return point_line_normal_jacobian(A[l], B[l], C[l]).eval(); + }); + check_lanes( + "unnormalized_hessian", + point_line_unnormalized_normal_hessian(a, b, c), [&](int l) { + return point_line_unnormalized_normal_hessian(A[l], B[l], C[l]) + .eval(); + }); + check_lanes( + "normal_hessian", point_line_normal_hessian(a, b, c), [&](int l) { + return point_line_normal_hessian(A[l], B[l], C[l]).eval(); + }); + } + + SECTION("triangle") + { + check_lanes( + "unnormalized", triangle_unnormalized_normal(a, b, c), [&](int l) { + return triangle_unnormalized_normal(A[l], B[l], C[l]).eval(); + }); + check_lanes("normal", triangle_normal(a, b, c), [&](int l) { + return triangle_normal(A[l], B[l], C[l]).eval(); + }); + check_lanes( + "unnormalized_jacobian", + triangle_unnormalized_normal_jacobian(a, b, c), [&](int l) { + return triangle_unnormalized_normal_jacobian(A[l], B[l], C[l]) + .eval(); + }); + check_lanes( + "normal_jacobian", triangle_normal_jacobian(a, b, c), [&](int l) { + return triangle_normal_jacobian(A[l], B[l], C[l]).eval(); + }); + check_lanes( + "unnormalized_hessian", + triangle_unnormalized_normal_hessian(a, b, c), [&](int l) { + return triangle_unnormalized_normal_hessian(A[l], B[l], C[l]) + .eval(); + }); + check_lanes( + "normal_hessian", triangle_normal_hessian(a, b, c), [&](int l) { + return triangle_normal_hessian(A[l], B[l], C[l]).eval(); + }); + } + + SECTION("line-line") + { + check_lanes( + "unnormalized", line_line_unnormalized_normal(a, b, c, d), + [&](int l) { + return line_line_unnormalized_normal(A[l], B[l], C[l], D[l]) + .eval(); + }); + check_lanes("normal", line_line_normal(a, b, c, d), [&](int l) { + return line_line_normal(A[l], B[l], C[l], D[l]).eval(); + }); + check_lanes( + "unnormalized_jacobian", + line_line_unnormalized_normal_jacobian(a, b, c, d), [&](int l) { + return line_line_unnormalized_normal_jacobian( + A[l], B[l], C[l], D[l]) + .eval(); + }); + check_lanes( + "normal_jacobian", line_line_normal_jacobian(a, b, c, d), + [&](int l) { + return line_line_normal_jacobian(A[l], B[l], C[l], D[l]).eval(); + }); + check_lanes( + "unnormalized_hessian", + line_line_unnormalized_normal_hessian(a, b, c, d), [&](int l) { + return line_line_unnormalized_normal_hessian( + A[l], B[l], C[l], D[l]) + .eval(); + }); + check_lanes( + "normal_hessian", line_line_normal_hessian(a, b, c, d), [&](int l) { + return line_line_normal_hessian(A[l], B[l], C[l], D[l]).eval(); + }); + } +} + +TEST_CASE( + "SIMD batch normalized normals leave a degenerate lane unscaled", + "[normal][simd]") +{ + // ipc::normalized applies Eigen's own rule -- a zero-length vector is + // returned unscaled -- per lane rather than behind one bool. Only the + // values are compared: the normalized Jacobian/Hessian of a degenerate + // configuration divides by zero on both paths. + // + // Lane 0 is degenerate and lane 1 is generic, and the roles swap on the + // second pass, so both orderings of the blend are exercised even when a + // batch holds only two lanes. + for (int flip = 0; flip < 2; ++flip) { + const auto is_degenerate = [&](int l) { return (l % 2) == flip; }; + + // point-line: the point lies on the line, so d is parallel to e. + // triangle: the three vertices are collinear. + // line-line: the two lines are parallel. + Points<3> P, E0, E1; + Points<3> TA, TB, TC; + Points<3> LA0, LA1, LB0, LB1; + for (int l = 0; l < L; ++l) { + // Exactly representable coordinates, so the degenerate lane's + // unnormalized normal cancels to a true zero rather than to a + // rounding residue that would take the non-degenerate branch. + const Eigen::Vector3d e(1, 2, 3); + E0[l] = Eigen::Vector3d(0.5 * l, -0.25, 0.75); + E1[l] = E0[l] + e; + P[l] = is_degenerate(l) + ? Eigen::Vector3d(E0[l] + 0.5 * e) + : Eigen::Vector3d(E0[l] + Eigen::Vector3d(0, 0, 1)); + + TA[l] = Eigen::Vector3d(0, 0, 0); + TB[l] = Eigen::Vector3d(1, 0, 0); + TC[l] = is_degenerate(l) ? Eigen::Vector3d(2, 0, 0) + : Eigen::Vector3d(0, 1, 0); + + LA0[l] = Eigen::Vector3d(0, 0, 0); + LA1[l] = Eigen::Vector3d(1, 0, 0); + LB0[l] = Eigen::Vector3d(0, 1, 0); + LB1[l] = LB0[l] + + (is_degenerate(l) ? Eigen::Vector3d(1, 0, 0) + : Eigen::Vector3d(0, 0, 1)); + } + + check_lanes( + "point_line_normal", point_line_normal(pack(P), pack(E0), pack(E1)), + [&](int l) { + return point_line_normal(P[l], E0[l], E1[l]).eval(); + }); + check_lanes( + "triangle_normal", triangle_normal(pack(TA), pack(TB), pack(TC)), + [&](int l) { return triangle_normal(TA[l], TB[l], TC[l]).eval(); }); + check_lanes( + "line_line_normal", + line_line_normal(pack(LA0), pack(LA1), pack(LB0), pack(LB1)), + [&](int l) { + return line_line_normal(LA0[l], LA1[l], LB0[l], LB1[l]).eval(); + }); + + // Agreeing with the scalar path is not enough on its own: assert the + // rule directly, so a degenerate lane is exactly zero rather than a + // NaN, and a generic lane in the same batch is still a unit vector. + const auto check_degenerate_rule = [&](const std::string& name, + const Eigen::Vector3& n) { + for (int l = 0; l < L; ++l) { + Eigen::Vector3d lane; + for (int k = 0; k < 3; ++k) { + lane[k] = n[k].get(l); + } + CAPTURE(name, flip, l, lane.transpose()); + if (is_degenerate(l)) { + CHECK(lane == Eigen::Vector3d::Zero()); + } else { + CHECK(std::abs(lane.norm() - 1.0) <= DERIVATIVE_TOL); + } + } + }; + + check_degenerate_rule( + "point_line_normal", + point_line_normal(pack(P), pack(E0), pack(E1))); + check_degenerate_rule( + "triangle_normal", triangle_normal(pack(TA), pack(TB), pack(TC))); + check_degenerate_rule( + "line_line_normal", + line_line_normal(pack(LA0), pack(LA1), pack(LB0), pack(LB1))); + } +} + +TEST_CASE( + "SIMD batch edge length and triangle area match the scalar ones lane-wise", + "[area][simd]") +{ + const Points<3> A = random_points<3>(5), B = random_points<3>(6), + C = random_points<3>(7); + + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C); + + check_scalar_lanes("edge_length", edge_length(a, b), [&](int l) { + return edge_length(A[l], B[l]); + }); + check_scalar_lanes("triangle_area", triangle_area(a, b, c), [&](int l) { + return triangle_area(A[l], B[l], C[l]); + }); + + check_lanes("edge_length_gradient", edge_length_gradient(a, b), [&](int l) { + return edge_length_gradient(A[l], B[l]).eval(); + }); + check_lanes( + "triangle_area_gradient", triangle_area_gradient(a, b, c), + [&](int l) { return triangle_area_gradient(A[l], B[l], C[l]).eval(); }); +} + +TEST_CASE( + "SIMD batch dihedral angle matches the scalar one lane-wise", + "[angle][simd]") +{ + // Random points give each lane a different fold angle, which is what the + // atan2 and its derivatives branch on internally. + const Points<3> X0 = random_points<3>(11), X1 = random_points<3>(12), + X2 = random_points<3>(13), X3 = random_points<3>(14); + + const Eigen::Vector3 x0 = pack(X0), x1 = pack(X1), x2 = pack(X2), + x3 = pack(X3); + + check_scalar_lanes( + "dihedral_angle", dihedral_angle(x0, x1, x2, x3), + [&](int l) { return dihedral_angle(X0[l], X1[l], X2[l], X3[l]); }); + + check_lanes( + "dihedral_angle_gradient", dihedral_angle_gradient(x0, x1, x2, x3), + [&](int l) { + return dihedral_angle_gradient(X0[l], X1[l], X2[l], X3[l]).eval(); + }); + + check_lanes( + "dihedral_angle_hessian", dihedral_angle_hessian(x0, x1, x2, x3), + [&](int l) { + return dihedral_angle_hessian(X0[l], X1[l], X2[l], X3[l]).eval(); + }); +} + +#endif diff --git a/tests/src/tests/potential/CMakeLists.txt b/tests/src/tests/potential/CMakeLists.txt index 2d1a42159..b7f00b894 100644 --- a/tests/src/tests/potential/CMakeLists.txt +++ b/tests/src/tests/potential/CMakeLists.txt @@ -12,6 +12,7 @@ set(SOURCES # Benchmarks benchmark_assembly.cpp benchmark_gradient_assembly.cpp + benchmark_simd_barrier_potential.cpp # Utilities assembly_scene.cpp diff --git a/tests/src/tests/potential/benchmark_simd_barrier_potential.cpp b/tests/src/tests/potential/benchmark_simd_barrier_potential.cpp new file mode 100644 index 000000000..391e87ff1 --- /dev/null +++ b/tests/src/tests/potential/benchmark_simd_barrier_potential.cpp @@ -0,0 +1,1245 @@ +// Per-collision barrier potential (value, gradient, Hessian) evaluated with +// four scalar types -- double, float, SimdBatch, SimdBatch -- +// on the same collision set, so the four are compared on identical work. Runs +// on every scene in `assembly_scene_specs()` that is available, except the two +// smallest. +// +// The chain each variant evaluates is the one `NormalPotential` uses: +// +// d = dist²(x), b = κ·w·f(d), ∇b = κ·w·f'(d)·∇d, +// ∇²b = κ·w·(f"(d)·∇d∇dᵀ + f'(d)·∇²d) +// +// with the distance type resolved scalar-side (from the collision set) and +// passed explicitly, which a batch requires. Collisions are grouped by kind +// and distance type, then packed into an array-of-structures-of-arrays layout +// with one collision per lane, so a batch load is one contiguous read. +// +// The edge-edge mollifier is *not* applied by any variant. It is instantiated +// for batch scalars, so a batch could now evaluate it; leaving it out keeps +// the four variants on the same arithmetic as when the numbers were first +// taken. The scene report below says how many edge-edge collisions are +// actually mollified (m < 1); for the rest the mollified and unmollified +// derivatives are identical, which is what the check against the library path +// relies on. +// +// The float variants come in two flavours. "float" converts the scene's +// coordinates as they are. "float (rescaled)" first re-centers each stencil +// on its centroid and divides by d̂, in double, so a float sees O(1) numbers: +// the distance functions are translation invariant and the barrier is +// homogeneous in (d, d̂), so the result is recovered exactly by scaling κ. The +// two flavours run the very same instructions; only the packing differs. +// +// Run with (single-threaded is the primary measurement): +// ./ipc_toolkit_tests "[simd_barrier_potential]" +// +// Environment: +// IPC_TOOLKIT_BENCH_SAMPLES number of timed runs per cell (default 5) +// IPC_TOOLKIT_BENCH_OUTPUT write the results as JSON to this path (one +// report per scene under "scenes") + +#include "assembly_scene.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace ipc; + +namespace { + +// -- Scalar-type abstraction -------------------------------------------------- + +/// @brief What the evaluation loop needs to know about a scalar type `T`. +template struct ScalarTraits { + using Real = T; + static constexpr int LANES = 1; + static T load(const Real* p) { return *p; } + static void store(Real* p, const T& v) { *p = v; } + static double reduce_add(const T& v) { return double(v); } +}; + +#ifdef IPC_TOOLKIT_WITH_SIMD +template struct ScalarTraits> { + using Batch = xsimd::batch; + using Real = R; + static constexpr int LANES = int(Batch::size); + // Aligned: every caller reads/writes a whole number of lanes into a + // PackedVector, whose allocator supplies the architecture's alignment. + // The asserts check that rather than assume it -- an aligned load off a + // misaligned address happens to work on some instruction sets and faults + // on others, so it cannot be validated by running on one machine. + static Batch load(const Real* p) + { + assert(xsimd::is_aligned(p)); + return Batch::load_aligned(p); + } + static void store(Real* p, const Batch& v) + { + assert(xsimd::is_aligned(p)); + v.store_aligned(p); + } + static double reduce_add(const Batch& v) + { + return double(xsimd::reduce_add(v)); + } +}; +#endif + +template std::string variant_name(const bool rescaled) +{ + using Tr = ScalarTraits; + const bool is_float = std::is_same_v; + std::string name; + if constexpr (Tr::LANES == 1) { + name = is_float ? "float" : "double"; + } else { + name = fmt::format( + "simd<{}> x{}", is_float ? "float" : "double", Tr::LANES); + } + return rescaled ? name + " (rescaled)" : name; +} + +// -- Collision kinds ---------------------------------------------------------- + +enum class Kind : uint8_t { VV, EV, EE, FV }; + +constexpr int num_vertices(const Kind k) +{ + switch (k) { + case Kind::VV: + return 2; + case Kind::EV: + return 3; + default: + return 4; + } +} + +const char* kind_name(const Kind k) +{ + switch (k) { + case Kind::VV: + return "vertex-vertex"; + case Kind::EV: + return "edge-vertex"; + case Kind::EE: + return "edge-edge"; + default: + return "face-vertex"; + } +} + +/// @brief The distance query for a kind, for any scalar `T`. +template struct Stencil; + +template struct Stencil { + static constexpr int NV = 2; + using V = Eigen::Vector3; + static T distance(const V* x, int /*dtype*/) + { + return point_point_distance(x[0], x[1]); + } + static Eigen::Vector gradient(const V* x, int /*dtype*/) + { + return point_point_distance_gradient(x[0], x[1]); + } + static Eigen::Matrix hessian(const V* x, int /*dtype*/) + { + return point_point_distance_hessian(x[0], x[1]); + } +}; + +template struct Stencil { + static constexpr int NV = 3; + using V = Eigen::Vector3; + static T distance(const V* x, int dtype) + { + return point_edge_distance( + x[0], x[1], x[2], PointEdgeDistanceType(dtype)); + } + static Eigen::Vector gradient(const V* x, int dtype) + { + return point_edge_distance_gradient( + x[0], x[1], x[2], PointEdgeDistanceType(dtype)); + } + static Eigen::Matrix hessian(const V* x, int dtype) + { + return point_edge_distance_hessian( + x[0], x[1], x[2], PointEdgeDistanceType(dtype)); + } +}; + +template struct Stencil { + static constexpr int NV = 4; + using V = Eigen::Vector3; + static T distance(const V* x, int dtype) + { + return edge_edge_distance( + x[0], x[1], x[2], x[3], EdgeEdgeDistanceType(dtype)); + } + static Eigen::Vector gradient(const V* x, int dtype) + { + return edge_edge_distance_gradient( + x[0], x[1], x[2], x[3], EdgeEdgeDistanceType(dtype)); + } + static Eigen::Matrix hessian(const V* x, int dtype) + { + return edge_edge_distance_hessian( + x[0], x[1], x[2], x[3], EdgeEdgeDistanceType(dtype)); + } +}; + +template struct Stencil { + static constexpr int NV = 4; + using V = Eigen::Vector3; + static T distance(const V* x, int dtype) + { + return point_triangle_distance( + x[0], x[1], x[2], x[3], PointTriangleDistanceType(dtype)); + } + static Eigen::Vector gradient(const V* x, int dtype) + { + return point_triangle_distance_gradient( + x[0], x[1], x[2], x[3], PointTriangleDistanceType(dtype)); + } + static Eigen::Matrix hessian(const V* x, int dtype) + { + return point_triangle_distance_hessian( + x[0], x[1], x[2], x[3], PointTriangleDistanceType(dtype)); + } +}; + +// -- Grouping and packing ----------------------------------------------------- + +/// @brief The collisions of one kind and distance type (lane-layout agnostic). +struct GroupSpec { + Kind kind; + int dtype; + std::vector ids; ///< Indices into the NormalCollisions. + + std::string name() const + { + return fmt::format("{} (dtype {})", kind_name(kind), dtype); + } +}; + +/// @brief One group packed for a lane count `L`, in real type `R`. +/// +/// Layout: block b (L consecutive collisions) occupies +/// `x[b*3*NV*L + comp*L + lane]`, i.e. within a block each of the 3·NV +/// coordinates is stored contiguously across lanes. For L = 1 this is the +/// plain per-collision DOF vector `CollisionStencil::dof` returns. +#ifdef IPC_TOOLKIT_WITH_SIMD +/// @brief Allocator giving a packed buffer the alignment a batch load wants. +/// +/// `xsimd::default_allocator` falls back to `std::allocator` on an +/// architecture with no alignment requirement, so this is safe either way. +template using PackedAllocator = xsimd::default_allocator; +#else +template using PackedAllocator = std::allocator; +#endif + +/// @brief A packed buffer, aligned for `load_aligned`/`store_aligned`. +/// +/// Every batch load and store below is at an offset that is a whole number of +/// lanes into one of these -- and one lane's width times the lane count *is* +/// the architecture's alignment -- so an aligned base makes every access +/// aligned. That matters here rather than in the tests: an unaligned load can +/// cost throughput on some instruction sets, which is the very thing this +/// benchmark measures. +template using PackedVector = std::vector>; + +template struct PackedGroup { + Kind kind = Kind::VV; + int dtype = 0; + size_t n = 0; ///< Real collisions. + size_t nblocks = 0; ///< ceil(n / L). + int lanes = 1; + PackedVector x; ///< nblocks * 3*NV * L + PackedVector w; ///< nblocks * L; zero on padding lanes. + PackedVector grad_out; + PackedVector hess_out; + + int nv() const { return num_vertices(kind); } + int ndof() const { return 3 * nv(); } + + R grad_at(size_t i, int comp) const + { + return grad_out + [(i / lanes) * ndof() * lanes + comp * lanes + (i % lanes)]; + } + R hess_at(size_t i, int entry) const + { + return hess_out + [(i / lanes) * ndof() * ndof() * lanes + entry * lanes + + (i % lanes)]; + } +}; + +std::vector group_collisions(const NormalCollisions& collisions) +{ + // A map from (kind, dtype) to its group, kept in first-seen order. + std::vector groups; + const auto group_for = [&](const Kind kind, const int dtype) -> GroupSpec& { + for (GroupSpec& g : groups) { + if (g.kind == kind && g.dtype == dtype) { + return g; + } + } + groups.push_back(GroupSpec { kind, dtype, {} }); + return groups.back(); + }; + + for (size_t i = 0; i < collisions.size(); i++) { + if (collisions.is_vertex_vertex(i)) { + group_for(Kind::VV, int(PointPointDistanceType::P_P)) + .ids.push_back(i); + } else if (collisions.is_edge_vertex(i)) { + const auto& ev = + dynamic_cast(collisions[i]); + group_for(Kind::EV, int(ev.known_dtype())).ids.push_back(i); + } else if (collisions.is_edge_edge(i)) { + const auto& ee = + dynamic_cast(collisions[i]); + group_for(Kind::EE, int(ee.known_dtype())).ids.push_back(i); + } else if (collisions.is_face_vertex(i)) { + const auto& fv = + dynamic_cast(collisions[i]); + group_for(Kind::FV, int(fv.known_dtype())).ids.push_back(i); + } else { + throw std::runtime_error("unsupported collision kind"); + } + } + + // Largest groups first, so a truncated run still covers the bulk. + std::stable_sort( + groups.begin(), groups.end(), + [](const GroupSpec& a, const GroupSpec& b) { + return a.ids.size() > b.ids.size(); + }); + return groups; +} + +/// @param scale Coordinates are stored as (x - centroid) / scale; 1 means as +/// they are (no re-centering). +template +PackedGroup pack_group( + const GroupSpec& spec, + const int lanes, + const ipc::tests::AssemblyScene& scene, + const double scale) +{ + const NormalCollisions& collisions = scene.collisions(); + const Eigen::MatrixXi& edges = scene.mesh().edges(); + const Eigen::MatrixXi& faces = scene.mesh().faces(); + const Eigen::MatrixXd& X = scene.vertices(); + + PackedGroup g; + g.kind = spec.kind; + g.dtype = spec.dtype; + g.lanes = lanes; + g.n = spec.ids.size(); + g.nblocks = (g.n + lanes - 1) / lanes; + const int ndof = g.ndof(); + + g.x.assign(g.nblocks * ndof * lanes, R(0)); + g.w.assign(g.nblocks * lanes, R(0)); + + for (size_t i = 0; i < g.nblocks * size_t(lanes); i++) { + // Padding lanes replay the last collision with zero weight so every + // lane holds a finite, in-range configuration. + const size_t src = std::min(i, g.n - 1); + const NormalCollision& c = collisions[spec.ids[src]]; + VectorMax12d dof = c.dof(X, edges, faces); + assert(dof.size() == ndof); + + if (scale != 1.0) { + const Eigen::Vector3d centroid = + dof.reshaped(3, dof.size() / 3).rowwise().mean(); + for (int v = 0; v < ndof / 3; v++) { + dof.segment<3>(3 * v) = + (dof.segment<3>(3 * v) - centroid) / scale; + } + } + + R* block = g.x.data() + (i / lanes) * ndof * lanes + (i % lanes); + for (int k = 0; k < ndof; k++) { + block[k * lanes] = R(dof[k]); + } + g.w[i] = i < g.n ? R(c.weight) : R(0); + } + return g; +} + +// -- The evaluation loop ------------------------------------------------------ + +struct Params { + double dhat_sqr; ///< The potential is a function of squared distance. + double kappa; +}; + +enum class Quantity : uint8_t { VALUE, GRADIENT, HESSIAN, HESSIAN_SUM }; + +const char* quantity_name(const Quantity q) +{ + switch (q) { + case Quantity::VALUE: + return "value"; + case Quantity::GRADIENT: + return "gradient"; + case Quantity::HESSIAN: + return "hessian"; + default: + return "hessian_sum"; + } +} + +constexpr Quantity QUANTITIES[] = { Quantity::VALUE, Quantity::GRADIENT, + Quantity::HESSIAN, Quantity::HESSIAN_SUM }; + +template struct Evaluator { + using Tr = ScalarTraits; + using R = typename Tr::Real; + using S = Stencil; + static constexpr int L = Tr::LANES; + static constexpr int NV = S::NV; + static constexpr int N = 3 * NV; + /// @brief Blocks summed in T before the partial sum is moved to double. + static constexpr size_t REDUCE_CHUNK = 256; + + static void + load(const PackedGroup& g, const size_t b, Eigen::Vector3* x, T& w) + { + const R* in = g.x.data() + b * (N * L); + for (int v = 0; v < NV; v++) { + for (int k = 0; k < 3; k++) { + x[v][k] = Tr::load(in + (3 * v + k) * L); + } + } + w = Tr::load(g.w.data() + b * L); + } + + static double + value(const PackedGroup& g, const Params& p, size_t b0, size_t b1) + { + const T dhat(R(p.dhat_sqr)); + const T kappa(R(p.kappa)); + Eigen::Vector3 x[NV]; + T w; + double total = 0; + // Sum in T over a chunk, then in double across chunks, so a float's + // accumulation error is bounded by the chunk length and the accuracy + // column reflects the per-collision precision. + for (size_t c0 = b0; c0 < b1; c0 += REDUCE_CHUNK) { + T acc(R(0)); + const size_t c1 = std::min(b1, c0 + REDUCE_CHUNK); + for (size_t b = c0; b < c1; b++) { + load(g, b, x, w); + const T d = S::distance(x, g.dtype); + acc += (kappa * w) * barrier(d, dhat); + } + total += Tr::reduce_add(acc); + } + return total; + } + + static void + gradient(PackedGroup& g, const Params& p, size_t b0, size_t b1) + { + const T dhat(R(p.dhat_sqr)); + const T kappa(R(p.kappa)); + Eigen::Vector3 x[NV]; + T w; + for (size_t b = b0; b < b1; b++) { + load(g, b, x, w); + const T d = S::distance(x, g.dtype); + const Eigen::Vector grad_d = S::gradient(x, g.dtype); + const T grad_f = barrier_first_derivative(d, dhat); + const Eigen::Vector grad = (kappa * w * grad_f) * grad_d; + + R* out = g.grad_out.data() + b * (N * L); + for (int k = 0; k < N; k++) { + Tr::store(out + k * L, grad[k]); + } + } + } + + static Eigen::Matrix local_hessian( + const Eigen::Vector3* x, + const T& w, + const int dtype, + const T& dhat, + const T& kappa) + { + const T d = S::distance(x, dtype); + const Eigen::Vector grad_d = S::gradient(x, dtype); + const Eigen::Matrix hess_d = S::hessian(x, dtype); + const T grad_f = barrier_first_derivative(d, dhat); + const T hess_f = barrier_second_derivative(d, dhat); + const T kw = kappa * w; + return (kw * hess_f) * (grad_d * grad_d.transpose()) + + (kw * grad_f) * hess_d; + } + + static void + hessian(PackedGroup& g, const Params& p, size_t b0, size_t b1) + { + const T dhat(R(p.dhat_sqr)); + const T kappa(R(p.kappa)); + Eigen::Vector3 x[NV]; + T w; + for (size_t b = b0; b < b1; b++) { + load(g, b, x, w); + const Eigen::Matrix hess = + local_hessian(x, w, g.dtype, dhat, kappa); + R* out = g.hess_out.data() + b * (N * N * L); + for (int k = 0; k < N * N; k++) { + Tr::store(out + k * L, hess.data()[k]); + } + } + } + + /// @brief The Hessian's compute cost alone: every entry is summed into + /// one accumulator instead of being written out, so the 144 stores per + /// collision -- which are memory-bound once threads share the bus -- are + /// not part of the measurement. + static double + hessian_sum(const PackedGroup& g, const Params& p, size_t b0, size_t b1) + { + const T dhat(R(p.dhat_sqr)); + const T kappa(R(p.kappa)); + Eigen::Vector3 x[NV]; + T w; + double total = 0; + for (size_t c0 = b0; c0 < b1; c0 += REDUCE_CHUNK) { + T acc(R(0)); + const size_t c1 = std::min(b1, c0 + REDUCE_CHUNK); + for (size_t b = c0; b < c1; b++) { + load(g, b, x, w); + acc += local_hessian(x, w, g.dtype, dhat, kappa).sum(); + } + total += Tr::reduce_add(acc); + } + return total; + } +}; + +/// @brief Dispatch on the kind at runtime; everything inside is static. +template +double run_group( + PackedGroup::Real>& g, + const Params& p, + const Quantity q, + const size_t b0, + const size_t b1) +{ + const auto run = [&](auto kind_tag) -> double { + using E = Evaluator; + switch (q) { + case Quantity::VALUE: + return E::value(g, p, b0, b1); + case Quantity::GRADIENT: + E::gradient(g, p, b0, b1); + return 0; + case Quantity::HESSIAN: + E::hessian(g, p, b0, b1); + return 0; + default: + return E::hessian_sum(g, p, b0, b1); + } + }; + switch (g.kind) { + case Kind::VV: + return run(std::integral_constant()); + case Kind::EV: + return run(std::integral_constant()); + case Kind::EE: + return run(std::integral_constant()); + default: + return run(std::integral_constant()); + } +} + +/// @brief All groups packed for one scalar type, plus its outputs. +template struct Variant { + using Tr = ScalarTraits; + using R = typename Tr::Real; + + /// @brief Coordinates are stored as (x - centroid) / scale. + double scale = 1.0; + std::vector> groups; + + void pack( + const std::vector& specs, + const ipc::tests::AssemblyScene& scene) + { + groups.clear(); + for (const GroupSpec& spec : specs) { + groups.push_back(pack_group(spec, Tr::LANES, scene, scale)); + } + } + + void allocate_outputs(const Quantity q) + { + for (PackedGroup& g : groups) { + const size_t per_block = size_t(g.ndof()) * g.lanes; + if (q == Quantity::GRADIENT) { + g.grad_out.assign(g.nblocks * per_block, R(0)); + } else if (q == Quantity::HESSIAN) { + g.hess_out.assign(g.nblocks * per_block * g.ndof(), R(0)); + } + } + } + + /// @brief The parameters in the packed coordinates. + /// + /// With x' = (x - c)/s: d'² = d²/s², and since the barrier is homogeneous, + /// b(d'², d̂²/s²) = b(d², d̂²)/s⁴. Its first derivative wrt d² picks up a + /// further s², ∇ₓ' a further s, so the value, gradient, and Hessian are + /// s⁴, s³, and s² too small; folding those into κ recovers them exactly. + Params effective(const Params& p, const Quantity q) const + { + if (scale == 1.0) { + return p; + } + const double s = scale; + const double kappa_scale = q == Quantity::VALUE ? s * s * s * s + : q == Quantity::GRADIENT ? s * s * s + : s * s; + return Params { p.dhat_sqr / (s * s), p.kappa * kappa_scale }; + } + + double run(const Params& params, const Quantity q, const bool parallel) + { + const Params p = effective(params, q); + double total = 0; + for (PackedGroup& g : groups) { + if (!parallel) { + total += run_group(g, p, q, 0, g.nblocks); + continue; + } + // Roughly 1k collisions per task, so the scheduling overhead is + // negligible next to the work. + const size_t grain = std::max(1, 1024 / Tr::LANES); + total += tbb::parallel_reduce( + tbb::blocked_range(0, g.nblocks, grain), 0.0, + [&](const tbb::blocked_range& r, double partial) { + return partial + run_group(g, p, q, r.begin(), r.end()); + }, + std::plus()); + } + return total; + } +}; + +// -- Timing ------------------------------------------------------------------- + +struct Timing { + double median_s = 0; + double min_s = 0; +}; + +template Timing time_runs(F&& f, const int num_samples) +{ + std::vector samples; + samples.reserve(num_samples); + for (int i = 0; i < num_samples; i++) { + const auto start = std::chrono::steady_clock::now(); + f(); + const auto end = std::chrono::steady_clock::now(); + samples.push_back(std::chrono::duration(end - start).count()); + } + std::sort(samples.begin(), samples.end()); + return Timing { samples[samples.size() / 2], samples.front() }; +} + +/// @brief |got - want| / |want|, where two infinities of the same sign agree: +/// a scene with a zero-distance collision has an infinite barrier value. +double relative_error(const double got, const double want) +{ + if (!std::isfinite(want)) { + return got == want ? 0.0 : std::numeric_limits::infinity(); + } + return std::abs(got - want) / std::abs(want); +} + +int env_int(const char* name, const int fallback) +{ + const char* s = std::getenv(name); + return s != nullptr ? std::atoi(s) : fallback; +} + +// -- Accuracy ----------------------------------------------------------------- + +struct Accuracy { + double value_rel = 0; + double grad_max_rel = 0; + double grad_median_rel = 0; + double hess_max_rel = 0; + double hess_median_rel = 0; + size_t non_finite = 0; + /// @brief Collisions with at least one non-finite entry, per group. + std::vector> non_finite_by_group; +}; + +double median_of(std::vector& v) +{ + if (v.empty()) { + return 0; + } + std::sort(v.begin(), v.end()); + return v[v.size() / 2]; +} + +/// @brief Per-collision relative error in the ∞-norm, scaled by the +/// reference's ∞-norm (derivatives of near-parallel edges are ill-conditioned, +/// so an entry-wise relative error would be dominated by cancellations). +/// @return Number of collisions with a non-finite entry. +template +size_t compare_outputs( + const PackedGroup& got, + const PackedGroup& ref, + const Quantity q, + std::vector& rel_errors, + size_t& non_finite) +{ + const int n_entries = + q == Quantity::GRADIENT ? got.ndof() : got.ndof() * got.ndof(); + size_t bad_collisions = 0; + for (size_t i = 0; i < got.n; i++) { + double max_diff = 0, max_ref = 0; + bool any_non_finite = false; + for (int k = 0; k < n_entries; k++) { + const double a = q == Quantity::GRADIENT + ? double(got.grad_at(i, k)) + : double(got.hess_at(i, k)); + const double b = q == Quantity::GRADIENT + ? double(ref.grad_at(i, k)) + : double(ref.hess_at(i, k)); + if (!std::isfinite(a)) { + non_finite++; + any_non_finite = true; + } + max_diff = std::max(max_diff, std::abs(a - b)); + max_ref = std::max(max_ref, std::abs(b)); + } + bad_collisions += any_non_finite; + rel_errors.push_back( + max_diff / std::max(max_ref, std::numeric_limits::min())); + } + return bad_collisions; +} + +// -- The library path, for reference ------------------------------------------ + +/// @brief The library's own per-collision evaluation (virtual dispatch, +/// dynamic-size `VectorMax12d`, and the mollifier for edge-edge), gathering +/// the DOF from the vertex matrix as `Potential::gradient` does. +double library_path( + const ipc::tests::AssemblyScene& scene, + const Quantity q, + const bool parallel, + std::vector& out) +{ + const NormalCollisions& collisions = scene.collisions(); + const BarrierPotential& potential = scene.potential(); + const Eigen::MatrixXi& edges = scene.mesh().edges(); + const Eigen::MatrixXi& faces = scene.mesh().faces(); + const Eigen::MatrixXd& X = scene.vertices(); + + const size_t stride = q == Quantity::GRADIENT ? 12 : 144; + const bool stores = q == Quantity::GRADIENT || q == Quantity::HESSIAN; + if (stores && out.size() != collisions.size() * stride) { + out.assign(collisions.size() * stride, 0.0); + } + + const auto body = [&](const size_t begin, const size_t end) { + double partial = 0; + for (size_t i = begin; i < end; i++) { + const NormalCollision& c = collisions[i]; + const VectorMax12d dof = c.dof(X, edges, faces); + if (q == Quantity::VALUE) { + partial += potential(c, dof); + } else if (q == Quantity::GRADIENT) { + const VectorMax12d grad = potential.gradient(c, dof); + std::copy( + grad.data(), grad.data() + grad.size(), + out.data() + i * stride); + } else if (q == Quantity::HESSIAN) { + const MatrixMax12d hess = + potential.hessian(c, dof, PSDProjectionMethod::NONE); + std::copy( + hess.data(), hess.data() + hess.size(), + out.data() + i * stride); + } else { + partial += + potential.hessian(c, dof, PSDProjectionMethod::NONE).sum(); + } + } + return partial; + }; + + if (!parallel) { + return body(0, collisions.size()); + } + return tbb::parallel_reduce( + tbb::blocked_range(0, collisions.size(), 1024), 0.0, + [&](const tbb::blocked_range& r, double partial) { + return partial + body(r.begin(), r.end()); + }, + std::plus()); +} + +// -- Results ------------------------------------------------------------------ + +/// @brief A double as a JSON value; JSON has no inf/nan, so those are strings. +std::string json_number(const double x) +{ + return std::isfinite(x) ? fmt::format("{:.6g}", x) + : fmt::format("\"{}\"", x); +} + +struct Cell { + std::string variant; + Quantity quantity; + bool parallel; + Timing timing; +}; + +struct Report { + std::string scene; + size_t num_collisions = 0; + std::vector> composition; + size_t num_mollified_ee = 0; + int num_samples = 0; + int num_threads = 1; + std::vector cells; + std::vector> pack_seconds; + std::vector> accuracy; + std::string reference_check; + + std::string to_json() const + { + fmt::memory_buffer buf; + auto f = std::back_inserter(buf); + fmt::format_to(f, "{{\n"); + fmt::format_to(f, " \"scene\": \"{}\",\n", scene); + fmt::format_to(f, " \"num_collisions\": {},\n", num_collisions); + fmt::format_to(f, " \"num_mollified_ee\": {},\n", num_mollified_ee); + fmt::format_to(f, " \"num_samples\": {},\n", num_samples); + fmt::format_to(f, " \"num_threads\": {},\n", num_threads); + fmt::format_to(f, " \"composition\": {{"); + for (size_t i = 0; i < composition.size(); i++) { + fmt::format_to( + f, "{}\"{}\": {}", i ? ", " : "", composition[i].first, + composition[i].second); + } + fmt::format_to(f, "}},\n \"pack_seconds\": {{"); + for (size_t i = 0; i < pack_seconds.size(); i++) { + fmt::format_to( + f, "{}\"{}\": {:.9g}", i ? ", " : "", pack_seconds[i].first, + pack_seconds[i].second); + } + fmt::format_to(f, "}},\n \"accuracy\": {{\n"); + for (size_t i = 0; i < accuracy.size(); i++) { + const Accuracy& a = accuracy[i].second; + fmt::format_to( + f, + " \"{}\": {{\"value_rel\": {}, \"grad_max_rel\": {}, " + "\"grad_median_rel\": {}, \"hess_max_rel\": {}, " + "\"hess_median_rel\": {}, \"non_finite\": {}, " + "\"non_finite_collisions_by_group\": {{", + accuracy[i].first, json_number(a.value_rel), + json_number(a.grad_max_rel), json_number(a.grad_median_rel), + json_number(a.hess_max_rel), json_number(a.hess_median_rel), + a.non_finite); + for (size_t j = 0; j < a.non_finite_by_group.size(); j++) { + fmt::format_to( + f, "{}\"{}\": {}", j ? ", " : "", + a.non_finite_by_group[j].first, + a.non_finite_by_group[j].second); + } + fmt::format_to(f, "}}}}{}\n", i + 1 < accuracy.size() ? "," : ""); + } + fmt::format_to(f, " }},\n \"timings\": [\n"); + for (size_t i = 0; i < cells.size(); i++) { + const Cell& c = cells[i]; + fmt::format_to( + f, + " {{\"variant\": \"{}\", \"quantity\": \"{}\", " + "\"parallel\": {}, \"median_s\": {:.9g}, \"min_s\": {:.9g}}}{}" + "\n", + c.variant, quantity_name(c.quantity), + c.parallel ? "true" : "false", c.timing.median_s, + c.timing.min_s, i + 1 < cells.size() ? "," : ""); + } + fmt::format_to(f, " ]\n}}"); + return fmt::to_string(buf); + } +}; + +/// @brief Time and check one scalar type; append to the report. +template +void bench_variant( + const ipc::tests::AssemblyScene& scene, + const std::vector& specs, + const Params& params, + Variant& reference, + Report& report, + Variant& variant) +{ + const std::string name = variant_name(variant.scale != 1.0); + + const Timing pack_time = time_runs([&] { variant.pack(specs, scene); }, 1); + report.pack_seconds.emplace_back(name, pack_time.median_s); + + Accuracy acc; + std::vector bad_by_group(specs.size(), 0); + for (const bool parallel : { false, true }) { + for (const Quantity q : QUANTITIES) { + variant.allocate_outputs(q); + // Warm-up touches the freshly allocated outputs so first-touch + // page faults are not in the timed runs. + double value = variant.run(params, q, parallel); + const Timing t = time_runs( + [&] { value = variant.run(params, q, parallel); }, + report.num_samples); + report.cells.push_back(Cell { name, q, parallel, t }); + + if (parallel) { + continue; + } + if (q == Quantity::VALUE) { + const double ref_value = reference.run(params, q, false); + acc.value_rel = relative_error(value, ref_value); + } else if (q != Quantity::HESSIAN_SUM) { + std::vector rel; + for (size_t i = 0; i < variant.groups.size(); i++) { + bad_by_group[i] += compare_outputs( + variant.groups[i], reference.groups[i], q, rel, + acc.non_finite); + } + const double max_rel = + rel.empty() ? 0 : *std::max_element(rel.begin(), rel.end()); + const double median_rel = median_of(rel); + if (q == Quantity::GRADIENT) { + acc.grad_max_rel = max_rel; + acc.grad_median_rel = median_rel; + } else { + acc.hess_max_rel = max_rel; + acc.hess_median_rel = median_rel; + } + } + } + } + for (size_t i = 0; i < specs.size(); i++) { + if (bad_by_group[i] > 0) { + acc.non_finite_by_group.emplace_back( + specs[i].name(), bad_by_group[i]); + } + } + report.accuracy.emplace_back(name, acc); +} + +/// @brief Check the double path against the library on every collision the +/// mollifier leaves untouched (m = 1), where the two must agree to rounding. +std::string check_against_library( + const ipc::tests::AssemblyScene& scene, + const std::vector& specs, + const Params& params, + Variant& variant, + size_t& num_mollified_ee) +{ + const NormalCollisions& collisions = scene.collisions(); + const BarrierPotential& potential = scene.potential(); + const Eigen::MatrixXi& edges = scene.mesh().edges(); + const Eigen::MatrixXi& faces = scene.mesh().faces(); + const Eigen::MatrixXd& X = scene.vertices(); + + variant.allocate_outputs(Quantity::GRADIENT); + variant.run(params, Quantity::GRADIENT, true); + variant.allocate_outputs(Quantity::HESSIAN); + variant.run(params, Quantity::HESSIAN, true); + const double value = variant.run(params, Quantity::VALUE, true); + + double ref_value = 0, max_grad_rel = 0, max_hess_rel = 0; + size_t checked = 0; + num_mollified_ee = 0; + for (size_t gi = 0; gi < specs.size(); gi++) { + const GroupSpec& spec = specs[gi]; + const PackedGroup& g = variant.groups[gi]; + for (size_t i = 0; i < spec.ids.size(); i++) { + const NormalCollision& c = collisions[spec.ids[i]]; + const VectorMax12d dof = c.dof(X, edges, faces); + + // The unmollified potential, from the library's own pieces. + ref_value += params.kappa * c.weight + * potential.barrier()(c.compute_distance(dof), params.dhat_sqr); + + if (c.is_mollified() && c.mollifier(dof) < 1.0) { + num_mollified_ee++; + continue; + } + + const VectorMax12d grad = potential.gradient(c, dof); + const MatrixMax12d hess = + potential.hessian(c, dof, PSDProjectionMethod::NONE); + double grad_diff = 0, hess_diff = 0; + for (int k = 0; k < grad.size(); k++) { + grad_diff = + std::max(grad_diff, std::abs(grad[k] - g.grad_at(i, k))); + } + for (int k = 0; k < hess.size(); k++) { + hess_diff = std::max( + hess_diff, std::abs(hess.data()[k] - g.hess_at(i, k))); + } + const double grad_scale = + std::max(grad.template lpNorm(), 1e-300); + const double hess_scale = + std::max(hess.template lpNorm(), 1e-300); + max_grad_rel = std::max(max_grad_rel, grad_diff / grad_scale); + max_hess_rel = std::max(max_hess_rel, hess_diff / hess_scale); + checked++; + } + } + + return fmt::format( + "double vs. library on {} unmollified collisions: value rel. err " + "{:.2e}{}, gradient max rel. err {:.2e}, Hessian max rel. err {:.2e}", + checked, relative_error(value, ref_value), + std::isfinite(ref_value) + ? "" + : " (the value is infinite: a collision has zero distance)", + max_grad_rel, max_hess_rel); +} + +void print_report(const Report& r) +{ + fmt::print("\n=== Barrier potential per scalar type: {} ===\n", r.scene); + fmt::print("{} collisions:", r.num_collisions); + for (const auto& [name, count] : r.composition) { + fmt::print(" {} {};", count, name); + } + fmt::print( + "\n{} edge-edge collisions are mollified (m < 1) and evaluated here " + "without the mollifier.\n", + r.num_mollified_ee); + fmt::print( + "{}\nMedian of {} runs; parallel columns use {} threads. " + "\"hess-sum\" evaluates the Hessian without storing it.\n\n", + r.reference_check, r.num_samples, r.num_threads); + + const auto find = [&](const std::string& v, Quantity q, bool par) { + for (const Cell& c : r.cells) { + if (c.variant == v && c.quantity == q && c.parallel == par) { + return c.timing.median_s; + } + } + return std::numeric_limits::quiet_NaN(); + }; + + std::vector variants; + for (const Cell& c : r.cells) { + if (std::find(variants.begin(), variants.end(), c.variant) + == variants.end()) { + variants.push_back(c.variant); + } + } + + const double per = 1e9 / double(r.num_collisions); + for (const bool parallel : { false, true }) { + fmt::print("--- {} ---\n", parallel ? "parallel" : "single-threaded"); + fmt::print("{:<24}", "variant"); + for (const char* h : { "value", "grad", "hess", "hess-sum" }) { + fmt::print(" {:>9}", fmt::format("{}(ms)", h)); + } + fmt::print(" |"); + for (const char* h : { "value", "grad", "hess", "hess-sum" }) { + fmt::print(" {:>9}", fmt::format("{} ns", h)); + } + fmt::print(" |"); + for (const char* h : { "value", "grad", "hess", "hess-sum" }) { + fmt::print(" {:>9}", fmt::format("{} x", h)); + } + fmt::print("\n"); + for (const std::string& v : variants) { + fmt::print("{:<24}", v); + for (const Quantity q : QUANTITIES) { + fmt::print(" {:>9.2f}", find(v, q, parallel) * 1e3); + } + fmt::print(" |"); + for (const Quantity q : QUANTITIES) { + fmt::print(" {:>9.1f}", find(v, q, parallel) * per); + } + fmt::print(" |"); + for (const Quantity q : QUANTITIES) { + fmt::print( + " {:>8.2f}x", + find("double", q, parallel) / find(v, q, parallel)); + } + fmt::print("\n"); + } + fmt::print("\n"); + } + + fmt::print("--- packing (gather into lane layout, once per variant) ---\n"); + for (const auto& [name, s] : r.pack_seconds) { + fmt::print("{:<24} {:>9.2f} ms\n", name, s * 1e3); + } + + fmt::print("\n--- accuracy relative to double ---\n"); + fmt::print( + "{:<24} {:>10} {:>12} {:>12} {:>12} {:>12} {:>10}\n", "variant", + "value rel", "grad max", "grad median", "hess max", "hess median", + "non-finite"); + for (const auto& [name, a] : r.accuracy) { + fmt::print( + "{:<24} {:>10.2e} {:>12.2e} {:>12.2e} {:>12.2e} {:>12.2e} {:>10}\n", + name, a.value_rel, a.grad_max_rel, a.grad_median_rel, + a.hess_max_rel, a.hess_median_rel, a.non_finite); + } + for (const auto& [name, a] : r.accuracy) { + if (a.non_finite_by_group.empty()) { + continue; + } + fmt::print(" {}: collisions with a non-finite entry:", name); + for (const auto& [group, count] : a.non_finite_by_group) { + fmt::print(" {} {};", count, group); + } + fmt::print("\n"); + } + fmt::print("\n"); + std::fflush(stdout); +} + +/// @brief Run every variant on one scene and return its report. +Report bench_scene(const ipc::tests::AssemblyScene& scene) +{ + Report report; + report.scene = scene.label(); + report.num_collisions = scene.num_collisions(); + report.num_samples = env_int("IPC_TOOLKIT_BENCH_SAMPLES", 5); + report.num_threads = tbb::this_task_arena::max_concurrency(); + + const std::vector groups = group_collisions(scene.collisions()); + for (const GroupSpec& g : groups) { + report.composition.emplace_back(g.name(), g.ids.size()); + } + + const double dhat = scene.potential().dhat(); + const Params params { dhat * dhat, scene.potential().stiffness() }; + + // The double path first: it is the reference the others are checked + // against, and is itself checked against the library. + Variant ref; + ref.pack(groups, scene); + report.reference_check = check_against_library( + scene, groups, params, ref, report.num_mollified_ee); + + const auto bench = [&](auto& variant, const double scale) { + variant.scale = scale; + bench_variant(scene, groups, params, ref, report, variant); + }; + { + Variant v; + bench(v, 1.0); + } + { + Variant v; + bench(v, 1.0); + bench(v, dhat); + } +#ifdef IPC_TOOLKIT_WITH_SIMD + { + Variant> v; + bench(v, 1.0); + } + { + Variant> v; + bench(v, 1.0); + bench(v, dhat); + } +#endif + + // The library's own path, so the hand-rolled double loop above can be + // placed relative to what `Potential::gradient`/`hessian` actually do. + { + std::vector out; + for (const bool parallel : { false, true }) { + for (const Quantity q : QUANTITIES) { + library_path(scene, q, parallel, out); // warm-up + const Timing t = time_runs( + [&] { library_path(scene, q, parallel, out); }, + report.num_samples); + report.cells.push_back( + Cell { "library (double)", q, parallel, t }); + } + } + } + + return report; +} + +} // namespace + +TEST_CASE( + "Barrier potential per scalar type", + "[!benchmark][simd][simd_barrier_potential]") +{ + // The two smallest scenes have a few hundred collisions: one or two TBB + // tasks, and timings dominated by fixed overhead rather than the kernels. + const std::vector skipped = { "two-cubes", "bunny" }; + + // One report per available scene, in the specs' order (increasing size). + std::vector reports; + for (const ipc::tests::AssemblySceneSpec& spec : + ipc::tests::assembly_scene_specs()) { + if (std::find(skipped.begin(), skipped.end(), spec.label) + != skipped.end()) { + continue; + } + const std::optional scene = + ipc::tests::build_assembly_scene(spec); + if (!scene.has_value()) { + fmt::print("Skipping {}: mesh not available\n", spec.label); + continue; + } + reports.push_back(bench_scene(scene.value())); + print_report(reports.back()); + } + REQUIRE(!reports.empty()); + + if (const char* path = std::getenv("IPC_TOOLKIT_BENCH_OUTPUT")) { + std::ofstream f(path); + f << "{\n \"scenes\": [\n"; + for (size_t i = 0; i < reports.size(); i++) { + f << reports[i].to_json() + << (i + 1 < reports.size() ? ",\n" : "\n"); + } + f << " ]\n}\n"; + fmt::print("Wrote {}\n", path); + } +} diff --git a/tests/src/tests/simd_utils.hpp b/tests/src/tests/simd_utils.hpp new file mode 100644 index 000000000..2ca30c7b8 --- /dev/null +++ b/tests/src/tests/simd_utils.hpp @@ -0,0 +1,251 @@ +#pragma once + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace ipc::tests { + +/// @brief The batch type the SIMD tests pack their lanes into. +using Batch = ipc::SimdBatch; + +/// @brief Lanes per batch: the number of independent problems one call solves. +constexpr int L = int(Batch::size); + +/// @brief The alignment a batch load/store wants. +/// +/// The scalar fallback architecture reports 0, which `alignas` treats as "no +/// effect" but which would under-align the array on its own, so take the +/// scalar's own alignment as the floor. +constexpr std::size_t LANE_ALIGNMENT = Batch::arch_type::alignment() + > alignof(double) + ? Batch::arch_type::alignment() + : alignof(double); + +/// @brief One lane's worth of scalars per lane, aligned for a batch +/// load/store. +/// +/// The `alignas` is what makes `load_aligned`/`store_aligned` usable: a plain +/// `std::array` is only guaranteed `alignof(double)`, and an +/// aligned load off an under-aligned address faults on the architectures that +/// require alignment rather than merely running slower. +struct alignas(LANE_ALIGNMENT) Lanes { + std::array values {}; + + double& operator[](const int l) { return values[size_t(l)]; } + const double& operator[](const int l) const { return values[size_t(l)]; } + double* data() { return values.data(); } + const double* data() const { return values.data(); } + + /// @brief Load these lanes into one batch. + Batch load() const { return Batch::load_aligned(data()); } + + /// @brief Unpack a batch back into its lanes. + static Lanes from(const Batch& b) + { + Lanes out; + b.store_aligned(out.data()); + return out; + } +}; + +static_assert( + alignof(Lanes) >= LANE_ALIGNMENT, + "Lanes must satisfy the alignment its load/store claims"); + +/// @brief `L` problems, one per lane. +template using Points = std::array, L>; + +// --------------------------------------------------------------------------- +// Tolerances + +/// @brief Agreement bound for a value. +/// +/// The batch and scalar paths differ only in how the compiler contracts +/// multiply-adds, which for a value costs no more than a rounding step. +constexpr double VALUE_TOL = 1e-14; + +/// @brief Agreement bound for a derivative. +/// +/// Applied relative to the magnitude of the *whole* result rather than +/// per-entry: a gradient or Hessian of a near-degenerate configuration has +/// entries that are catastrophic cancellations of much larger terms, so a +/// per-entry bound would reject a correct result. A structurally wrong entry +/// differs by order of the result's own magnitude. +constexpr double DERIVATIVE_TOL = 1e-9; + +// --------------------------------------------------------------------------- +// Packing + +/// @brief Pack the k-th coordinate of `L` problems into one batch each. +template inline Eigen::Vector pack(const Points& v) +{ + Eigen::Vector out; + for (int k = 0; k < dim; ++k) { + Lanes lane; + for (int l = 0; l < L; ++l) { + lane[l] = v[size_t(l)][k]; + } + out[k] = lane.load(); + } + return out; +} + +/// @brief `L` random points, deterministic in `seed`. +template inline Points random_points(const int seed) +{ + std::srand(unsigned(seed)); + Points v; + for (int l = 0; l < L; ++l) { + v[size_t(l)] = Eigen::Vector::Random(); + } + return v; +} + +/// @brief `cases` scaled by `s`, for a case list written as multiples of a +/// threshold (speeds as multiples of eps_v, distances as multiples of dhat). +template +inline std::array +scaled(const std::array& cases, const double s) +{ + std::array out {}; + for (std::size_t i = 0; i < N; ++i) { + out[i] = cases[i] * s; + } + return out; +} + +/// @brief Assign `cases` round-robin to the `L` lanes, starting at `offset`. +/// +/// A batch may hold fewer lanes than there are cases, so a test sweeps the +/// offset over `cases.size()` to reach every case — and, where the two counts +/// differ, to land different cases in the same batch, which is what exercises +/// the per-lane blend. +template +inline Lanes lane_cases(const Container& cases, const int offset) +{ + Lanes lanes; + for (int l = 0; l < L; ++l) { + lanes[l] = cases[(size_t(l) + size_t(offset)) % cases.size()]; + } + return lanes; +} + +// --------------------------------------------------------------------------- +// Comparison + +/// @brief The bound one lane must meet, as a Catch2 `Approx`. +/// +/// @warning `epsilon(0)` is not optional. `Approx` **ors** its margin test +/// with an epsilon test, and its default epsilon is float-grade +/// (`FLT_EPSILON * 100`, about 1.19e-5 relative), so a bare +/// `Approx(x).margin(m)` would also accept anything within 1.19e-5 of `x` -- +/// silently defeating a 1e-14 double comparison. Zeroing it leaves the +/// absolute `margin` as the only bound. +/// +/// `Approx` compares without subtracting, so `±inf` matches only itself and +/// only with the same sign, and a NaN never compares equal to anything. That +/// is the contract the barrier tests rely on: a barrier at penetration must +/// resolve to `+inf` or 0, never NaN. +inline Catch::Approx +approx(const double expected, const double tol, const double scale) +{ + // A non-finite scale must not reach the margin. `tol * inf` is `inf`, and + // an infinite margin compares equal to *everything* -- including a finite + // value where `+inf` was expected, which is precisely the case the barrier + // tests rely on catching. + const double margin = std::isfinite(scale) ? tol * scale : tol; + return Catch::Approx(expected).epsilon(0).margin(margin); +} + +/// @brief `approx` relative to the magnitude of `expected` itself. +inline Catch::Approx approx(const double expected, const double tol = VALUE_TOL) +{ + return approx(expected, tol, std::max(1.0, std::abs(expected))); +} + +/// @brief Compare a batch-valued Eigen object against the scalar result, lane +/// by lane, where `scalar_of(l)` produces lane `l`'s scalar result. +template +void check_lanes( + const std::string& name, + const BatchMatrix& batched, + ScalarOf&& scalar_of, + const double tol = DERIVATIVE_TOL) +{ + for (int l = 0; l < L; ++l) { + const auto expected = scalar_of(l); + REQUIRE(batched.rows() == expected.rows()); + REQUIRE(batched.cols() == expected.cols()); + const double scale = std::max(1.0, expected.array().abs().maxCoeff()); + for (Eigen::Index i = 0; i < expected.size(); ++i) { + CAPTURE(name, l, i, scale, tol); + CHECK(batched(i).get(l) == approx(expected(i), tol, scale)); + } + } +} + +/// @brief Compare a batch of scalars against the scalar result, lane by lane. +template +void check_scalar_lanes( + const std::string& name, + const Batch& batched, + ScalarOf&& scalar_of, + const double tol = VALUE_TOL) +{ + const Lanes actual = Lanes::from(batched); + for (int l = 0; l < L; ++l) { + CAPTURE(name, l, tol); + CHECK(actual[l] == approx(scalar_of(l), tol)); + } +} + +/// @brief Sweep `cases` across every lane offset, comparing a one-argument +/// batch call against its scalar counterpart on each lane. +template +void check_swept_lanes( + const std::string& name, + const Container& cases, + ScalarFn&& scalar_fn, + BatchFn&& batch_fn, + const double tol = VALUE_TOL) +{ + for (int offset = 0; offset < int(cases.size()); ++offset) { + const Lanes xs = lane_cases(cases, offset); + const Lanes actual = Lanes::from(batch_fn(xs.load())); + for (int l = 0; l < L; ++l) { + CAPTURE(name, offset, l, xs[l], tol); + CHECK(actual[l] == approx(scalar_fn(xs[l]), tol)); + } + } +} + +/// @brief `check_swept_lanes` for a function of the swept value and fixed +/// parameters. One generic callable serves both paths: the scalar call gets the +/// parameters as `double`s and the batch call gets them broadcast to a `Batch`. +template +void check_swept_lanes_with( + const std::string& name, + const Container& cases, + Fn&& f, + const Params... params) +{ + check_swept_lanes( + name, cases, [&](const double x) { return f(x, params...); }, + [&](const Batch& x) { return f(x, Batch(params)...); }); +} + +} // namespace ipc::tests + +#endif diff --git a/tests/src/tests/tangent/CMakeLists.txt b/tests/src/tests/tangent/CMakeLists.txt index 32a642309..6a265b934 100644 --- a/tests/src/tests/tangent/CMakeLists.txt +++ b/tests/src/tests/tangent/CMakeLists.txt @@ -2,6 +2,9 @@ set(SOURCES # Tests test_closest_point.cpp test_relative_velocity.cpp + test_simd_closest_point.cpp + test_simd_relative_velocity.cpp + test_simd_tangent_basis.cpp test_tangent_basis.cpp # Benchmarks diff --git a/tests/src/tests/tangent/test_closest_point.cpp b/tests/src/tests/tangent/test_closest_point.cpp index d9ebeb938..cb2be0fbf 100644 --- a/tests/src/tests/tangent/test_closest_point.cpp +++ b/tests/src/tests/tangent/test_closest_point.cpp @@ -1,5 +1,6 @@ #include #include +#include #include @@ -43,6 +44,28 @@ TEST_CASE( J_FD); CHECK(fd::compare_jacobian(J, J_FD)); + + // test Hessian: one 12x12 block per barycentric coordinate, each the + // Jacobian of that coordinate's row of J. + const std::array, 2> H = + point_triangle_closest_point_hessian(p, t0, t1, t2); + + for (int c = 0; c < 2; c++) { + CAPTURE(c); + Eigen::MatrixXd H_FD; + fd::finite_jacobian( + x, + [c](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + return point_triangle_closest_point_jacobian( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)) + .row(c) + .transpose(); + }, + H_FD); + + CHECK(fd::compare_jacobian(H[c], H_FD)); + } } TEST_CASE("Edge-edge closest point", "[friction][edge-edge][closest_point]") @@ -52,8 +75,10 @@ TEST_CASE("Edge-edge closest point", "[friction][edge-edge][closest_point]") Eigen::Vector2d barycentric_coords = edge_edge_closest_point(ea0, ea1, eb0, eb1); CAPTURE(barycentric_coords); - CHECK(barycentric_coords[0] == Catch::Approx(0.5)); - CHECK(barycentric_coords[1] == Catch::Approx(0.5)); + // Perpendicular edges centered on the same axis meet at their midpoints, + // so the answer is exactly (0.5, 0.5) with no rounding to hide behind. + CHECK(barycentric_coords[0] == Catch::Approx(0.5).epsilon(0).margin(1e-15)); + CHECK(barycentric_coords[1] == Catch::Approx(0.5).epsilon(0).margin(1e-15)); // test Jacobian Eigen::Matrix J = @@ -73,6 +98,28 @@ TEST_CASE("Edge-edge closest point", "[friction][edge-edge][closest_point]") J_FD); CHECK(fd::compare_jacobian(J, J_FD)); + + // test Hessian: one 12x12 block per barycentric coordinate, each the + // Jacobian of that coordinate's row of J. + const std::array, 2> H = + edge_edge_closest_point_hessian(ea0, ea1, eb0, eb1); + + for (int c = 0; c < 2; c++) { + CAPTURE(c); + Eigen::MatrixXd H_FD; + fd::finite_jacobian( + x, + [c](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + return edge_edge_closest_point_jacobian( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)) + .row(c) + .transpose(); + }, + H_FD); + + CHECK(fd::compare_jacobian(H[c], H_FD)); + } } TEST_CASE("Point-edge closest point", "[friction][point-edge][closest_point]") @@ -98,6 +145,22 @@ TEST_CASE("Point-edge closest point", "[friction][point-edge][closest_point]") J_FD); CHECK(fd::compare_gradient(J, J_FD)); + + // test Hessian: the closest point is a scalar here, so its Hessian is the + // Jacobian of the gradient checked above. + const Eigen::Matrix H = + point_edge_closest_point_hessian(p, e0, e1); + + Eigen::MatrixXd H_FD; + fd::finite_jacobian( + x, + [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + return point_edge_closest_point_jacobian( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6)); + }, + H_FD); + + CHECK(fd::compare_jacobian(H, H_FD)); } TEST_CASE( @@ -125,4 +188,65 @@ TEST_CASE( J_FD); CHECK(fd::compare_gradient(J, J_FD)); + + // test Hessian: covers the 2D branch of the kernel, which the 3D case + // above never reaches. + const Eigen::Matrix H = + point_edge_closest_point_hessian(p, e0, e1); + + Eigen::MatrixXd H_FD; + fd::finite_jacobian( + x, + [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + return point_edge_closest_point_jacobian( + _x.segment<2>(0), _x.segment<2>(2), _x.segment<2>(4)); + }, + H_FD); + + CHECK(fd::compare_jacobian(H, H_FD)); +} + +TEST_CASE( + "Point-edge closest point agrees whether or not the dimension is known at " + "compile time", + "[friction][point-edge][closest_point]") +{ + // The front ends dispatch on `dim_v` when the argument type + // carries its size and fall back to a runtime branch on `size()` when it + // does not. Every other test here passes a fixed-size vector, so only the + // `if constexpr` arms ever run. A VectorMax3d holds the same numbers but + // keeps its size at runtime, taking the fallback instead; both arms reach + // the same kernel, so any difference is a dispatch bug. + const int dim = GENERATE(2, 3); + CAPTURE(dim); + + const Eigen::VectorXd p = Eigen::VectorXd::LinSpaced(dim, 0.25, 1.0); + const Eigen::VectorXd e0 = Eigen::VectorXd::LinSpaced(dim, -1.0, 0.5); + const Eigen::VectorXd e1 = Eigen::VectorXd::LinSpaced(dim, 0.75, -0.5); + + const VectorMax3d p_dyn = p, e0_dyn = e0, e1_dyn = e1; + + if (dim == 2) { + const Eigen::Vector2d a = p, b = e0, c = e1; + CHECK( + point_edge_closest_point(p_dyn, e0_dyn, e1_dyn) + == point_edge_closest_point(a, b, c)); + CHECK( + point_edge_closest_point_jacobian(p_dyn, e0_dyn, e1_dyn) + == point_edge_closest_point_jacobian(a, b, c)); + CHECK( + point_edge_closest_point_hessian(p_dyn, e0_dyn, e1_dyn) + == point_edge_closest_point_hessian(a, b, c)); + } else { + const Eigen::Vector3d a = p, b = e0, c = e1; + CHECK( + point_edge_closest_point(p_dyn, e0_dyn, e1_dyn) + == point_edge_closest_point(a, b, c)); + CHECK( + point_edge_closest_point_jacobian(p_dyn, e0_dyn, e1_dyn) + == point_edge_closest_point_jacobian(a, b, c)); + CHECK( + point_edge_closest_point_hessian(p_dyn, e0_dyn, e1_dyn) + == point_edge_closest_point_hessian(a, b, c)); + } } diff --git a/tests/src/tests/tangent/test_simd_closest_point.cpp b/tests/src/tests/tangent/test_simd_closest_point.cpp new file mode 100644 index 000000000..d9e08dc4a --- /dev/null +++ b/tests/src/tests/tangent/test_simd_closest_point.cpp @@ -0,0 +1,183 @@ +#include +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +#include +#include +#include + +using namespace ipc; +using namespace ipc::tests; + +namespace { + +/// @brief Two edges crossing at `theta`, offset along z so the closest points +/// land in the interior of both. That interior-interior case is the only one +/// these functions are called for, so it is what we test. +struct EdgePair { + Eigen::Vector3d ea0, ea1, eb0, eb1; +}; + +EdgePair crossing_edges(const double theta) +{ + return { Eigen::Vector3d(-1, 0, 0), Eigen::Vector3d(1, 0, 0), + Eigen::Vector3d(-std::cos(theta), -std::sin(theta), -0.5), + Eigen::Vector3d(std::cos(theta), std::sin(theta), -0.5) }; +} + +/// @brief Two exactly parallel edges, separated in z and slid past each other +/// along their shared direction by `shift`. +/// +/// Parallel is what makes the 2x2 Gram matrix singular. The shift is what +/// makes the case worth testing: without it the offset between the edges is +/// perpendicular to both, the right-hand side comes out exactly zero, and a +/// lane that was wrongly solved rather than zeroed would still leave no +/// residual to catch. Sliding the edges gives that lane a residual of order +/// ‖b‖, so the singular path has to actually be taken. +EdgePair parallel_edges(const double shift) +{ + return { Eigen::Vector3d(-1, 0, 0), Eigen::Vector3d(1, 0, 0), + Eigen::Vector3d(-1 + shift, 0, -0.5), + Eigen::Vector3d(1 + shift, 0, -0.5) }; +} + +} // namespace + +TEST_CASE( + "A batch of closest points agrees with the scalar path, one problem per " + "lane", + "[closest_point][simd]") +{ + const Points<3> A = random_points<3>(61), B = random_points<3>(62), + C = random_points<3>(63), D = random_points<3>(64); + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + SECTION("point-edge") + { + check_scalar_lanes( + "coordinate", point_edge_closest_point(a, b, c), + [&](int l) { return point_edge_closest_point(A[l], B[l], C[l]); }); + } + + SECTION("point-triangle") + { + check_lanes( + "coordinates", point_triangle_closest_point(a, b, c, d), + [&](int l) { + return point_triangle_closest_point(A[l], B[l], C[l], D[l]) + .eval(); + }, + VALUE_TOL); + check_lanes( + "jacobian", point_triangle_closest_point_jacobian(a, b, c, d), + [&](int l) { + return point_triangle_closest_point_jacobian( + A[l], B[l], C[l], D[l]); + }); + } + + SECTION("edge-edge") + { + // These coordinates get the derivative bound, not VALUE_TOL. The two + // segments here are unconstrained in both direction and offset, so + // the closest points routinely land far off the ends of both: a + // coordinate of 20+ is an ordinary draw, not a degenerate one. Such a + // coordinate is an amplification of the inputs, and the batch and + // scalar paths contract multiply-adds differently, so their agreement + // is bounded by the conditioning of the 2x2 solve rather than by a + // rounding step on the answer. That is the same reason the + // deliberately near-parallel test below uses this bound. A lane that + // is structurally wrong still differs by order of its own magnitude, + // far above this. + check_lanes( + "coordinates", edge_edge_closest_point(a, b, c, d), + [&](int l) { + return edge_edge_closest_point(A[l], B[l], C[l], D[l]).eval(); + }, + DERIVATIVE_TOL); + check_lanes( + "jacobian", edge_edge_closest_point_jacobian(a, b, c, d), + [&](int l) { + return edge_edge_closest_point_jacobian(A[l], B[l], C[l], D[l]); + }); + } +} + +TEST_CASE( + "Closest points agree with the scalar path even for nearly parallel edges", + "[closest_point][simd]") +{ + // The angle between the edges sets how well conditioned the 2x2 system is, + // so we pack one batch with lanes ranging from well separated to nearly + // parallel. That is the regime where the solve is worst conditioned, and + // so where a batch and a scalar are most likely to drift apart. Rotating + // the offset puts a different angle in each lane on every pass. + constexpr std::array THETAS = { { 1.0, 0.1, 1e-2, 1e-3 } }; + const int offset = GENERATE(range(0, 4)); + + const Lanes thetas = lane_cases(THETAS, offset); + + Points<3> EA0, EA1, EB0, EB1; + for (int l = 0; l < L; ++l) { + const EdgePair e = crossing_edges(thetas[l]); + EA0[l] = e.ea0, EA1[l] = e.ea1, EB0[l] = e.eb0, EB1[l] = e.eb1; + } + + check_lanes( + "coordinates", + edge_edge_closest_point(pack(EA0), pack(EA1), pack(EB0), pack(EB1)), + [&](int l) { + return edge_edge_closest_point(EA0[l], EA1[l], EB0[l], EB1[l]) + .eval(); + }); +} + +TEST_CASE( + "A singular lane is zeroed without disturbing the lanes beside it", + "[closest_point][simd]") +{ + // theta = 0 stands for a parallel pair, whose Gram matrix is singular, so + // the solve zeroes that lane instead of dividing by the determinant. Every + // offset below mixes such a lane with a well conditioned one, which is the + // case a single batch has to answer two ways at once: zero one lane, solve + // the other, and hold the residual check to the solved lane only. + constexpr std::array THETAS = { { 0.0, 1.0, 0.0, 0.1 } }; + const int offset = GENERATE(range(0, 4)); + + const Lanes thetas = lane_cases(THETAS, offset); + + Points<3> EA0, EA1, EB0, EB1; + for (int l = 0; l < L; ++l) { + const EdgePair e = + thetas[l] == 0.0 ? parallel_edges(0.75) : crossing_edges(thetas[l]); + EA0[l] = e.ea0, EA1[l] = e.ea1, EB0[l] = e.eb0, EB1[l] = e.eb1; + } + + const Eigen::Vector2 coords = + edge_edge_closest_point(pack(EA0), pack(EA1), pack(EB0), pack(EB1)); + + check_lanes("coordinates", coords, [&](int l) { + return edge_edge_closest_point(EA0[l], EA1[l], EB0[l], EB1[l]).eval(); + }); + + // Pin down which lanes were the degenerate ones. Without this the check + // above would still pass if the batch and the scalar path agreed on some + // other answer for a parallel pair. + for (int l = 0; l < L; ++l) { + CAPTURE(l, thetas[l]); + if (thetas[l] == 0.0) { + CHECK(coords[0].get(l) == 0.0); + CHECK(coords[1].get(l) == 0.0); + } else { + CHECK(coords[0].get(l) != 0.0); + } + } +} + +#endif diff --git a/tests/src/tests/tangent/test_simd_relative_velocity.cpp b/tests/src/tests/tangent/test_simd_relative_velocity.cpp new file mode 100644 index 000000000..8282bc8cb --- /dev/null +++ b/tests/src/tests/tangent/test_simd_relative_velocity.cpp @@ -0,0 +1,204 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +using namespace ipc; +using namespace ipc::tests; + +namespace { + +// A relative velocity is a linear combination of the input velocities, so +// unlike a distance derivative it involves no cancellation of large terms and +// the two paths should agree to a rounding step. Hence VALUE_TOL rather than +// the DERIVATIVE_TOL that check_lanes defaults to. +constexpr double TOL = VALUE_TOL; + +/// @brief A distinct barycentric coordinate per lane, so the Jacobians and +/// dx_dbeta tensors -- which depend on the coordinate and nothing else -- +/// differ between lanes and the comparison is not vacuous. +Lanes lane_alphas() +{ + Lanes alphas; + for (int l = 0; l < L; ++l) { + alphas[l] = 0.15 + 0.2 * l; + } + return alphas; +} + +Points<2> lane_coords() +{ + Points<2> coords; + for (int l = 0; l < L; ++l) { + coords[l] = Eigen::Vector2d(0.2 + 0.15 * l, 0.35 - 0.1 * l); + } + return coords; +} + +} // namespace + +TEST_CASE( + "SIMD batch relative velocities match the scalar ones lane-wise (3D)", + "[relative_velocity][simd]") +{ + const Points<3> A = random_points<3>(41), B = random_points<3>(42), + C = random_points<3>(43), D = random_points<3>(44); + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + const Lanes ALPHA = lane_alphas(); + const Batch alpha = ALPHA.load(); + const Points<2> COORDS = lane_coords(); + const Eigen::Vector2 coords = pack(COORDS); + + SECTION("point-point") + { + check_lanes( + "value", point_point_relative_velocity(a, b), + [&](int l) { + return point_point_relative_velocity(A[l], B[l]).eval(); + }, + TOL); + check_lanes( + "jacobian", point_point_relative_velocity_jacobian(3), + [&](int) { + return point_point_relative_velocity_jacobian(3); + }, + TOL); + // Γ does not depend on β here, so both paths must give exactly zero. + check_lanes( + "dx_dbeta", point_point_relative_velocity_dx_dbeta(3), + [&](int) { + return point_point_relative_velocity_dx_dbeta(3); + }, + TOL); + } + + SECTION("point-edge") + { + check_lanes( + "value", point_edge_relative_velocity(a, b, c, alpha), + [&](int l) { + return point_edge_relative_velocity(A[l], B[l], C[l], ALPHA[l]) + .eval(); + }, + TOL); + check_lanes( + "jacobian", point_edge_relative_velocity_jacobian(3, alpha), + [&](int l) { + return point_edge_relative_velocity_jacobian(3, ALPHA[l]); + }, + TOL); + check_lanes( + "dx_dbeta", point_edge_relative_velocity_dx_dbeta(3, alpha), + [&](int l) { + return point_edge_relative_velocity_dx_dbeta(3, ALPHA[l]); + }, + TOL); + } + + SECTION("edge-edge") + { + check_lanes( + "value", edge_edge_relative_velocity(a, b, c, d, coords), + [&](int l) { + return edge_edge_relative_velocity( + A[l], B[l], C[l], D[l], COORDS[l]) + .eval(); + }, + TOL); + check_lanes( + "jacobian", edge_edge_relative_velocity_jacobian(coords), + [&](int l) { + return edge_edge_relative_velocity_jacobian(COORDS[l]).eval(); + }, + TOL); + check_lanes( + "dx_dbeta", edge_edge_relative_velocity_dx_dbeta(coords), + [&](int l) { + return edge_edge_relative_velocity_dx_dbeta(COORDS[l]).eval(); + }, + TOL); + } + + SECTION("point-triangle") + { + check_lanes( + "value", point_triangle_relative_velocity(a, b, c, d, coords), + [&](int l) { + return point_triangle_relative_velocity( + A[l], B[l], C[l], D[l], COORDS[l]) + .eval(); + }, + TOL); + check_lanes( + "jacobian", point_triangle_relative_velocity_jacobian(coords), + [&](int l) { + return point_triangle_relative_velocity_jacobian(COORDS[l]) + .eval(); + }, + TOL); + check_lanes( + "dx_dbeta", point_triangle_relative_velocity_dx_dbeta(coords), + [&](int l) { + return point_triangle_relative_velocity_dx_dbeta(COORDS[l]) + .eval(); + }, + TOL); + } +} + +TEST_CASE( + "SIMD batch relative velocities match the scalar ones lane-wise (2D)", + "[relative_velocity][simd]") +{ + // Only point-point and point-edge are defined in 2D; the runtime `dim` + // branch in the front ends is a size test, not a value test, so a batch + // takes it the same way a scalar does. + const Points<2> A = random_points<2>(51), B = random_points<2>(52), + C = random_points<2>(53); + const Eigen::Vector2 a = pack(A), b = pack(B), c = pack(C); + + const Lanes ALPHA = lane_alphas(); + const Batch alpha = ALPHA.load(); + + check_lanes( + "point_point_value", point_point_relative_velocity(a, b), + [&](int l) { return point_point_relative_velocity(A[l], B[l]).eval(); }, + TOL); + check_lanes( + "point_point_jacobian", + point_point_relative_velocity_jacobian(2), + [&](int) { return point_point_relative_velocity_jacobian(2); }, + TOL); + check_lanes( + "point_point_dx_dbeta", + point_point_relative_velocity_dx_dbeta(2), + [&](int) { return point_point_relative_velocity_dx_dbeta(2); }, + TOL); + + check_lanes( + "point_edge_value", point_edge_relative_velocity(a, b, c, alpha), + [&](int l) { + return point_edge_relative_velocity(A[l], B[l], C[l], ALPHA[l]) + .eval(); + }, + TOL); + check_lanes( + "point_edge_jacobian", point_edge_relative_velocity_jacobian(2, alpha), + [&](int l) { + return point_edge_relative_velocity_jacobian(2, ALPHA[l]); + }, + TOL); + check_lanes( + "point_edge_dx_dbeta", point_edge_relative_velocity_dx_dbeta(2, alpha), + [&](int l) { + return point_edge_relative_velocity_dx_dbeta(2, ALPHA[l]); + }, + TOL); +} + +#endif diff --git a/tests/src/tests/tangent/test_simd_tangent_basis.cpp b/tests/src/tests/tangent/test_simd_tangent_basis.cpp new file mode 100644 index 000000000..e63b145ca --- /dev/null +++ b/tests/src/tests/tangent/test_simd_tangent_basis.cpp @@ -0,0 +1,87 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +using namespace ipc; +using namespace ipc::tests; + +TEST_CASE( + "SIMD batch tangent bases match the scalar ones lane-wise", + "[tangent_basis][simd]") +{ + const Points<3> A = random_points<3>(1), B = random_points<3>(2), + C = random_points<3>(3), D = random_points<3>(4); + + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + check_lanes("point_point", point_point_tangent_basis(a, b), [&](int l) { + return point_point_tangent_basis(A[l], B[l]).eval(); + }); + check_lanes( + "point_point_jacobian", point_point_tangent_basis_jacobian(a, b), + [&](int l) { + return point_point_tangent_basis_jacobian(A[l], B[l]).eval(); + }); + + check_lanes("point_edge", point_edge_tangent_basis(a, b, c), [&](int l) { + return point_edge_tangent_basis(A[l], B[l], C[l]).eval(); + }); + check_lanes( + "point_edge_jacobian", point_edge_tangent_basis_jacobian(a, b, c), + [&](int l) { + return point_edge_tangent_basis_jacobian(A[l], B[l], C[l]).eval(); + }); + + check_lanes("edge_edge", edge_edge_tangent_basis(a, b, c, d), [&](int l) { + return edge_edge_tangent_basis(A[l], B[l], C[l], D[l]).eval(); + }); + check_lanes( + "edge_edge_jacobian", edge_edge_tangent_basis_jacobian(a, b, c, d), + [&](int l) { + return edge_edge_tangent_basis_jacobian(A[l], B[l], C[l], D[l]) + .eval(); + }); + + check_lanes( + "point_triangle", point_triangle_tangent_basis(a, b, c, d), [&](int l) { + return point_triangle_tangent_basis(A[l], B[l], C[l], D[l]).eval(); + }); + check_lanes( + "point_triangle_jacobian", + point_triangle_tangent_basis_jacobian(a, b, c, d), [&](int l) { + return point_triangle_tangent_basis_jacobian(A[l], B[l], C[l], D[l]) + .eval(); + }); +} + +TEST_CASE( + "SIMD batch point-point tangent basis blends the reference axis per lane", + "[tangent_basis][simd]") +{ + // Lanes deliberately straddle the cross_x / cross_y branch: a pair along + // x prefers one reference axis, a pair along y the other. + Points<3> A, B; + for (int l = 0; l < L; ++l) { + A[l] = Eigen::Vector3d::Zero(); + B[l] = + (l % 2 == 0) ? Eigen::Vector3d(1, 0, 0) : Eigen::Vector3d(0, 1, 0); + } + + const Eigen::Vector3 a = pack(A), b = pack(B); + + check_lanes("point_point", point_point_tangent_basis(a, b), [&](int l) { + return point_point_tangent_basis(A[l], B[l]).eval(); + }); + check_lanes( + "point_point_jacobian", point_point_tangent_basis_jacobian(a, b), + [&](int l) { + return point_point_tangent_basis_jacobian(A[l], B[l]).eval(); + }); +} + +#endif diff --git a/tests/src/tests/tangent/test_tangent_basis.cpp b/tests/src/tests/tangent/test_tangent_basis.cpp index c095718e5..20fb4273a 100644 --- a/tests/src/tests/tangent/test_tangent_basis.cpp +++ b/tests/src/tests/tangent/test_tangent_basis.cpp @@ -1,5 +1,6 @@ #include #include +#include #include @@ -202,3 +203,56 @@ TEST_CASE( CHECK(fd::compare_jacobian(J, J_fd)); } + +TEST_CASE( + "Tangent bases agree whether or not the dimension is known at compile time", + "[friction][tangent_basis][tangent_basis_jacobian]") +{ + // The front ends dispatch on `dim_v` when the argument type + // carries its size, and fall back to a runtime branch on `size()` when it + // does not. Every other test here passes a fixed-size vector, so only the + // `if constexpr` arms ever run. Passing the same points as a dynamically + // sized vector takes the fallback instead, and the two must agree exactly: + // they call the same kernel, so any difference is a dispatch bug. + const int dim = GENERATE(2, 3); + CAPTURE(dim); + + const Eigen::VectorXd p = Eigen::VectorXd::LinSpaced(dim, 0.25, 1.0); + const Eigen::VectorXd q = Eigen::VectorXd::LinSpaced(dim, -1.0, 0.5); + + // VectorMax3d keeps its size at runtime, so dim_v is not a constant. + const VectorMax3d p_dyn = p, q_dyn = q; + + const Eigen::VectorXd r = Eigen::VectorXd::LinSpaced(dim, 0.75, -0.5); + const VectorMax3d r_dyn = r; + + if (dim == 2) { + const Eigen::Vector2d a = p, b = q, c = r; + CHECK( + point_point_tangent_basis(p_dyn, q_dyn) + == point_point_tangent_basis(a, b)); + CHECK( + point_point_tangent_basis_jacobian(p_dyn, q_dyn) + == point_point_tangent_basis_jacobian(a, b)); + CHECK( + point_edge_tangent_basis(p_dyn, q_dyn, r_dyn) + == point_edge_tangent_basis(a, b, c)); + CHECK( + point_edge_tangent_basis_jacobian(p_dyn, q_dyn, r_dyn) + == point_edge_tangent_basis_jacobian(a, b, c)); + } else { + const Eigen::Vector3d a = p, b = q, c = r; + CHECK( + point_point_tangent_basis(p_dyn, q_dyn) + == point_point_tangent_basis(a, b)); + CHECK( + point_point_tangent_basis_jacobian(p_dyn, q_dyn) + == point_point_tangent_basis_jacobian(a, b)); + CHECK( + point_edge_tangent_basis(p_dyn, q_dyn, r_dyn) + == point_edge_tangent_basis(a, b, c)); + CHECK( + point_edge_tangent_basis_jacobian(p_dyn, q_dyn, r_dyn) + == point_edge_tangent_basis_jacobian(a, b, c)); + } +} diff --git a/tests/src/tests/utils/CMakeLists.txt b/tests/src/tests/utils/CMakeLists.txt index 8acd17887..c386e8915 100644 --- a/tests/src/tests/utils/CMakeLists.txt +++ b/tests/src/tests/utils/CMakeLists.txt @@ -3,6 +3,7 @@ set(SOURCES test_local_to_global.cpp test_matrixcache.cpp test_profiler.cpp + test_simd_utils.cpp test_utils.cpp # Benchmarks diff --git a/tests/src/tests/utils/test_simd_utils.cpp b/tests/src/tests/utils/test_simd_utils.cpp new file mode 100644 index 000000000..770e50aea --- /dev/null +++ b/tests/src/tests/utils/test_simd_utils.cpp @@ -0,0 +1,88 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include +#include + +using namespace ipc::tests; + +// Every SIMD test compares through `approx`, so its contract is worth pinning +// down here rather than rediscovering it as a mysteriously passing test. + +TEST_CASE("SIMD test approx zeroes Approx's default epsilon", "[utils][simd]") +{ + // The reason `approx` sets epsilon(0): Approx ORs its margin test with an + // epsilon test whose default is float-grade (FLT_EPSILON * 100, about + // 1.19e-5 relative). Left in place, it would accept a value that misses by + // far more than VALUE_TOL, which is exactly the bug this guards. + const double x = 1.0; + const double off_by_1e7 = x + 1e-7; + + CHECK(off_by_1e7 != approx(x, VALUE_TOL)); + CHECK(off_by_1e7 == Catch::Approx(x)); // the default that must not leak in +} + +TEST_CASE( + "SIMD test approx bounds relative to the given scale", "[utils][simd]") +{ + // The scale is the caller's choice -- for a derivative it is the magnitude + // of the whole result, so an entry that is a cancellation of much larger + // terms is not held to its own tiny magnitude. + CHECK(1e-8 == approx(0.0, DERIVATIVE_TOL, /*scale=*/1e3)); + CHECK(1e-8 != approx(0.0, DERIVATIVE_TOL, /*scale=*/1.0)); +} + +TEST_CASE("SIMD test approx matches infinities by sign", "[utils][simd]") +{ + // Approx compares without subtracting, which is what makes this work. The + // barrier tests depend on it: a barrier at penetration must resolve to + // +inf or 0, never NaN. + constexpr double INF = std::numeric_limits::infinity(); + constexpr double NAN_ = std::numeric_limits::quiet_NaN(); + + CHECK(INF == approx(INF, VALUE_TOL)); + CHECK(-INF == approx(-INF, VALUE_TOL)); + CHECK(-INF != approx(INF, VALUE_TOL)); + CHECK(1.0 != approx(INF, VALUE_TOL)); + CHECK(INF != approx(1.0, VALUE_TOL)); + + // A NaN never passes, on either side. + CHECK(NAN_ != approx(INF, VALUE_TOL)); + CHECK(NAN_ != approx(1.0, VALUE_TOL)); + CHECK(1.0 != approx(NAN_, VALUE_TOL)); +} + +TEST_CASE("SIMD test Lanes round-trip through a batch", "[utils][simd]") +{ + Lanes in; + for (int l = 0; l < L; ++l) { + in[l] = 1.0 + 0.25 * l; + } + + const Lanes out = Lanes::from(in.load()); + for (int l = 0; l < L; ++l) { + CAPTURE(l); + CHECK(out[l] == in[l]); + } +} + +TEST_CASE("SIMD test lane_cases wraps round-robin", "[utils][simd]") +{ + // Two cases and (usually) more lanes, so the wrap is exercised whatever + // the batch width; the offset rotates which case each lane gets. + const std::array cases = { { 10.0, 20.0 } }; + + for (int offset = 0; offset < 2; ++offset) { + CAPTURE(offset); + const Lanes lanes = lane_cases(cases, offset); + for (int l = 0; l < L; ++l) { + CAPTURE(l); + CHECK(lanes[l] == cases[size_t(l + offset) % cases.size()]); + } + } +} + +#endif