Check distance when no motion in CCD (#71)

Author: zfergus

src/ipc/ccd/ccd.cpp

@@ -26,6 +26,22 @@ namespace {
         assert(v.size() == 2 || v.size() == 3);
         return v.size() == 2 ? Eigen::Vector3d(v.x(), v.y(), 0) : v.head<3>();
     }
+
+    inline bool check_initial_distance(
+        const double initial_distance, const double min_distance, double& toi)
+    {
+        if (initial_distance > min_distance) {
+            return false;
+        }
+
+        logger().warn(
+            "Initial distance {} ≤ d_min={}, returning toi=0!",
+            initial_distance, min_distance);
+
+        toi = 0; // Initially touching
+
+        return true;
+    }
 } // namespace
 
 /// @brief Scale the distance tolerance to be at most this fraction of the initial distance.
@@ -49,11 +65,7 @@ bool ccd_strategy(
     const double conservative_rescaling,
     double& toi)
 {
-    if (initial_distance <= min_distance) {
-        logger().warn(
-            "Initial distance {} ≤ d_min={}, returning toi=0!",
-            initial_distance, min_distance);
-        toi = 0;
+    if (check_initial_distance(initial_distance, min_distance, toi)) {
         return true;
     }
 
@@ -111,12 +123,12 @@ bool point_point_ccd_3D(
 {
     assert(tmax >= 0 && tmax <= 1.0);
 
-    if (p0_t0 == p0_t1 && p1_t0 == p1_t1) {
-        return false; // No motion
-    }
-
     const double initial_distance = sqrt(point_point_distance(p0_t0, p1_t0));
 
+    if (p0_t0 == p0_t1 && p1_t0 == p1_t1) { // No motion
+        return check_initial_distance(initial_distance, min_distance, toi);
+    }
+
     const double adjusted_tolerance = std::min(
         INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);
 
@@ -182,13 +194,13 @@ bool point_edge_ccd_3D(
 {
     assert(tmax >= 0 && tmax <= 1.0);
 
-    if (p_t0 == p_t1 && e0_t0 == e0_t1 && e1_t0 == e1_t1) {
-        return false; // No motion
-    }
-
     const double initial_distance =
         sqrt(point_edge_distance(p_t0, e0_t0, e1_t0));
 
+    if (p_t0 == p_t1 && e0_t0 == e0_t1 && e1_t0 == e1_t1) { // No motion
+        return check_initial_distance(initial_distance, min_distance, toi);
+    }
+
     const double adjusted_tolerance = std::min(
         INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);
 
@@ -244,15 +256,6 @@ bool point_edge_ccd(
     assert(p_t1.size() == dim);
     assert(e0_t0.size() == dim && e1_t0.size() == dim);
     assert(e0_t1.size() == dim && e1_t1.size() == dim);
-
-#ifndef IPC_TOOLKIT_WITH_CORRECT_CCD
-    if (dim == 2) {
-        return inexact_point_edge_ccd_2D(
-            p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, toi,
-            conservative_rescaling);
-    }
-#endif
-
     return point_edge_ccd_3D(
         to_3D(p_t0), to_3D(e0_t0), to_3D(e1_t0), to_3D(p_t1), to_3D(e0_t1),
         to_3D(e1_t1), toi, min_distance, tmax, tolerance, max_iterations,
@@ -277,14 +280,14 @@ bool edge_edge_ccd(
 {
     assert(tmax >= 0 && tmax <= 1.0);
 
-    if (ea0_t0 == ea0_t1 && ea1_t0 == ea1_t1 && eb0_t0 == eb0_t1
-        && eb1_t0 == eb1_t1) {
-        return false; // No motion
-    }
-
     const double initial_distance =
         sqrt(edge_edge_distance(ea0_t0, ea1_t0, eb0_t0, eb1_t0));
 
+    if (ea0_t0 == ea0_t1 && ea1_t0 == ea1_t1 && eb0_t0 == eb0_t1
+        && eb1_t0 == eb1_t1) { // No motion
+        return check_initial_distance(initial_distance, min_distance, toi);
+    }
+
     const double adjusted_tolerance = std::min(
         INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);
 
@@ -340,13 +343,14 @@ bool point_triangle_ccd(
 {
     assert(tmax >= 0 && tmax <= 1.0);
 
-    if (p_t0 == p_t1 && t0_t0 == t0_t1 && t1_t0 == t1_t1 && t2_t0 == t2_t1) {
-        return false; // No motion
-    }
-
     const double initial_distance =
         sqrt(point_triangle_distance(p_t0, t0_t0, t1_t0, t2_t0));
 
+    if (p_t0 == p_t1 && t0_t0 == t0_t1 && t1_t0 == t1_t1 && t2_t0 == t2_t1) {
+        // No motion
+        return check_initial_distance(initial_distance, min_distance, toi);
+    }
+
     const double adjusted_tolerance = std::min(
         INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);
 

src/ipc/ccd/inexact_point_edge.cpp

@@ -1,3 +1,10 @@
+//
+// NOTE: This method is provided for reference comparison and is not utilized by
+// the high-level functionality. In compairson to Tight Inclusion CCD, this CCD
+// method is not provably conservative and so can potentially produce false
+// negatives (i.e., miss collisions) due to floating-point rounding error.
+//
+
 #include "inexact_point_edge.hpp"
 
 #include <ipc/distance/distance_type.hpp>

src/ipc/ccd/inexact_point_edge.hpp

@@ -1,3 +1,10 @@
+//
+// NOTE: This method is provided for reference comparison and is not utilized by
+// the high-level functionality. In compairson to Tight Inclusion CCD, this CCD
+// method is not provably conservative and so can potentially produce false
+// negatives (i.e., miss collisions) due to floating-point rounding error.
+//
+
 #pragma once
 
 #include <Eigen/Core>

tests/src/tests/ccd/test_ccd_benchmark.cpp

@@ -256,30 +256,37 @@ TEST_CASE(
 // TEST_CASE("Failing Benchmark Cases", "[ccd]")
 // {
 //     using Matrix8x3 = Eigen::Matrix<double, 8, 3, Eigen::RowMajor>;
-
-//     const static std::vector<std::string> paths;
-//     const static std::vector<std::vector<int>> qids;
-
+//
+//     const static std::vector<std::filesystem::path> paths = { {
+//         tests::NEW_CCD_BENCHMARK_DIR / "n-body-simulation/queries/42ee.csv",
+//     } };
+//     const static std::vector<std::vector<int>> qids { {
+//         2647,
+//         2648,
+//     } };
+//
 //     for (int i = 0; i < paths.size(); i++) {
 //         const std::vector<ccd_io::CCDQuery> queries =
 //         ccd_io::read_ccd_queries(
-//             std::string(CCD_IO_SAMPLE_QUERIES_DIR) + paths[i]);
+//             paths[i].string(),
+//             paths[i].parent_path().parent_path() / "mma_bool"
+//                 / (paths[i].stem().string() + "_mma_bool.json"));
 //         for (const auto& qi : qids[i]) {
 //             Eigen::Map<const Matrix8x3> V(&queries[qi].vertices[0][0]);
 //             const bool expected_result = queries[qi].ground_truth;
-
+//
 //             bool result;
 //             double toi;
-//             // if (subfolder == "edge-edge") {
-//             //     result = edge_edge_ccd(
-//             //         V.row(0), V.row(1), V.row(2), V.row(3), V.row(4),
-//             //         V.row(5), V.row(6), V.row(7), toi);
-//             // } else {
-//             result = additive_ccd::point_triangle_ccd(
-//                 V.row(0), V.row(1), V.row(2), V.row(3), V.row(4), V.row(5),
-//                 V.row(6), V.row(7), toi);
-//             // }
-//             CHECK(result || !expected_result); // false positive is ok
+//             if (paths[i].stem().string().find("ee") != std::string::npos) {
+//                 result = edge_edge_ccd(
+//                     V.row(0), V.row(1), V.row(2), V.row(3), V.row(4),
+//                     V.row(5), V.row(6), V.row(7), toi);
+//             } else {
+//                 result = point_triangle_ccd(
+//                     V.row(0), V.row(1), V.row(2), V.row(3), V.row(4),
+//                     V.row(5), V.row(6), V.row(7), toi);
+//             }
+//             CHECK((result || !expected_result)); // false positive is ok
 //         }
 //     }
 // }