Bucket DFS

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;

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")
+CPMAddPackage("gh:CLIUtils/CLI11@2.6.2")

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<is_vertex_face>(
+                        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);
 

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

src/tight_inclusion/interval_root_finder.cpp

@@ -5,6 +5,7 @@
 #include <tight_inclusion/avx.hpp>
 #include <tight_inclusion/logger.hpp>
 
+#include <map>
 #include <queue>
 
 namespace ticcd {
@@ -571,6 +572,111 @@ 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 dedicated traversal
+    // stack for each t lower bound.
+    template <bool is_vertex_face>
+    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<Scalar>::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<NumCCD, std::vector<Interval3>> stacks;
+
+        Interval3 current = initial;
+        bool has_current = true;
+
+        long iteration_count = 0;
+        while (has_current || !stacks.empty()) {
+            auto bucket = stacks.end();
+            if (!has_current) {
+                bucket = stacks.begin();
+                std::vector<Interval3> &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.
+            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<is_vertex_face>(
+                    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;
+            }
+
+            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;
+            }
+            // 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;
+    }
+
     void print_times()
     {
         logger().trace("[time] origin predicates, {}", time_predicates);
@@ -708,6 +814,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<false>(
+            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<true>(
+            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 +873,8 @@ namespace ticcd {
     template bool interval_root_finder_DFS<true>(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<false>(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<true>(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<false>(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<true>(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

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 is_vertex_face>
+    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 <typename T>
     T function_f_ee(