From a0a8e9278e06fd013b847dd9e39c9ef3096d45ca Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 18 Oct 2023 17:45:16 -0400 Subject: [PATCH] Check distance when no motion in CCD --- src/ipc/ccd/ccd.cpp | 66 ++++++++++++---------- src/ipc/ccd/inexact_point_edge.cpp | 7 +++ src/ipc/ccd/inexact_point_edge.hpp | 7 +++ tests/src/tests/ccd/test_ccd_benchmark.cpp | 39 +++++++------ 4 files changed, 72 insertions(+), 47 deletions(-) diff --git a/src/ipc/ccd/ccd.cpp b/src/ipc/ccd/ccd.cpp index 5ed45ebd1..e46170ba0 100644 --- a/src/ipc/ccd/ccd.cpp +++ b/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); diff --git a/src/ipc/ccd/inexact_point_edge.cpp b/src/ipc/ccd/inexact_point_edge.cpp index a64053dd9..5f9c36aff 100644 --- a/src/ipc/ccd/inexact_point_edge.cpp +++ b/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 diff --git a/src/ipc/ccd/inexact_point_edge.hpp b/src/ipc/ccd/inexact_point_edge.hpp index 99c11dcfe..d1c1fe2b9 100644 --- a/src/ipc/ccd/inexact_point_edge.hpp +++ b/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 diff --git a/tests/src/tests/ccd/test_ccd_benchmark.cpp b/tests/src/tests/ccd/test_ccd_benchmark.cpp index e4ed3b969..8092aa555 100644 --- a/tests/src/tests/ccd/test_ccd_benchmark.cpp +++ b/tests/src/tests/ccd/test_ccd_benchmark.cpp @@ -256,30 +256,37 @@ TEST_CASE( // TEST_CASE("Failing Benchmark Cases", "[ccd]") // { // using Matrix8x3 = Eigen::Matrix; - -// const static std::vector paths; -// const static std::vector> qids; - +// +// const static std::vector paths = { { +// tests::NEW_CCD_BENCHMARK_DIR / "n-body-simulation/queries/42ee.csv", +// } }; +// const static std::vector> qids { { +// 2647, +// 2648, +// } }; +// // for (int i = 0; i < paths.size(); i++) { // const std::vector 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 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 // } // } // }