From 464a723a226065b3ad5147b7be16c48895347bd3 Mon Sep 17 00:00:00 2001 From: Ian Su Date: Tue, 4 Aug 2026 20:17:59 -0400 Subject: [PATCH 1/4] initial commit --- app/main.cpp | 2 +- src/tight_inclusion/ccd.cpp | 8 + src/tight_inclusion/ccd.hpp | 9 +- src/tight_inclusion/interval_root_finder.cpp | 147 +++++++++++++++++++ src/tight_inclusion/interval_root_finder.hpp | 111 ++++++++++++++ 5 files changed, 272 insertions(+), 5 deletions(-) diff --git a/app/main.cpp b/app/main.cpp index 9059595..3a513f0 100755 --- a/app/main.cpp +++ b/app/main.cpp @@ -57,7 +57,7 @@ void check_sample_queries( constexpr double t_max = 1; constexpr bool no_zero_toi = false; constexpr CCDRootFindingMethod ccd_method = - CCDRootFindingMethod::BREADTH_FIRST_SEARCH; + CCDRootFindingMethod::BUCKET_DEPTH_FIRST_SEARCH; int total_positives = 0; int total_false_positives = 0; diff --git a/src/tight_inclusion/ccd.cpp b/src/tight_inclusion/ccd.cpp index a431d19..9f1cf04 100644 --- a/src/tight_inclusion/ccd.cpp +++ b/src/tight_inclusion/ccd.cpp @@ -207,6 +207,14 @@ namespace ticcd { a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, tol, tolerance, err, ms, t_max, max_itr, toi, output_tolerance); break; + case CCDRootFindingMethod::BUCKET_DEPTH_FIRST_SEARCH: + assert(t_max >= 0 && t_max <= 1); + tmp_is_impacting = + interval_root_finder_bucket_DFS( + a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, tol, + tolerance, err, ms, t_max, max_itr, toi, + output_tolerance); + break; } assert(!tmp_is_impacting || toi >= 0); diff --git a/src/tight_inclusion/ccd.hpp b/src/tight_inclusion/ccd.hpp index 69a6475..e71378c 100644 --- a/src/tight_inclusion/ccd.hpp +++ b/src/tight_inclusion/ccd.hpp @@ -12,6 +12,7 @@ namespace ticcd { enum class CCDRootFindingMethod { DEPTH_FIRST_SEARCH, BREADTH_FIRST_SEARCH, + BUCKET_DEPTH_FIRST_SEARCH, }; /// @brief This function can give you the answer of continuous collision detection with minimum @@ -55,7 +56,7 @@ namespace ticcd { Scalar &output_tolerance, bool no_zero_toi = DEFAULT_NO_ZERO_TOI, const CCDRootFindingMethod ccd_method = - CCDRootFindingMethod::BREADTH_FIRST_SEARCH); + CCDRootFindingMethod::BUCKET_DEPTH_FIRST_SEARCH); /// This function can give you the answer of continuous collision detection with minimum /// separation, and the earliest collision time if collision happens. @@ -98,7 +99,7 @@ namespace ticcd { Scalar &output_tolerance, bool no_zero_toi = DEFAULT_NO_ZERO_TOI, const CCDRootFindingMethod ccd_method = - CCDRootFindingMethod::BREADTH_FIRST_SEARCH); + CCDRootFindingMethod::BUCKET_DEPTH_FIRST_SEARCH); Array3 compute_vertex_face_tolerances( const Vector3 &v_t0, @@ -144,7 +145,7 @@ namespace ticcd { double &output_tolerance, bool no_zero_toi = DEFAULT_NO_ZERO_TOI, const CCDRootFindingMethod ccd_method = - CCDRootFindingMethod::BREADTH_FIRST_SEARCH); + CCDRootFindingMethod::BUCKET_DEPTH_FIRST_SEARCH); bool vertexFaceCCD( const Eigen::Vector3d &v_t0, @@ -164,6 +165,6 @@ namespace ticcd { double &output_tolerance, bool no_zero_toi = DEFAULT_NO_ZERO_TOI, const CCDRootFindingMethod ccd_method = - CCDRootFindingMethod::BREADTH_FIRST_SEARCH); + CCDRootFindingMethod::BUCKET_DEPTH_FIRST_SEARCH); #endif } // namespace ticcd diff --git a/src/tight_inclusion/interval_root_finder.cpp b/src/tight_inclusion/interval_root_finder.cpp index 98501a2..fd198f9 100644 --- a/src/tight_inclusion/interval_root_finder.cpp +++ b/src/tight_inclusion/interval_root_finder.cpp @@ -5,6 +5,7 @@ #include #include +#include #include namespace ticcd { @@ -571,6 +572,100 @@ namespace ticcd { output_tolerance); } + // Based on narrowphase benchmark, >95% of CPU time is dominated by queries with + // deep traversal level (10+). However, compared to u and v the refinement level + // of time is low. Thus instead of BFS we choose DFS with dedicate traversal + // stack for each t lower bound. + template + bool interval_root_finder_bucket_DFS( + const Vector3 &a_t0, + const Vector3 &b_t0, + const Vector3 &c_t0, + const Vector3 &d_t0, + const Vector3 &a_t1, + const Vector3 &b_t1, + const Vector3 &c_t1, + const Vector3 &d_t1, + const Array3 &tolerances, + Scalar co_domain_tolerance, + const Array3 &err, + Scalar minimum_separation, + Scalar max_time, + long max_iterations, + Scalar &toi, + Scalar &output_tolerance) + { + output_tolerance = co_domain_tolerance; + toi = std::numeric_limits::infinity(); + + Interval zero_to_one(NumCCD(0, 0), NumCCD(1, 0)); + Interval3 initial = {{zero_to_one, zero_to_one, zero_to_one}}; + + // Each exact t.lower owns a LIFO traversal stack. Processing the smallest + // key first preserves chronological TOI order while exploring u/v deeply. + std::map> stacks; + stacks[initial[0].lower].push_back(initial); + + long iteration_count = 0; + while (!stacks.empty()) { + auto bucket = stacks.begin(); + std::vector &stack = bucket->second; + Interval3 current = std::move(stack.back()); + stack.pop_back(); + if (stack.empty()) { + stacks.erase(bucket); + } + + ++iteration_count; + // True if L1 distance function eval resides completely inside zero interval. + bool bbox_in_eps; + Array3 true_tolerance; + // True if L1 distance function eval intersects with zero interval. + bool origin_in_bbox = + origin_in_function_bounding_box_vector( + current, a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, + err, bbox_in_eps, minimum_separation, &true_tolerance); + if (!origin_in_bbox) { + continue; + } + + Array3 widths = ticcd::width(current); + bool parameter_tolerance_reached = (widths <= tolerances).all(); + bool co_domain_tolerance_reached = + (true_tolerance <= co_domain_tolerance).all(); + if (parameter_tolerance_reached || co_domain_tolerance_reached + || bbox_in_eps) { + toi = current[0].lower.value(); + return true; + } + + // The current box has the smallest pending t.lower, so it is the earliest + // conservative fallback when the iteration cap is reached. + if (max_iterations > 0 && iteration_count > max_iterations) { + toi = current[0].lower.value(); + output_tolerance = std::max( + {true_tolerance[0], true_tolerance[1], true_tolerance[2], + co_domain_tolerance}); + return true; + } + + int split_index = find_next_split(widths, tolerances); + auto push = [&stacks](const Interval3 &child) { + stacks[child[0].lower].push_back(child); + }; + if (split_and_push( + current, split_index, push, is_vertex_face, max_time)) { + toi = current[0].lower.value(); + output_tolerance = std::max( + {true_tolerance[0], true_tolerance[1], true_tolerance[2], + co_domain_tolerance}); + return true; + } + } + + return false; + } + void print_times() { logger().trace("[time] origin predicates, {}", time_predicates); @@ -708,6 +803,56 @@ namespace ticcd { output_tolerance); } + bool edge_edge_interval_root_finder_bucket_DFS( + const Vector3 &ea0_t0, + const Vector3 &ea1_t0, + const Vector3 &eb0_t0, + const Vector3 &eb1_t0, + const Vector3 &ea0_t1, + const Vector3 &ea1_t1, + const Vector3 &eb0_t1, + const Vector3 &eb1_t1, + const Array3 &tol, + const Scalar co_domain_tolerance, + // this is the maximum error on each axis when calculating the vertices, err, aka, filter + const Array3 &err, + const Scalar ms, + const Scalar max_time, + const long max_itr, + Scalar &toi, + Scalar &output_tolerance) + { + return interval_root_finder_bucket_DFS( + ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, tol, + co_domain_tolerance, err, ms, max_time, max_itr, toi, + output_tolerance); + } + + bool vertex_face_interval_root_finder_bucket_DFS( + const Vector3 &v_t0, + const Vector3 &f0_t0, + const Vector3 &f1_t0, + const Vector3 &f2_t0, + const Vector3 &v_t1, + const Vector3 &f0_t1, + const Vector3 &f1_t1, + const Vector3 &f2_t1, + const Array3 &tol, + const Scalar co_domain_tolerance, + // this is the maximum error on each axis when calculating the vertices, err, aka, filter + const Array3 &err, + const Scalar ms, + const Scalar max_time, + const long max_itr, + Scalar &toi, + Scalar &output_tolerance) + { + return interval_root_finder_bucket_DFS( + v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1, tol, + co_domain_tolerance, err, ms, max_time, max_itr, toi, + output_tolerance); + } + // ------------------------------------------------------------------------ // Template instantiation // ------------------------------------------------------------------------ @@ -717,6 +862,8 @@ namespace ticcd { template bool interval_root_finder_DFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Array3 &,const Scalar,Scalar &); template bool interval_root_finder_BFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Scalar,const Array3 &,const Scalar,const Scalar,const long,Scalar &,Scalar &); template bool interval_root_finder_BFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Scalar,const Array3 &,const Scalar,const Scalar,const long,Scalar &,Scalar &); + template bool interval_root_finder_bucket_DFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Scalar,const Array3 &,const Scalar,const Scalar,const long,Scalar &,Scalar &); + template bool interval_root_finder_bucket_DFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Scalar,const Array3 &,const Scalar,const Scalar,const long,Scalar &,Scalar &); // clang-format on } // namespace ticcd diff --git a/src/tight_inclusion/interval_root_finder.hpp b/src/tight_inclusion/interval_root_finder.hpp index 8e39552..644f9e5 100755 --- a/src/tight_inclusion/interval_root_finder.hpp +++ b/src/tight_inclusion/interval_root_finder.hpp @@ -217,6 +217,117 @@ namespace ticcd { Scalar &toi, Scalar &output_tolerance); + /// @brief Perform interval root finding for CCD using bucket DFS. + /// @param[in] a_t0 Vertex a at t=0 + /// @param[in] b_t0 Vertex b at t=0 + /// @param[in] c_t0 Vertex c at t=0 + /// @param[in] d_t0 Vertex d at t=0 + /// @param[in] a_t1 Vertex a at t=1 + /// @param[in] b_t1 Vertex b at t=1 + /// @param[in] c_t1 Vertex c at t=1 + /// @param[in] d_t1 Vertex d at t=1 + /// @param[in] tol The tolerance of the interval. + /// @param[in] co_domain_tolerance The tolerance of the co-domain. + /// @param[in] err The maximum error on each axis when calculating the vertices, err, aka, filter. + /// @param[in] ms The minimum separation. + /// @param[in] max_time The maximum time to check. + /// @param[in] max_itr The maximum number of iterations. + /// @param[out] toi The time of impact. + /// @param[out] output_tolerance The resulting tolerance. + /// @tparam is_vertex_face Whether to check vertex-face or edge-edge collision. + /// @return True if there is a root (collision), false otherwise. + template + bool interval_root_finder_bucket_DFS( + const Vector3 &a_t0, + const Vector3 &b_t0, + const Vector3 &c_t0, + const Vector3 &d_t0, + const Vector3 &a_t1, + const Vector3 &b_t1, + const Vector3 &c_t1, + const Vector3 &d_t1, + const Array3 &tol, + const Scalar co_domain_tolerance, + const Array3 &err, + const Scalar ms, + const Scalar max_time, + const long max_itr, + Scalar &toi, + Scalar &output_tolerance); + + /// @brief Perform interval root finding for edge-edge CCD using bucket DFS. + /// @param[in] ea0_t0 The start position of the first vertex of the first edge. + /// @param[in] ea1_t0 The start position of the second vertex of the first edge. + /// @param[in] eb0_t0 The start position of the first vertex of the second edge. + /// @param[in] eb1_t0 The start position of the second vertex of the second edge. + /// @param[in] ea0_t1 The end position of the first vertex of the first edge. + /// @param[in] ea1_t1 The end position of the second vertex of the first edge. + /// @param[in] eb0_t1 The end position of the first vertex of the second edge. + /// @param[in] eb1_t1 The end position of the second vertex of the second edge. + /// @param[in] tol The tolerance of the interval. + /// @param[in] co_domain_tolerance The tolerance of the co-domain. + /// @param[in] err The maximum error on each axis when calculating the vertices, err, aka, filter. + /// @param[in] ms The minimum separation. + /// @param[in] max_time The maximum time to check. + /// @param[in] max_itr The maximum number of iterations. + /// @param[out] toi The time of impact. + /// @param[out] output_tolerance The resulting tolerance. + /// @return True if there is a root (collision), false otherwise. + bool edge_edge_interval_root_finder_bucket_DFS( + const Vector3 &ea0_t0, + const Vector3 &ea1_t0, + const Vector3 &eb0_t0, + const Vector3 &eb1_t0, + const Vector3 &ea0_t1, + const Vector3 &ea1_t1, + const Vector3 &eb0_t1, + const Vector3 &eb1_t1, + const Array3 &tol, + const Scalar co_domain_tolerance, + const Array3 &err, + const Scalar ms, + const Scalar max_time, + const long max_itr, + Scalar &toi, + Scalar &output_tolerance); + + /// @brief Perform interval root finding for vertex-face CCD using bucket DFS. + /// @param[in] v_t0 The start position of the vertex. + /// @param[in] f0_t0 The start position of the first vertex of the face. + /// @param[in] f1_t0 The start position of the second vertex of the face. + /// @param[in] f2_t0 The start position of the third vertex of the face. + /// @param[in] v_t1 The end position of the vertex. + /// @param[in] f0_t1 The end position of the first vertex of the face. + /// @param[in] f1_t1 The end position of the second vertex of the face. + /// @param[in] f2_t1 The end position of the third vertex of the face. + /// @param[in] tol The tolerance of the interval. + /// @param[in] co_domain_tolerance The tolerance of the co-domain. + /// @param[in] err The maximum error on each axis when calculating the vertices, err, aka, filter. + /// @param[in] ms The minimum separation. + /// @param[in] max_time The maximum time to check. + /// @param[in] max_itr The maximum number of iterations. + /// @param[out] toi The time of impact. + /// @param[out] output_tolerance The resulting tolerance. + /// @return True if there is a root (collision), false otherwise. + bool vertex_face_interval_root_finder_bucket_DFS( + const Vector3 &v_t0, + const Vector3 &f0_t0, + const Vector3 &f1_t0, + const Vector3 &f2_t0, + const Vector3 &v_t1, + const Vector3 &f0_t1, + const Vector3 &f1_t1, + const Vector3 &f2_t1, + const Array3 &tol, + const Scalar co_domain_tolerance, + // this is the maximum error on each axis when calculating the vertices, err, aka, filter + const Array3 &err, + const Scalar ms, + const Scalar max_time, + const long max_itr, + Scalar &toi, + Scalar &output_tolerance); + // calculate the sign of f. dim is the dimension we are evaluating. template T function_f_ee( From e0eb65831da5eec8dffbe333eb38f9619c3c9c02 Mon Sep 17 00:00:00 2001 From: Ian Su Date: Tue, 4 Aug 2026 20:33:43 -0400 Subject: [PATCH 2/4] bump cli11 version for newer CI runner --- cmake/recipes/cli11.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/recipes/cli11.cmake b/cmake/recipes/cli11.cmake index 38d91e2..2339e06 100644 --- a/cmake/recipes/cli11.cmake +++ b/cmake/recipes/cli11.cmake @@ -16,4 +16,4 @@ endif() message(STATUS "Third-party: creating target 'CLI11::CLI11'") include(CPM) -CPMAddPackage("gh:CLIUtils/CLI11@2.3.2") \ No newline at end of file +CPMAddPackage("gh:CLIUtils/CLI11@2.6.2") From e3d280fa3b26a35bb530acaa99393f25af079895 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 5 Aug 2026 01:27:53 -0500 Subject: [PATCH 3/4] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- src/tight_inclusion/interval_root_finder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tight_inclusion/interval_root_finder.cpp b/src/tight_inclusion/interval_root_finder.cpp index fd198f9..fd08b28 100644 --- a/src/tight_inclusion/interval_root_finder.cpp +++ b/src/tight_inclusion/interval_root_finder.cpp @@ -574,7 +574,7 @@ namespace ticcd { // Based on narrowphase benchmark, >95% of CPU time is dominated by queries with // deep traversal level (10+). However, compared to u and v the refinement level - // of time is low. Thus instead of BFS we choose DFS with dedicate traversal + // of time is low. Thus instead of BFS we choose DFS with dedicated traversal // stack for each t lower bound. template bool interval_root_finder_bucket_DFS( From 6f0d5b701a0b191d17c30419fed8c6427a3607c1 Mon Sep 17 00:00:00 2001 From: Ian Su Date: Wed, 5 Aug 2026 20:20:09 -0400 Subject: [PATCH 4/4] Reduce heap allocation --- src/tight_inclusion/interval_root_finder.cpp | 27 ++++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/src/tight_inclusion/interval_root_finder.cpp b/src/tight_inclusion/interval_root_finder.cpp index fd08b28..7d3d0c0 100644 --- a/src/tight_inclusion/interval_root_finder.cpp +++ b/src/tight_inclusion/interval_root_finder.cpp @@ -604,17 +604,20 @@ namespace ticcd { // Each exact t.lower owns a LIFO traversal stack. Processing the smallest // key first preserves chronological TOI order while exploring u/v deeply. std::map> stacks; - stacks[initial[0].lower].push_back(initial); + + Interval3 current = initial; + bool has_current = true; long iteration_count = 0; - while (!stacks.empty()) { - auto bucket = stacks.begin(); - std::vector &stack = bucket->second; - Interval3 current = std::move(stack.back()); - stack.pop_back(); - if (stack.empty()) { - stacks.erase(bucket); + while (has_current || !stacks.empty()) { + auto bucket = stacks.end(); + if (!has_current) { + bucket = stacks.begin(); + std::vector &stack = bucket->second; + current = std::move(stack.back()); + stack.pop_back(); } + has_current = false; ++iteration_count; // True if L1 distance function eval resides completely inside zero interval. @@ -626,6 +629,9 @@ namespace ticcd { current, a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, err, bbox_in_eps, minimum_separation, &true_tolerance); if (!origin_in_bbox) { + if (bucket != stacks.end() && bucket->second.empty()) { + stacks.erase(bucket); + } continue; } @@ -661,6 +667,11 @@ namespace ticcd { co_domain_tolerance}); return true; } + // Keep an emptied bucket alive through evaluation and splitting so + // children with the same t.lower reuse its vector allocation. + if (bucket != stacks.end() && bucket->second.empty()) { + stacks.erase(bucket); + } } return false;