From e1f3606064a82f3fd8ed8136fbeecfccbc542640 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 31 Aug 2026 00:50:52 -0400 Subject: [PATCH 01/34] Support xsimd batch scalars in the distance functions Specialize `Eigen::NumTraits` for `xsimd::batch` and add `ipc::SimdBatch` in the new `ipc/utils/simd.hpp`, so `Eigen::Vector3>` is a valid argument to the distance functions. A caller holding a structure-of-arrays layout can then evaluate one independent problem per SIMD lane per call. The scalar templating this builds on is what makes it possible; the values, gradients, and Hessians of point-point, point-line, point-edge, line-line, edge-edge, and point-triangle are instantiated for `SimdBatch` and `SimdBatch`. The generated `autogen` kernels needed instantiating for the batch types too, since they are where the derivatives actually land. Verified lane-by-lane against the scalar path: the values agree to one ulp, and every gradient and Hessian entry agrees to 1e-9 relative to the magnitude of the result. The derivatives need the looser, norm-scaled bound because they are ill-conditioned for near-parallel edges -- a random configuration routinely produces entries of order 1e6 whose small differences are cancellations of large terms -- while the batch and scalar paths contract multiply-adds differently. AUTO and the `*_distance_type` predicates are unavailable for batch scalars and throw. This is semantic, not an omission: 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, which is the natural SoA layout anyway. [Breaking] `xsimd` and `SIMD_CXX_FLAGS` become PUBLIC rather than PRIVATE. `xsimd::default_arch` is selected from each translation unit's own compiler flags: with the flags PRIVATE, this library's TUs resolve it to `i8mm+neon64` while a consumer's resolve it to `arm64+neon`, a genuinely different type, so the instantiations here would not match what a caller names. The cost is that consumers are now compiled with the detected SIMD flags (typically `-march=native`), which makes their binaries non-portable; disable IPC_TOOLKIT_WITH_SIMD if that is unwanted. Moving the edge-edge and point-triangle kernel definitions into a `.tpp` so callers instantiate in their own TU would avoid this, at the cost of a larger refactor. A caveat on the payoff: on an Apple M-series (NEON, 2 lanes per double) a batched point-line sweep measured 1.06-1.13x over the scalar loop, and 1.13-1.18x with float at 4 lanes -- well short of the lane count, because the compiler already auto-vectorizes a loop over independent problems and these kernels are largely memory-bound. Wider ISAs may do better; that is unmeasured. Only `SimdBatch` is covered by the tests; the `float` instantiations compile and link but are not numerically verified. Co-Authored-By: Claude Opus 5 --- CMakeLists.txt | 10 +- docs/source/about/release_notes.rst | 11 + src/ipc/distance/edge_edge.cpp | 6 + src/ipc/distance/line_line.cpp | 7 + src/ipc/distance/point_edge.cpp | 7 + src/ipc/distance/point_line.cpp | 6 + src/ipc/distance/point_plane.cpp | 7 + src/ipc/distance/point_triangle.cpp | 9 + src/ipc/utils/CMakeLists.txt | 1 + src/ipc/utils/simd.hpp | 85 ++++++ tests/src/tests/distance/CMakeLists.txt | 1 + .../src/tests/distance/test_simd_distance.cpp | 245 ++++++++++++++++++ 12 files changed, 392 insertions(+), 3 deletions(-) create mode 100644 src/ipc/utils/simd.hpp create mode 100644 tests/src/tests/distance/test_simd_distance.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 177954f9f..68ef06a3d 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. diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 062018a69..4a08c520d 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -65,6 +65,17 @@ API Changes |:wrench:| - 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. +- Add SIMD batch support to the distance functions via the new ``ipc/utils/simd.hpp`` (requires ``IPC_TOOLKIT_WITH_SIMD``). + + - ``Eigen::NumTraits`` is specialized for ``xsimd::batch``, and ``ipc::SimdBatch`` aliases the batch type for the build's architecture. Passing ``Eigen::Vector3>`` evaluates one independent problem per SIMD lane, letting a caller with a structure-of-arrays layout compute several distances per call. + - The values, gradients, and Hessians of the point-point, point-line, point-edge, line-line, edge-edge, and point-triangle distances are instantiated for ``SimdBatch`` and ``SimdBatch``. Measured agreement with the scalar path is one ulp on the values; the derivatives agree to ~1e-11 relative to their own magnitude, since the two paths contract multiply-adds differently and these derivatives are ill-conditioned for near-parallel edges. + - 💥 **[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. + - ``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. + + A caveat on the payoff: on an Apple M-series (NEON, 2 lanes per ``double``) a batched point-line sweep measured only 1.06-1.13x over the scalar loop, and 1.13-1.18x with ``float`` at 4 lanes. The compiler already auto-vectorizes a loop over independent problems, and the kernels are largely memory-bound. Wider ISAs may do better; that has not been measured. + +- Add ``float`` aliases to ``ipc/utils/eigen_ext.hpp`` (``Vector1f``, ``Vector6f``, ``Matrix6f``, ``VectorMax3f``, ``MatrixMax9f``, …) mirroring the existing ``double`` ones. + Performance |:zap:| ~~~~~~~~~~~~~~~~~~~ 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/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/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/simd.hpp b/src/ipc/utils/simd.hpp new file mode 100644 index 000000000..ed92a3d32 --- /dev/null +++ b/src/ipc/utils/simd.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include +#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; + + 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 + }; + + static inline Real epsilon() { return Real(NumTraits::epsilon()); } + static inline Real dummy_precision() + { + return Real(NumTraits::dummy_precision()); + } + static inline int digits10() { return NumTraits::digits10(); } + static inline Real highest() { return Real(NumTraits::highest()); } + static inline 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. +/// +/// @warning Only the distance *values* are instantiated. The gradients and +/// Hessians additionally require the ``autogen`` kernels to be instantiated for +/// the batch type. +/// +/// @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; + +} // namespace ipc + +#endif diff --git a/tests/src/tests/distance/CMakeLists.txt b/tests/src/tests/distance/CMakeLists.txt index 84e05e6d5..942dc3170 100644 --- a/tests/src/tests/distance/CMakeLists.txt +++ b/tests/src/tests/distance/CMakeLists.txt @@ -9,6 +9,7 @@ set(SOURCES test_point_plane.cpp test_point_point.cpp test_point_triangle.cpp + test_simd_distance.cpp test_signed_distance.cpp # Benchmarks 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..982133261 --- /dev/null +++ b/tests/src/tests/distance/test_simd_distance.cpp @@ -0,0 +1,245 @@ +#include +#include +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace ipc; + +namespace { + +using Batch = SimdBatch; +constexpr int L = int(Batch::size); + +/// @brief Pack the k-th coordinate of L problems into one batch. +Eigen::Vector3 pack(const std::vector& v) +{ + Eigen::Vector3 out; + for (int k = 0; k < 3; ++k) { + std::vector tmp(L); + for (int l = 0; l < L; ++l) { + tmp[l] = v[l][k]; + } + out[k] = Batch::load_unaligned(tmp.data()); + } + return out; +} + +/// @brief Agreement to within rounding; the batch and scalar paths differ only +/// in how the compiler contracts multiply-adds. +bool close(const double got, const double want) +{ + return std::abs(got - want) <= 1e-14 * std::max(1.0, std::abs(want)); +} + +std::vector random_points(const int seed) +{ + std::srand(seed); + std::vector v(L); + for (int l = 0; l < L; ++l) { + v[l] = Eigen::Vector3d::Random(); + } + return v; +} + +} // namespace + +TEST_CASE( + "SIMD batch distances match the scalar ones lane-wise", "[distance][simd]") +{ + const std::vector A = random_points(1), + B = random_points(2), + C = random_points(3), + D = random_points(4); + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + const Batch pp = point_point_distance(a, b); + const Batch pl = point_line_distance(a, b, c); + const Batch ll = line_line_distance(a, b, c, d); + const Batch pe = point_edge_distance(a, b, c, PointEdgeDistanceType::P_E); + const Batch ee = + edge_edge_distance(a, b, c, d, EdgeEdgeDistanceType::EA_EB); + const Batch pt = + point_triangle_distance(a, b, c, d, PointTriangleDistanceType::P_T); + + for (int l = 0; l < L; ++l) { + CAPTURE(l); + // A batch lane must agree with the scalar answer to within rounding; + // the two differ only in how the compiler contracts multiply-adds. + CHECK(close(pp.get(l), point_point_distance(A[l], B[l]))); + CHECK(close(pl.get(l), point_line_distance(A[l], B[l], C[l]))); + CHECK(close(ll.get(l), line_line_distance(A[l], B[l], C[l], D[l]))); + CHECK(close( + pe.get(l), + point_edge_distance(A[l], B[l], C[l], PointEdgeDistanceType::P_E))); + CHECK(close( + ee.get(l), + edge_edge_distance( + A[l], B[l], C[l], D[l], EdgeEdgeDistanceType::EA_EB))); + CHECK(close( + pt.get(l), + 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 std::vector A = random_points(11), + B = random_points(12), + C = random_points(13), + D = random_points(14); + const Eigen::Vector3 a = pack(A), b = pack(B), c = pack(C), + d = pack(D); + + // Compare a batched derivative against the scalar one, lane by lane. + // + // NOTE: the tolerance is scaled by the magnitude of the whole result, not + // by each entry. These derivatives are ill-conditioned for near-parallel + // edges -- a random configuration routinely produces entries of order 1e6 + // whose small differences are catastrophic cancellations of large terms. + // The batch and scalar paths contract multiply-adds differently, so they + // agree to roughly 1e-11 relative to the norm rather than to an ulp. A + // structurally wrong entry would differ by order of the norm itself. + const auto check_all = [&](const auto& batched, const auto& scalar_of) { + for (int l = 0; l < L; ++l) { + CAPTURE(l); + const auto want = scalar_of(l); + REQUIRE(batched.size() == want.size()); + const double scale = std::max(1.0, want.array().abs().maxCoeff()); + for (Eigen::Index i = 0; i < want.size(); ++i) { + CAPTURE(i); + const double got = batched(i).get(l), expect = want(i); + CAPTURE(got, expect, std::abs(got - expect), scale); + CHECK(std::abs(got - expect) <= 1e-9 * scale); + } + } + }; + + check_all(point_point_distance_gradient(a, b), [&](int l) { + return point_point_distance_gradient(A[l], B[l]).eval(); + }); + check_all(point_point_distance_hessian(a, b), [&](int l) { + return point_point_distance_hessian(A[l], B[l]).eval(); + }); + + check_all(point_line_distance_gradient(a, b, c), [&](int l) { + return point_line_distance_gradient(A[l], B[l], C[l]).eval(); + }); + check_all(point_line_distance_hessian(a, b, c), [&](int l) { + return point_line_distance_hessian(A[l], B[l], C[l]).eval(); + }); + + check_all(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_all(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_all( + 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_all( + 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_all( + 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_all( + 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_all( + 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_all( + 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 std::vector A = random_points(5), + B = random_points(6), + C = random_points(7), + D = random_points(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 From cc767d2af8508e3a5051d1b9ba8b8e01ec70ccd4 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 3 Sep 2026 00:51:39 -0400 Subject: [PATCH 02/34] Consolidate sqr/cubic and split math.hpp by dependency `sqr` was defined four times across the library: as `Math::sqr` in math.hpp and as three separate anonymous-namespace copies, two of which were `double`-only and so unusable from templated code. - Add free `ipc::sqr` and `ipc::cubic` to a new `math/scalar_math.hpp`. - `Math::sqr`/`cubic` forward to them, so the public API is unchanged and there is a single implementation. - Drop the duplicates in normal_collisions.cpp and voxel_size_heuristic.cpp. Split math.hpp along its dependencies so consumers that only want the scalar helpers do not pay for the rest: - `math/scalar_math.hpp` -- no includes. - `math/heaviside_type.hpp` -- `HeavisideType`, `OrientationTypes`. - `math/math.hpp` -- the parts needing eigen_ext, and math.tpp which pulls in the autodiff scalars. It still includes both new headers, so existing `#include ` keeps working. normal_collisions.cpp and voxel_size_heuristic.cpp no longer pull in TinyAD as a result (verified with -H). Co-Authored-By: Claude Opus 5 --- src/ipc/broad_phase/voxel_size_heuristic.cpp | 4 +-- .../collisions/normal/normal_collisions.cpp | 5 +-- src/ipc/math/CMakeLists.txt | 2 ++ src/ipc/math/heaviside_type.hpp | 32 +++++++++++++++++ src/ipc/math/math.hpp | 36 ++++--------------- src/ipc/math/scalar_math.hpp | 14 ++++++++ 6 files changed, 57 insertions(+), 36 deletions(-) create mode 100644 src/ipc/math/heaviside_type.hpp create mode 100644 src/ipc/math/scalar_math.hpp 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/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..35b3c87b5 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; @@ -40,8 +18,8 @@ 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 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/scalar_math.hpp b/src/ipc/math/scalar_math.hpp new file mode 100644 index 000000000..0fc7a7a71 --- /dev/null +++ b/src/ipc/math/scalar_math.hpp @@ -0,0 +1,14 @@ +#pragma once + +namespace ipc { + +/// @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; } + +constexpr double MOLLIFIER_THRESHOLD_EPS = 1e-2; + +} // namespace ipc From 0f7c7268750ac664ca619d7cb44232c907990730 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 3 Sep 2026 00:52:50 -0400 Subject: [PATCH 03/34] Extend SIMD batch support beyond the distance functions Instantiate the remaining scalar-templated geometry for `SimdBatch` and `SimdBatch`: the barrier functions and classes, the tangent bases and their Jacobians, the closest-point Jacobians/Hessians, the unnormalized-normal Jacobians/Hessians, the signed-distance Hessians, and the triangle-area gradient. Three things blocked this and are fixed here: - Qualified `std::sqrt` cannot find `xsimd::sqrt`, which is only reachable by ADL. `ipc::sqrt` does the lookup in one place via a block-scope `using std::sqrt`, so it also covers the autodiff scalars. - Eigen's `normalized()` guards a zero-length vector with an `if`, which a batch cannot answer with one bool. `ipc::normalized` applies that rule per-lane and defers to Eigen for scalars. - Functions that pick a case from the values themselves (a barrier clamping at dhat, the 3D point-point tangent basis choosing a reference axis) have no single answer for a batch. `select_lazy` expresses these as one first-match-wins cascade: a scalar evaluates only the case it lands in, while a batch evaluates every case and blends per-lane. Each barrier is now written once rather than as separate scalar and batch bodies. The `*_distance_type` predicates remain scalar-only by design -- they return an enum, which has no per-lane blend. Also fixes TwoStageBarrier at penetration. It had no `d <= 0` guard, so its log returned NaN for d < 0 and its first derivative flipped sign, reporting an attractive force. It now matches the other log barriers: +inf for the value, 0 for the derivatives. Tests cover the batch/scalar agreement lane-wise, sweeping lane offsets so every branch is exercised even when a batch holds fewer lanes than there are cases, plus the barrier penetration and stage-boundary conventions. Co-Authored-By: Claude Opus 5 --- docs/source/about/release_notes.rst | 3 + src/ipc/barrier/barrier.cpp | 185 ++++++++++-------- src/ipc/barrier/barrier.hpp | 7 +- src/ipc/distance/signed/line_line.cpp | 21 +- src/ipc/distance/signed/point_line.cpp | 24 ++- src/ipc/distance/signed/point_plane.cpp | 21 +- src/ipc/geometry/area.cpp | 8 +- src/ipc/geometry/normal.cpp | 46 ++--- src/ipc/tangent/closest_point.cpp | 6 + src/ipc/tangent/tangent_basis.cpp | 101 ++++++---- src/ipc/tangent/tangent_basis.hpp | 42 ++-- src/ipc/utils/simd.hpp | 139 ++++++++++++- tests/src/tests/barrier/CMakeLists.txt | 1 + tests/src/tests/barrier/test_barrier.cpp | 75 ++++++- tests/src/tests/barrier/test_simd_barrier.cpp | 150 ++++++++++++++ tests/src/tests/tangent/CMakeLists.txt | 1 + .../tests/tangent/test_simd_tangent_basis.cpp | 148 ++++++++++++++ 17 files changed, 795 insertions(+), 183 deletions(-) create mode 100644 tests/src/tests/barrier/test_simd_barrier.cpp create mode 100644 tests/src/tests/tangent/test_simd_tangent_basis.cpp diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 4a08c520d..4b7bf14df 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -69,6 +69,9 @@ API Changes |:wrench:| - ``Eigen::NumTraits`` is specialized for ``xsimd::batch``, and ``ipc::SimdBatch`` aliases the batch type for the build's architecture. Passing ``Eigen::Vector3>`` evaluates one independent problem per SIMD lane, letting a caller with a structure-of-arrays layout compute several distances per call. - The values, gradients, and Hessians of the point-point, point-line, point-edge, line-line, edge-edge, and point-triangle distances are instantiated for ``SimdBatch`` and ``SimdBatch``. Measured agreement with the scalar path is one ulp on the values; the derivatives agree to ~1e-11 relative to their own magnitude, since the two paths contract multiply-adds differently and these derivatives are ill-conditioned for near-parallel edges. + - The barrier functions and classes (``barrier``, ``ClampedLogBarrier``, ``ClampedLogSqBarrier``, ``CubicBarrier``, ``TwoStageBarrier``), the tangent bases and their Jacobians, the closest-point Jacobians/Hessians, the unnormalized-normal Jacobians/Hessians, the signed-distance Hessians, and the triangle-area gradient are instantiated for batch scalars as well. + - Functions that pick a case from the values themselves — a barrier clamping at ``d̂``, the 3D point-point tangent basis choosing a reference axis — evaluate every case for a batch and blend the results per-lane, so one batch may carry lanes in different cases. The scalar instantiations keep their original early-return form and are unchanged. + - ``ipc::normalized(v)`` replaces ``v.normalized()`` in the tangent bases: Eigen's own ``normalized()`` guards a zero-length vector with an ``if``, which a batch cannot answer with one ``bool``. It applies the same rule per-lane and defers to Eigen for scalars. - 💥 **[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. - ``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. diff --git a/src/ipc/barrier/barrier.cpp b/src/ipc/barrier/barrier.cpp index 742f1712f..0dc6504ee 100644 --- a/src/ipc/barrier/barrier.cpp +++ b/src/ipc/barrier/barrier.cpp @@ -5,43 +5,50 @@ // 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 * 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 * 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 * 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 +56,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 = 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 = 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 = 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 +105,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 +135,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 +182,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/distance/signed/line_line.cpp b/src/ipc/distance/signed/line_line.cpp index b010cad45..33bb3db80 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 @@ -70,9 +72,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..72e3b856d 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 @@ -34,7 +36,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 +73,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..01498ab52 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 @@ -69,9 +71,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/geometry/area.cpp b/src/ipc/geometry/area.cpp index 99a54fcbe..9b6a753e4 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::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/normal.cpp b/src/ipc/geometry/normal.cpp index 3c669194c..df4b2ea01 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::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::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::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/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/tangent_basis.cpp b/src/ipc/tangent/tangent_basis.cpp index 762117cd5..32db9fa45 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,7 @@ 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::sqrt(x); } } // namespace // J is (2×4) flattened in column-major order @@ -164,7 +174,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::sqrt(t4); const T t8 = T(1) / t4; const T t9 = t2 * t8 - 1; const T t10 = t3 * t8 - 1; @@ -189,65 +199,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::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::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::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 +266,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 +295,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 +317,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 +335,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::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::sqrt(t4)); const T t9 = t3 * t6 - 1; const T t10 = -t8; J[0] = 0; @@ -384,7 +395,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::sqrt(t25); const T t27 = -t5; const T t28 = -t1; const T t29 = t11 * t27 - t15 * t28; @@ -393,7 +404,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::sqrt(t22); const T t36 = -e0_y; const T t37 = T(1) / t22; const T t38 = t37 * t8; @@ -411,7 +422,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::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 +527,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::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 +569,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::sqrt(t49); const T t51 = t27 * t29; const T t52 = -t51; const T t53 = T(1) / t49; @@ -573,7 +584,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::sqrt(t63); const T t65 = 2 * t34; const T t66 = t36 + t65; const T t67 = T(1) / t63; @@ -743,7 +754,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::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 +797,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::sqrt(t50); const T t52 = t17 + t1_z; const T t53 = -t52; const T t54 = t1_y + t30; @@ -803,7 +814,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::sqrt(t67); const T t69 = t18 * t2; const T t70 = t22 * t4; const T t71 = -t70; @@ -946,6 +957,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/simd.hpp b/src/ipc/utils/simd.hpp index ed92a3d32..7f0af6b97 100644 --- a/src/ipc/utils/simd.hpp +++ b/src/ipc/utils/simd.hpp @@ -2,9 +2,93 @@ #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 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() +{ + if constexpr (is_simd_batch_v) { + return T(std::numeric_limits::infinity()); + } else { + return 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)...)); + } +} + +/// @brief `sqrt` for any scalar the library templates on. +/// +/// The block-scope using-declaration is what makes this work: unqualified +/// lookup stops at it (so this does not recurse), while ADL still reaches +/// `xsimd::sqrt` for a batch and TinyAD's hidden friend for an autodiff +/// scalar. +template inline T sqrt(const T& x) +{ + using std::sqrt; + return sqrt(x); +} + +} // namespace ipc + #ifdef IPC_TOOLKIT_WITH_SIMD -#include #include namespace Eigen { @@ -20,6 +104,7 @@ struct NumTraits> : GenericNumTraits> { using Nested = xsimd::batch; using Literal = xsimd::batch; + // NOLINTBEGIN(readability-identifier-naming,performance-enum-size) enum { IsComplex = 0, IsInteger = 0, @@ -31,15 +116,16 @@ struct NumTraits> : GenericNumTraits> { AddCost = 1, MulCost = 1 }; + // NOLINTEND(readability-identifier-naming,performance-enum-size) - static inline Real epsilon() { return Real(NumTraits::epsilon()); } - static inline Real dummy_precision() + static Real epsilon() { return Real(NumTraits::epsilon()); } + static Real dummy_precision() { return Real(NumTraits::dummy_precision()); } - static inline int digits10() { return NumTraits::digits10(); } - static inline Real highest() { return Real(NumTraits::highest()); } - static inline Real lowest() { return Real(NumTraits::lowest()); } + static int digits10() { return NumTraits::digits10(); } + static Real highest() { return Real(NumTraits::highest()); } + static Real lowest() { return Real(NumTraits::lowest()); } }; } // namespace Eigen @@ -69,9 +155,11 @@ namespace ipc { /// scalar-side and group the problems by type before batching, which is the /// natural structure-of-arrays layout anyway. /// -/// @warning Only the distance *values* are instantiated. The gradients and -/// Hessians additionally require the ``autogen`` kernels to be instantiated for -/// the batch type. +/// @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 @@ -80,6 +168,39 @@ namespace ipc { /// them as ``SIMD_CXX_FLAGS``. template using SimdBatch = xsimd::batch; +template +inline constexpr bool is_simd_batch_v> = true; + } // 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::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/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..24e7e5648 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; @@ -502,4 +504,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..321bc3e3f --- /dev/null +++ b/tests/src/tests/barrier/test_simd_barrier.cpp @@ -0,0 +1,150 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +#include +#include +#include +#include + +using namespace ipc; + +namespace { + +using Batch = SimdBatch; +constexpr int L = int(Batch::size); + +/// @brief d <= 0 (penetration), d in (0, dhat/2), d in (dhat/2, dhat), and +/// d >= dhat (inactive) — every branch of every barrier. +std::vector regions(const double dhat) +{ + return { -0.1 * dhat, 0.1 * dhat, 0.7 * dhat, 1.5 * dhat }; +} + +/// @brief Fill the lanes from `regions` starting at `offset`. A batch may hold +/// fewer lanes than there are regions, so the caller sweeps the offset to reach +/// every region. +std::vector lane_ds(const double dhat, const int offset) +{ + const std::vector region_ds = regions(dhat); + std::vector ds(L); + for (int l = 0; l < L; ++l) { + ds[l] = region_ds[(l + offset) % region_ds.size()]; + } + return ds; +} + +bool close(const double got, const double want) +{ + // NaN is deliberately not tolerated: every barrier must resolve d <= 0 to + // +inf (value) or 0 (derivative), never NaN. + if (std::isinf(want)) { + return std::isinf(got) && (got > 0) == (want > 0); + } + return std::abs(got - want) <= 1e-12 * std::max(1.0, std::abs(want)); +} + +template +void check_matches_scalar( + const std::string& name, + const double dhat, + BarrierValue&& value, + BarrierBatch&& batch_value) +{ + for (int offset = 0; offset < int(regions(dhat).size()); ++offset) { + const std::vector ds = lane_ds(dhat, offset); + + const Batch d_batch = Batch::load_unaligned(ds.data()); + const Batch dhat_batch(dhat); + const Batch got_batch = batch_value(d_batch, dhat_batch); + + std::vector got(L); + got_batch.store_unaligned(got.data()); + + for (int l = 0; l < L; ++l) { + const double want = value(ds[l], dhat); + CAPTURE(name, offset, l, ds[l], dhat, got[l], want); + CHECK(close(got[l], want)); + } + } +} + +} // 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/tangent/CMakeLists.txt b/tests/src/tests/tangent/CMakeLists.txt index 32a642309..99b002b78 100644 --- a/tests/src/tests/tangent/CMakeLists.txt +++ b/tests/src/tests/tangent/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests test_closest_point.cpp test_relative_velocity.cpp + test_simd_tangent_basis.cpp test_tangent_basis.cpp # Benchmarks 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..c135bfd8d --- /dev/null +++ b/tests/src/tests/tangent/test_simd_tangent_basis.cpp @@ -0,0 +1,148 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +#include +#include +#include + +using namespace ipc; + +namespace { + +using Batch = SimdBatch; +constexpr int L = int(Batch::size); + +/// @brief The batch and scalar paths differ only in how the compiler contracts +/// multiply-adds. As with the distance derivatives, the tolerance is scaled by +/// the magnitude of the whole result rather than per-entry: a tangent-basis +/// Jacobian for a near-degenerate configuration has entries that are +/// catastrophic cancellations of much larger terms. A structurally wrong entry +/// would differ by order of the result's own magnitude. +constexpr double TOL = 1e-9; + +/// @brief Pack the k-th coordinate of L problems into one batch. +Eigen::Vector3 pack(const std::vector& v) +{ + Eigen::Vector3 out; + for (int k = 0; k < 3; ++k) { + std::vector tmp(L); + for (int l = 0; l < L; ++l) { + tmp[l] = v[l][k]; + } + out[k] = Batch::load_unaligned(tmp.data()); + } + return out; +} + +std::vector random_points(const int seed) +{ + std::srand(seed); + std::vector v(L); + for (int l = 0; l < L; ++l) { + v[l] = Eigen::Vector3d::Random(); + } + return v; +} + +/// @brief Compare a batch-valued matrix against the scalar result per lane. +template +void check_lanes( + const std::string& name, const BatchMatrix& batched, ScalarOf&& scalar_of) +{ + for (int l = 0; l < L; ++l) { + const auto want = scalar_of(l); + REQUIRE(batched.rows() == want.rows()); + REQUIRE(batched.cols() == want.cols()); + const double scale = std::max(1.0, want.array().abs().maxCoeff()); + for (Eigen::Index i = 0; i < want.size(); ++i) { + const double got = batched(i).get(l); + CAPTURE(name, l, i, got, want(i), scale); + CHECK(std::abs(got - want(i)) <= TOL * scale); + } + } +} + +} // namespace + +TEST_CASE( + "SIMD batch tangent bases match the scalar ones lane-wise", + "[tangent_basis][simd]") +{ + const std::vector A = random_points(1); + const std::vector B = random_points(2); + const std::vector C = random_points(3); + const std::vector D = random_points(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. + std::vector A(L, Eigen::Vector3d::Zero()), B(L); + for (int l = 0; l < L; ++l) { + 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 From 54e1aa81f3da0d38ab1ed40eacfbb84a5a6f0947 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 3 Sep 2026 15:34:57 -0400 Subject: [PATCH 04/34] Add a benchmark of the barrier potential per scalar type Time the per-collision barrier value, gradient, and Hessian with double, float, SimdBatch, and SimdBatch on the assembly benchmark scenes, so the four are compared on identical work. Collisions are grouped by kind and distance type and packed one per lane; a single templated loop evaluates the chain d -> f(d) -> derivatives for every scalar type, and the library path is timed alongside as a reference. A compute-only Hessian column sums the entries instead of storing them, separating the arithmetic from the 144 stores per collision, which are memory-bound once threads share the bus. Every variant is checked against the double path, and the double path against the library on the unmollified collisions. Two caveats the report states explicitly: the edge-edge mollifier has no batch implementation and is excluded, and raw float overflows the generated line-line and point-plane Hessians at scene scale (edge lengths ~1e-4), so "rescaled" float variants re-center each stencil and divide by dhat before conversion, folding the scale back into kappa. On puffer-ball (512k collisions, AVX2, single-threaded) SimdBatch is 3.1x faster than the scalar loop on the gradient and 2.2x on the Hessian without stores; SimdBatch 7.1x and 4.5x. Co-Authored-By: Claude Fable 5.1 --- tests/src/tests/potential/CMakeLists.txt | 1 + .../benchmark_simd_barrier_potential.cpp | 1210 +++++++++++++++++ 2 files changed, 1211 insertions(+) create mode 100644 tests/src/tests/potential/benchmark_simd_barrier_potential.cpp 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..337ec499f --- /dev/null +++ b/tests/src/tests/potential/benchmark_simd_barrier_potential.cpp @@ -0,0 +1,1210 @@ +// 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: its +// implementation branches on a scalar `if` and is only instantiated for +// `float`/`double`, so a batch cannot evaluate it. 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 + +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); + static Batch load(const Real* p) { return Batch::load_unaligned(p); } + static void store(Real* p, const Batch& v) { v.store_unaligned(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. +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; + std::vector x; ///< nblocks * 3*NV * L + std::vector w; ///< nblocks * L; zero on padding lanes. + std::vector grad_out; + std::vector 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); + } +} From d06ea9aaece6bf98a9e3537f1bda4faaf183bba1 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 4 Sep 2026 00:26:40 -0400 Subject: [PATCH 05/34] Implement SIMD handling for edge-edge mollification --- docs/source/about/release_notes.rst | 1 + src/ipc/distance/edge_edge_mollifier.cpp | 83 +++--- src/ipc/distance/edge_edge_mollifier.hpp | 94 +++--- tests/src/tests/distance/CMakeLists.txt | 1 + .../test_simd_edge_edge_mollifier.cpp | 276 ++++++++++++++++++ 5 files changed, 376 insertions(+), 79 deletions(-) create mode 100644 tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 4b7bf14df..96ace5df2 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -70,6 +70,7 @@ API Changes |:wrench:| - ``Eigen::NumTraits`` is specialized for ``xsimd::batch``, and ``ipc::SimdBatch`` aliases the batch type for the build's architecture. Passing ``Eigen::Vector3>`` evaluates one independent problem per SIMD lane, letting a caller with a structure-of-arrays layout compute several distances per call. - The values, gradients, and Hessians of the point-point, point-line, point-edge, line-line, edge-edge, and point-triangle distances are instantiated for ``SimdBatch`` and ``SimdBatch``. Measured agreement with the scalar path is one ulp on the values; the derivatives agree to ~1e-11 relative to their own magnitude, since the two paths contract multiply-adds differently and these derivatives are ill-conditioned for near-parallel edges. - The barrier functions and classes (``barrier``, ``ClampedLogBarrier``, ``ClampedLogSqBarrier``, ``CubicBarrier``, ``TwoStageBarrier``), the tangent bases and their Jacobians, the closest-point Jacobians/Hessians, the unnormalized-normal Jacobians/Hessians, the signed-distance Hessians, and the triangle-area gradient are instantiated for batch scalars as well. + - The edge-edge mollifier is instantiated for batch scalars too: the mollifier and its gradient/Hessian, their derivatives with respect to the threshold, the threshold and its gradient, and the edge-edge cross-product squared norm with its gradient/Hessian. - Functions that pick a case from the values themselves — a barrier clamping at ``d̂``, the 3D point-point tangent basis choosing a reference axis — evaluate every case for a batch and blend the results per-lane, so one batch may carry lanes in different cases. The scalar instantiations keep their original early-return form and are unchanged. - ``ipc::normalized(v)`` replaces ``v.normalized()`` in the tangent bases: Eigen's own ``normalized()`` guards a zero-length vector with an ``if``, which a batch cannot answer with one ``bool``. It applies the same rule per-lane and defers to Eigen for scalars. - 💥 **[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. 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 38c2013d9..7ce26413f 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -2,6 +2,7 @@ #include #include +#include namespace ipc { @@ -28,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. @@ -43,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 * 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 @@ -60,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. @@ -71,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 @@ -88,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 --------------------------------------------------- @@ -145,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. @@ -167,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. @@ -186,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. diff --git a/tests/src/tests/distance/CMakeLists.txt b/tests/src/tests/distance/CMakeLists.txt index 942dc3170..76211339b 100644 --- a/tests/src/tests/distance/CMakeLists.txt +++ b/tests/src/tests/distance/CMakeLists.txt @@ -10,6 +10,7 @@ set(SOURCES test_point_point.cpp test_point_triangle.cpp test_simd_distance.cpp + test_simd_edge_edge_mollifier.cpp test_signed_distance.cpp # Benchmarks 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..e6ebc6c07 --- /dev/null +++ b/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp @@ -0,0 +1,276 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +#include + +#include +#include +#include + +using namespace ipc; + +namespace { + +using Batch = SimdBatch; +constexpr int L = int(Batch::size); + +/// @brief The threshold of two unit-length rest edges. +constexpr double EPS_X = 1e-3; + +/// @brief The batch and scalar paths differ only in how the compiler contracts +/// multiply-adds. As in the other SIMD tests, the tolerance is relative to the +/// magnitude of the whole result: the mollifier derivatives scale as 1/eps_x +/// and 1/eps_x², so a per-entry absolute tolerance would be meaningless. +constexpr double TOL = 1e-9; + +/// @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. +const std::vector THETAS = { 0.0, 0.02, 0.0316, 0.4 }; + +/// @brief Fill the lanes from `THETAS` starting at `offset`. A batch may hold +/// fewer lanes than there are cases, so the caller sweeps the offset to reach +/// every one. +std::vector lane_thetas(const int offset) +{ + std::vector thetas(L); + for (int l = 0; l < L; ++l) { + thetas[l] = THETAS[(l + offset) % THETAS.size()]; + } + return thetas; +} + +/// @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 }; +} + +/// @brief Pack the k-th coordinate of L problems into one batch. +Eigen::Vector3 pack(const std::vector& v) +{ + Eigen::Vector3 out; + for (int k = 0; k < 3; ++k) { + std::vector tmp(L); + for (int l = 0; l < L; ++l) { + tmp[l] = v[l][k]; + } + out[k] = Batch::load_unaligned(tmp.data()); + } + return out; +} + +void check_lane( + const std::string& name, + const int offset, + const int l, + const double got, + const double want) +{ + const double scale = std::max(1.0, std::abs(want)); + CAPTURE(name, offset, l, got, want, scale); + CHECK(std::abs(got - want) <= TOL * scale); +} + +/// @brief Compare a batch-valued matrix against the scalar result per lane. +template +void check_lanes( + const std::string& name, + const int offset, + const BatchMatrix& batched, + ScalarOf&& scalar_of) +{ + for (int l = 0; l < L; ++l) { + const auto want = scalar_of(l); + REQUIRE(batched.rows() == want.rows()); + REQUIRE(batched.cols() == want.cols()); + // One scale for the whole result: entries of a mollifier Hessian are + // cancellations of much larger terms. + const double scale = std::max(1.0, want.array().abs().maxCoeff()); + for (Eigen::Index i = 0; i < want.size(); ++i) { + CAPTURE(name, offset, l, i, scale); + CHECK(std::abs(batched(i).get(l) - want(i)) <= TOL * scale); + } + } +} + +} // 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. + const std::vector region_xs = { 0.0, 0.1 * EPS_X, 0.9 * EPS_X, + 2.0 * EPS_X }; + + for (int offset = 0; offset < int(region_xs.size()); ++offset) { + std::vector xs(L); + for (int l = 0; l < L; ++l) { + xs[l] = region_xs[(l + offset) % region_xs.size()]; + } + + const Batch x = Batch::load_unaligned(xs.data()); + const Batch eps_x(EPS_X); + + const Batch m = edge_edge_mollifier(x, eps_x); + const Batch dm = edge_edge_mollifier_gradient(x, eps_x); + const Batch d2m = edge_edge_mollifier_hessian(x, eps_x); + const Batch dm_deps = + edge_edge_mollifier_derivative_wrt_eps_x(x, eps_x); + const Batch d2m_deps = + edge_edge_mollifier_gradient_derivative_wrt_eps_x(x, eps_x); + + for (int l = 0; l < L; ++l) { + check_lane( + "mollifier", offset, l, m.get(l), + edge_edge_mollifier(xs[l], EPS_X)); + check_lane( + "gradient", offset, l, dm.get(l), + edge_edge_mollifier_gradient(xs[l], EPS_X)); + check_lane( + "hessian", offset, l, d2m.get(l), + edge_edge_mollifier_hessian(xs[l], EPS_X)); + check_lane( + "derivative_wrt_eps_x", offset, l, dm_deps.get(l), + edge_edge_mollifier_derivative_wrt_eps_x(xs[l], EPS_X)); + check_lane( + "gradient_derivative_wrt_eps_x", offset, l, d2m_deps.get(l), + edge_edge_mollifier_gradient_derivative_wrt_eps_x( + xs[l], EPS_X)); + } + } +} + +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 std::vector thetas = lane_thetas(offset); + + std::vector configs(L); + std::vector EA0(L), EA1(L), EB0(L), EB1(L); + // 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. + std::vector RA0(L), RA1(L), RB0(L), RB1(L); + 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. + const Batch eps_x_batch = + edge_edge_mollifier_threshold(ra0, ra1, rb0, rb1); + const Batch x_batch = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + const Batch m_batch = edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x); + for (int l = 0; l < L; ++l) { + check_lane( + "threshold", offset, l, eps_x_batch.get(l), + edge_edge_mollifier_threshold(RA0[l], RA1[l], RB0[l], RB1[l])); + check_lane( + "cross_squarednorm", offset, l, x_batch.get(l), + edge_edge_cross_squarednorm(EA0[l], EA1[l], EB0[l], EB1[l])); + check_lane( + "mollifier", offset, l, m_batch.get(l), + edge_edge_mollifier(EA0[l], EA1[l], EB0[l], EB1[l], EPS_X)); + } + + check_lanes( + "cross_squarednorm_gradient", offset, + 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", offset, + 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", offset, + 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", offset, + 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", offset, + 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", offset, + 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", offset, + 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 From 16f396001068472ddbb19b1f2c0a0ea5c24d9d62 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 4 Sep 2026 10:44:31 -0400 Subject: [PATCH 06/34] Add SIMD batch support to the normals and signed distances The normalized normals (`point_line_normal`, `triangle_normal`, `line_line_normal`) called Eigen's `normalized()`, whose zero-length guard is an `if` on a value that a batch cannot answer with one bool. They now use `ipc::normalized`, which applies that rule per-lane, as the tangent bases already do. That was the only thing blocking the values and gradients of the `point_line`, `line_line`, and `point_plane` signed distances, which route through those normals; previously only their Hessians were instantiated for batch scalars. All of these are header-only templates, so no new instantiations were needed. `edge_length_gradient` asserted `(e1 - e0).norm() != 0`, which likewise has no single answer for a batch. That assert is now scalar-only, matching how `tangent_basis.hpp` handles the same pattern. The relative-velocity functions turned out to already be batch-compatible; they are noted in the release notes and covered by tests separately. Co-Authored-By: Claude Opus 5 --- docs/source/about/release_notes.rst | 3 +++ src/ipc/geometry/area.hpp | 7 ++++++- src/ipc/geometry/normal.hpp | 7 ++++--- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 96ace5df2..273a63087 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -71,6 +71,9 @@ API Changes |:wrench:| - The values, gradients, and Hessians of the point-point, point-line, point-edge, line-line, edge-edge, and point-triangle distances are instantiated for ``SimdBatch`` and ``SimdBatch``. Measured agreement with the scalar path is one ulp on the values; the derivatives agree to ~1e-11 relative to their own magnitude, since the two paths contract multiply-adds differently and these derivatives are ill-conditioned for near-parallel edges. - The barrier functions and classes (``barrier``, ``ClampedLogBarrier``, ``ClampedLogSqBarrier``, ``CubicBarrier``, ``TwoStageBarrier``), the tangent bases and their Jacobians, the closest-point Jacobians/Hessians, the unnormalized-normal Jacobians/Hessians, the signed-distance Hessians, and the triangle-area gradient are instantiated for batch scalars as well. - The edge-edge mollifier is instantiated for batch scalars too: the mollifier and its gradient/Hessian, their derivatives with respect to the threshold, the threshold and its gradient, and the edge-edge cross-product squared norm with its gradient/Hessian. + - The normalized normals (``point_line_normal``, ``triangle_normal``, ``line_line_normal``) use ``ipc::normalized`` instead of Eigen's ``normalized()``, which makes them and the values/gradients of the ``point_line``, ``line_line``, and ``point_plane`` signed distances usable with batch scalars (previously only the signed-distance Hessians were). + - ``edge_length_gradient`` asserts its non-degeneracy only for a plain scalar, so it accepts batch scalars as well. + - The relative-velocity functions (values, Jacobians, and ``dx_dbeta`` tensors for point-point, point-edge, edge-edge, and point-triangle, in 2D and 3D) were already batch-compatible and are now covered by tests, agreeing with the scalar path to 1e-14 relative. - Functions that pick a case from the values themselves — a barrier clamping at ``d̂``, the 3D point-point tangent basis choosing a reference axis — evaluate every case for a batch and blend the results per-lane, so one batch may carry lanes in different cases. The scalar instantiations keep their original early-return form and are unchanged. - ``ipc::normalized(v)`` replaces ``v.normalized()`` in the tangent bases: Eigen's own ``normalized()`` guards a zero-length vector with an ``if``, which a batch cannot answer with one ``bool``. It applies the same rule per-lane and defers to Eigen for scalars. - 💥 **[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. 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.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 From faeb6542da0a771c3af23b51435cf903bd8cbf1c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 4 Sep 2026 10:44:58 -0400 Subject: [PATCH 07/34] Consolidate the SIMD test helpers and cover the remaining batch functions Six SIMD test files each carried a private copy of the same lane-packing and lane-comparison helpers. They now share `tests/simd_utils.hpp`, which owns `Batch`, `L`, `Lanes`, `Points`, `pack`, `random_points`, `lane_cases`, `approx`, `check_lanes`, `check_scalar_lanes`, and `check_swept_lanes`. The four pre-existing files shrank by 37-81 lines each. Three changes beyond a straight extraction: - `Lanes` is a `std::array` aligned to the architecture's requirement, so packing uses `load_aligned`/`store_aligned` rather than staging through an unaligned buffer. A `static_assert` checks that the type actually meets the alignment its load claims. - Comparisons go through `Catch::Approx` instead of a hand-rolled `close`. `epsilon(0)` is not optional there: `Approx` ors its margin test with an epsilon test whose default is float-grade (~1e-5 relative), which would silently pass a wrong double. `Approx` also compares without subtracting, so it already gives the sign-respecting infinity matching the barriers need. - The barrier's region sweep and the mollifier's threshold sweep were the same loop, and are now one `check_swept_lanes`. New coverage for the batch functions that had none: the normals and the signed distances, the triangle area and edge length, and the relative velocities, which were already batch-compatible but untested. The relative velocities are held to the value tolerance rather than the derivative one, since they are linear combinations with no cancellation of large terms. `test_simd_utils.cpp` pins down the shared helper's own contract, six files now depending on it: the epsilon leak above, infinity matching by sign, and NaN never passing. It caught `tol * scale` going infinite when the expected value is `+inf` -- the case the barrier tests rely on -- which would have made those lanes accept anything. The signed-distance values needed a looser bound than the unsigned ones, and structurally so: a signed distance is `normal.dot(p - x0)` with the normal a normalized cross product, so the cross cancels and the sqrt and division that follow amplify the two paths' differing multiply-add contraction. The reason is recorded at the call site rather than hidden in a per-file constant. --- tests/src/tests/CMakeLists.txt | 1 + tests/src/tests/barrier/test_simd_barrier.cpp | 61 +---- tests/src/tests/distance/CMakeLists.txt | 1 + .../src/tests/distance/test_simd_distance.cpp | 215 ++++++--------- .../test_simd_edge_edge_mollifier.cpp | 208 +++++--------- .../distance/test_simd_signed_distance.cpp | 122 +++++++++ tests/src/tests/geometry/CMakeLists.txt | 1 + .../src/tests/geometry/test_simd_geometry.cpp | 253 ++++++++++++++++++ tests/src/tests/simd_utils.hpp | 223 +++++++++++++++ tests/src/tests/tangent/CMakeLists.txt | 1 + .../tangent/test_simd_relative_velocity.cpp | 204 ++++++++++++++ .../tests/tangent/test_simd_tangent_basis.cpp | 73 +---- tests/src/tests/utils/CMakeLists.txt | 1 + tests/src/tests/utils/test_simd_utils.cpp | 88 ++++++ 14 files changed, 1066 insertions(+), 386 deletions(-) create mode 100644 tests/src/tests/distance/test_simd_signed_distance.cpp create mode 100644 tests/src/tests/geometry/test_simd_geometry.cpp create mode 100644 tests/src/tests/simd_utils.hpp create mode 100644 tests/src/tests/tangent/test_simd_relative_velocity.cpp create mode 100644 tests/src/tests/utils/test_simd_utils.cpp 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/barrier/test_simd_barrier.cpp b/tests/src/tests/barrier/test_simd_barrier.cpp index 321bc3e3f..e4b5fc1b3 100644 --- a/tests/src/tests/barrier/test_simd_barrier.cpp +++ b/tests/src/tests/barrier/test_simd_barrier.cpp @@ -1,53 +1,29 @@ #include -#include +#include #ifdef IPC_TOOLKIT_WITH_SIMD #include -#include -#include -#include -#include +#include using namespace ipc; +using namespace ipc::tests; namespace { -using Batch = SimdBatch; -constexpr int L = int(Batch::size); - /// @brief d <= 0 (penetration), d in (0, dhat/2), d in (dhat/2, dhat), and -/// d >= dhat (inactive) — every branch of every barrier. -std::vector regions(const double dhat) +/// 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 Fill the lanes from `regions` starting at `offset`. A batch may hold -/// fewer lanes than there are regions, so the caller sweeps the offset to reach -/// every region. -std::vector lane_ds(const double dhat, const int offset) -{ - const std::vector region_ds = regions(dhat); - std::vector ds(L); - for (int l = 0; l < L; ++l) { - ds[l] = region_ds[(l + offset) % region_ds.size()]; - } - return ds; -} - -bool close(const double got, const double want) -{ - // NaN is deliberately not tolerated: every barrier must resolve d <= 0 to - // +inf (value) or 0 (derivative), never NaN. - if (std::isinf(want)) { - return std::isinf(got) && (got > 0) == (want > 0); - } - return std::abs(got - want) <= 1e-12 * std::max(1.0, std::abs(want)); -} - +/// @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, @@ -55,22 +31,9 @@ void check_matches_scalar( BarrierValue&& value, BarrierBatch&& batch_value) { - for (int offset = 0; offset < int(regions(dhat).size()); ++offset) { - const std::vector ds = lane_ds(dhat, offset); - - const Batch d_batch = Batch::load_unaligned(ds.data()); - const Batch dhat_batch(dhat); - const Batch got_batch = batch_value(d_batch, dhat_batch); - - std::vector got(L); - got_batch.store_unaligned(got.data()); - - for (int l = 0; l < L; ++l) { - const double want = value(ds[l], dhat); - CAPTURE(name, offset, l, ds[l], dhat, got[l], want); - CHECK(close(got[l], want)); - } - } + check_swept_lanes( + name, regions(dhat), [&](double d) { return value(d, dhat); }, + [&](Batch d) { return batch_value(d, Batch(dhat)); }, 1e-12); } } // namespace diff --git a/tests/src/tests/distance/CMakeLists.txt b/tests/src/tests/distance/CMakeLists.txt index 76211339b..829389de9 100644 --- a/tests/src/tests/distance/CMakeLists.txt +++ b/tests/src/tests/distance/CMakeLists.txt @@ -11,6 +11,7 @@ set(SOURCES 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_simd_distance.cpp b/tests/src/tests/distance/test_simd_distance.cpp index 982133261..8fb15c46a 100644 --- a/tests/src/tests/distance/test_simd_distance.cpp +++ b/tests/src/tests/distance/test_simd_distance.cpp @@ -2,7 +2,7 @@ #include #include -#include +#include #ifdef IPC_TOOLKIT_WITH_SIMD @@ -13,153 +13,100 @@ #include #include -#include -#include - using namespace ipc; - -namespace { - -using Batch = SimdBatch; -constexpr int L = int(Batch::size); - -/// @brief Pack the k-th coordinate of L problems into one batch. -Eigen::Vector3 pack(const std::vector& v) -{ - Eigen::Vector3 out; - for (int k = 0; k < 3; ++k) { - std::vector tmp(L); - for (int l = 0; l < L; ++l) { - tmp[l] = v[l][k]; - } - out[k] = Batch::load_unaligned(tmp.data()); - } - return out; -} - -/// @brief Agreement to within rounding; the batch and scalar paths differ only -/// in how the compiler contracts multiply-adds. -bool close(const double got, const double want) -{ - return std::abs(got - want) <= 1e-14 * std::max(1.0, std::abs(want)); -} - -std::vector random_points(const int seed) -{ - std::srand(seed); - std::vector v(L); - for (int l = 0; l < L; ++l) { - v[l] = Eigen::Vector3d::Random(); - } - return v; -} - -} // namespace +using namespace ipc::tests; TEST_CASE( "SIMD batch distances match the scalar ones lane-wise", "[distance][simd]") { - const std::vector A = random_points(1), - B = random_points(2), - C = random_points(3), - D = random_points(4); + 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); - const Batch pp = point_point_distance(a, b); - const Batch pl = point_line_distance(a, b, c); - const Batch ll = line_line_distance(a, b, c, d); - const Batch pe = point_edge_distance(a, b, c, PointEdgeDistanceType::P_E); - const Batch ee = - edge_edge_distance(a, b, c, d, EdgeEdgeDistanceType::EA_EB); - const Batch pt = - point_triangle_distance(a, b, c, d, PointTriangleDistanceType::P_T); - - for (int l = 0; l < L; ++l) { - CAPTURE(l); - // A batch lane must agree with the scalar answer to within rounding; - // the two differ only in how the compiler contracts multiply-adds. - CHECK(close(pp.get(l), point_point_distance(A[l], B[l]))); - CHECK(close(pl.get(l), point_line_distance(A[l], B[l], C[l]))); - CHECK(close(ll.get(l), line_line_distance(A[l], B[l], C[l], D[l]))); - CHECK(close( - pe.get(l), - point_edge_distance(A[l], B[l], C[l], PointEdgeDistanceType::P_E))); - CHECK(close( - ee.get(l), - edge_edge_distance( - A[l], B[l], C[l], D[l], EdgeEdgeDistanceType::EA_EB))); - CHECK(close( - pt.get(l), - point_triangle_distance( - A[l], B[l], C[l], D[l], PointTriangleDistanceType::P_T))); - } + // 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 std::vector A = random_points(11), - B = random_points(12), - C = random_points(13), - D = random_points(14); + 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); - // Compare a batched derivative against the scalar one, lane by lane. - // - // NOTE: the tolerance is scaled by the magnitude of the whole result, not - // by each entry. These derivatives are ill-conditioned for near-parallel - // edges -- a random configuration routinely produces entries of order 1e6 - // whose small differences are catastrophic cancellations of large terms. - // The batch and scalar paths contract multiply-adds differently, so they - // agree to roughly 1e-11 relative to the norm rather than to an ulp. A - // structurally wrong entry would differ by order of the norm itself. - const auto check_all = [&](const auto& batched, const auto& scalar_of) { - for (int l = 0; l < L; ++l) { - CAPTURE(l); - const auto want = scalar_of(l); - REQUIRE(batched.size() == want.size()); - const double scale = std::max(1.0, want.array().abs().maxCoeff()); - for (Eigen::Index i = 0; i < want.size(); ++i) { - CAPTURE(i); - const double got = batched(i).get(l), expect = want(i); - CAPTURE(got, expect, std::abs(got - expect), scale); - CHECK(std::abs(got - expect) <= 1e-9 * scale); - } - } - }; - - check_all(point_point_distance_gradient(a, b), [&](int l) { - return point_point_distance_gradient(A[l], B[l]).eval(); - }); - check_all(point_point_distance_hessian(a, b), [&](int l) { - return point_point_distance_hessian(A[l], B[l]).eval(); - }); - - check_all(point_line_distance_gradient(a, b, c), [&](int l) { - return point_line_distance_gradient(A[l], B[l], C[l]).eval(); - }); - check_all(point_line_distance_hessian(a, b, c), [&](int l) { - return point_line_distance_hessian(A[l], B[l], C[l]).eval(); - }); - - check_all(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_all(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_all( + 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_all( + check_lanes( + "point_edge_distance_hessian", point_edge_distance_hessian(a, b, c, PointEdgeDistanceType::P_E), [&](int l) { return point_edge_distance_hessian( @@ -167,14 +114,16 @@ TEST_CASE( .eval(); }); - check_all( + 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_all( + check_lanes( + "edge_edge_distance_hessian", edge_edge_distance_hessian(a, b, c, d, EdgeEdgeDistanceType::EA_EB), [&](int l) { return edge_edge_distance_hessian( @@ -182,7 +131,8 @@ TEST_CASE( .eval(); }); - check_all( + check_lanes( + "point_triangle_distance_gradient", point_triangle_distance_gradient( a, b, c, d, PointTriangleDistanceType::P_T), [&](int l) { @@ -190,7 +140,8 @@ TEST_CASE( A[l], B[l], C[l], D[l], PointTriangleDistanceType::P_T) .eval(); }); - check_all( + check_lanes( + "point_triangle_distance_hessian", point_triangle_distance_hessian( a, b, c, d, PointTriangleDistanceType::P_T), [&](int l) { @@ -205,10 +156,8 @@ 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 std::vector A = random_points(5), - B = random_points(6), - C = random_points(7), - D = random_points(8); + 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); diff --git a/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp b/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp index e6ebc6c07..b74faf782 100644 --- a/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp +++ b/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp @@ -1,6 +1,6 @@ #include -#include +#include #ifdef IPC_TOOLKIT_WITH_SIMD @@ -8,43 +8,23 @@ #include +#include #include #include -#include using namespace ipc; +using namespace ipc::tests; namespace { -using Batch = SimdBatch; -constexpr int L = int(Batch::size); - /// @brief The threshold of two unit-length rest edges. constexpr double EPS_X = 1e-3; -/// @brief The batch and scalar paths differ only in how the compiler contracts -/// multiply-adds. As in the other SIMD tests, the tolerance is relative to the -/// magnitude of the whole result: the mollifier derivatives scale as 1/eps_x -/// and 1/eps_x², so a per-entry absolute tolerance would be meaningless. -constexpr double TOL = 1e-9; - /// @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. -const std::vector THETAS = { 0.0, 0.02, 0.0316, 0.4 }; - -/// @brief Fill the lanes from `THETAS` starting at `offset`. A batch may hold -/// fewer lanes than there are cases, so the caller sweeps the offset to reach -/// every one. -std::vector lane_thetas(const int offset) -{ - std::vector thetas(L); - for (int l = 0; l < L; ++l) { - thetas[l] = THETAS[(l + offset) % THETAS.size()]; - } - return thetas; -} +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. @@ -78,54 +58,6 @@ Config config(const double theta, const int l) return { R * ea0 + t, R * ea1 + t, R * eb0 + t, R * eb1 + t }; } -/// @brief Pack the k-th coordinate of L problems into one batch. -Eigen::Vector3 pack(const std::vector& v) -{ - Eigen::Vector3 out; - for (int k = 0; k < 3; ++k) { - std::vector tmp(L); - for (int l = 0; l < L; ++l) { - tmp[l] = v[l][k]; - } - out[k] = Batch::load_unaligned(tmp.data()); - } - return out; -} - -void check_lane( - const std::string& name, - const int offset, - const int l, - const double got, - const double want) -{ - const double scale = std::max(1.0, std::abs(want)); - CAPTURE(name, offset, l, got, want, scale); - CHECK(std::abs(got - want) <= TOL * scale); -} - -/// @brief Compare a batch-valued matrix against the scalar result per lane. -template -void check_lanes( - const std::string& name, - const int offset, - const BatchMatrix& batched, - ScalarOf&& scalar_of) -{ - for (int l = 0; l < L; ++l) { - const auto want = scalar_of(l); - REQUIRE(batched.rows() == want.rows()); - REQUIRE(batched.cols() == want.cols()); - // One scale for the whole result: entries of a mollifier Hessian are - // cancellations of much larger terms. - const double scale = std::max(1.0, want.array().abs().maxCoeff()); - for (Eigen::Index i = 0; i < want.size(); ++i) { - CAPTURE(name, offset, l, i, scale); - CHECK(std::abs(batched(i).get(l) - want(i)) <= TOL * scale); - } - } -} - } // namespace TEST_CASE( @@ -133,45 +65,44 @@ TEST_CASE( "[distance][mollifier][simd]") { // x straddling eps_x, so one batch carries mollified and inactive lanes. - const std::vector region_xs = { 0.0, 0.1 * EPS_X, 0.9 * EPS_X, - 2.0 * EPS_X }; - - for (int offset = 0; offset < int(region_xs.size()); ++offset) { - std::vector xs(L); - for (int l = 0; l < L; ++l) { - xs[l] = region_xs[(l + offset) % region_xs.size()]; - } - - const Batch x = Batch::load_unaligned(xs.data()); - const Batch eps_x(EPS_X); - - const Batch m = edge_edge_mollifier(x, eps_x); - const Batch dm = edge_edge_mollifier_gradient(x, eps_x); - const Batch d2m = edge_edge_mollifier_hessian(x, eps_x); - const Batch dm_deps = - edge_edge_mollifier_derivative_wrt_eps_x(x, eps_x); - const Batch d2m_deps = - edge_edge_mollifier_gradient_derivative_wrt_eps_x(x, eps_x); - - for (int l = 0; l < L; ++l) { - check_lane( - "mollifier", offset, l, m.get(l), - edge_edge_mollifier(xs[l], EPS_X)); - check_lane( - "gradient", offset, l, dm.get(l), - edge_edge_mollifier_gradient(xs[l], EPS_X)); - check_lane( - "hessian", offset, l, d2m.get(l), - edge_edge_mollifier_hessian(xs[l], EPS_X)); - check_lane( - "derivative_wrt_eps_x", offset, l, dm_deps.get(l), - edge_edge_mollifier_derivative_wrt_eps_x(xs[l], EPS_X)); - check_lane( - "gradient_derivative_wrt_eps_x", offset, l, d2m_deps.get(l), - edge_edge_mollifier_gradient_derivative_wrt_eps_x( - xs[l], EPS_X)); - } - } + 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( @@ -179,13 +110,13 @@ TEST_CASE( "[distance][mollifier][simd]") { for (int offset = 0; offset < int(THETAS.size()); ++offset) { - const std::vector thetas = lane_thetas(offset); + const Lanes thetas = lane_cases(THETAS, offset); + const std::string at = " (offset " + std::to_string(offset) + ")"; - std::vector configs(L); - std::vector EA0(L), EA1(L), EB0(L), EB1(L); + 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. - std::vector RA0(L), RA1(L), RB0(L), RB1(L); + 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; @@ -200,31 +131,34 @@ TEST_CASE( const Batch eps_x(EPS_X); // The threshold itself must agree before anything built on it does. - const Batch eps_x_batch = - edge_edge_mollifier_threshold(ra0, ra1, rb0, rb1); - const Batch x_batch = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - const Batch m_batch = edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x); - for (int l = 0; l < L; ++l) { - check_lane( - "threshold", offset, l, eps_x_batch.get(l), - edge_edge_mollifier_threshold(RA0[l], RA1[l], RB0[l], RB1[l])); - check_lane( - "cross_squarednorm", offset, l, x_batch.get(l), - edge_edge_cross_squarednorm(EA0[l], EA1[l], EB0[l], EB1[l])); - check_lane( - "mollifier", offset, l, m_batch.get(l), - edge_edge_mollifier(EA0[l], EA1[l], EB0[l], EB1[l], EPS_X)); - } + 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", offset, + "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", offset, + "cross_squarednorm_hessian" + at, edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1), [&](int l) { return edge_edge_cross_squarednorm_hessian( @@ -232,28 +166,28 @@ TEST_CASE( }); check_lanes( - "mollifier_gradient", offset, + "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", offset, + "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", offset, + "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", offset, + "mollifier_gradient_wrt_x" + at, edge_edge_mollifier_gradient_wrt_x( ra0, ra1, rb0, rb1, ea0, ea1, eb0, eb1), [&](int l) { @@ -262,7 +196,7 @@ TEST_CASE( EB1[l]); }); check_lanes( - "mollifier_gradient_jacobian_wrt_x", offset, + "mollifier_gradient_jacobian_wrt_x" + at, edge_edge_mollifier_gradient_jacobian_wrt_x( ra0, ra1, rb0, rb1, ea0, ea1, eb0, eb1), [&](int l) { 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/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..f490b89bc --- /dev/null +++ b/tests/src/tests/geometry/test_simd_geometry.cpp @@ -0,0 +1,253 @@ +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#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(); }); +} + +#endif diff --git a/tests/src/tests/simd_utils.hpp b/tests/src/tests/simd_utils.hpp new file mode 100644 index 000000000..81e3445c0 --- /dev/null +++ b/tests/src/tests/simd_utils.hpp @@ -0,0 +1,223 @@ +#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 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)); + } + } +} + +} // namespace ipc::tests + +#endif diff --git a/tests/src/tests/tangent/CMakeLists.txt b/tests/src/tests/tangent/CMakeLists.txt index 99b002b78..35297e6b8 100644 --- a/tests/src/tests/tangent/CMakeLists.txt +++ b/tests/src/tests/tangent/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests test_closest_point.cpp test_relative_velocity.cpp + test_simd_relative_velocity.cpp test_simd_tangent_basis.cpp test_tangent_basis.cpp 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 index c135bfd8d..e63b145ca 100644 --- a/tests/src/tests/tangent/test_simd_tangent_basis.cpp +++ b/tests/src/tests/tangent/test_simd_tangent_basis.cpp @@ -1,82 +1,20 @@ #include -#include +#include #ifdef IPC_TOOLKIT_WITH_SIMD #include -#include -#include -#include - using namespace ipc; - -namespace { - -using Batch = SimdBatch; -constexpr int L = int(Batch::size); - -/// @brief The batch and scalar paths differ only in how the compiler contracts -/// multiply-adds. As with the distance derivatives, the tolerance is scaled by -/// the magnitude of the whole result rather than per-entry: a tangent-basis -/// Jacobian for a near-degenerate configuration has entries that are -/// catastrophic cancellations of much larger terms. A structurally wrong entry -/// would differ by order of the result's own magnitude. -constexpr double TOL = 1e-9; - -/// @brief Pack the k-th coordinate of L problems into one batch. -Eigen::Vector3 pack(const std::vector& v) -{ - Eigen::Vector3 out; - for (int k = 0; k < 3; ++k) { - std::vector tmp(L); - for (int l = 0; l < L; ++l) { - tmp[l] = v[l][k]; - } - out[k] = Batch::load_unaligned(tmp.data()); - } - return out; -} - -std::vector random_points(const int seed) -{ - std::srand(seed); - std::vector v(L); - for (int l = 0; l < L; ++l) { - v[l] = Eigen::Vector3d::Random(); - } - return v; -} - -/// @brief Compare a batch-valued matrix against the scalar result per lane. -template -void check_lanes( - const std::string& name, const BatchMatrix& batched, ScalarOf&& scalar_of) -{ - for (int l = 0; l < L; ++l) { - const auto want = scalar_of(l); - REQUIRE(batched.rows() == want.rows()); - REQUIRE(batched.cols() == want.cols()); - const double scale = std::max(1.0, want.array().abs().maxCoeff()); - for (Eigen::Index i = 0; i < want.size(); ++i) { - const double got = batched(i).get(l); - CAPTURE(name, l, i, got, want(i), scale); - CHECK(std::abs(got - want(i)) <= TOL * scale); - } - } -} - -} // namespace +using namespace ipc::tests; TEST_CASE( "SIMD batch tangent bases match the scalar ones lane-wise", "[tangent_basis][simd]") { - const std::vector A = random_points(1); - const std::vector B = random_points(2); - const std::vector C = random_points(3); - const std::vector D = random_points(4); + 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); @@ -127,8 +65,9 @@ TEST_CASE( { // Lanes deliberately straddle the cross_x / cross_y branch: a pair along // x prefers one reference axis, a pair along y the other. - std::vector A(L, Eigen::Vector3d::Zero()), B(L); + 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); } 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..a5aa7fa87 --- /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 From 36c061e01451514b8f98b07661794b2ecefaa6fe Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 4 Sep 2026 10:45:57 -0400 Subject: [PATCH 08/34] Align the SIMD benchmark's packed collision buffers The packed buffers were plain `std::vector`, guaranteeing only `alignof(R)`, so the batch loads and stores had to be unaligned. They now use `xsimd::default_allocator`, which falls back to `std::allocator` on an architecture with no alignment requirement. Every batch access is a whole number of lanes into one of these buffers, and one lane's width times the lane count is the architecture's alignment, so an aligned base makes all of them aligned. This matters here rather than in the tests: an unaligned load can cost throughput on some instruction sets, which is the very thing the benchmark measures. The traits assert `xsimd::is_aligned` on every load and store instead of leaving that reasoning as a comment. An aligned load off a misaligned address happens to work on NEON and faults on AVX, so it cannot be validated by running on one machine; the assert checks it on every access in a debug build, including after any future change to the packing layout. Also drops the note that a batch cannot evaluate the edge-edge mollifier, which stopped being true when the mollifier gained batch instantiations. The benchmark still does not apply it, so that the four variants stay on the same arithmetic the original numbers were taken with. Co-Authored-By: Claude Opus 5 --- .../benchmark_simd_barrier_potential.cpp | 59 +++++++++++++++---- 1 file changed, 47 insertions(+), 12 deletions(-) diff --git a/tests/src/tests/potential/benchmark_simd_barrier_potential.cpp b/tests/src/tests/potential/benchmark_simd_barrier_potential.cpp index 337ec499f..391e87ff1 100644 --- a/tests/src/tests/potential/benchmark_simd_barrier_potential.cpp +++ b/tests/src/tests/potential/benchmark_simd_barrier_potential.cpp @@ -14,12 +14,13 @@ // 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: its -// implementation branches on a scalar `if` and is only instantiated for -// `float`/`double`, so a batch cannot evaluate it. 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 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 @@ -55,6 +56,7 @@ #include #include +#include #include #include #include @@ -84,8 +86,21 @@ template struct ScalarTraits> { using Batch = xsimd::batch; using Real = R; static constexpr int LANES = int(Batch::size); - static Batch load(const Real* p) { return Batch::load_unaligned(p); } - static void store(Real* p, const Batch& v) { v.store_unaligned(p); } + // 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)); @@ -237,16 +252,36 @@ struct GroupSpec { /// `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; - std::vector x; ///< nblocks * 3*NV * L - std::vector w; ///< nblocks * L; zero on padding lanes. - std::vector grad_out; - std::vector hess_out; + 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(); } From b356e55bf5e1c5addc797677934d8911d55ea33c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 4 Sep 2026 10:45:58 -0400 Subject: [PATCH 09/34] Ignore __cmake_systeminformation --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 From 1191effd7cc44b027baebd2aa8002d0c1eb23b21 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 12:29:52 -0400 Subject: [PATCH 10/34] Fix single bracker errors on Linux --- tests/src/tests/barrier/test_simd_barrier.cpp | 2 +- tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp | 7 ++++--- tests/src/tests/utils/test_simd_utils.cpp | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/tests/src/tests/barrier/test_simd_barrier.cpp b/tests/src/tests/barrier/test_simd_barrier.cpp index e4b5fc1b3..e6c2d13da 100644 --- a/tests/src/tests/barrier/test_simd_barrier.cpp +++ b/tests/src/tests/barrier/test_simd_barrier.cpp @@ -17,7 +17,7 @@ namespace { /// 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 }; + 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. diff --git a/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp b/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp index b74faf782..a34079725 100644 --- a/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp +++ b/tests/src/tests/distance/test_simd_edge_edge_mollifier.cpp @@ -24,7 +24,7 @@ constexpr double EPS_X = 1e-3; /// 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 }; +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. @@ -65,8 +65,9 @@ TEST_CASE( "[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 }; + 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) { diff --git a/tests/src/tests/utils/test_simd_utils.cpp b/tests/src/tests/utils/test_simd_utils.cpp index a5aa7fa87..770e50aea 100644 --- a/tests/src/tests/utils/test_simd_utils.cpp +++ b/tests/src/tests/utils/test_simd_utils.cpp @@ -73,7 +73,7 @@ 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 }; + const std::array cases = { { 10.0, 20.0 } }; for (int offset = 0; offset < 2; ++offset) { CAPTURE(offset); From 67cc4367c34b6ea7ffb8e76fb84cc236494ba5ee Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 4 Sep 2026 22:12:10 -0400 Subject: [PATCH 11/34] Add SIMD support to the closest point solves - Replace `A.ldlt().solve()` with a branchless closed form, since pivoting branches on matrix values and cannot vectorize - Add `scalar_of_t` and `all_of` to `ipc/utils/simd.hpp`, fixing the residual tolerance for batches and keeping its assert alive per-lane Co-Authored-By: Claude Opus 5 --- docs/source/about/release_notes.rst | 1 + src/ipc/tangent/closest_point.hpp | 49 +++++- src/ipc/utils/simd.hpp | 37 ++++- tests/src/tests/tangent/CMakeLists.txt | 1 + .../tests/tangent/test_simd_closest_point.cpp | 148 ++++++++++++++++++ 5 files changed, 224 insertions(+), 12 deletions(-) create mode 100644 tests/src/tests/tangent/test_simd_closest_point.cpp diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 273a63087..feb23a830 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -74,6 +74,7 @@ API Changes |:wrench:| - The normalized normals (``point_line_normal``, ``triangle_normal``, ``line_line_normal``) use ``ipc::normalized`` instead of Eigen's ``normalized()``, which makes them and the values/gradients of the ``point_line``, ``line_line``, and ``point_plane`` signed distances usable with batch scalars (previously only the signed-distance Hessians were). - ``edge_length_gradient`` asserts its non-degeneracy only for a plain scalar, so it accepts batch scalars as well. - The relative-velocity functions (values, Jacobians, and ``dx_dbeta`` tensors for point-point, point-edge, edge-edge, and point-triangle, in 2D and 3D) were already batch-compatible and are now covered by tests, agreeing with the scalar path to 1e-14 relative. + - ``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. They complete the batch coverage of the closest-point functions, whose Jacobians and Hessians were already instantiated. Accuracy is unchanged in practice: measured against exactly known solutions, the closed form and Eigen's LDLT trade places within about 2× at every conditioning level, and on realistic edge-edge Gram matrices they agree to three significant figures at every angle down to 1e-4 rad. - Functions that pick a case from the values themselves — a barrier clamping at ``d̂``, the 3D point-point tangent basis choosing a reference axis — evaluate every case for a batch and blend the results per-lane, so one batch may carry lanes in different cases. The scalar instantiations keep their original early-return form and are unchanged. - ``ipc::normalized(v)`` replaces ``v.normalized()`` in the tangent bases: Eigen's own ``normalized()`` guards a zero-length vector with an ``if``, which a batch cannot answer with one ``bool``. It applies the same rule per-lane and defers to Eigen for scalars. - 💥 **[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. diff --git a/src/ipc/tangent/closest_point.hpp b/src/ipc/tangent/closest_point.hpp index 2fa7570d6..91c1d7ae7 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,47 @@ 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 write this out manually instead of calling `A.ldlt().solve(b)` + /// because Eigen's LDLT uses pivoting. Pivoting requires branching based on + /// matrix values, which breaks vectorization since different batch lanes + /// can't easily take different control flow paths. + /// + /// The entire implementation is just Cramer's rule. We chose this over a + /// hand-written pivoted LDLT because testing showed that accuracy is + /// limited by the conditioning of `A`, not the algorithm. Across random SPD + /// matrices, Cramer's rule, our manual LDLT, and Eigen's LDLT all perform + /// within 2x of each other. On realistic edge-edge Gram matrices, they + /// agree to three significant figures down to tiny angles (1e-4 rad). + /// Ultimately, Cramer's rule wins because it's completely branchless and + /// avoids per-lane blending. + /// + /// @warning `A` must be nonsingular. If `det == 0`, it returns a non-finite + /// result rather than throwing an error. This is safe here because + /// callers only use this for interior-interior distances, which + /// naturally excludes the parallel edges and degenerate triangles + /// that cause singularities. The residual asserts at the call + /// sites act as our safety net. + template + inline Eigen::Vector2 solve_spd_2x2( + Eigen::ConstRef> A, + Eigen::ConstRef> b) + { + const T det = A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0); + return Eigen::Vector2( + (A(1, 1) * b[0] - A(0, 1) * b[1]) / det, + (A(0, 0) * b[1] - A(1, 0) * b[0]) / det); + } + // ======================================================================== // Point - Edge @@ -251,8 +286,8 @@ 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); + const Eigen::Vector2 x = solve_spd_2x2(A, rhs); + assert(all_of((A * x - rhs).norm() < T(CLOSEST_POINT_RESIDUAL_TOL))); return x; } @@ -324,8 +359,8 @@ 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); + const Eigen::Vector2 x = solve_spd_2x2(A, b); + assert(all_of((A * x - b).norm() < T(CLOSEST_POINT_RESIDUAL_TOL))); return x; } diff --git a/src/ipc/utils/simd.hpp b/src/ipc/utils/simd.hpp index b11aa56af..ecd1f085b 100644 --- a/src/ipc/utils/simd.hpp +++ b/src/ipc/utils/simd.hpp @@ -18,6 +18,30 @@ namespace ipc { /// 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 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 @@ -30,11 +54,7 @@ template inline T select(const bool mask, const T& a, const T& b) /// @brief `+infinity` for any scalar the library templates on. template inline T infinity() { - if constexpr (is_simd_batch_v) { - return T(std::numeric_limits::infinity()); - } else { - return std::numeric_limits::infinity(); - } + return T(std::numeric_limits>::infinity()); } /// @brief A first-match-wins cascade of cases: @@ -159,6 +179,13 @@ 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 diff --git a/tests/src/tests/tangent/CMakeLists.txt b/tests/src/tests/tangent/CMakeLists.txt index 35297e6b8..6a265b934 100644 --- a/tests/src/tests/tangent/CMakeLists.txt +++ b/tests/src/tests/tangent/CMakeLists.txt @@ -2,6 +2,7 @@ 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 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..1e6652459 --- /dev/null +++ b/tests/src/tests/tangent/test_simd_closest_point.cpp @@ -0,0 +1,148 @@ +#include +#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) }; +} + +} // 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(); + }); + 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") + { + 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(); + }); + 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( + "Closest points of a symmetric crossing are the edge midpoints", + "[closest_point]") +{ + // Two 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. This anchors the result to the geometry rather than to whatever + // a particular decomposition happens to return. + const EdgePair e = crossing_edges(std::acos(0.0)); // perpendicular + + const Eigen::Vector2d coords = + edge_edge_closest_point(e.ea0, e.ea1, e.eb0, e.eb1); + + CAPTURE(coords); + CHECK(coords[0] == Catch::Approx(0.5).epsilon(0).margin(1e-15)); + CHECK(coords[1] == Catch::Approx(0.5).epsilon(0).margin(1e-15)); +} + +TEST_CASE( + "A point on a triangle recovers its own barycentric coordinates", + "[closest_point]") +{ + // Projecting a point that already lies in the plane must return the + // coordinates it was built from, whatever the solve does internally. + const Eigen::Vector3d t0(-1, 0, 1), t1(1, 0, 1), t2(0, 0, -1); + const Eigen::Vector2d expected(0.25, 0.5); + const Eigen::Vector3d p = + t0 + expected[0] * (t1 - t0) + expected[1] * (t2 - t0); + + const Eigen::Vector2d coords = point_triangle_closest_point(p, t0, t1, t2); + + CAPTURE(coords); + CHECK(coords[0] == Catch::Approx(expected[0]).epsilon(0).margin(1e-15)); + CHECK(coords[1] == Catch::Approx(expected[1]).epsilon(0).margin(1e-15)); +} + +#endif From c6f1c864631ee2467b912285888921c3062ee59e Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 4 Sep 2026 23:38:38 -0400 Subject: [PATCH 12/34] Template friction, adhesion, and angle on scalar These were the last hardcoded-`double` potentials, which is what ruled out a batch-capable friction or adhesion path. - Move the friction mollifier, smooth-mu, and adhesion functions into header-only templates, deleting `smooth_friction_mollifier.cpp` and `adhesion.cpp` - Rewrite their piecewise branches as `select_lazy` cascades, so one batch may carry lanes on either side of a threshold. Several inactive branches divide by `y`, which a batch evaluates even where `y == 0`; the per-lane blend discards the infinity rather than propagating a NaN - Split `dihedral_angle` and its derivatives into `ipc::detail` kernels instantiated for `float`, `double`, and both batch types, behind front ends that deduce `T` - Add `ipc::abs` and `ipc::atan2`. `Math::abs` picks the sign with a ternary, which asks a batch for one `bool` its lanes may disagree on - Keep the anisotropic friction helpers `double`-only: their branches test the material rather than a per-problem speed, so there is nothing per-lane Co-Authored-By: Claude Opus 5 --- docs/source/about/release_notes.rst | 11 + python/src/adhesion/adhesion.cpp | 28 +-- .../friction/smooth_friction_mollifier.cpp | 10 +- python/src/friction/smooth_mu.cpp | 14 +- python/src/geometry/angle.cpp | 4 +- src/ipc/adhesion/CMakeLists.txt | 3 +- src/ipc/adhesion/adhesion.cpp | 204 ---------------- src/ipc/adhesion/adhesion.hpp | 223 ++++++++++++++++-- src/ipc/friction/CMakeLists.txt | 1 - .../friction/smooth_friction_mollifier.cpp | 55 ----- .../friction/smooth_friction_mollifier.hpp | 70 +++++- src/ipc/friction/smooth_mu.cpp | 115 +-------- src/ipc/friction/smooth_mu.hpp | 132 +++++++++-- src/ipc/geometry/angle.cpp | 182 +++++++------- src/ipc/geometry/angle.hpp | 85 +++++-- src/ipc/math/scalar_math.hpp | 23 ++ tests/src/tests/adhesion/CMakeLists.txt | 1 + .../src/tests/adhesion/test_simd_adhesion.cpp | 127 ++++++++++ tests/src/tests/friction/CMakeLists.txt | 1 + .../src/tests/friction/test_simd_friction.cpp | 110 +++++++++ .../src/tests/geometry/test_simd_geometry.cpp | 30 +++ 21 files changed, 886 insertions(+), 543 deletions(-) delete mode 100644 src/ipc/adhesion/adhesion.cpp delete mode 100644 src/ipc/friction/smooth_friction_mollifier.cpp create mode 100644 tests/src/tests/adhesion/test_simd_adhesion.cpp create mode 100644 tests/src/tests/friction/test_simd_friction.cpp diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index feb23a830..2dc09b173 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -65,12 +65,23 @@ API Changes |:wrench:| - 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. +- Templatize the friction, adhesion, and dihedral-angle functions on the scalar type. + + - The smooth friction mollifier, the smooth-μ family, and the adhesion functions (normal adhesion and its derivatives, the tangential-adhesion mollifiers, and the ``smooth_mu_a*`` variants) now take a scalar template parameter ``T``. These were hardcoded ``double``, which is what previously ruled out a batch-capable friction or adhesion path. + - ``smooth_friction_mollifier.cpp`` and ``adhesion.cpp`` are gone; both headers are header-only templates now, following ``edge_edge_mollifier.hpp``. + - ``dihedral_angle`` and its gradient/Hessian follow the distance family's two-layer split: ``ipc::detail`` kernels templated on ```` (3D-only) and instantiated for ``float``, ``double``, and both batch types, behind front ends that deduce ``T``. Existing calls are unaffected. + - The anisotropic-friction helpers (``anisotropic_mu_eff_f``, ``anisotropic_x_from_tau_aniso``, ``anisotropic_mu_eff_from_tau_aniso``) stay ``double``-only. Their branches test the material — whether an ellipse axis is set, whether the caller disabled μ — rather than a per-problem speed, so there is nothing for a batch to evaluate per-lane. + - Add ``ipc::abs`` and ``ipc::atan2`` to ``ipc/math/scalar_math.hpp``. ``Math::abs`` picks the sign with a ternary, which asks a batch for one ``bool`` its lanes may disagree on; ``ipc::abs`` reaches ``xsimd::abs``, which clears the sign bit per-lane instead. + - 💥 **[Breaking]** As with the distance functions, mixed-precision calls no longer deduce: every argument must share one scalar type, so ``smooth_mu(float_y, 0.5, 0.3, 0.001)`` must become ``smooth_mu(float_y, 0.5f, 0.3f, 0.001f)``. + - Add SIMD batch support to the distance functions via the new ``ipc/utils/simd.hpp`` (requires ``IPC_TOOLKIT_WITH_SIMD``). - ``Eigen::NumTraits`` is specialized for ``xsimd::batch``, and ``ipc::SimdBatch`` aliases the batch type for the build's architecture. Passing ``Eigen::Vector3>`` evaluates one independent problem per SIMD lane, letting a caller with a structure-of-arrays layout compute several distances per call. - The values, gradients, and Hessians of the point-point, point-line, point-edge, line-line, edge-edge, and point-triangle distances are instantiated for ``SimdBatch`` and ``SimdBatch``. Measured agreement with the scalar path is one ulp on the values; the derivatives agree to ~1e-11 relative to their own magnitude, since the two paths contract multiply-adds differently and these derivatives are ill-conditioned for near-parallel edges. - The barrier functions and classes (``barrier``, ``ClampedLogBarrier``, ``ClampedLogSqBarrier``, ``CubicBarrier``, ``TwoStageBarrier``), the tangent bases and their Jacobians, the closest-point Jacobians/Hessians, the unnormalized-normal Jacobians/Hessians, the signed-distance Hessians, and the triangle-area gradient are instantiated for batch scalars as well. - The edge-edge mollifier is instantiated for batch scalars too: the mollifier and its gradient/Hessian, their derivatives with respect to the threshold, the threshold and its gradient, and the edge-edge cross-product squared norm with its gradient/Hessian. + - The friction mollifier, smooth-μ, adhesion, and dihedral-angle functions accept batch scalars. Their piecewise branches are ``select_lazy`` cascades, so one batch may carry lanes on either side of ``ε_v``/``ε_a``/``d̂ₚ``. Several of the inactive branches divide by ``y``, which a batch evaluates even on a lane where ``y == 0``; the blend is a per-lane select, so it discards the resulting infinity rather than propagating it into a NaN. + - The ``mu_s == mu_k`` test opening most smooth-μ functions is a fast path, not a special case: when the coefficients are equal the general formulas reduce to the same value, so a batch blending across that mask stays correct. It still buys a scalar caller the cheaper formula in the common single-coefficient setting. - The normalized normals (``point_line_normal``, ``triangle_normal``, ``line_line_normal``) use ``ipc::normalized`` instead of Eigen's ``normalized()``, which makes them and the values/gradients of the ``point_line``, ``line_line``, and ``point_plane`` signed distances usable with batch scalars (previously only the signed-distance Hessians were). - ``edge_length_gradient`` asserts its non-degeneracy only for a plain scalar, so it accepts batch scalars as well. - The relative-velocity functions (values, Jacobians, and ``dx_dbeta`` tensors for point-point, point-edge, edge-edge, and point-triangle, in 2D and 3D) were already batch-compatible and are now covered by tests, agreeing with the scalar path to 1e-14 relative. 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..1e42909e5 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,199 @@ 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 = T(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) - T(0.4) * z) * delta_mu - mu_s / T(3)) + + mu_s); + }, + [&] { + return y * z + * (z + * (z * (T(0.4) * z - T(2)) * delta_mu + T(3) * mu_k + - T(10.0 / 3.0) * mu_s) + - mu_k + T(2) * mu_s) + + T(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/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..82ef7a295 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::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::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::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::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::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..7c154dac1 100644 --- a/src/ipc/friction/smooth_mu.cpp +++ b/src/ipc/friction/smooth_mu.cpp @@ -1,116 +1,11 @@ #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, unlike the scalar μ functions in +// the header. Their branches test the *material* -- whether an ellipse axis is +// set at all, whether the caller disabled μ -- rather than the per-problem +// speed, so there is nothing here a batch would evaluate per-lane. std::pair anisotropic_mu_eff_f( Eigen::ConstRef tau_dir, @@ -169,4 +64,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..41a4c2adf 100644 --- a/src/ipc/friction/smooth_mu.hpp +++ b/src/ipc/friction/smooth_mu.hpp @@ -1,75 +1,177 @@ #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::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::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::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) - T(0.4) * z) * delta_mu - mu_s / T(3)) + + mu_s) + + T(9.0 / 16.0) * eps_v * mu_k - T(11.0 / 48.0) * eps_v * mu_s; + }, + [&] { + return y * z + * (z + * (z * (T(0.4) * z - T(2)) * delta_mu + + (T(3) * mu_k - T(10.0 / 3.0) * mu_s)) + + (T(2) * mu_s - mu_k)) + + T(0.6) * eps_v * mu_k - T(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::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 +241,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..8630f1590 100644 --- a/src/ipc/geometry/angle.cpp +++ b/src/ipc/geometry/angle.cpp @@ -1,80 +1,81 @@ #include "angle.hpp" +#include #include +#include +#include -namespace ipc { +namespace ipc::detail { -double dihedral_angle( - Eigen::ConstRef x0, - Eigen::ConstRef x1, - Eigen::ConstRef x2, - Eigen::ConstRef x3) +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::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 dn0_dx; + dn0_dx.template leftCols<9>() = triangle_normal_jacobian(x0, x1, x2); + dn0_dx.template rightCols<3>().setZero(); - Eigen::Matrix dn1_dx; + 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(); + dn1_dx.template middleCols<3>(6).setZero(); // --- 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 @@ -83,14 +84,14 @@ Matrix12d dihedral_angle_hessian( // 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 dn0_dx; + dn0_dx.template leftCols<9>() = triangle_normal_jacobian(x0, x1, x2); + dn0_dx.template rightCols<3>().setZero(); - Eigen::Matrix dn1_dx; + 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(); + dn1_dx.template middleCols<3>(6).setZero(); // ------------------------------------------------------------------------- // Hessians of n0 and n1 — shape (27 × 9) each, then embedded in 12-DOF @@ -104,9 +105,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 +147,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 +159,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 +197,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 +234,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 +245,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 +272,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 +282,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 +303,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 +339,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/math/scalar_math.hpp b/src/ipc/math/scalar_math.hpp index 52e612a99..83c81250a 100644 --- a/src/ipc/math/scalar_math.hpp +++ b/src/ipc/math/scalar_math.hpp @@ -48,6 +48,29 @@ template inline T fma(const T& x, const T& y, const T& z) return fma(x, y, z); } +/// @brief `abs` for any scalar the library templates on. +/// +/// We need this instead of `Math::abs`, which picks the sign with a +/// ternary. A ternary asks the scalar to answer `x >= 0` with one `bool`, +/// which a batch cannot do: its lanes may disagree. `xsimd::abs` clears the +/// sign bit per-lane instead, and the block-scope using-declaration above is +/// what lets ADL reach it. +template inline T abs(const T& x) +{ + using std::abs; + return abs(x); +} + +/// @brief `atan2` for any scalar the library templates on. +/// +/// Same block-scope using-declaration trick as `sqrt`/`log` above. Note the +/// argument order matches `std::atan2(y, x)` — the sine-like argument first. +template inline T atan2(const T& y, const T& x) +{ + using std::atan2; + return atan2(y, x); +} + constexpr double MOLLIFIER_THRESHOLD_EPS = 1e-2; } // namespace ipc 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..1ed319223 --- /dev/null +++ b/tests/src/tests/adhesion/test_simd_adhesion.cpp @@ -0,0 +1,127 @@ +#include +#include + +#include + +#ifdef IPC_TOOLKIT_WITH_SIMD + +#include + +#include +#include + +using namespace ipc; +using namespace ipc::tests; + +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( + name, DS, + [&](const double d) { return f(d, DHAT_P, DHAT_A, max_slope); }, + [&](const Batch& d) { + return f(d, Batch(DHAT_P), Batch(DHAT_A), Batch(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); + + // Speeds as multiples of eps_a. These functions clamp at y <= 0 rather than + // mirroring on |y|, so the negative entry checks that clamp; zero is where + // the `1/y` branch is singular, which only a batch evaluates. + constexpr std::array MULTIPLES = { -1.0, 0.0, 0.25, 0.49, + 0.5, 0.75, 1.0, 2.5 }; + std::array ys {}; + for (size_t i = 0; i < ys.size(); ++i) { + ys[i] = MULTIPLES[i] * eps_a; + } + + auto check = [&](const std::string& name, auto&& f) { + check_swept_lanes( + name, ys, [&](const double y) { return f(y, eps_a); }, + [&](const Batch& y) { return f(y, Batch(eps_a)); }); + }; + + check("f0", [](auto y, auto e) { return tangential_adhesion_f0(y, e); }); + check("f1", [](auto y, auto e) { return tangential_adhesion_f1(y, e); }); + check("f2", [](auto y, auto e) { return tangential_adhesion_f2(y, e); }); + check("f1_over_x", [](auto y, auto e) { + return tangential_adhesion_f1_over_x(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. + constexpr std::array MULTIPLES = { 0.0, 0.25, 0.49, 0.5, + 0.75, 1.0, 2.5 }; + std::array ys {}; + for (size_t i = 0; i < ys.size(); ++i) { + ys[i] = MULTIPLES[i] * eps_a; + } + + auto check = [&](const std::string& name, auto&& f) { + check_swept_lanes( + name, ys, [&](const double y) { return f(y, mu_s, mu_k, eps_a); }, + [&](const Batch& y) { + return f(y, Batch(mu_s), Batch(mu_k), Batch(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/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..865b8642a --- /dev/null +++ b/tests/src/tests/friction/test_simd_friction.cpp @@ -0,0 +1,110 @@ +#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 }; + +std::array speeds(const double eps_v) +{ + std::array ys {}; + for (size_t i = 0; i < ys.size(); ++i) { + ys[i] = Y_MULTIPLES[i] * eps_v; + } + return ys; +} + +} // 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 = speeds(eps_v); + + auto check = [&](const std::string& name, auto&& f) { + check_swept_lanes( + name, ys, [&](const double y) { return f(y, eps_v); }, + [&](const Batch& y) { return f(y, Batch(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 = speeds(eps_v); + + auto check = [&](const std::string& name, auto&& f) { + check_swept_lanes( + name, ys, [&](const double y) { return f(y, mu_s, mu_k, eps_v); }, + [&](const Batch& y) { + return f(y, Batch(mu_s), Batch(mu_k), Batch(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/test_simd_geometry.cpp b/tests/src/tests/geometry/test_simd_geometry.cpp index f490b89bc..e788c8185 100644 --- a/tests/src/tests/geometry/test_simd_geometry.cpp +++ b/tests/src/tests/geometry/test_simd_geometry.cpp @@ -4,6 +4,7 @@ #ifdef IPC_TOOLKIT_WITH_SIMD +#include #include #include @@ -250,4 +251,33 @@ TEST_CASE( [&](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 From 889509caec5e38a36e8ab746ec74614a02d66fb2 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 16:16:11 -0400 Subject: [PATCH 13/34] Fix ADL, singular solves, and literal narrowing - Return `decltype(auto)` from `ipc::abs` so it stops hijacking Eigen's array `abs` under `using namespace ipc` - Zero the 2x2 closest-point solve on a singular Gram matrix instead of dividing by zero, and scale its residual assert to the system's magnitude instead of an absolute bound - Add `ipc::literal` and use it wherever a fractional constant would narrow for a `SimdBatch` - Collapse the generic `std::` math forwarders into one variadic macro over a single name list - Delegate `Math::abs` to `ipc::abs` instead of a batch-incompatible ternary - Fix the point-edge distance test's inverted transition-point guard and add the degenerate-edge guard its finite-difference comparison needs - Factor the dihedral-angle Jacobian assembly out of the gradient and Hessian, consolidate the SIMD test helpers, and correct several stale or misleading comments - Decouple config.hpp.in from by mirroring the StorageOptions values it needs Co-Authored-By: Claude Fable 5.1 --- docs/source/about/release_notes.rst | 2 +- src/ipc/adhesion/adhesion.hpp | 12 ++- src/ipc/config.hpp.in | 11 ++- src/ipc/distance/edge_edge_mollifier.hpp | 6 +- src/ipc/friction/smooth_mu.cpp | 12 ++- src/ipc/friction/smooth_mu.hpp | 14 ++- src/ipc/geometry/angle.cpp | 52 ++++++---- src/ipc/math/math.hpp | 2 +- src/ipc/math/math.tpp | 8 +- src/ipc/math/scalar_math.hpp | 96 +++++++------------ src/ipc/smooth_contact/common.hpp | 8 +- src/ipc/tangent/closest_point.hpp | 60 ++++++------ src/ipc/utils/simd.hpp | 11 +++ .../src/tests/adhesion/test_simd_adhesion.cpp | 74 +++++++------- tests/src/tests/barrier/test_barrier.cpp | 17 ++-- tests/src/tests/broad_phase/test_lbvh.cpp | 2 - tests/src/tests/distance/test_point_edge.cpp | 26 +++-- .../src/tests/friction/test_simd_friction.cpp | 23 +---- tests/src/tests/simd_utils.hpp | 28 ++++++ .../src/tests/tangent/test_closest_point.cpp | 6 +- .../tests/tangent/test_simd_closest_point.cpp | 46 ++------- 21 files changed, 266 insertions(+), 250 deletions(-) diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 2dc09b173..2870b8c97 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -70,7 +70,7 @@ API Changes |:wrench:| - The smooth friction mollifier, the smooth-μ family, and the adhesion functions (normal adhesion and its derivatives, the tangential-adhesion mollifiers, and the ``smooth_mu_a*`` variants) now take a scalar template parameter ``T``. These were hardcoded ``double``, which is what previously ruled out a batch-capable friction or adhesion path. - ``smooth_friction_mollifier.cpp`` and ``adhesion.cpp`` are gone; both headers are header-only templates now, following ``edge_edge_mollifier.hpp``. - ``dihedral_angle`` and its gradient/Hessian follow the distance family's two-layer split: ``ipc::detail`` kernels templated on ```` (3D-only) and instantiated for ``float``, ``double``, and both batch types, behind front ends that deduce ``T``. Existing calls are unaffected. - - The anisotropic-friction helpers (``anisotropic_mu_eff_f``, ``anisotropic_x_from_tau_aniso``, ``anisotropic_mu_eff_from_tau_aniso``) stay ``double``-only. Their branches test the material — whether an ellipse axis is set, whether the caller disabled μ — rather than a per-problem speed, so there is nothing for a batch to evaluate per-lane. + - The anisotropic-friction helpers (``anisotropic_mu_eff_f``, ``anisotropic_x_from_tau_aniso``, ``anisotropic_mu_eff_from_tau_aniso``) stay ``double``-only for now, since no batch caller exists for them yet. The first two do depend on the per-collision tangential velocity, so a batch friction path with anisotropic μ would need to template them as well; only ``anisotropic_mu_eff_from_tau_aniso`` tests the material alone. - Add ``ipc::abs`` and ``ipc::atan2`` to ``ipc/math/scalar_math.hpp``. ``Math::abs`` picks the sign with a ternary, which asks a batch for one ``bool`` its lanes may disagree on; ``ipc::abs`` reaches ``xsimd::abs``, which clears the sign bit per-lane instead. - 💥 **[Breaking]** As with the distance functions, mixed-precision calls no longer deduce: every argument must share one scalar type, so ``smooth_mu(float_y, 0.5, 0.3, 0.001)`` must become ``smooth_mu(float_y, 0.5f, 0.3f, 0.001f)``. diff --git a/src/ipc/adhesion/adhesion.hpp b/src/ipc/adhesion/adhesion.hpp index 1e42909e5..50823f966 100644 --- a/src/ipc/adhesion/adhesion.hpp +++ b/src/ipc/adhesion/adhesion.hpp @@ -209,22 +209,24 @@ inline T smooth_mu_a0(const T y, const T mu_s, const T mu_k, const T eps_a) y <= T(0), [&] { return T(0); }, // mu_s == mu_k || y >= eps_a, [&] { - const T c = T(11 / 48.) * eps_a * delta_mu; + 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) - T(0.4) * z) * delta_mu - mu_s / T(3)) + * (z + * (z * (T(1) - literal(0.4) * z) * delta_mu + - mu_s / T(3)) + mu_s); }, [&] { return y * z * (z - * (z * (T(0.4) * z - T(2)) * delta_mu + T(3) * mu_k - - T(10.0 / 3.0) * mu_s) + * (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) - + T(3.0 / 80.0) * eps_a * delta_mu; + + literal(3.0 / 80.0) * eps_a * delta_mu; }); } 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_mollifier.hpp b/src/ipc/distance/edge_edge_mollifier.hpp index 7ce26413f..d3a20cddc 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -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 @@ -235,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(); } @@ -251,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/friction/smooth_mu.cpp b/src/ipc/friction/smooth_mu.cpp index 7c154dac1..d1ed795df 100644 --- a/src/ipc/friction/smooth_mu.cpp +++ b/src/ipc/friction/smooth_mu.cpp @@ -2,10 +2,14 @@ namespace ipc { -// The anisotropic helpers stay `double`-only, unlike the scalar μ functions in -// the header. Their branches test the *material* -- whether an ellipse axis is -// set at all, whether the caller disabled μ -- rather than the per-problem -// speed, so there is nothing here a batch would evaluate per-lane. +// 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, diff --git a/src/ipc/friction/smooth_mu.hpp b/src/ipc/friction/smooth_mu.hpp index 41a4c2adf..61bd4b0dd 100644 --- a/src/ipc/friction/smooth_mu.hpp +++ b/src/ipc/friction/smooth_mu.hpp @@ -81,17 +81,21 @@ inline T smooth_mu_f0(const T y, const T mu_s, const T mu_k, const T eps_v) abs_y < T(0.5) * eps_v, [&] { return y * z - * (z * (z * (T(1) - T(0.4) * z) * delta_mu - mu_s / T(3)) + * (z + * (z * (T(1) - literal(0.4) * z) * delta_mu + - mu_s / T(3)) + mu_s) - + T(9.0 / 16.0) * eps_v * mu_k - T(11.0 / 48.0) * eps_v * 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 * (T(0.4) * z - T(2)) * delta_mu - + (T(3) * mu_k - T(10.0 / 3.0) * mu_s)) + * (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)) - + T(0.6) * eps_v * mu_k - T(4.0 / 15.0) * eps_v * mu_s; + + literal(0.6) * eps_v * mu_k + - literal(4.0 / 15.0) * eps_v * mu_s; }); } diff --git a/src/ipc/geometry/angle.cpp b/src/ipc/geometry/angle.cpp index 8630f1590..b75e8c940 100644 --- a/src/ipc/geometry/angle.cpp +++ b/src/ipc/geometry/angle.cpp @@ -5,8 +5,39 @@ #include #include +#include +#include + 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, @@ -37,14 +68,7 @@ Eigen::Vector dihedral_angle_gradient( // --- Normal gradients --- - 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(); + const auto [dn0_dx, dn1_dx] = dihedral_normal_jacobians(x0, x1, x2, x3); // --- Angle gradient --- @@ -79,19 +103,9 @@ Eigen::Matrix dihedral_angle_hessian( // ------------------------------------------------------------------------- // 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.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(); + 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 diff --git a/src/ipc/math/math.hpp b/src/ipc/math/math.hpp index 35b3c87b5..53f634413 100644 --- a/src/ipc/math/math.hpp +++ b/src/ipc/math/math.hpp @@ -17,7 +17,7 @@ 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 abs(const T& x) { return ipc::abs(x); } static T sqr(const T& x) { return ipc::sqr(x); } static T cubic(const T& x) { return ipc::cubic(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 index 83c81250a..5ab6f81ad 100644 --- a/src/ipc/math/scalar_math.hpp +++ b/src/ipc/math/scalar_math.hpp @@ -1,9 +1,43 @@ #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 Define `ipc::FUNC` forwarding its arguments 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 + +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; } @@ -11,66 +45,4 @@ 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; } -/// @brief `sqrt` for any scalar the library templates on. -/// -/// The block-scope using-declaration is what makes this work: unqualified -/// lookup stops at it (so this does not recurse), while ADL still reaches -/// `xsimd::sqrt` for a batch and TinyAD's hidden friend for an autodiff -/// scalar. -template inline T sqrt(const T& x) -{ - using std::sqrt; - return sqrt(x); -} - -/// @brief `log` for any scalar the library templates on. -/// -/// Same block-scope using-declaration trick as `sqrt` above: without it, -/// unqualified `log(float_val)` finds only the global `::log(double)` -/// declared by ``, promoting the argument to `double` and narrowing -/// the result back, which trips `-Wfloat-conversion` for no reason. -template inline T log(const T& x) -{ - using std::log; - return log(x); -} - -/// @brief `fma` for any scalar the library templates on. -/// -/// Same block-scope using-declaration trick as `sqrt`/`log` above, but the -/// stakes are different: `std::fma` has no generic/arithmetic-type overload -/// that would silently compile for a batch, so a qualified `std::fma` call -/// on `SimdBatch` is a hard compile error, not a quiet precision bug. -/// Unqualified lookup here lets ADL reach `xsimd::fma`. -template inline T fma(const T& x, const T& y, const T& z) -{ - using std::fma; - return fma(x, y, z); -} - -/// @brief `abs` for any scalar the library templates on. -/// -/// We need this instead of `Math::abs`, which picks the sign with a -/// ternary. A ternary asks the scalar to answer `x >= 0` with one `bool`, -/// which a batch cannot do: its lanes may disagree. `xsimd::abs` clears the -/// sign bit per-lane instead, and the block-scope using-declaration above is -/// what lets ADL reach it. -template inline T abs(const T& x) -{ - using std::abs; - return abs(x); -} - -/// @brief `atan2` for any scalar the library templates on. -/// -/// Same block-scope using-declaration trick as `sqrt`/`log` above. Note the -/// argument order matches `std::atan2(y, x)` — the sine-like argument first. -template inline T atan2(const T& y, const T& x) -{ - using std::atan2; - return atan2(y, x); -} - -constexpr double MOLLIFIER_THRESHOLD_EPS = 1e-2; - } // 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.hpp b/src/ipc/tangent/closest_point.hpp index 91c1d7ae7..1cf11b5b1 100644 --- a/src/ipc/tangent/closest_point.hpp +++ b/src/ipc/tangent/closest_point.hpp @@ -149,35 +149,45 @@ namespace detail { /// @brief Solves `Ax = b` for a 2x2 symmetric positive-definite matrix `A`. /// - /// We write this out manually instead of calling `A.ldlt().solve(b)` - /// because Eigen's LDLT uses pivoting. Pivoting requires branching based on - /// matrix values, which breaks vectorization since different batch lanes - /// can't easily take different control flow paths. + /// 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. /// - /// The entire implementation is just Cramer's rule. We chose this over a - /// hand-written pivoted LDLT because testing showed that accuracy is - /// limited by the conditioning of `A`, not the algorithm. Across random SPD - /// matrices, Cramer's rule, our manual LDLT, and Eigen's LDLT all perform - /// within 2x of each other. On realistic edge-edge Gram matrices, they - /// agree to three significant figures down to tiny angles (1e-4 rad). - /// Ultimately, Cramer's rule wins because it's completely branchless and - /// avoids per-lane blending. + /// `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. /// - /// @warning `A` must be nonsingular. If `det == 0`, it returns a non-finite - /// result rather than throwing an error. This is safe here because - /// callers only use this for interior-interior distances, which - /// naturally excludes the parallel edges and degenerate triangles - /// that cause singularities. The residual asserts at the call - /// sites act as our safety net. + /// 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) { const T det = A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0); - return Eigen::Vector2( - (A(1, 1) * b[0] - A(0, 1) * b[1]) / det, - (A(0, 0) * b[1] - A(1, 0) * b[0]) / det); + 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; } // ======================================================================== @@ -286,9 +296,7 @@ namespace detail { rhs[0] = -eb_to_ea.dot(ea); rhs[1] = eb_to_ea.dot(eb); - const Eigen::Vector2 x = solve_spd_2x2(A, rhs); - assert(all_of((A * x - rhs).norm() < T(CLOSEST_POINT_RESIDUAL_TOL))); - return x; + return solve_spd_2x2(A, rhs); } /// @brief Compute the Jacobian of the closest points between two edges. @@ -359,9 +367,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 = solve_spd_2x2(A, b); - assert(all_of((A * x - b).norm() < T(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/utils/simd.hpp b/src/ipc/utils/simd.hpp index ecd1f085b..51e168d1d 100644 --- a/src/ipc/utils/simd.hpp +++ b/src/ipc/utils/simd.hpp @@ -31,6 +31,17 @@ template struct ScalarOf { }; 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 diff --git a/tests/src/tests/adhesion/test_simd_adhesion.cpp b/tests/src/tests/adhesion/test_simd_adhesion.cpp index 1ed319223..0a5fc7297 100644 --- a/tests/src/tests/adhesion/test_simd_adhesion.cpp +++ b/tests/src/tests/adhesion/test_simd_adhesion.cpp @@ -13,6 +13,23 @@ 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]") @@ -28,12 +45,7 @@ TEST_CASE( DHAT_A, 3e-3, 1.0 }; auto check = [&](const std::string& name, auto&& f) { - check_swept_lanes( - name, DS, - [&](const double d) { return f(d, DHAT_P, DHAT_A, max_slope); }, - [&](const Batch& d) { - return f(d, Batch(DHAT_P), Batch(DHAT_A), Batch(max_slope)); - }); + 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) { @@ -55,28 +67,29 @@ TEST_CASE( { const double eps_a = GENERATE(1e-3, 0.1, 1.0); - // Speeds as multiples of eps_a. These functions clamp at y <= 0 rather than - // mirroring on |y|, so the negative entry checks that clamp; zero is where - // the `1/y` branch is singular, which only a batch evaluates. - constexpr std::array MULTIPLES = { -1.0, 0.0, 0.25, 0.49, - 0.5, 0.75, 1.0, 2.5 }; - std::array ys {}; - for (size_t i = 0; i < ys.size(); ++i) { - ys[i] = MULTIPLES[i] * eps_a; - } - - auto check = [&](const std::string& name, auto&& f) { - check_swept_lanes( - name, ys, [&](const double y) { return f(y, eps_a); }, - [&](const Batch& y) { return f(y, Batch(eps_a)); }); + auto check = [&](const std::string& name, const auto& ys, auto&& f) { + check_swept_lanes_with(name, ys, f, eps_a); }; - check("f0", [](auto y, auto e) { return tangential_adhesion_f0(y, e); }); - check("f1", [](auto y, auto e) { return tangential_adhesion_f1(y, e); }); - check("f2", [](auto y, auto e) { return tangential_adhesion_f2(y, e); }); - check("f1_over_x", [](auto y, auto e) { + 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( @@ -92,19 +105,10 @@ TEST_CASE( 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. - constexpr std::array MULTIPLES = { 0.0, 0.25, 0.49, 0.5, - 0.75, 1.0, 2.5 }; - std::array ys {}; - for (size_t i = 0; i < ys.size(); ++i) { - ys[i] = MULTIPLES[i] * eps_a; - } + const auto ys = scaled(NONNEGATIVE_MULTIPLES, eps_a); auto check = [&](const std::string& name, auto&& f) { - check_swept_lanes( - name, ys, [&](const double y) { return f(y, mu_s, mu_k, eps_a); }, - [&](const Batch& y) { - return f(y, Batch(mu_s), Batch(mu_k), Batch(eps_a)); - }); + check_swept_lanes_with(name, ys, f, mu_s, mu_k, eps_a); }; check("a0", [](auto y, auto s, auto k, auto e) { diff --git a/tests/src/tests/barrier/test_barrier.cpp b/tests/src/tests/barrier/test_barrier.cpp index 24e7e5648..d50c67f32 100644 --- a/tests/src/tests/barrier/test_barrier.cpp +++ b/tests/src/tests/barrier/test_barrier.cpp @@ -61,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))); } } @@ -80,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))); } } @@ -102,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); @@ -238,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); } 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/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/friction/test_simd_friction.cpp b/tests/src/tests/friction/test_simd_friction.cpp index 865b8642a..6d6b450c2 100644 --- a/tests/src/tests/friction/test_simd_friction.cpp +++ b/tests/src/tests/friction/test_simd_friction.cpp @@ -27,15 +27,6 @@ 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 }; -std::array speeds(const double eps_v) -{ - std::array ys {}; - for (size_t i = 0; i < ys.size(); ++i) { - ys[i] = Y_MULTIPLES[i] * eps_v; - } - return ys; -} - } // namespace TEST_CASE( @@ -43,12 +34,10 @@ TEST_CASE( "[friction][mollifier][simd]") { const double eps_v = GENERATE(1e-3, 0.1, 1.0); - const auto ys = speeds(eps_v); + const auto ys = scaled(Y_MULTIPLES, eps_v); auto check = [&](const std::string& name, auto&& f) { - check_swept_lanes( - name, ys, [&](const double y) { return f(y, eps_v); }, - [&](const Batch& y) { return f(y, Batch(eps_v)); }); + check_swept_lanes_with(name, ys, f, eps_v); }; check("f0", [](auto y, auto e) { return smooth_friction_f0(y, e); }); @@ -74,14 +63,10 @@ TEST_CASE( 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 = speeds(eps_v); + const auto ys = scaled(Y_MULTIPLES, eps_v); auto check = [&](const std::string& name, auto&& f) { - check_swept_lanes( - name, ys, [&](const double y) { return f(y, mu_s, mu_k, eps_v); }, - [&](const Batch& y) { - return f(y, Batch(mu_s), Batch(mu_k), Batch(eps_v)); - }); + check_swept_lanes_with(name, ys, f, mu_s, mu_k, eps_v); }; check("mu", [](auto y, auto s, auto k, auto e) { diff --git a/tests/src/tests/simd_utils.hpp b/tests/src/tests/simd_utils.hpp index 81e3445c0..2ca30c7b8 100644 --- a/tests/src/tests/simd_utils.hpp +++ b/tests/src/tests/simd_utils.hpp @@ -113,6 +113,19 @@ template inline Points random_points(const int seed) 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 @@ -218,6 +231,21 @@ void check_swept_lanes( } } +/// @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/test_closest_point.cpp b/tests/src/tests/tangent/test_closest_point.cpp index d9ebeb938..f9c837a9f 100644 --- a/tests/src/tests/tangent/test_closest_point.cpp +++ b/tests/src/tests/tangent/test_closest_point.cpp @@ -52,8 +52,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 = diff --git a/tests/src/tests/tangent/test_simd_closest_point.cpp b/tests/src/tests/tangent/test_simd_closest_point.cpp index 1e6652459..ff6ddb883 100644 --- a/tests/src/tests/tangent/test_simd_closest_point.cpp +++ b/tests/src/tests/tangent/test_simd_closest_point.cpp @@ -1,4 +1,3 @@ -#include #include #include @@ -57,7 +56,8 @@ TEST_CASE( [&](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) { @@ -69,9 +69,11 @@ TEST_CASE( SECTION("edge-edge") { check_lanes( - "coordinates", edge_edge_closest_point(a, b, c, d), [&](int l) { + "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(); - }); + }, + VALUE_TOL); check_lanes( "jacobian", edge_edge_closest_point_jacobian(a, b, c, d), [&](int l) { @@ -109,40 +111,4 @@ TEST_CASE( }); } -TEST_CASE( - "Closest points of a symmetric crossing are the edge midpoints", - "[closest_point]") -{ - // Two 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. This anchors the result to the geometry rather than to whatever - // a particular decomposition happens to return. - const EdgePair e = crossing_edges(std::acos(0.0)); // perpendicular - - const Eigen::Vector2d coords = - edge_edge_closest_point(e.ea0, e.ea1, e.eb0, e.eb1); - - CAPTURE(coords); - CHECK(coords[0] == Catch::Approx(0.5).epsilon(0).margin(1e-15)); - CHECK(coords[1] == Catch::Approx(0.5).epsilon(0).margin(1e-15)); -} - -TEST_CASE( - "A point on a triangle recovers its own barycentric coordinates", - "[closest_point]") -{ - // Projecting a point that already lies in the plane must return the - // coordinates it was built from, whatever the solve does internally. - const Eigen::Vector3d t0(-1, 0, 1), t1(1, 0, 1), t2(0, 0, -1); - const Eigen::Vector2d expected(0.25, 0.5); - const Eigen::Vector3d p = - t0 + expected[0] * (t1 - t0) + expected[1] * (t2 - t0); - - const Eigen::Vector2d coords = point_triangle_closest_point(p, t0, t1, t2); - - CAPTURE(coords); - CHECK(coords[0] == Catch::Approx(expected[0]).epsilon(0).margin(1e-15)); - CHECK(coords[1] == Catch::Approx(expected[1]).epsilon(0).margin(1e-15)); -} - #endif From 1cd5e9407839ed25e66eed328d85c01590cff1d7 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 20:12:55 -0400 Subject: [PATCH 14/34] Fix alignment issues with dynamic reshaped matrices - Fix single bracket array initializer in tests --- src/ipc/distance/signed/line_line.cpp | 3 ++- src/ipc/distance/signed/point_line.cpp | 3 ++- src/ipc/distance/signed/point_plane.cpp | 3 ++- tests/src/tests/adhesion/test_simd_adhesion.cpp | 4 ++-- tests/src/tests/friction/test_simd_friction.cpp | 6 +++--- tests/src/tests/tangent/test_simd_closest_point.cpp | 2 +- 6 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/ipc/distance/signed/line_line.cpp b/src/ipc/distance/signed/line_line.cpp index 33bb3db80..c82e8a7aa 100644 --- a/src/ipc/distance/signed/line_line.cpp +++ b/src/ipc/distance/signed/line_line.cpp @@ -28,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) diff --git a/src/ipc/distance/signed/point_line.cpp b/src/ipc/distance/signed/point_line.cpp index 72e3b856d..6000aaf20 100644 --- a/src/ipc/distance/signed/point_line.cpp +++ b/src/ipc/distance/signed/point_line.cpp @@ -25,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) diff --git a/src/ipc/distance/signed/point_plane.cpp b/src/ipc/distance/signed/point_plane.cpp index 01498ab52..3b5b41c3a 100644 --- a/src/ipc/distance/signed/point_plane.cpp +++ b/src/ipc/distance/signed/point_plane.cpp @@ -43,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 diff --git a/tests/src/tests/adhesion/test_simd_adhesion.cpp b/tests/src/tests/adhesion/test_simd_adhesion.cpp index 0a5fc7297..b6e1d72e0 100644 --- a/tests/src/tests/adhesion/test_simd_adhesion.cpp +++ b/tests/src/tests/adhesion/test_simd_adhesion.cpp @@ -41,8 +41,8 @@ TEST_CASE( // 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 }; + 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); diff --git a/tests/src/tests/friction/test_simd_friction.cpp b/tests/src/tests/friction/test_simd_friction.cpp index 6d6b450c2..d6894301b 100644 --- a/tests/src/tests/friction/test_simd_friction.cpp +++ b/tests/src/tests/friction/test_simd_friction.cpp @@ -23,9 +23,9 @@ namespace { /// 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 }; +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 diff --git a/tests/src/tests/tangent/test_simd_closest_point.cpp b/tests/src/tests/tangent/test_simd_closest_point.cpp index ff6ddb883..baedbeef4 100644 --- a/tests/src/tests/tangent/test_simd_closest_point.cpp +++ b/tests/src/tests/tangent/test_simd_closest_point.cpp @@ -91,7 +91,7 @@ TEST_CASE( // 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 }; + 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); From 2033cd0343c8bed72357fe6dd9be3d2d56f2abec Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 20:57:59 -0400 Subject: [PATCH 15/34] Narrow the warning suppressions and drop the Eigen abs workaround Suppress -Warray-bounds only on GCC 16+, the one compiler that trips it. In a translation unit instantiating tbb::enumerable_thread_specific for more than one type, GCC 16 speculatively devirtualizes the type-erased construct callback in combine() to an override for the wrong type and reports the placement-new into the smaller ets_element as out of bounds. GCC <= 15 and Clang are clean across all 182 translation units, so they keep the warning instead of losing it project-wide. Drop -Wmaybe-uninitialized from the non-GNU branch: it is GCC-only, so check_cxx_compiler_flag filters it out on Clang while the -Wno- form disables it on GCC. It was enabled nowhere. Remove and the EIGEN_USING_STD(abs) workaround from eigen_ext.hpp; neither the header nor its .tpp uses either one, and the latter injected a using-declaration into the global namespace from a public header. --- cmake/ipc_toolkit/ipc_toolkit_warnings.cmake | 8 ++++++++ src/ipc/utils/eigen_ext.hpp | 8 -------- 2 files changed, 8 insertions(+), 8 deletions(-) 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/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>; From 22391bbb9f4d43e33c6233ddd71df7d4ce8a1356 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 21:07:06 -0400 Subject: [PATCH 16/34] Configure MeshFEM_export.h instead of write --- cmake/recipes/meshfem_sparse.cmake | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 From fec48ce6f95aef3455d5ca8c3013ebfa21cc9b7c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 22:02:14 -0400 Subject: [PATCH 17/34] Document xsimd's public linkage in dependencies --- docs/source/about/dependencies.rst | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/docs/source/about/dependencies.rst b/docs/source/about/dependencies.rst index 82b6162d2..15908a5dd 100644 --- a/docs/source/about/dependencies.rst +++ b/docs/source/about/dependencies.rst @@ -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). From b4be016f84d856a0bd4fda1c040b469ae5a7ffc0 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 22:04:28 -0400 Subject: [PATCH 18/34] Color the xsimd dependency edge as public --- docs/source/_static/graphviz/dependencies.dot | 2 +- docs/source/_static/graphviz/dependencies.svg | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/source/_static/graphviz/dependencies.dot b/docs/source/_static/graphviz/dependencies.dot index 50643f46d..2d971e3e3 100644 --- a/docs/source/_static/graphviz/dependencies.dot +++ b/docs/source/_static/graphviz/dependencies.dot @@ -47,7 +47,7 @@ digraph "IPC Toolkit Dependencies" { "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";]; + "node5" -> "node15" [color = "#8FB976";]; // ipc_toolkit -> xsimd "node6" [label = "robin_map\n(tsl::robin_map)";shape = box;style = "rounded,filled";fillcolor = "#FFE6CC";color = "#DAA52D";]; "node5" -> "node6" [color = "#BE6562";]; diff --git a/docs/source/_static/graphviz/dependencies.svg b/docs/source/_static/graphviz/dependencies.svg index d502307ca..76ecebdb4 100644 --- a/docs/source/_static/graphviz/dependencies.svg +++ b/docs/source/_static/graphviz/dependencies.svg @@ -121,8 +121,8 @@ node5->node15 - - + + From 3db577d3e443b8e12f73c2afaf3a59b7eb5959de Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 22:13:27 -0400 Subject: [PATCH 19/34] Show all default dependencies in the graph - Add MeshFEMSparse, which is on by default but was missing - Drop nlohmann/json and Tracy, which are off by default - Dash the edges of optional dependencies and note it in the legend - Correct several node comments that named the wrong target --- docs/source/_static/graphviz/dependencies.dot | 36 +- docs/source/_static/graphviz/dependencies.svg | 340 +++++++++--------- docs/source/about/dependencies.rst | 2 +- 3 files changed, 199 insertions(+), 179 deletions(-) diff --git a/docs/source/_static/graphviz/dependencies.dot b/docs/source/_static/graphviz/dependencies.dot index 2d971e3e3..a22be4de5 100644 --- a/docs/source/_static/graphviz/dependencies.dot +++ b/docs/source/_static/graphviz/dependencies.dot @@ -17,9 +17,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 +44,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 = "#8FB976";]; - // 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 +79,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 76ecebdb4..0460a93e4 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 15908a5dd..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. From 647e189dabbc7e9f5c1b00884b8646ed6d794aa5 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 22:48:00 -0400 Subject: [PATCH 20/34] Give the ortho router room to turn into igl_core --- docs/source/_static/graphviz/dependencies.dot | 5 +- docs/source/_static/graphviz/dependencies.svg | 230 +++++++++--------- 2 files changed, 119 insertions(+), 116 deletions(-) diff --git a/docs/source/_static/graphviz/dependencies.dot b/docs/source/_static/graphviz/dependencies.dot index a22be4de5..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";]; diff --git a/docs/source/_static/graphviz/dependencies.svg b/docs/source/_static/graphviz/dependencies.svg index 0460a93e4..6e9d89f80 100644 --- a/docs/source/_static/graphviz/dependencies.svg +++ b/docs/source/_static/graphviz/dependencies.svg @@ -4,20 +4,20 @@ - + IPC Toolkit Dependencies clusterLegend - -Legend + +Legend legendNode0 - -Static Library + +Static Library @@ -28,297 +28,297 @@ legendNode0->legendNode1 - - + + Public legendNode2 - -Interface Library + +Interface Library legendNode1->legendNode2 - - + + Private legendNode2->legendNode0 - - -Interface + + +Interface legendNode3 - -Optional Dependency + +Optional Dependency legendNode2->legendNode3 - - -Optional + + +Optional node5 - -ipc_toolkit -(ipc::toolkit) + +ipc_toolkit +(ipc::toolkit) node0 - -Eigen3_Eigen -(Eigen3::Eigen) + +Eigen3_Eigen +(Eigen3::Eigen) node5->node0 - - + + node1 - -filib -(filib::filib) + +filib +(filib::filib) node5->node1 - - + + 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 - - + + node17 - -MeshFEMSparse -(MeshFEM::Sparse) + +MeshFEMSparse +(MeshFEM::Sparse) 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 - - + + From 7b589c12c2e54d008036aaa211f2e36a1b4a582f Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 22:48:43 -0400 Subject: [PATCH 21/34] Add Benchmarks section to CLAUDE.md --- CLAUDE.md | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) 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: From f14e5a21e9f1e962e1cc508073c5fb5f7ba2fa0d Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 23:24:08 -0400 Subject: [PATCH 22/34] Test a singular lane beside well-conditioned ones MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The nearly-parallel sweep stopped at 1e-3, so the singular branch of solve_spd_2x2 was never taken per-lane. Slide the parallel pair along its shared direction so the zeroed lane leaves a residual of order ‖b‖; neutering the det <= 0 guard now trips the assert. --- .../tests/tangent/test_simd_closest_point.cpp | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/tests/src/tests/tangent/test_simd_closest_point.cpp b/tests/src/tests/tangent/test_simd_closest_point.cpp index baedbeef4..f1d76694f 100644 --- a/tests/src/tests/tangent/test_simd_closest_point.cpp +++ b/tests/src/tests/tangent/test_simd_closest_point.cpp @@ -30,6 +30,22 @@ EdgePair crossing_edges(const double theta) 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( @@ -111,4 +127,46 @@ TEST_CASE( }); } +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 From 491a163cbad150f9810a7c7c8f1e83c6357ae62b Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 23:29:54 -0400 Subject: [PATCH 23/34] Hold random edge-edge coordinates to the derivative bound Random segments are unconstrained in direction and offset, so the closest points routinely land far off both ends and the coordinates amplify the inputs. On an 8-lane runner one draw drifted 1.2e-14 relative, just over VALUE_TOL, while the same test passes at 2 lanes. The sibling near-parallel test already uses this bound for the same quantity. --- tests/src/tests/tangent/test_simd_closest_point.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/tests/src/tests/tangent/test_simd_closest_point.cpp b/tests/src/tests/tangent/test_simd_closest_point.cpp index f1d76694f..d9e08dc4a 100644 --- a/tests/src/tests/tangent/test_simd_closest_point.cpp +++ b/tests/src/tests/tangent/test_simd_closest_point.cpp @@ -84,12 +84,23 @@ TEST_CASE( 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(); }, - VALUE_TOL); + DERIVATIVE_TOL); check_lanes( "jacobian", edge_edge_closest_point_jacobian(a, b, c, d), [&](int l) { From 3caa7642432556e99e22f62a809d2b738fa2f01e Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 5 Sep 2026 23:41:16 -0400 Subject: [PATCH 24/34] Instrument the tests in coverage builds The --coverage compile options were PRIVATE, so only the library's own translation units were instrumented. Much of the library is header-only templates instantiated in the caller's TU, so a header exercised solely by tests recorded no coverage and read as untested: adhesion.hpp at 34.6% despite scalar and batch tests covering every function in it. The link options were already PUBLIC, and the workflow strips '*tests/*' from the report, which only has an effect if tests are instrumented. --- CMakeLists.txt | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 68ef06a3d..f9c357bce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -343,7 +343,14 @@ endif() if(IPC_TOOLKIT_WITH_CODE_COVERAGE AND CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") # Add required flags (GCC & LLVM/Clang) - target_compile_options(ipc_toolkit PRIVATE + # NOTE: PUBLIC, so the tests are instrumented too. Much of the library is + # header-only templates, which are only instantiated in the translation unit + # that calls them. With these flags PRIVATE the tests compile uninstrumented, + # so a header exercised solely by tests records no coverage at all and reads + # as untested. That is also why the workflow strips "*tests/*" from the + # report afterwards -- it only has something to strip if tests are built with + # these flags. + target_compile_options(ipc_toolkit PUBLIC -g # generate debug info --coverage # sets all required flags -fprofile-update=atomic From b49ca519b00acbf1755244f280ae2f2dae9aa75c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 00:09:06 -0400 Subject: [PATCH 25/34] Ignore geninfo's mismatch error in coverage capture --- .github/workflows/coverage.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index f4239d455..5dfd9c090 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -68,7 +68,12 @@ jobs: run: | cd build ctest --verbose -j ${{ steps.cpu-cores.outputs.count }} - lcov --directory . --capture --output-file coverage.info --ignore-errors inconsistent,format,gcov + # NOTE: "mismatch" is needed because the tests are instrumented too. + # geninfo reads their .gcda and trips over Catch2's TEMPLATE_TEST_CASE + # macros, whose recorded end line does not match the one gcov derives + # for each instantiation. It is a quirk of reading the macro, not a + # problem with the data. + 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 From d795223bafd04b8ae9ff2199ae94f4f3039d3d64 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 00:33:05 -0400 Subject: [PATCH 26/34] Ignore the whole tests tree in codecov A single "*" does not cross a "/", so "tests/*" only matched files sitting directly in tests/ and everything under tests/src/tests/ was reported. Confirmed with codecov's validator: the old pattern compiles to (?s:tests/[^/]*)\Z, the new one to (?s:tests/.*)\Z. --- codecov.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/codecov.yml b/codecov.yml index b28f2ce69..f9c7cdf98 100644 --- a/codecov.yml +++ b/codecov.yml @@ -10,4 +10,7 @@ coverage: threshold: 5% only_pulls: true ignore: - - "tests/*" + # "**" so this reaches the whole tree: a single "*" does not cross a "/", so + # "tests/*" only ever matched files sitting directly in tests/, leaving + # everything under tests/src/tests/ reported. + - "tests/**" From ffed4a8182d9f79a9ddcbc8944bf858ad4a0eb1e Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 00:51:16 -0400 Subject: [PATCH 27/34] Test the closest-point Hessians and the runtime dimension dispatch The closest-point Hessians had no test at all: the existing cases finite-difference each Jacobian against the value, then stop. Check each Hessian against a finite difference of the corresponding Jacobian, in the same cases. The front ends also have a runtime branch on size() for argument types that do not carry their dimension. Every test passed a fixed-size vector, so only the if constexpr arms ran. Passing the same points as a VectorMax3d takes the fallback; both reach the same kernel, so the results must be identical. Takes tangent_basis.hpp from 73.3% to 100% and closest_point.hpp from 65.4% to 100%. --- .../src/tests/tangent/test_closest_point.cpp | 122 ++++++++++++++++++ .../src/tests/tangent/test_tangent_basis.cpp | 54 ++++++++ 2 files changed, 176 insertions(+) diff --git a/tests/src/tests/tangent/test_closest_point.cpp b/tests/src/tests/tangent/test_closest_point.cpp index f9c837a9f..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]") @@ -75,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]") @@ -100,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( @@ -127,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_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)); + } +} From c1874d06c1d8eee5ade6a5c1135dbc70e6351d15 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 09:02:55 -0400 Subject: [PATCH 28/34] Remove coverage comments --- .github/workflows/coverage.yml | 5 ----- codecov.yml | 3 --- 2 files changed, 8 deletions(-) diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 5dfd9c090..b71a818e5 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -68,11 +68,6 @@ jobs: run: | cd build ctest --verbose -j ${{ steps.cpu-cores.outputs.count }} - # NOTE: "mismatch" is needed because the tests are instrumented too. - # geninfo reads their .gcda and trips over Catch2's TEMPLATE_TEST_CASE - # macros, whose recorded end line does not match the one gcov derives - # for each instantiation. It is a quirk of reading the macro, not a - # problem with the data. 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 diff --git a/codecov.yml b/codecov.yml index f9c7cdf98..d83565144 100644 --- a/codecov.yml +++ b/codecov.yml @@ -10,7 +10,4 @@ coverage: threshold: 5% only_pulls: true ignore: - # "**" so this reaches the whole tree: a single "*" does not cross a "/", so - # "tests/*" only ever matched files sitting directly in tests/, leaving - # everything under tests/src/tests/ reported. - "tests/**" From 00c8b7c67e7d9f77159d831b725432b7853a322e Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 09:05:20 -0400 Subject: [PATCH 29/34] Instrument the tests from the test target, not via PUBLIC Coverage flags on ipc_toolkit go back to PRIVATE; the test target sets the same flags on itself. Instrumentation still reaches the tests, which it must because a template is only compiled where it is instantiated, but consumers of ipc::toolkit no longer inherit --coverage. --- CMakeLists.txt | 10 ++-------- tests/CMakeLists.txt | 17 +++++++++++++++++ 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f9c357bce..ee04d3984 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -343,14 +343,8 @@ endif() if(IPC_TOOLKIT_WITH_CODE_COVERAGE AND CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") # Add required flags (GCC & LLVM/Clang) - # NOTE: PUBLIC, so the tests are instrumented too. Much of the library is - # header-only templates, which are only instantiated in the translation unit - # that calls them. With these flags PRIVATE the tests compile uninstrumented, - # so a header exercised solely by tests records no coverage at all and reads - # as untested. That is also why the workflow strips "*tests/*" from the - # report afterwards -- it only has something to strip if tests are built with - # these flags. - target_compile_options(ipc_toolkit PUBLIC + # 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 -fprofile-update=atomic 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( From 8343e3b154c3abf167271495a6b4b1dfd0f17a26 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 09:27:59 -0400 Subject: [PATCH 30/34] Compute the 2x2 determinant with Kahan's FMA form The naive a00*a11 - a01*a10 cancels catastrophically as the edges approach parallel, which is where the closest-point solve is used. Recovering the dropped rounding error with fma makes the determinant good to a couple of ulp however badly the products cancel. Measured against the exact determinant of the same double inputs, the relative error at 1e-4 rad drops from 2.5e-9 to 4.5e-17, and the full solve improves by the same factor: the determinant, not the numerators, was the dominant error for this geometry. Well-conditioned angles are unchanged. Degrades to the naive expression, rather than misbehaving, where xsimd has no hardware fma; NEON and FMA3 both provide a fused one. --- src/ipc/tangent/closest_point.hpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/ipc/tangent/closest_point.hpp b/src/ipc/tangent/closest_point.hpp index 1cf11b5b1..9f2780b7e 100644 --- a/src/ipc/tangent/closest_point.hpp +++ b/src/ipc/tangent/closest_point.hpp @@ -176,7 +176,21 @@ namespace detail { Eigen::ConstRef> A, Eigen::ConstRef> b) { - const T det = A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0); + // 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::fma(A(0, 1), A(1, 0), -bc); + const T det = ipc::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( From 8a10c4db38e642dee86a4af553431e9b1c3761b3 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 09:33:39 -0400 Subject: [PATCH 31/34] Rename DETECTED_FMA to DETECTED_FMA3_X86 The probe needs -mavx2 -mfma and , so it can only ever answer for x86. On AArch64 it failed while the chip does have hardware FMA, which reads like a missing capability. Only the log line changes; FMA_FOUND and FMA_FLAGS are untouched, and an empty FMA_FLAGS stays correct on AArch64 because fmadd and vfmaq_* are mandatory there with no flag to opt into. --- cmake/find/FindSIMD.cmake | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) 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") From 1c96cc062083153ac42472f7267d5c7f20227572 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 09:35:45 -0400 Subject: [PATCH 32/34] Update the closest-point solve accuracy claim The old note said accuracy was unchanged, which was measured before the determinant used Kahan's fused form. Measured against the exact solution of the same double inputs, the closed form now beats LDLT by up to 5.6e7x near parallel, because LDLT's second pivot suffers the same cancellation the naive determinant did. --- docs/source/about/release_notes.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 2870b8c97..eabdc3ee1 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -85,7 +85,7 @@ API Changes |:wrench:| - The normalized normals (``point_line_normal``, ``triangle_normal``, ``line_line_normal``) use ``ipc::normalized`` instead of Eigen's ``normalized()``, which makes them and the values/gradients of the ``point_line``, ``line_line``, and ``point_plane`` signed distances usable with batch scalars (previously only the signed-distance Hessians were). - ``edge_length_gradient`` asserts its non-degeneracy only for a plain scalar, so it accepts batch scalars as well. - The relative-velocity functions (values, Jacobians, and ``dx_dbeta`` tensors for point-point, point-edge, edge-edge, and point-triangle, in 2D and 3D) were already batch-compatible and are now covered by tests, agreeing with the scalar path to 1e-14 relative. - - ``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. They complete the batch coverage of the closest-point functions, whose Jacobians and Hessians were already instantiated. Accuracy is unchanged in practice: measured against exactly known solutions, the closed form and Eigen's LDLT trade places within about 2× at every conditioning level, and on realistic edge-edge Gram matrices they agree to three significant figures at every angle down to 1e-4 rad. + - ``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. They complete the batch coverage of the closest-point functions, whose Jacobians and Hessians were already instantiated. The determinant uses Kahan's fused form, recovering the rounding error of one product with ``fma`` so that it stays accurate no matter how badly the two products cancel. That cancellation, not the algorithm, is what limits accuracy as the edges approach parallel, and it limits ``LDLT`` equally: its second pivot ``a11 - a01²/a00`` loses the same digits the naive determinant does. 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. On hardware with no fused multiply-add the determinant degrades to the naive expression rather than misbehaving. - Functions that pick a case from the values themselves — a barrier clamping at ``d̂``, the 3D point-point tangent basis choosing a reference axis — evaluate every case for a batch and blend the results per-lane, so one batch may carry lanes in different cases. The scalar instantiations keep their original early-return form and are unchanged. - ``ipc::normalized(v)`` replaces ``v.normalized()`` in the tangent bases: Eigen's own ``normalized()`` guards a zero-length vector with an ``if``, which a batch cannot answer with one ``bool``. It applies the same rule per-lane and defers to Eigen for scalars. - 💥 **[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. From 0df5486204461fe5c112ab83e2b934496be3c077 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 10:55:01 -0400 Subject: [PATCH 33/34] Move the std math forwarders to ipc::numext abs, atan2, fma, log and sqrt 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 already found to hijack Eigen's array abs. They are mechanism, not API: each exists only so a template can reach the xsimd and autodiff overloads by ADL. Following Eigen::numext, they now live in their own namespace. Not ipc::detail, since the kernels live there and would pick these up by ordinary lookup ahead of ADL, reproducing the same hijack one level down. sqr, cubic and MOLLIFIER_THRESHOLD_EPS stay at ipc scope; they collide with nothing and Math::sqr forwards to them as public API. --- docs/source/about/release_notes.rst | 2 +- .../friction/smooth_friction_mollifier.hpp | 10 +++--- src/ipc/friction/smooth_mu.hpp | 8 ++--- src/ipc/geometry/angle.cpp | 2 +- src/ipc/geometry/area.cpp | 2 +- src/ipc/geometry/normal.cpp | 6 ++-- src/ipc/math/math.hpp | 2 +- src/ipc/math/scalar_math.hpp | 24 +++++++++---- src/ipc/tangent/closest_point.hpp | 4 +-- src/ipc/tangent/tangent_basis.cpp | 35 ++++++++++--------- src/ipc/utils/simd.hpp | 2 +- 11 files changed, 56 insertions(+), 41 deletions(-) diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index eabdc3ee1..0d1073a3b 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -71,7 +71,7 @@ API Changes |:wrench:| - ``smooth_friction_mollifier.cpp`` and ``adhesion.cpp`` are gone; both headers are header-only templates now, following ``edge_edge_mollifier.hpp``. - ``dihedral_angle`` and its gradient/Hessian follow the distance family's two-layer split: ``ipc::detail`` kernels templated on ```` (3D-only) and instantiated for ``float``, ``double``, and both batch types, behind front ends that deduce ``T``. Existing calls are unaffected. - The anisotropic-friction helpers (``anisotropic_mu_eff_f``, ``anisotropic_x_from_tau_aniso``, ``anisotropic_mu_eff_from_tau_aniso``) stay ``double``-only for now, since no batch caller exists for them yet. The first two do depend on the per-collision tangential velocity, so a batch friction path with anisotropic μ would need to template them as well; only ``anisotropic_mu_eff_from_tau_aniso`` tests the material alone. - - Add ``ipc::abs`` and ``ipc::atan2`` to ``ipc/math/scalar_math.hpp``. ``Math::abs`` picks the sign with a ternary, which asks a batch for one ``bool`` its lanes may disagree on; ``ipc::abs`` reaches ``xsimd::abs``, which clears the sign bit per-lane instead. + - Add ``ipc::numext::abs`` and ``ipc::numext::atan2`` to ``ipc/math/scalar_math.hpp``, alongside ``fma``, ``log`` and ``sqrt``. ``Math::abs`` picks the sign with a ternary, which asks a batch for one ``bool`` its lanes may disagree on; ``numext::abs`` reaches ``xsimd::abs``, which clears the sign bit per-lane instead. These forwarders sit in ``ipc::numext`` rather than ``ipc``, following ``Eigen::numext``: they carry names (``abs``, ``log``, ``sqrt``) that would otherwise join the overload set of anyone writing ``using namespace ipc;``. ``ipc::sqr``, ``ipc::cubic`` and ``ipc::MOLLIFIER_THRESHOLD_EPS`` stay at ``ipc`` scope. - 💥 **[Breaking]** As with the distance functions, mixed-precision calls no longer deduce: every argument must share one scalar type, so ``smooth_mu(float_y, 0.5, 0.3, 0.001)`` must become ``smooth_mu(float_y, 0.5f, 0.3f, 0.001f)``. - Add SIMD batch support to the distance functions via the new ``ipc/utils/simd.hpp`` (requires ``IPC_TOOLKIT_WITH_SIMD``). diff --git a/src/ipc/friction/smooth_friction_mollifier.hpp b/src/ipc/friction/smooth_friction_mollifier.hpp index 82ef7a295..99d97447a 100644 --- a/src/ipc/friction/smooth_friction_mollifier.hpp +++ b/src/ipc/friction/smooth_friction_mollifier.hpp @@ -38,7 +38,7 @@ template inline T smooth_friction_f0(const T y, const T eps_v) { assert(all_of(eps_v > T(0))); return select_lazy( - ipc::abs(y) >= eps_v, [&] { return y; }, + ipc::numext::abs(y) >= eps_v, [&] { return y; }, [&] { return y * y * (T(1) - y / (T(3) * eps_v)) / eps_v + eps_v / T(3); }); @@ -61,7 +61,7 @@ template inline T smooth_friction_f1(const T y, const T eps_v) { assert(all_of(eps_v > T(0))); return select_lazy( - ipc::abs(y) >= eps_v, [&] { return T(1); }, + 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); @@ -85,7 +85,7 @@ template inline T smooth_friction_f2(const T y, const T eps_v) { assert(all_of(eps_v > T(0))); return select_lazy( - ipc::abs(y) >= eps_v, [&] { return T(0); }, + ipc::numext::abs(y) >= eps_v, [&] { return T(0); }, [&] { return (T(2) - T(2) * y / eps_v) / eps_v; }); } @@ -108,7 +108,7 @@ 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::abs(y) >= eps_v, [&] { return T(1) / y; }, + ipc::numext::abs(y) >= eps_v, [&] { return T(1) / y; }, [&] { return (T(2) - y / eps_v) / eps_v; }); } @@ -131,7 +131,7 @@ 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::abs(y) >= eps_v, [&] { return T(-1) / (y * y * y); }, + ipc::numext::abs(y) >= eps_v, [&] { return T(-1) / (y * y * y); }, [&] { return T(-1) / (y * eps_v * eps_v); }); } diff --git a/src/ipc/friction/smooth_mu.hpp b/src/ipc/friction/smooth_mu.hpp index 61bd4b0dd..ae2a9d79a 100644 --- a/src/ipc/friction/smooth_mu.hpp +++ b/src/ipc/friction/smooth_mu.hpp @@ -31,7 +31,7 @@ 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::abs(y); + 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; }, @@ -52,7 +52,7 @@ 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::abs(y); + 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); }, @@ -72,7 +72,7 @@ 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::abs(y); + 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( @@ -160,7 +160,7 @@ 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::abs(y); + 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( diff --git a/src/ipc/geometry/angle.cpp b/src/ipc/geometry/angle.cpp index b75e8c940..0014899aa 100644 --- a/src/ipc/geometry/angle.cpp +++ b/src/ipc/geometry/angle.cpp @@ -52,7 +52,7 @@ T dihedral_angle( const T sin_theta = n0.cross(n1).dot(e); const T cos_theta = n0.dot(n1); - return ipc::atan2(sin_theta, cos_theta); + return ipc::numext::atan2(sin_theta, cos_theta); } template diff --git a/src/ipc/geometry/area.cpp b/src/ipc/geometry/area.cpp index 9b6a753e4..150ac7245 100644 --- a/src/ipc/geometry/area.cpp +++ b/src/ipc/geometry/area.cpp @@ -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) / ipc::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); diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp index 32efa7ffb..ebba14be2 100644 --- a/src/ipc/geometry/normal.cpp +++ b/src/ipc/geometry/normal.cpp @@ -183,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 = ipc::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) @@ -270,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 = ipc::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); @@ -349,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 = ipc::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 = diff --git a/src/ipc/math/math.hpp b/src/ipc/math/math.hpp index 53f634413..18a98bf3b 100644 --- a/src/ipc/math/math.hpp +++ b/src/ipc/math/math.hpp @@ -17,7 +17,7 @@ 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 ipc::abs(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); } diff --git a/src/ipc/math/scalar_math.hpp b/src/ipc/math/scalar_math.hpp index 5ab6f81ad..b8cf484ae 100644 --- a/src/ipc/math/scalar_math.hpp +++ b/src/ipc/math/scalar_math.hpp @@ -16,7 +16,17 @@ namespace ipc { -/// @brief Define `ipc::FUNC` forwarding its arguments to the `std` counterpart. +/// @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, \ @@ -27,15 +37,17 @@ namespace ipc { 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) + 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. diff --git a/src/ipc/tangent/closest_point.hpp b/src/ipc/tangent/closest_point.hpp index 9f2780b7e..9f5c4603d 100644 --- a/src/ipc/tangent/closest_point.hpp +++ b/src/ipc/tangent/closest_point.hpp @@ -189,8 +189,8 @@ namespace detail { // `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::fma(A(0, 1), A(1, 0), -bc); - const T det = ipc::fma(A(0, 0), A(1, 1), -bc) - bc_err; + 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( diff --git a/src/ipc/tangent/tangent_basis.cpp b/src/ipc/tangent/tangent_basis.cpp index 32db9fa45..182bba771 100644 --- a/src/ipc/tangent/tangent_basis.cpp +++ b/src/ipc/tangent/tangent_basis.cpp @@ -159,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 * ipc::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 @@ -174,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) / ipc::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; @@ -208,7 +211,7 @@ void point_point_tangent_basis_3D_jacobian( const T t11 = T(0.5) * select(t6, T(0), T(2) * t0); const T t12 = t10 * t11; const T t13 = select(t6, t2, T(0)); - const T t14 = T(1) / ipc::sqrt(t9); + 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 = select(t6, t16, t0); @@ -224,7 +227,7 @@ void point_point_tangent_basis_3D_jacobian( 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) / ipc::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 = select(t22, t2, T(0)); @@ -242,7 +245,7 @@ void point_point_tangent_basis_3D_jacobian( 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) / ipc::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; @@ -335,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) / ipc::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 * ipc::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; @@ -395,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) / ipc::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; @@ -404,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) / ipc::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; @@ -422,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) / ipc::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); @@ -527,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) / ipc::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); @@ -569,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) / ipc::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; @@ -584,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) / ipc::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; @@ -754,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) / ipc::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); @@ -797,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) / ipc::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; @@ -814,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) / ipc::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; diff --git a/src/ipc/utils/simd.hpp b/src/ipc/utils/simd.hpp index 51e168d1d..a731f960a 100644 --- a/src/ipc/utils/simd.hpp +++ b/src/ipc/utils/simd.hpp @@ -216,7 +216,7 @@ normalized(const Eigen::MatrixBase& v) 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::sqrt(z); + const typename Derived::PlainObject scaled = n / ipc::numext::sqrt(z); const auto is_nonzero = z > T(0); typename Derived::PlainObject out = n; From 93d09f70ff560a38e589390d78960a016f2e06d2 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 6 Sep 2026 11:56:26 -0400 Subject: [PATCH 34/34] Update release_notes.rst --- docs/source/about/release_notes.rst | 52 ++++++++--------------------- 1 file changed, 14 insertions(+), 38 deletions(-) diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 0d1073a3b..08793fa8b 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -42,58 +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 - - - 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. - -- Templatize the friction, adhesion, and dihedral-angle functions on the scalar type. - - - The smooth friction mollifier, the smooth-μ family, and the adhesion functions (normal adhesion and its derivatives, the tangential-adhesion mollifiers, and the ``smooth_mu_a*`` variants) now take a scalar template parameter ``T``. These were hardcoded ``double``, which is what previously ruled out a batch-capable friction or adhesion path. - - ``smooth_friction_mollifier.cpp`` and ``adhesion.cpp`` are gone; both headers are header-only templates now, following ``edge_edge_mollifier.hpp``. - - ``dihedral_angle`` and its gradient/Hessian follow the distance family's two-layer split: ``ipc::detail`` kernels templated on ```` (3D-only) and instantiated for ``float``, ``double``, and both batch types, behind front ends that deduce ``T``. Existing calls are unaffected. - - The anisotropic-friction helpers (``anisotropic_mu_eff_f``, ``anisotropic_x_from_tau_aniso``, ``anisotropic_mu_eff_from_tau_aniso``) stay ``double``-only for now, since no batch caller exists for them yet. The first two do depend on the per-collision tangential velocity, so a batch friction path with anisotropic μ would need to template them as well; only ``anisotropic_mu_eff_from_tau_aniso`` tests the material alone. - - Add ``ipc::numext::abs`` and ``ipc::numext::atan2`` to ``ipc/math/scalar_math.hpp``, alongside ``fma``, ``log`` and ``sqrt``. ``Math::abs`` picks the sign with a ternary, which asks a batch for one ``bool`` its lanes may disagree on; ``numext::abs`` reaches ``xsimd::abs``, which clears the sign bit per-lane instead. These forwarders sit in ``ipc::numext`` rather than ``ipc``, following ``Eigen::numext``: they carry names (``abs``, ``log``, ``sqrt``) that would otherwise join the overload set of anyone writing ``using namespace ipc;``. ``ipc::sqr``, ``ipc::cubic`` and ``ipc::MOLLIFIER_THRESHOLD_EPS`` stay at ``ipc`` scope. - - 💥 **[Breaking]** As with the distance functions, mixed-precision calls no longer deduce: every argument must share one scalar type, so ``smooth_mu(float_y, 0.5, 0.3, 0.001)`` must become ``smooth_mu(float_y, 0.5f, 0.3f, 0.001f)``. - -- Add SIMD batch support to the distance functions via the new ``ipc/utils/simd.hpp`` (requires ``IPC_TOOLKIT_WITH_SIMD``). - - - ``Eigen::NumTraits`` is specialized for ``xsimd::batch``, and ``ipc::SimdBatch`` aliases the batch type for the build's architecture. Passing ``Eigen::Vector3>`` evaluates one independent problem per SIMD lane, letting a caller with a structure-of-arrays layout compute several distances per call. - - The values, gradients, and Hessians of the point-point, point-line, point-edge, line-line, edge-edge, and point-triangle distances are instantiated for ``SimdBatch`` and ``SimdBatch``. Measured agreement with the scalar path is one ulp on the values; the derivatives agree to ~1e-11 relative to their own magnitude, since the two paths contract multiply-adds differently and these derivatives are ill-conditioned for near-parallel edges. - - The barrier functions and classes (``barrier``, ``ClampedLogBarrier``, ``ClampedLogSqBarrier``, ``CubicBarrier``, ``TwoStageBarrier``), the tangent bases and their Jacobians, the closest-point Jacobians/Hessians, the unnormalized-normal Jacobians/Hessians, the signed-distance Hessians, and the triangle-area gradient are instantiated for batch scalars as well. - - The edge-edge mollifier is instantiated for batch scalars too: the mollifier and its gradient/Hessian, their derivatives with respect to the threshold, the threshold and its gradient, and the edge-edge cross-product squared norm with its gradient/Hessian. - - The friction mollifier, smooth-μ, adhesion, and dihedral-angle functions accept batch scalars. Their piecewise branches are ``select_lazy`` cascades, so one batch may carry lanes on either side of ``ε_v``/``ε_a``/``d̂ₚ``. Several of the inactive branches divide by ``y``, which a batch evaluates even on a lane where ``y == 0``; the blend is a per-lane select, so it discards the resulting infinity rather than propagating it into a NaN. - - The ``mu_s == mu_k`` test opening most smooth-μ functions is a fast path, not a special case: when the coefficients are equal the general formulas reduce to the same value, so a batch blending across that mask stays correct. It still buys a scalar caller the cheaper formula in the common single-coefficient setting. - - The normalized normals (``point_line_normal``, ``triangle_normal``, ``line_line_normal``) use ``ipc::normalized`` instead of Eigen's ``normalized()``, which makes them and the values/gradients of the ``point_line``, ``line_line``, and ``point_plane`` signed distances usable with batch scalars (previously only the signed-distance Hessians were). - - ``edge_length_gradient`` asserts its non-degeneracy only for a plain scalar, so it accepts batch scalars as well. - - The relative-velocity functions (values, Jacobians, and ``dx_dbeta`` tensors for point-point, point-edge, edge-edge, and point-triangle, in 2D and 3D) were already batch-compatible and are now covered by tests, agreeing with the scalar path to 1e-14 relative. - - ``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. They complete the batch coverage of the closest-point functions, whose Jacobians and Hessians were already instantiated. The determinant uses Kahan's fused form, recovering the rounding error of one product with ``fma`` so that it stays accurate no matter how badly the two products cancel. That cancellation, not the algorithm, is what limits accuracy as the edges approach parallel, and it limits ``LDLT`` equally: its second pivot ``a11 - a01²/a00`` loses the same digits the naive determinant does. 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. On hardware with no fused multiply-add the determinant degrades to the naive expression rather than misbehaving. - - Functions that pick a case from the values themselves — a barrier clamping at ``d̂``, the 3D point-point tangent basis choosing a reference axis — evaluate every case for a batch and blend the results per-lane, so one batch may carry lanes in different cases. The scalar instantiations keep their original early-return form and are unchanged. - - ``ipc::normalized(v)`` replaces ``v.normalized()`` in the tangent bases: Eigen's own ``normalized()`` guards a zero-length vector with an ``if``, which a batch cannot answer with one ``bool``. It applies the same rule per-lane and defers to Eigen for scalars. - - 💥 **[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. - - ``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. - - A caveat on the payoff: on an Apple M-series (NEON, 2 lanes per ``double``) a batched point-line sweep measured only 1.06-1.13x over the scalar loop, and 1.13-1.18x with ``float`` at 4 lanes. The compiler already auto-vectorizes a loop over independent problems, and the kernels are largely memory-bound. Wider ISAs may do better; that has not been measured. +- 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. -- Add ``float`` aliases to ``ipc/utils/eigen_ext.hpp`` (``Vector1f``, ``Vector6f``, ``Matrix6f``, ``VectorMax3f``, ``MatrixMax9f``, …) mirroring the existing ``double`` ones. Performance |:zap:| ~~~~~~~~~~~~~~~~~~~