Add CCD Query IO and handle corner cases

cmake/recipes/ccd_query_io.cmake

@@ -0,0 +1,11 @@
+if(TARGET ccd_io::ccd_io)
+    return()
+endif()
+
+message(STATUS "Third-party: creating target 'ccd_io::ccd_io'")
+
+set(CCD_IO_DOWNLOAD_SAMPLE_QUERIES ON CACHE BOOL "Download sample CCD queries" FORCE)
+set(CCD_IO_SAMPLE_QUERIES_DIR "${PROJECT_SOURCE_DIR}/tests/sample-queries/" CACHE PATH "Where should we download sample queries?")
+
+include(CPM)
+CPMAddPackage("gh:Continuous-Collision-Detection/CCD-Query-IO#36f6093af81a65acc27d9f05ad32d6b5729e8d15")

src/ccd.cpp

@@ -86,6 +86,7 @@ bool fast_approximate_root_ccd(
                 return true;
             }
         }
+        return false; // no roots in [0, 1]
     } else if (4 * d.b * d.b - 12 * d.a * d.c < 0) {
         return false;
     }
@@ -113,7 +114,7 @@ std::optional<RootInterval>
 determine_cubic_root_interval(const CubicEquation& d)
 {
     constexpr double t0 = 0, t1 = 1;
-    assert(d(t0) > 0);
+    assert(d(t0) >= 0);
 
     const auto [tm0, tm1] = d.extrema();
     assert(tm0 <= tm1);

src/cubic.cpp

@@ -43,13 +43,13 @@ bool CubicEquation::is_nearly_quadratic(const double tol) const
 
 bool CubicEquation::is_nearly_linear(const double tol) const
 {
-    return is_nearly_quadratic()
+    return is_nearly_quadratic(tol)
         && (std::abs(b) < tol || std::abs(b / (c != 0 ? c : 1)) < tol);
 }
 
 bool CubicEquation::is_nearly_constant(const double tol) const
 {
-    return is_nearly_linear()
+    return is_nearly_linear(tol)
         && (std::abs(c) < tol || std::abs(c / (d != 0 ? d : 1)) < tol);
 }
 

src/math.cpp

@@ -9,8 +9,14 @@ namespace ccd {
 std::array<double, 2>
 solve_quadratic_equation(const double a, const double b, const double c)
 {
-    assert(b * b - 4 * a * c >= 0);
-    const double tmp = b + sgn(b) * std::sqrt(b * b - 4 * a * c);
+    const double delta = b * b - 4 * a * c;
+    assert(delta >= -std::numeric_limits<double>::epsilon());
+
+    double tmp = b;
+    if (delta > std::numeric_limits<double>::epsilon()) {
+        tmp += sgn(b) * std::sqrt(delta);
+    }
+
     std::array<double, 2> roots = { { -2 * c / tmp, -tmp / (2 * a) } };
     if (roots[0] > roots[1])
         std::swap(roots[0], roots[1]);
@@ -32,13 +38,15 @@ double modified_newton_raphson(
     const CubicEquation& f,
     const double x0,
     const double locally_min_gradient,
-    const double tolerance)
+    const double tolerance,
+    const int max_iter)
 {
     double prev_x, x = x0;
+    int iter = 0;
     do {
         prev_x = x;
         x -= std::clamp(f(x) / locally_min_gradient, -1.0, 1.0);
-    } while (std::abs(x - prev_x) > tolerance);
+    } while (std::abs(x - prev_x) > tolerance && ++iter < max_iter);
     return x;
 }
 

src/math.hpp

@@ -39,6 +39,7 @@ double modified_newton_raphson(
     const CubicEquation& f,
     const double x0,
     const double locally_min_gradient,
-    const double tolerance = 1e-6);
+    const double tolerance = 1e-6,
+    const int max_iter = 100);
 
 } // namespace ccd

tests/CMakeLists.txt

@@ -5,7 +5,7 @@
 add_executable(far_ccd_tests
   test_edge_edge_ccd.cpp
   test_point_triangle_ccd.cpp
-  # test_dataset.cpp
+  test_dataset.cpp
 )
 
 target_include_directories(far_ccd_tests PUBLIC ".")
@@ -19,11 +19,8 @@ target_link_libraries(far_ccd_tests PUBLIC far_ccd::far_ccd)
 include(catch2)
 target_link_libraries(far_ccd_tests PUBLIC Catch2::Catch2WithMain)
 
-find_package(GMP REQUIRED)
-target_link_libraries(far_ccd_tests PUBLIC gmp::gmp)
-
-include(sample-queries)
-target_link_libraries(far_ccd_tests PUBLIC sample_queries::sample_queries)
+include(ccd_query_io)
+target_link_libraries(far_ccd_tests PUBLIC ccd_io::ccd_io)
 
 # Extra warnings (link last for highest priority)
 include(far_ccd_warnings)

tests/test_dataset.cpp

@@ -2,15 +2,12 @@
 
 #include <ccd.hpp>
 
-#include <gmp.h>
+#include <ccd_io/read_ccd_queries.hpp>
+
 #include <filesystem>
-#include <fstream>
 
 enum Case { POINT_TRIANGLE, EDGE_EDGE };
 
-std::pair<Eigen::MatrixXd, std::vector<bool>>
-read_rational_csv(const std::string& filename);
-
 TEST_CASE("Dataset", "[ccd][point-triangle][edge-edge][dataset][!mayfail]")
 {
     using namespace ccd;
@@ -24,7 +21,7 @@ TEST_CASE("Dataset", "[ccd][point-triangle][edge-edge][dataset][!mayfail]")
         "cow-heads", "golf-ball", "mat-twist");
     const Case test_case = GENERATE(POINT_TRIANGLE, EDGE_EDGE);
 
-    const fs::path path = fs::path(SAMPLE_QUERIES_DIR) / folder_name
+    const fs::path path = fs::path(CCD_IO_SAMPLE_QUERIES_DIR) / folder_name
         / (test_case == POINT_TRIANGLE ? "vertex-face" : "edge-edge");
     REQUIRE(fs::exists(path));
     REQUIRE(fs::is_directory(path));
@@ -35,88 +32,27 @@ TEST_CASE("Dataset", "[ccd][point-triangle][edge-edge][dataset][!mayfail]")
         }
 
         CAPTURE(f.path().string());
-        const auto& [queries, results] = read_rational_csv(f.path().string());
-        assert(queries.rows() % 8 == 0 && queries.cols() == 3);
+        const std::vector<ccd_io::CCDQuery> queries =
+            ccd_io::read_ccd_queries(f.path().string());
 
-        const int n_queries = queries.rows() / 8;
-        for (int i = 0; i < n_queries; i++) {
-            const Eigen::Matrix<double, 8, 3> query =
-                queries.middleRows<8>(8 * i);
-            const bool expected_result = results[i * 8];
+        for (const auto& [vertices, is_collision_expected] : queries) {
+            Eigen::Map<const Eigen::Matrix<double, 8, 3>> V(&vertices[0][0]);
 
             bool result;
             double toi;
             switch (test_case) {
             case POINT_TRIANGLE:
                 result = point_triangle_ccd(
-                    query.row(0), query.row(1), query.row(2), query.row(3),
-                    query.row(4), query.row(5), query.row(6), query.row(7),
-                    toi);
+                    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);
                 break;
             case EDGE_EDGE:
                 result = edge_edge_ccd(
-                    query.row(0), query.row(1), query.row(2), query.row(3),
-                    query.row(4), query.row(5), query.row(6), query.row(7),
-                    toi);
+                    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); // conservative check
-        }
-    }
-}
-
-double
-double_from_rational_strings(const std::string& num, const std::string& denom)
-{
-    static mpq_t value;
-    std::string tmp = num + "/" + denom;
-    mpq_set_str(value, tmp.c_str(), 10);
-    return mpq_get_d(value);
-}
-
-std::pair<Eigen::MatrixXd, std::vector<bool>>
-read_rational_csv(const std::string& filename)
-{
-    Eigen::MatrixXd queries;
-    std::vector<bool> results;
-
-    // be careful, there are n lines which means there are n/8 queries, but has
-    // n results, which means results are duplicated
-    std::vector<Eigen::Vector3d> vs;
-    std::ifstream f(filename);
-    assert(f.is_open());
-
-    while (f) {
-        std::string s;
-        if (!getline(f, s))
-            break;
-
-        if (s[0] == '#') {
-            continue;
-        }
-
-        std::istringstream ss(s);
-        // the first six are one vetex, the seventh is the result
-        std::array<std::string, 7> record;
-        int c = 0;
-        while (ss) {
-            std::string line;
-            if (!getline(ss, line, ','))
-                break;
-            record[c++] = line;
-        }
-        vs.emplace_back(
-            double_from_rational_strings(record[0], record[1]),
-            double_from_rational_strings(record[2], record[3]),
-            double_from_rational_strings(record[4], record[5]));
-        results.push_back(std::stoi(record[6]));
-    }
-    queries.resize(vs.size(), 3);
-    for (int i = 0; i < vs.size(); i++) {
-        for (int j = 0; j < 3; j++) {
-            queries(i, j) = vs[i][j];
+            CHECK((result || !is_collision_expected)); // conservative check
         }
     }
-
-    return std::make_pair(queries, results);
 }

tests/test_edge_edge_ccd.cpp

@@ -1,6 +1,7 @@
 #include <catch2/catch_all.hpp>
 
 #include <ccd.hpp>
+#include <autogen.hpp>
 
 using namespace ccd;
 
@@ -111,11 +112,18 @@ TEST_CASE("Edge-Edge CCD", "[ccd][3D][edge-edge][!mayfail]")
     }
     INFO(name);
 
+    CAPTURE(autogen::edge_edge_ccd_equation(
+        ea0_t0.x(), ea0_t0.y(), ea0_t0.z(), ea1_t0.x(), ea1_t0.y(), ea1_t0.z(),
+        eb0_t0.x(), eb0_t0.y(), eb0_t0.z(), eb1_t0.x(), eb1_t0.y(), eb1_t0.z(),
+        ea0_t1.x(), ea0_t1.y(), ea0_t1.z(), ea1_t1.x(), ea1_t1.y(), ea1_t1.z(),
+        eb0_t1.x(), eb0_t1.y(), eb0_t1.z(), eb1_t1.x(), eb1_t1.y(),
+        eb1_t1.z()));
+
     double toi;
     bool is_colliding = edge_edge_ccd(
         ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, toi);
     if (conservative_check) {
-        CHECK(is_colliding >= is_collision_expected);
+        CHECK((is_colliding || !is_collision_expected));
     } else {
         CHECK(is_colliding == is_collision_expected);
     }