From 791b2ae46b5f2bbb030a53f40d6e66aa214150b5 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 31 Aug 2025 21:29:49 -0400 Subject: [PATCH 1/4] Refactor BroadPhase to build from vertex boxes directly. - Moved the inclusion of the CCD library to a new position in CMakeLists.txt for better organization. - Updated CPM version from 0.40.2 to 0.42.0 in CPM.cmake. - Modified ccd_query_io.cmake to use the new CPMAddPackage syntax with options. - Updated finite_diff.cmake to use the latest version of the finite-diff package (1.0.3). - Adjusted libigl.cmake to use the new CPMAddPackage syntax with options. - Updated scalable_ccd.cmake to use the new CPMAddPackage syntax with options. - Refactored BroadPhase class methods in broad_phase.cpp and broad_phase.hpp to streamline the build process. - Enhanced HashGrid class methods in hash_grid.cpp and hash_grid.hpp for better clarity and efficiency. - Improved SpatialHash class methods in spatial_hash.cpp and spatial_hash.hpp to utilize precomputed AABBs. - Added new methods to SweepAndPrune and SweepAndTiniestQueue classes for building from precomputed AABBs. - Introduced a new function to suggest a good voxel size based on AABBs in voxel_size_heuristic.cpp and voxel_size_heuristic.hpp. - Optimized edge_edge_mollifier.cpp with FMA operator in gradient calculations. - Updated eigen_ext.tpp to improve error handling during matrix projections. - Added a static method to create an empty interval in interval.hpp. --- CMakeLists.txt | 14 +-- cmake/recipes/CPM.cmake | 2 +- cmake/recipes/ccd_query_io.cmake | 9 +- cmake/recipes/finite_diff.cmake | 2 +- cmake/recipes/libigl.cmake | 7 +- cmake/recipes/scalable_ccd.cmake | 7 +- src/ipc/broad_phase/broad_phase.cpp | 30 ++++-- src/ipc/broad_phase/broad_phase.hpp | 17 +++ src/ipc/broad_phase/bvh.cpp | 25 +---- src/ipc/broad_phase/bvh.hpp | 32 ++---- src/ipc/broad_phase/hash_grid.cpp | 48 ++------- src/ipc/broad_phase/hash_grid.hpp | 32 ++---- src/ipc/broad_phase/spatial_hash.cpp | 59 ++++++---- src/ipc/broad_phase/spatial_hash.hpp | 102 +++++++++++------- src/ipc/broad_phase/sweep_and_prune.cpp | 25 +++++ src/ipc/broad_phase/sweep_and_prune.hpp | 9 ++ .../broad_phase/sweep_and_tiniest_queue.cpp | 29 +++++ .../broad_phase/sweep_and_tiniest_queue.hpp | 9 ++ src/ipc/broad_phase/voxel_size_heuristic.cpp | 21 ++++ src/ipc/broad_phase/voxel_size_heuristic.hpp | 7 +- src/ipc/candidates/candidates.hpp | 18 ++++ src/ipc/ccd/narrow_phase_ccd.hpp | 46 ++++++++ src/ipc/distance/edge_edge_mollifier.cpp | 2 +- src/ipc/utils/eigen_ext.tpp | 7 +- src/ipc/utils/interval.hpp | 8 ++ 25 files changed, 373 insertions(+), 194 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4210e8376..612f1ff9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -168,13 +168,6 @@ target_link_libraries(ipc_toolkit PRIVATE tight_inclusion::tight_inclusion) include(scalable_ccd) target_link_libraries(ipc_toolkit PRIVATE scalable_ccd::scalable_ccd) -# CCD -if(IPC_TOOLKIT_WITH_INEXACT_CCD) - # Etienne Vouga's CTCD Library for the floating point root finding algorithm - include(evouga_ccd) - target_link_libraries(ipc_toolkit PRIVATE evouga::ccd) -endif() - # SimpleBVH include(simple_bvh) target_link_libraries(ipc_toolkit PRIVATE simple_bvh::simple_bvh) @@ -183,6 +176,13 @@ target_link_libraries(ipc_toolkit PRIVATE simple_bvh::simple_bvh) include(spdlog) target_link_libraries(ipc_toolkit PUBLIC spdlog::spdlog) +# CCD +if(IPC_TOOLKIT_WITH_INEXACT_CCD) + # Etienne Vouga's CTCD Library for the floating point root finding algorithm + include(evouga_ccd) + target_link_libraries(ipc_toolkit PRIVATE evouga::ccd) +endif() + # rational-cpp (requires GMP) if(IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION) include(rational_cpp) diff --git a/cmake/recipes/CPM.cmake b/cmake/recipes/CPM.cmake index fb1846742..c7b9c090c 100644 --- a/cmake/recipes/CPM.cmake +++ b/cmake/recipes/CPM.cmake @@ -1,4 +1,4 @@ -set(CPM_DOWNLOAD_VERSION 0.40.2) +set(CPM_DOWNLOAD_VERSION 0.42.0) if(CPM_SOURCE_CACHE) set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") diff --git a/cmake/recipes/ccd_query_io.cmake b/cmake/recipes/ccd_query_io.cmake index be26e87cc..ca8c4ad46 100644 --- a/cmake/recipes/ccd_query_io.cmake +++ b/cmake/recipes/ccd_query_io.cmake @@ -8,8 +8,9 @@ message(STATUS "Third-party: creating target 'ccd_io::ccd_io'") include(ipc_toolkit_tests_data) -set(CCD_IO_DOWNLOAD_SAMPLE_QUERIES ON CACHE BOOL "Download sample CCD queries" FORCE) -set(CCD_IO_SAMPLE_QUERIES_DIR "${IPC_TOOLKIT_TESTS_DATA_DIR}/ccd-queries/" CACHE PATH "Where should we download sample queries?") - include(CPM) -CPMAddPackage("gh:Continuous-Collision-Detection/CCD-Query-IO#36f6093af81a65acc27d9f05ad32d6b5729e8d15") \ No newline at end of file +CPMAddPackage( + URI "gh:Continuous-Collision-Detection/CCD-Query-IO#36f6093af81a65acc27d9f05ad32d6b5729e8d15" + OPTIONS "CCD_IO_DOWNLOAD_SAMPLE_QUERIES ON" + "CCD_IO_SAMPLE_QUERIES_DIR ${IPC_TOOLKIT_TESTS_DATA_DIR}/ccd-queries/" +) \ No newline at end of file diff --git a/cmake/recipes/finite_diff.cmake b/cmake/recipes/finite_diff.cmake index 35661aff8..bf2ee6ab6 100644 --- a/cmake/recipes/finite_diff.cmake +++ b/cmake/recipes/finite_diff.cmake @@ -7,7 +7,7 @@ endif() message(STATUS "Third-party: creating target 'finitediff::finitediff'") include(CPM) -CPMAddPackage("gh:zfergus/finite-diff@1.0.1") +CPMAddPackage("gh:zfergus/finite-diff@1.0.3") # Folder name for IDE set_target_properties(finitediff_finitediff PROPERTIES FOLDER "ThirdParty") \ No newline at end of file diff --git a/cmake/recipes/libigl.cmake b/cmake/recipes/libigl.cmake index 11046a192..681cee078 100644 --- a/cmake/recipes/libigl.cmake +++ b/cmake/recipes/libigl.cmake @@ -6,12 +6,13 @@ endif() message(STATUS "Third-party: creating target 'igl::core'") -set(LIBIGL_PREDICATES ON CACHE BOOL "Use exact predicates" FORCE) - include(eigen) include(CPM) -CPMAddPackage("gh:libigl/libigl#89267b4a80b1904de3f6f2812a2053e5e9332b7e") +CPMAddPackage( + URI "gh:libigl/libigl#89267b4a80b1904de3f6f2812a2053e5e9332b7e" + OPTIONS "LIBIGL_PREDICATES ON" +) # Folder name for IDE foreach(target_name IN ITEMS core predicates) diff --git a/cmake/recipes/scalable_ccd.cmake b/cmake/recipes/scalable_ccd.cmake index a6a41e170..5fea4ab23 100644 --- a/cmake/recipes/scalable_ccd.cmake +++ b/cmake/recipes/scalable_ccd.cmake @@ -6,10 +6,11 @@ endif() message(STATUS "Third-party: creating target 'scalable_ccd::scalable_ccd'") -set(SCALABLE_CCD_WITH_CUDA ${IPC_TOOLKIT_WITH_CUDA} CACHE BOOL "Enable CUDA CCD" FORCE) - include(CPM) -CPMAddPackage("gh:continuous-collision-detection/scalable-ccd#4fa806f533b19132e696a2dddeab16537025b5f9") +CPMAddPackage( + URI "gh:continuous-collision-detection/scalable-ccd#4fa806f533b19132e696a2dddeab16537025b5f9" + OPTIONS "SCALABLE_CCD_WITH_CUDA ${IPC_TOOLKIT_WITH_CUDA}" +) # Folder name for IDE set_target_properties(scalable_ccd PROPERTIES FOLDER "ThirdParty") \ No newline at end of file diff --git a/src/ipc/broad_phase/broad_phase.cpp b/src/ipc/broad_phase/broad_phase.cpp index c5589c53c..95e5674b0 100644 --- a/src/ipc/broad_phase/broad_phase.cpp +++ b/src/ipc/broad_phase/broad_phase.cpp @@ -11,12 +11,9 @@ void BroadPhase::build( Eigen::ConstRef faces, const double inflation_radius) { - assert(edges.size() == 0 || edges.cols() == 2); - assert(faces.size() == 0 || faces.cols() == 3); clear(); build_vertex_boxes(vertices, vertex_boxes, inflation_radius); - build_edge_boxes(vertex_boxes, edges, edge_boxes); - build_face_boxes(vertex_boxes, faces, face_boxes); + build(edges, faces); } void BroadPhase::build( @@ -26,11 +23,32 @@ void BroadPhase::build( Eigen::ConstRef faces, const double inflation_radius) { - assert(edges.size() == 0 || edges.cols() == 2); - assert(faces.size() == 0 || faces.cols() == 3); clear(); build_vertex_boxes( vertices_t0, vertices_t1, vertex_boxes, inflation_radius); + build(edges, faces); +} + +void BroadPhase::build( + const std::vector& _vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces) +{ + clear(); + + assert(&(this->vertex_boxes) != &_vertex_boxes); + this->vertex_boxes = _vertex_boxes; + + build(edges, faces); +} + +void BroadPhase::build( + Eigen::ConstRef edges, + Eigen::ConstRef faces) +{ + assert(vertex_boxes.size() > 0); + assert(edges.size() == 0 || edges.cols() == 2); + assert(faces.size() == 0 || faces.cols() == 3); build_edge_boxes(vertex_boxes, edges, edge_boxes); build_face_boxes(vertex_boxes, faces, face_boxes); } diff --git a/src/ipc/broad_phase/broad_phase.hpp b/src/ipc/broad_phase/broad_phase.hpp index 446e0f598..d45518f96 100644 --- a/src/ipc/broad_phase/broad_phase.hpp +++ b/src/ipc/broad_phase/broad_phase.hpp @@ -48,6 +48,15 @@ class BroadPhase { Eigen::ConstRef faces, const double inflation_radius = 0); + /// @brief Build the broad phase for collision detection. + /// @param vertex_boxes AABBs for the vertices. + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + virtual void build( + const std::vector& vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces); + /// @brief Clear any built data. virtual void clear(); @@ -91,6 +100,14 @@ class BroadPhase { default_can_vertices_collide; protected: + /// @brief Build the broad phase for collision detection. + /// @note Assumes the vertex_boxes have been built. + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + virtual void build( + Eigen::ConstRef edges, + Eigen::ConstRef faces); + virtual bool can_edge_vertex_collide(size_t ei, size_t vi) const; virtual bool can_edges_collide(size_t eai, size_t ebi) const; virtual bool can_face_vertex_collide(size_t fi, size_t vi) const; diff --git a/src/ipc/broad_phase/bvh.cpp b/src/ipc/broad_phase/bvh.cpp index 57b5df729..668333726 100644 --- a/src/ipc/broad_phase/bvh.cpp +++ b/src/ipc/broad_phase/bvh.cpp @@ -22,25 +22,10 @@ BVH::BVH() BVH::~BVH() = default; void BVH::build( - Eigen::ConstRef vertices, Eigen::ConstRef edges, - Eigen::ConstRef faces, - const double inflation_radius) + Eigen::ConstRef faces) { - BroadPhase::build(vertices, edges, faces, inflation_radius); - init_bvh(vertex_boxes, *vertex_bvh); - init_bvh(edge_boxes, *edge_bvh); - init_bvh(face_boxes, *face_bvh); -} - -void BVH::build( - Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1, - Eigen::ConstRef edges, - Eigen::ConstRef faces, - const double inflation_radius) -{ - BroadPhase::build(vertices_t0, vertices_t1, edges, faces, inflation_radius); + BroadPhase::build(edges, faces); // Build edge_boxes and face_boxes init_bvh(vertex_boxes, *vertex_bvh); init_bvh(edge_boxes, *edge_bvh); init_bvh(face_boxes, *face_bvh); @@ -74,9 +59,9 @@ void BVH::detect_candidates( const std::function& can_collide, std::vector& candidates) { - // O(n^2) or O(n^3) to build - // O(klog(n)) to do a single look up - // O(knlog(n)) to do all look ups + // O(n) or O(n⋅log(n)) to build + // O(k⋅log(n)) to do a single look up + // O(k⋅n⋅log(n)) to do all look ups tbb::enumerable_thread_specific> storage; diff --git a/src/ipc/broad_phase/bvh.hpp b/src/ipc/broad_phase/bvh.hpp index 0b19ec4de..4d97d358d 100644 --- a/src/ipc/broad_phase/bvh.hpp +++ b/src/ipc/broad_phase/bvh.hpp @@ -20,29 +20,7 @@ class BVH : public BroadPhase { /// @return The name of the broad phase method. std::string name() const override { return "BVH"; } - /// @brief Build the broad phase for static collision detection. - /// @param vertices Vertex positions - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @param inflation_radius Radius of inflation around all elements. - void build( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces, - const double inflation_radius = 0) override; - - /// @brief Build the broad phase for continuous collision detection. - /// @param vertices_t0 Starting vertices of the vertices. - /// @param vertices_t1 Ending vertices of the vertices. - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @param inflation_radius Radius of inflation around all elements. - void build( - Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1, - Eigen::ConstRef edges, - Eigen::ConstRef faces, - const double inflation_radius = 0) override; + using BroadPhase::build; /// @brief Clear any built data. void clear() override; @@ -78,6 +56,14 @@ class BVH : public BroadPhase { std::vector& candidates) const override; protected: + /// @brief Build the broad phase for collision detection. + /// @note Assumes the vertex_boxes have been built. + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + void build( + Eigen::ConstRef edges, + Eigen::ConstRef faces) override; + /// @brief Initialize a BVH from a set of boxes. /// @param[in] boxes Set of boxes to initialize the BVH with. /// @param[out] bvh The BVH to initialize. diff --git a/src/ipc/broad_phase/hash_grid.cpp b/src/ipc/broad_phase/hash_grid.cpp index ac0f68123..fbf7f11bb 100644 --- a/src/ipc/broad_phase/hash_grid.cpp +++ b/src/ipc/broad_phase/hash_grid.cpp @@ -18,46 +18,20 @@ using namespace std::placeholders; namespace ipc { void HashGrid::build( - Eigen::ConstRef vertices, Eigen::ConstRef edges, - Eigen::ConstRef faces, - const double inflation_radius) + Eigen::ConstRef faces) { - BroadPhase::build(vertices, edges, faces, inflation_radius); - // BroadPhase::build also calls clear() + BroadPhase::build(edges, faces); - ArrayMax3d mesh_min = vertices.colwise().minCoeff().array(); - ArrayMax3d mesh_max = vertices.colwise().maxCoeff().array(); - AABB::conservative_inflation(mesh_min, mesh_max, inflation_radius); + ArrayMax3d mesh_min = vertex_boxes[0].min; + ArrayMax3d mesh_max = vertex_boxes[0].max; + for (const auto& box : vertex_boxes) { + mesh_min = mesh_min.min(box.min); + mesh_max = mesh_max.max(box.max); + } const double cell_size = - suggest_good_voxel_size(vertices, edges, inflation_radius); - resize(mesh_min, mesh_max, cell_size); - - insert_boxes(); -} - -void HashGrid::build( - Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1, - Eigen::ConstRef edges, - Eigen::ConstRef faces, - const double inflation_radius) -{ - BroadPhase::build(vertices_t0, vertices_t1, edges, faces, inflation_radius); - // BroadPhase::build also calls clear() - - const ArrayMax3d mesh_min_t0 = vertices_t0.colwise().minCoeff(); - const ArrayMax3d mesh_max_t0 = vertices_t0.colwise().maxCoeff(); - const ArrayMax3d mesh_min_t1 = vertices_t1.colwise().minCoeff(); - const ArrayMax3d mesh_max_t1 = vertices_t1.colwise().maxCoeff(); - - ArrayMax3d mesh_min = mesh_min_t0.min(mesh_min_t1); - ArrayMax3d mesh_max = mesh_max_t0.max(mesh_max_t1); - AABB::conservative_inflation(mesh_min, mesh_max, inflation_radius); - - const double cell_size = suggest_good_voxel_size( - vertices_t0, vertices_t1, edges, inflation_radius); + suggest_good_voxel_size(edges.rows() > 0 ? edge_boxes : vertex_boxes); resize(mesh_min, mesh_max, cell_size); insert_boxes(); @@ -66,9 +40,9 @@ void HashGrid::build( void HashGrid::resize( Eigen::ConstRef domain_min, Eigen::ConstRef domain_max, - double cell_size) + const double cell_size) { - assert(cell_size != 0.0); + assert(cell_size > 0.0); assert(std::isfinite(cell_size)); m_domain_min = domain_min; diff --git a/src/ipc/broad_phase/hash_grid.hpp b/src/ipc/broad_phase/hash_grid.hpp index 823feabda..eb3c168bb 100644 --- a/src/ipc/broad_phase/hash_grid.hpp +++ b/src/ipc/broad_phase/hash_grid.hpp @@ -33,29 +33,7 @@ class HashGrid : public BroadPhase { /// @return The name of the broad phase method. std::string name() const override { return "HashGrid"; } - /// @brief Build the broad phase for static collision detection. - /// @param vertices Vertex positions - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @param inflation_radius Radius of inflation around all elements. - void build( - Eigen::ConstRef vertices, - Eigen::ConstRef edges, - Eigen::ConstRef faces, - double inflation_radius = 0) override; - - /// @brief Build the broad phase for continuous collision detection. - /// @param vertices_t0 Starting vertices of the vertices. - /// @param vertices_t1 Ending vertices of the vertices. - /// @param edges Collision mesh edges - /// @param faces Collision mesh faces - /// @param inflation_radius Radius of inflation around all elements. - void build( - Eigen::ConstRef vertices_t0, - Eigen::ConstRef vertices_t1, - Eigen::ConstRef edges, - Eigen::ConstRef faces, - double inflation_radius = 0) override; + using BroadPhase::build; /// @brief Clear the hash grid. void clear() override @@ -101,6 +79,14 @@ class HashGrid : public BroadPhase { const ArrayMax3d& domain_max() const { return m_domain_max; } protected: + /// @brief Build the broad phase for collision detection. + /// @note Assumes the vertex_boxes have been built. + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + void build( + Eigen::ConstRef edges, + Eigen::ConstRef faces) override; + void resize( Eigen::ConstRef domain_min, Eigen::ConstRef domain_max, diff --git a/src/ipc/broad_phase/spatial_hash.cpp b/src/ipc/broad_phase/spatial_hash.cpp index d4e44ff91..25154fc16 100644 --- a/src/ipc/broad_phase/spatial_hash.cpp +++ b/src/ipc/broad_phase/spatial_hash.cpp @@ -76,7 +76,9 @@ void SpatialHash::build( double inflation_radius, double voxel_size) { - build(vertices, vertices, edges, faces, inflation_radius, voxel_size); + clear(); + build_vertex_boxes(vertices, vertex_boxes, inflation_radius); + build(edges, faces, voxel_size); } void SpatialHash::build( @@ -87,28 +89,47 @@ void SpatialHash::build( double inflation_radius, double voxel_size) { - const size_t num_vertices = vertices_t0.rows(); - dim = vertices_t0.cols(); + clear(); + build_vertex_boxes( + vertices_t0, vertices_t1, vertex_boxes, inflation_radius); + build(edges, faces, voxel_size); +} - assert(vertices_t1.rows() == num_vertices && vertices_t1.cols() == dim); +void SpatialHash::build( + const std::vector& _vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + double voxel_size) +{ + // WARNING: Clear will reset vertex_boxes if this assert is triggered + assert(&(this->vertex_boxes) != &_vertex_boxes); + clear(); + this->vertex_boxes = _vertex_boxes; + build(edges, faces, voxel_size); +} - // also calls clear() - BroadPhase::build(vertices_t0, vertices_t1, edges, faces, inflation_radius); +void SpatialHash::build( + Eigen::ConstRef edges, + Eigen::ConstRef faces, + double voxel_size) +{ + const size_t num_vertices = vertex_boxes.size(); + assert(num_vertices > 0); + dim = vertex_boxes[0].min.size(); - built_in_radius = inflation_radius; + BroadPhase::build(edges, faces); if (voxel_size <= 0) { voxel_size = suggest_good_voxel_size( - vertices_t0, vertices_t1, edges, inflation_radius); + edges.rows() > 0 ? edge_boxes : vertex_boxes); } - left_bottom_corner = vertices_t0.colwise().minCoeff().cwiseMin( - vertices_t1.colwise().minCoeff()); - right_top_corner = vertices_t0.colwise().maxCoeff().cwiseMax( - vertices_t1.colwise().maxCoeff()); - - AABB::conservative_inflation( - left_bottom_corner, right_top_corner, inflation_radius); + left_bottom_corner = vertex_boxes[0].min; + right_top_corner = vertex_boxes[0].max; + for (const auto& box : vertex_boxes) { + left_bottom_corner = left_bottom_corner.min(box.min); + right_top_corner = right_top_corner.max(box.max); + } one_div_voxelSize = 1.0 / voxel_size; @@ -129,14 +150,10 @@ void SpatialHash::build( std::vector vertex_max_voxel_axis_index( num_vertices, Eigen::Array3i::Zero()); tbb::parallel_for(size_t(0), num_vertices, [&](size_t vi) { - ArrayMax3d v_min = vertices_t0.row(vi).cwiseMin(vertices_t1.row(vi)); - ArrayMax3d v_max = vertices_t0.row(vi).cwiseMax(vertices_t1.row(vi)); - AABB::conservative_inflation(v_min, v_max, inflation_radius); - vertex_min_voxel_axis_index[vi].head(dim) = - locate_voxel_axis_index(v_min); + locate_voxel_axis_index(vertex_boxes[vi].min); vertex_max_voxel_axis_index[vi].head(dim) = - locate_voxel_axis_index(v_max); + locate_voxel_axis_index(vertex_boxes[vi].max); }); // ------------------------------------------------------------------------ diff --git a/src/ipc/broad_phase/spatial_hash.hpp b/src/ipc/broad_phase/spatial_hash.hpp index 7b783d00f..933ec39f8 100644 --- a/src/ipc/broad_phase/spatial_hash.hpp +++ b/src/ipc/broad_phase/spatial_hash.hpp @@ -12,43 +12,6 @@ namespace ipc { /// @brief Spatial hash broad phase collision detection. class SpatialHash : public BroadPhase { -public: // data - /// @brief The left bottom corner of the world bounding box. - ArrayMax3d left_bottom_corner; - - /// @brief The right top corner of the world bounding box. - ArrayMax3d right_top_corner; - - /// @brief The number of voxels in each dimension. - ArrayMax3i voxel_count; - - /// @brief 1.0 / voxel_size - double one_div_voxelSize; - - /// @brief The number of voxels in the first two dimensions. - int voxel_count_0x1; - - // // The index of the first edge in voxel_occupancies - int edge_start_ind; - // // The index of the first triangle in voxel_occupancies - int tri_start_ind; - - /// @brief Map from voxel index to the primitive indices it contains. - unordered_map> voxel_to_primitives; - - /// @brief Map from point index to the voxel indices it occupies. - std::vector> point_to_voxels; - - /// @brief Map from edge index to the voxel indices it occupies. - std::vector> edge_to_voxels; - - /// @brief Map from face index to the voxel indices it occupies. - std::vector> face_to_voxels; - -protected: - int dim; - double built_in_radius; - public: // constructor SpatialHash() = default; @@ -79,7 +42,11 @@ class SpatialHash : public BroadPhase { /// @return The name of the broad phase method. std::string name() const override { return "SpatialHash"; } -public: // API + // ------------------------------------------------------------------------ + // BroadPhase::build() + + using BroadPhase::build; + void build( Eigen::ConstRef vertices, Eigen::ConstRef edges, @@ -101,6 +68,17 @@ class SpatialHash : public BroadPhase { /*voxel_size=*/-1); } + void build( + const std::vector& vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces) override + { + build(vertex_boxes, edges, faces, /*voxel_size=*/-1); + } + + // ------------------------------------------------------------------------ + // SpatialHash::build(..., voxel_size) + void build( Eigen::ConstRef vertices, Eigen::ConstRef edges, @@ -116,6 +94,12 @@ class SpatialHash : public BroadPhase { double inflation_radius, double voxel_size); + void build( + const std::vector& vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + double voxel_size); + void clear() override { BroadPhase::clear(); @@ -182,7 +166,47 @@ class SpatialHash : public BroadPhase { void detect_face_face_candidates( std::vector& candidates) const override; + // ======================================================================== + // Data + + /// @brief The left bottom corner of the world bounding box. + ArrayMax3d left_bottom_corner; + + /// @brief The right top corner of the world bounding box. + ArrayMax3d right_top_corner; + + /// @brief The number of voxels in each dimension. + ArrayMax3i voxel_count; + + /// @brief 1.0 / voxel_size + double one_div_voxelSize; + + /// @brief The number of voxels in the first two dimensions. + int voxel_count_0x1; + + // // The index of the first edge in voxel_occupancies + int edge_start_ind; + // // The index of the first triangle in voxel_occupancies + int tri_start_ind; + + /// @brief Map from voxel index to the primitive indices it contains. + unordered_map> voxel_to_primitives; + + /// @brief Map from point index to the voxel indices it occupies. + std::vector> point_to_voxels; + + /// @brief Map from edge index to the voxel indices it occupies. + std::vector> edge_to_voxels; + + /// @brief Map from face index to the voxel indices it occupies. + std::vector> face_to_voxels; + protected: // helper functions + void build( + Eigen::ConstRef edges, + Eigen::ConstRef faces, + double voxel_size); + void query_point_for_points(int vi, unordered_set& vert_inds) const; void query_point_for_edges(int vi, unordered_set& edge_inds) const; @@ -214,6 +238,8 @@ class SpatialHash : public BroadPhase { int voxel_axis_index_to_voxel_index(int ix, int iy, int iz) const; + int dim; + private: /// @brief Detect candidate collisions between type A and type B. /// @tparam Candidate Type of candidate collision. diff --git a/src/ipc/broad_phase/sweep_and_prune.cpp b/src/ipc/broad_phase/sweep_and_prune.cpp index 4f5ae6f97..2e5302903 100644 --- a/src/ipc/broad_phase/sweep_and_prune.cpp +++ b/src/ipc/broad_phase/sweep_and_prune.cpp @@ -54,6 +54,31 @@ void SweepAndPrune::build( scalable_ccd::build_face_boxes(boxes->vertices, faces, boxes->faces); } +void SweepAndPrune::build( + const std::vector& vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces) +{ + assert(edges.size() == 0 || edges.cols() == 2); + assert(faces.size() == 0 || faces.cols() == 3); + + clear(); + + // Convert from ipc::AABB to scalable_ccd::AABB (additional element_id) + boxes->vertices.resize(vertex_boxes.size()); + for (int i = 0; i < vertex_boxes.size(); ++i) { + boxes->vertices[i].min = vertex_boxes[i].min; + boxes->vertices[i].max = vertex_boxes[i].max; + for (int j = 0; j < 3; ++j) { + boxes->vertices[i].vertex_ids[j] = vertex_boxes[i].vertex_ids[j]; + } + boxes->vertices[i].element_id = i; + } + + scalable_ccd::build_edge_boxes(boxes->vertices, edges, boxes->edges); + scalable_ccd::build_face_boxes(boxes->vertices, faces, boxes->faces); +} + void SweepAndPrune::clear() { BroadPhase::clear(); diff --git a/src/ipc/broad_phase/sweep_and_prune.hpp b/src/ipc/broad_phase/sweep_and_prune.hpp index e157fbbcc..f996b7848 100644 --- a/src/ipc/broad_phase/sweep_and_prune.hpp +++ b/src/ipc/broad_phase/sweep_and_prune.hpp @@ -38,6 +38,15 @@ class SweepAndPrune : public BroadPhase { Eigen::ConstRef faces, double inflation_radius = 0) override; + /// @brief Build the broad phase from precomputed AABBs. + /// @param vertex_boxes Precomputed vertex AABBs + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + void build( + const std::vector& vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces) override; + /// @brief Clear any built data. void clear() override; diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp b/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp index d6f198643..eacb22175 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp @@ -66,6 +66,35 @@ void SweepAndTiniestQueue::build( scalable_ccd::cuda::build_face_boxes(boxes->vertices, faces, boxes->faces); } +void SweepAndTiniestQueue::build( + const std::vector& vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces) +{ + assert(edges.size() == 0 || edges.cols() == 2); + assert(faces.size() == 0 || faces.cols() == 3); + + clear(); + + // Convert from ipc::AABB to scalable_ccd::cuda::AABB + boxes->vertices.resize(vertex_boxes.size()); + for (int i = 0; i < vertex_boxes.size(); ++i) { + boxes->vertices[i].min = Scalar3( + vertex_boxes[i].min.x(), vertex_boxes[i].min.y(), + vertex_boxes[i].min.size() > 2 ? vertex_boxes[i].min.z() : 0); + boxes->vertices[i].max = Scalar3( + vertex_boxes[i].max.x(), vertex_boxes[i].max.y(), + vertex_boxes[i].max.size() > 2 ? vertex_boxes[i].max.z() : 0); + boxes->vertices[i].vertex_ids = int3( + vertex_boxes[i].vertex_ids[0], vertex_boxes[i].vertex_ids[1], + vertex_boxes[i].vertex_ids[2]); + boxes->vertices[i].element_id = i; + } + + scalable_ccd::cuda::build_edge_boxes(boxes->vertices, edges, boxes->edges); + scalable_ccd::cuda::build_face_boxes(boxes->vertices, faces, boxes->faces); +} + void SweepAndTiniestQueue::clear() { BroadPhase::clear(); diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp index 1d7e308ab..8f249b812 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp @@ -42,6 +42,15 @@ class SweepAndTiniestQueue : public BroadPhase { Eigen::ConstRef faces, double inflation_radius = 0) override; + /// @brief Build the broad phase from precomputed AABBs. + /// @param vertex_boxes Precomputed vertex AABBs + /// @param edges Collision mesh edges + /// @param faces Collision mesh faces + void build( + const std::vector& vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces) override; + /// @brief Clear any built data. void clear() override; diff --git a/src/ipc/broad_phase/voxel_size_heuristic.cpp b/src/ipc/broad_phase/voxel_size_heuristic.cpp index 9712c8a43..3f6455e1c 100644 --- a/src/ipc/broad_phase/voxel_size_heuristic.cpp +++ b/src/ipc/broad_phase/voxel_size_heuristic.cpp @@ -77,6 +77,27 @@ double suggest_good_voxel_size( return voxel_size; } +double suggest_good_voxel_size(const std::vector& boxes) +{ + assert(boxes.size() > 0); + + Eigen::VectorXd box_sizes(boxes.size()); + for (size_t i = 0; i < boxes.size(); ++i) { + box_sizes(i) = (boxes[i].max - boxes[i].min).maxCoeff(); + } + + double voxel_size; + igl::median(box_sizes, voxel_size); + + if (voxel_size <= 0) { + voxel_size = std::numeric_limits::max(); + } + assert(std::isfinite(voxel_size)); + + logger().trace("suggesting voxel size of {}", voxel_size); + return voxel_size; +} + double mean_edge_length( Eigen::ConstRef vertices_t0, Eigen::ConstRef vertices_t1, diff --git a/src/ipc/broad_phase/voxel_size_heuristic.hpp b/src/ipc/broad_phase/voxel_size_heuristic.hpp index a124ad39d..5bb3f1016 100644 --- a/src/ipc/broad_phase/voxel_size_heuristic.hpp +++ b/src/ipc/broad_phase/voxel_size_heuristic.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include namespace ipc { @@ -26,6 +26,11 @@ double suggest_good_voxel_size( Eigen::ConstRef edges, const double inflation_radius = 0); +/// @brief Suggest a good voxel size for the given mesh. +/// @param boxes The axis-aligned bounding boxes of the mesh. +/// @return The suggested voxel size. +double suggest_good_voxel_size(const std::vector& boxes); + /// @brief Compute the average edge length of a mesh. /// @param vertices_t0 The vertex positions of the mesh at time t0. /// @param vertices_t1 The vertex positions of the mesh at time t1. diff --git a/src/ipc/candidates/candidates.hpp b/src/ipc/candidates/candidates.hpp index f1170dc47..5c947fecf 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -44,13 +44,25 @@ class Candidates { const std::shared_ptr broad_phase = make_default_broad_phase()); + /// @brief Get the number of collision candidates. + /// @return The number of collision candidates. size_t size() const; + /// @brief Check if there are no collision candidates. + /// @return True if there are no collision candidates, false otherwise. bool empty() const; + /// @brief Clear all collision candidates. void clear(); + /// @brief Get a collision stencil by index. + /// @param i The index of the collision stencil. + /// @return A reference to the collision stencil. CollisionStencil& operator[](size_t i); + + /// @brief Get a collision stencil by index. + /// @param i The index of the collision stencil. + /// @return A const reference to the collision stencil. const CollisionStencil& operator[](size_t i) const; /// @brief Determine if the step is collision free from the set of candidates. @@ -113,6 +125,12 @@ class Candidates { const NarrowPhaseCCD& narrow_phase_ccd = DEFAULT_NARROW_PHASE_CCD) const; + /// @brief Write collision candidates to a file. + /// @param filename The name of the file to write to. + /// @param vertices The vertex positions. + /// @param edges The edge connectivity. + /// @param faces The face connectivity. + /// @return True if the write was successful, false otherwise. bool save_obj( const std::string& filename, Eigen::ConstRef vertices, diff --git a/src/ipc/ccd/narrow_phase_ccd.hpp b/src/ipc/ccd/narrow_phase_ccd.hpp index 04c44441b..3911f9954 100644 --- a/src/ipc/ccd/narrow_phase_ccd.hpp +++ b/src/ipc/ccd/narrow_phase_ccd.hpp @@ -11,6 +11,15 @@ class NarrowPhaseCCD { virtual ~NarrowPhaseCCD() = default; + /// @brief Perform narrow phase CCD between two points. + /// @param p0_t0 The starting position of the first point. + /// @param p1_t0 The starting position of the second point. + /// @param p0_t1 The ending position of the first point. + /// @param p1_t1 The ending position of the second point. + /// @param toi The time of impact. + /// @param min_distance The minimum distance between the two points. + /// @param tmax The maximum time to check for collision. + /// @return True if a collision was detected, false otherwise. virtual bool point_point_ccd( Eigen::ConstRef p0_t0, Eigen::ConstRef p1_t0, @@ -20,6 +29,17 @@ class NarrowPhaseCCD { const double min_distance = 0.0, const double tmax = 1.0) const = 0; + /// @brief Perform narrow phase CCD between a point and a linear edge. + /// @param p_t0 The starting position of the point. + /// @param e0_t0 The starting position of the first endpoint of the edge. + /// @param e1_t0 The starting position of the second endpoint of the edge. + /// @param p_t1 The ending position of the point. + /// @param e0_t1 The ending position of the first endpoint of the edge. + /// @param e1_t1 The ending position of the second endpoint of the edge. + /// @param toi The time of impact. + /// @param min_distance The minimum distance between the point and the edge. + /// @param tmax The maximum time to check for collision. + /// @return True if a collision was detected, false otherwise. virtual bool point_edge_ccd( Eigen::ConstRef p_t0, Eigen::ConstRef e0_t0, @@ -31,6 +51,19 @@ class NarrowPhaseCCD { const double min_distance = 0.0, const double tmax = 1.0) const = 0; + /// @brief Perform narrow phase CCD between a point and a linear triangle. + /// @param p_t0 The starting position of the point. + /// @param t0_t0 The starting position of the first vertex of the triangle. + /// @param t1_t0 The starting position of the second vertex of the triangle. + /// @param t2_t0 The starting position of the third vertex of the triangle. + /// @param p_t1 The ending position of the point. + /// @param t0_t1 The ending position of the first vertex of the triangle. + /// @param t1_t1 The ending position of the second vertex of the triangle. + /// @param t2_t1 The ending position of the third vertex of the triangle. + /// @param toi The time of impact. + /// @param min_distance The minimum distance between the point and the triangle. + /// @param tmax The maximum time to check for collision. + /// @return True if a collision was detected, false otherwise. virtual bool point_triangle_ccd( Eigen::ConstRef p_t0, Eigen::ConstRef t0_t0, @@ -44,6 +77,19 @@ class NarrowPhaseCCD { const double min_distance = 0.0, const double tmax = 1.0) const = 0; + /// @brief Perform narrow phase CCD between two linear edges. + /// @param ea0_t0 The starting position of the first edge's first endpoint. + /// @param ea1_t0 The starting position of the first edge's second endpoint. + /// @param eb0_t0 The starting position of the second edge's first endpoint. + /// @param eb1_t0 The starting position of the second edge's second endpoint. + /// @param ea0_t1 The ending position of the first edge's first endpoint. + /// @param ea1_t1 The ending position of the first edge's second endpoint. + /// @param eb0_t1 The ending position of the second edge's first endpoint. + /// @param eb1_t1 The ending position of the second edge's second endpoint. + /// @param toi The time of impact. + /// @param min_distance The minimum distance between the two edges. + /// @param tmax The maximum time to check for collision. + /// @return True if a collision was detected, false otherwise. virtual bool edge_edge_ccd( Eigen::ConstRef ea0_t0, Eigen::ConstRef ea1_t0, diff --git a/src/ipc/distance/edge_edge_mollifier.cpp b/src/ipc/distance/edge_edge_mollifier.cpp index 5be0b25aa..c106f1dc5 100644 --- a/src/ipc/distance/edge_edge_mollifier.cpp +++ b/src/ipc/distance/edge_edge_mollifier.cpp @@ -53,7 +53,7 @@ double edge_edge_mollifier_gradient(const double x, const double eps_x) { if (x < eps_x) { const double one_div_eps_x = 1 / eps_x; - return 2 * one_div_eps_x * (-one_div_eps_x * x + 1); + return 2 * one_div_eps_x * fma(-one_div_eps_x, x, 1); } else { return 0; } diff --git a/src/ipc/utils/eigen_ext.tpp b/src/ipc/utils/eigen_ext.tpp index 182b9c84a..2b3b3da89 100644 --- a/src/ipc/utils/eigen_ext.tpp +++ b/src/ipc/utils/eigen_ext.tpp @@ -31,8 +31,7 @@ project_to_pd( Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>> eigensolver(A); if (eigensolver.info() != Eigen::Success) { - logger().error("unable to project matrix onto positive definite cone"); - throw std::runtime_error( + log_and_throw_error( "unable to project matrix onto positive definite cone"); } // Check if all eigen values are positive. @@ -76,10 +75,8 @@ project_to_psd( Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>> eigensolver(A); if (eigensolver.info() != Eigen::Success) { - logger().error( + log_and_throw_error( "unable to project matrix onto positive semi-definite cone"); - throw std::runtime_error( - "unable to project matrix onto positive definite cone"); } // Check if all eigen values are zero or positive. // The eigenvalues are sorted in increasing order. diff --git a/src/ipc/utils/interval.hpp b/src/ipc/utils/interval.hpp index 1699b1f02..32f1cbc11 100644 --- a/src/ipc/utils/interval.hpp +++ b/src/ipc/utils/interval.hpp @@ -41,6 +41,13 @@ class Interval : public interval { this->SUP = y; } + static Interval empty() + { + return Interval( + std::numeric_limits::infinity(), + -std::numeric_limits::infinity()); + } + // friend std::ostream& operator<<(std::ostream& out, const Interval& i) // { // return out << "[" << i.INF << ", " << i.SUP << "]"; @@ -105,6 +112,7 @@ template struct ScalarBinaryOpTraits { typedef filib::Interval ReturnType; }; + } // namespace Eigen #endif \ No newline at end of file From d1b0f8489eac22803aa9e83e2687cd014cea3694 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 31 Aug 2025 21:44:24 -0400 Subject: [PATCH 2/4] Fix vertex box conversion in SweepAndTiniestQueue::build method --- .../broad_phase/sweep_and_tiniest_queue.cpp | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp b/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp index eacb22175..1fad63548 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp @@ -79,15 +79,20 @@ void SweepAndTiniestQueue::build( // Convert from ipc::AABB to scalable_ccd::cuda::AABB boxes->vertices.resize(vertex_boxes.size()); for (int i = 0; i < vertex_boxes.size(); ++i) { - boxes->vertices[i].min = Scalar3( - vertex_boxes[i].min.x(), vertex_boxes[i].min.y(), - vertex_boxes[i].min.size() > 2 ? vertex_boxes[i].min.z() : 0); - boxes->vertices[i].max = Scalar3( - vertex_boxes[i].max.x(), vertex_boxes[i].max.y(), - vertex_boxes[i].max.size() > 2 ? vertex_boxes[i].max.z() : 0); - boxes->vertices[i].vertex_ids = int3( - vertex_boxes[i].vertex_ids[0], vertex_boxes[i].vertex_ids[1], - vertex_boxes[i].vertex_ids[2]); + boxes->vertices[i].min.x = vertex_boxes[i].min.x(); + boxes->vertices[i].min.y = vertex_boxes[i].min.y(); + boxes->vertices[i].min.z = + vertex_boxes[i].min.size() > 2 ? vertex_boxes[i].min.z() : 0; + + boxes->vertices[i].max.x = vertex_boxes[i].max.x(); + boxes->vertices[i].max.y = vertex_boxes[i].max.y(); + boxes->vertices[i].max.z = + vertex_boxes[i].max.size() > 2 ? vertex_boxes[i].max.z() : 0; + + boxes->vertices[i].min.x = vertex_boxes[i].vertex_ids[0]; + boxes->vertices[i].min.y = vertex_boxes[i].vertex_ids[1]; + boxes->vertices[i].min.z = vertex_boxes[i].vertex_ids[2]; + boxes->vertices[i].element_id = i; } From 91c355b3b51dbd8951f9d9286c7fe4fd26382d79 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 31 Aug 2025 23:35:57 -0400 Subject: [PATCH 3/4] Test broad phase build from boxes --- src/ipc/broad_phase/sweep_and_prune.cpp | 15 ++++++++---- .../tests/broad_phase/test_broad_phase.cpp | 23 +++++++++++++++++++ 2 files changed, 33 insertions(+), 5 deletions(-) diff --git a/src/ipc/broad_phase/sweep_and_prune.cpp b/src/ipc/broad_phase/sweep_and_prune.cpp index 2e5302903..d7e186fc4 100644 --- a/src/ipc/broad_phase/sweep_and_prune.cpp +++ b/src/ipc/broad_phase/sweep_and_prune.cpp @@ -67,11 +67,16 @@ void SweepAndPrune::build( // Convert from ipc::AABB to scalable_ccd::AABB (additional element_id) boxes->vertices.resize(vertex_boxes.size()); for (int i = 0; i < vertex_boxes.size(); ++i) { - boxes->vertices[i].min = vertex_boxes[i].min; - boxes->vertices[i].max = vertex_boxes[i].max; - for (int j = 0; j < 3; ++j) { - boxes->vertices[i].vertex_ids[j] = vertex_boxes[i].vertex_ids[j]; - } + boxes->vertices[i].min = + vertex_boxes[i].min.cast(); + boxes->vertices[i].max = + vertex_boxes[i].max.cast(); + assert(vertex_boxes[i].vertex_ids[0] >= 0); + assert(vertex_boxes[i].vertex_ids[1] < 0); + assert(vertex_boxes[i].vertex_ids[2] < 0); + boxes->vertices[i].vertex_ids[0] = vertex_boxes[i].vertex_ids[0]; + boxes->vertices[i].vertex_ids[1] = -vertex_boxes[i].vertex_ids[0] - 1; + boxes->vertices[i].vertex_ids[2] = boxes->vertices[i].vertex_ids[1]; boxes->vertices[i].element_id = i; } diff --git a/tests/src/tests/broad_phase/test_broad_phase.cpp b/tests/src/tests/broad_phase/test_broad_phase.cpp index ed767231a..5b315e431 100644 --- a/tests/src/tests/broad_phase/test_broad_phase.cpp +++ b/tests/src/tests/broad_phase/test_broad_phase.cpp @@ -266,3 +266,26 @@ TEST_CASE("Cloth-Ball", "[ccd][broad_phase][cloth-ball][.]") mesh, V0, V1, broad_phase, true, (tests::DATA_DIR / "cloth_ball_bf_ccd_candidates.json").string()); } + +TEST_CASE("Broad phase build from boxes", "[broad_phase]") +{ + using namespace ipc; + + std::vector boxes(100); + for (int i = 0; i < boxes.size(); ++i) { + boxes[i].min = Eigen::Array3d(i * 0.6, 0, 0); + boxes[i].max = Eigen::Array3d(boxes[i].min.x() + 1.0, 0, 0); + boxes[i].vertex_ids = { i, -1, -1 }; + } + REQUIRE(boxes[0].intersects(boxes[1])); + REQUIRE(!boxes[0].intersects(boxes[2])); + + const auto broad_phase = GENERATE(tests::BroadPhaseGenerator::create()); + broad_phase->build(boxes, Eigen::MatrixXi(), Eigen::MatrixXi()); + + std::vector candidates; + broad_phase->detect_vertex_vertex_candidates(candidates); + + CAPTURE(broad_phase->name()); + CHECK(candidates.size() == boxes.size() - 1); +} \ No newline at end of file From ec5a059575763387505236dea7c0e7904c3f521b Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 31 Aug 2025 23:43:21 -0400 Subject: [PATCH 4/4] Fix initialization of vertex_ids in broad phase box test --- tests/src/tests/broad_phase/test_broad_phase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/src/tests/broad_phase/test_broad_phase.cpp b/tests/src/tests/broad_phase/test_broad_phase.cpp index 5b315e431..25ac29132 100644 --- a/tests/src/tests/broad_phase/test_broad_phase.cpp +++ b/tests/src/tests/broad_phase/test_broad_phase.cpp @@ -275,7 +275,7 @@ TEST_CASE("Broad phase build from boxes", "[broad_phase]") for (int i = 0; i < boxes.size(); ++i) { boxes[i].min = Eigen::Array3d(i * 0.6, 0, 0); boxes[i].max = Eigen::Array3d(boxes[i].min.x() + 1.0, 0, 0); - boxes[i].vertex_ids = { i, -1, -1 }; + boxes[i].vertex_ids = { { i, -1, -1 } }; } REQUIRE(boxes[0].intersects(boxes[1])); REQUIRE(!boxes[0].intersects(boxes[2]));