From 9b644f2a3fe6dc13e66931358882b88b2745a1c4 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 22 Oct 2023 20:12:40 -0600 Subject: [PATCH 1/4] Update warnings --- cmake/ipc_toolkit/ipc_toolkit_warnings.cmake | 28 +++++++++----------- src/ipc/broad_phase/voxel_size_heuristic.cpp | 4 +-- 2 files changed, 14 insertions(+), 18 deletions(-) diff --git a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake index 7e1ded74b..97bb96d57 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake @@ -10,20 +10,22 @@ endif() set(IPC_TOOLKIT_WARNING_FLAGS -Wall -Wextra - -pedantic + -Wpedantic + # -Werror # -Wconversion + # -Wsign-conversion # -Wunsafe-loop-optimizations # broken with C++11 loops -Wunused - -Wno-long-long + -Wno-long-long # disable warnings about using long long -Wpointer-arith -Wformat=2 -Wuninitialized -Wcast-qual - # -Wmissing-noreturn + -Wmissing-noreturn -Wmissing-format-attribute - # -Wredundant-decls + -Wredundant-decls -Werror=implicit -Werror=nonnull @@ -43,11 +45,10 @@ set(IPC_TOOLKIT_WARNING_FLAGS -Wunused-but-set-variable -Wno-unused-parameter - #-Weffc++ + -Weffc++ -Wold-style-cast - # -Wsign-conversion - # -Wshadow + -Wshadow -Wstrict-null-sentinel -Woverloaded-virtual @@ -60,7 +61,7 @@ set(IPC_TOOLKIT_WARNING_FLAGS # lacks a case for one or more of the named codes of that enumeration. -Wswitch # This is annoying if all cases are already covered. - # -Wswitch-default + -Wswitch-default # This is annoying if there is a default that covers the rest. # -Wswitch-enum -Wswitch-unreachable @@ -70,20 +71,17 @@ set(IPC_TOOLKIT_WARNING_FLAGS -Wdisabled-optimization # -Winline # produces warning on default implicit destructor -Winvalid-pch - # -Wmissing-include-dirs + -Wmissing-include-dirs -Wpacked -Wno-padded -Wstrict-overflow -Wstrict-overflow=2 - # -Wctor-dtor-privacy + -Wctor-dtor-privacy -Wlogical-op - # -Wnoexcept -Woverloaded-virtual # -Wundef - -Wnon-virtual-dtor - -Wdelete-non-virtual-dtor -Werror=non-virtual-dtor -Werror=delete-non-virtual-dtor @@ -126,7 +124,7 @@ set(IPC_TOOLKIT_WARNING_FLAGS ################################################ #-Wimplicit-atomic-properties - #-Wmissing-declarations + -Wmissing-declarations #-Wmissing-prototypes #-Wstrict-selector-match #-Wundeclared-selector @@ -141,8 +139,6 @@ set(IPC_TOOLKIT_WARNING_FLAGS -fno-omit-frame-pointer -fno-optimize-sibling-calls - -Wno-pedantic - -Wno-redundant-decls ) diff --git a/src/ipc/broad_phase/voxel_size_heuristic.cpp b/src/ipc/broad_phase/voxel_size_heuristic.cpp index fad0e9529..80628d078 100644 --- a/src/ipc/broad_phase/voxel_size_heuristic.cpp +++ b/src/ipc/broad_phase/voxel_size_heuristic.cpp @@ -86,7 +86,7 @@ double mean_edge_length( double sum = 0; for (int i = 0; i < edges.rows(); i++) { - const size_t e0i = edges(i, 0), e1i = edges(i, 1); + const int e0i = edges(i, 0), e1i = edges(i, 1); sum += (vertices_t0.row(e0i) - vertices_t0.row(e1i)).norm(); sum += (vertices_t1.row(e0i) - vertices_t1.row(e1i)).norm(); } @@ -94,7 +94,7 @@ double mean_edge_length( std_deviation = 0; for (int i = 0; i < edges.rows(); i++) { - const size_t e0i = edges(i, 0), e1i = edges(i, 1); + const int e0i = edges(i, 0), e1i = edges(i, 1); std_deviation += std::pow( (vertices_t0.row(e0i) - vertices_t0.row(e1i)).norm() - mean, 2); std_deviation += std::pow( From c9177f8488806ea3562c6c4b5bb209ef87949995 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 23 Oct 2023 00:17:29 -0400 Subject: [PATCH 2/4] Fix warnings --- cmake/ipc_toolkit/ipc_toolkit_warnings.cmake | 4 +- src/ipc/broad_phase/aabb.cpp | 4 +- src/ipc/broad_phase/hash_grid.cpp | 24 +-- src/ipc/broad_phase/hash_grid.hpp | 2 +- .../broad_phase/sweep_and_tiniest_queue.cpp | 24 +-- .../broad_phase/sweep_and_tiniest_queue.hpp | 16 +- src/ipc/candidates/candidates.cpp | 12 +- src/ipc/candidates/edge_edge.cpp | 6 +- src/ipc/candidates/edge_face.cpp | 6 +- src/ipc/candidates/edge_vertex.cpp | 6 +- src/ipc/candidates/face_vertex.cpp | 6 +- src/ipc/candidates/vertex_vertex.cpp | 6 +- src/ipc/ccd/additive_ccd.cpp | 20 +- src/ipc/ccd/additive_ccd.hpp | 2 +- src/ipc/ccd/ccd.cpp | 40 ++-- src/ipc/ccd/ccd.hpp | 20 ++ src/ipc/ccd/nonlinear_ccd.cpp | 56 +++--- src/ipc/ccd/nonlinear_ccd.hpp | 28 ++- src/ipc/collisions/collision_constraint.cpp | 6 +- .../collision_constraints_builder.cpp | 11 +- src/ipc/collisions/edge_edge.cpp | 42 ++-- src/ipc/collisions/edge_vertex.hpp | 12 +- src/ipc/collisions/face_vertex.hpp | 12 +- src/ipc/collisions/plane_vertex.cpp | 12 +- src/ipc/collisions/vertex_vertex.hpp | 12 +- src/ipc/distance/point_point.cpp | 2 +- src/ipc/friction/constraints/edge_edge.cpp | 12 +- src/ipc/friction/constraints/edge_vertex.cpp | 18 +- src/ipc/friction/constraints/face_vertex.cpp | 12 +- .../friction/constraints/vertex_vertex.cpp | 8 +- src/ipc/ipc.cpp | 4 +- src/ipc/utils/intersection.cpp | 180 ++++++++++-------- tests/src/tests/barrier/test_barrier.cpp | 18 +- tests/src/tests/ccd/collision_generator.cpp | 4 +- tests/src/tests/ccd/test_nonlinear_ccd.cpp | 27 ++- tests/src/tests/distance/test_edge_edge.cpp | 27 +-- tests/src/tests/distance/test_line_line.cpp | 25 ++- tests/src/tests/distance/test_point_edge.cpp | 71 ++++--- tests/src/tests/distance/test_point_line.cpp | 52 ++--- tests/src/tests/distance/test_point_plane.cpp | 25 ++- tests/src/tests/distance/test_point_point.cpp | 35 ++-- .../tests/distance/test_point_triangle.cpp | 48 ++--- .../src/tests/friction/test_closest_point.cpp | 20 +- .../friction/test_normal_force_magnitude.cpp | 30 ++- .../test_smooth_friction_mollifier.cpp | 8 +- .../src/tests/friction/test_tangent_basis.cpp | 28 +-- 46 files changed, 556 insertions(+), 487 deletions(-) diff --git a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake index 97bb96d57..a29395cff 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake @@ -45,7 +45,7 @@ set(IPC_TOOLKIT_WARNING_FLAGS -Wunused-but-set-variable -Wno-unused-parameter - -Weffc++ + # -Weffc++ -Wold-style-cast -Wshadow @@ -124,7 +124,7 @@ set(IPC_TOOLKIT_WARNING_FLAGS ################################################ #-Wimplicit-atomic-properties - -Wmissing-declarations + #-Wmissing-declarations #-Wmissing-prototypes #-Wstrict-selector-match #-Wundeclared-selector diff --git a/src/ipc/broad_phase/aabb.cpp b/src/ipc/broad_phase/aabb.cpp index 5e5d964a5..a98f44524 100644 --- a/src/ipc/broad_phase/aabb.cpp +++ b/src/ipc/broad_phase/aabb.cpp @@ -7,7 +7,9 @@ namespace ipc { -AABB::AABB(const ArrayMax3d& min, const ArrayMax3d& max) : min(min), max(max) +AABB::AABB(const ArrayMax3d& _min, const ArrayMax3d& _max) + : min(_min) + , max(_max) { assert(min.size() == max.size()); assert((min <= max).all()); diff --git a/src/ipc/broad_phase/hash_grid.cpp b/src/ipc/broad_phase/hash_grid.cpp index f25078bb1..26310ffae 100644 --- a/src/ipc/broad_phase/hash_grid.cpp +++ b/src/ipc/broad_phase/hash_grid.cpp @@ -9,7 +9,7 @@ #include #include -#include // std::min/max +#include // std::min/max #define IPC_TOOLKIT_HASH_GRID_USE_SORT_UNIQUE // else use unordered_set @@ -149,20 +149,22 @@ void HashGrid::detect_candidates( size_t num_items = items0.size() + items1.size(); std::vector merged_item_indices; merged_item_indices.reserve(num_items); - long i = 0, j = 0; - while (i < items0.size() && j < items1.size()) { - if (items0[i] < items1[j]) { + { + long i = 0, j = 0; + while (i < items0.size() && j < items1.size()) { + if (items0[i] < items1[j]) { + merged_item_indices.push_back(-(i++) - 1); + } else { + merged_item_indices.push_back(j++); + } + } + while (i < items0.size()) { merged_item_indices.push_back(-(i++) - 1); - } else { + } + while (j < items1.size()) { merged_item_indices.push_back(j++); } } - while (i < items0.size()) { - merged_item_indices.push_back(-(i++) - 1); - } - while (j < items1.size()) { - merged_item_indices.push_back(j++); - } assert(merged_item_indices.size() == num_items); const auto get_item = [&](long i) -> const HashItem& { diff --git a/src/ipc/broad_phase/hash_grid.hpp b/src/ipc/broad_phase/hash_grid.hpp index 94e0e8d07..0dfd4061f 100644 --- a/src/ipc/broad_phase/hash_grid.hpp +++ b/src/ipc/broad_phase/hash_grid.hpp @@ -10,7 +10,7 @@ struct HashItem { long id; /// @brief The value of the item. /// @brief Construct a hash item as a (key, value) pair. - HashItem(int key, int id) : key(key), id(id) { } + HashItem(int _key, int _id) : key(_key), id(_id) { } /// @brief Compare HashItems by their keys for sorting. bool operator<(const HashItem& other) const diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp b/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp index 7e7c9b274..4b2cb9cb7 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.cpp @@ -12,21 +12,21 @@ namespace ipc { void SweepAndTiniestQueue::build( const Eigen::MatrixXd& vertices, - const Eigen::MatrixXi& edges, - const Eigen::MatrixXi& faces, + const Eigen::MatrixXi& _edges, + const Eigen::MatrixXi& _faces, double inflation_radius) { - build(vertices, vertices, edges, faces, inflation_radius); + build(vertices, vertices, _edges, _faces, inflation_radius); } void SweepAndTiniestQueue::build( const Eigen::MatrixXd& vertices_t0, const Eigen::MatrixXd& vertices_t1, - const Eigen::MatrixXi& edges, - const Eigen::MatrixXi& faces, + const Eigen::MatrixXi& _edges, + const Eigen::MatrixXi& _faces, double inflation_radius) { - CopyMeshBroadPhase::copy_mesh(edges, faces); + CopyMeshBroadPhase::copy_mesh(_edges, _faces); num_vertices = vertices_t0.rows(); stq::cpu::constructBoxes( vertices_t0, vertices_t1, edges, faces, boxes, inflation_radius); @@ -122,11 +122,11 @@ bool SweepAndTiniestQueue::is_face(long id) const #ifdef IPC_TOOLKIT_WITH_CUDA void SweepAndTiniestQueueGPU::build( const Eigen::MatrixXd& vertices, - const Eigen::MatrixXi& edges, - const Eigen::MatrixXi& faces, + const Eigen::MatrixXi& _edges, + const Eigen::MatrixXi& _faces, double inflation_radius) { - CopyMeshBroadPhase::copy_mesh(edges, faces); + CopyMeshBroadPhase::copy_mesh(_edges, _faces); ccd::gpu::construct_static_collision_candidates( vertices, edges, faces, overlaps, boxes, inflation_radius); } @@ -134,11 +134,11 @@ void SweepAndTiniestQueueGPU::build( void SweepAndTiniestQueueGPU::build( const Eigen::MatrixXd& vertices_t0, const Eigen::MatrixXd& vertices_t1, - const Eigen::MatrixXi& edges, - const Eigen::MatrixXi& faces, + const Eigen::MatrixXi& _edges, + const Eigen::MatrixXi& _faces, double inflation_radius) { - CopyMeshBroadPhase::copy_mesh(edges, faces); + CopyMeshBroadPhase::copy_mesh(_edges, _faces); ccd::gpu::construct_continuous_collision_candidates( vertices_t0, vertices_t1, edges, faces, overlaps, boxes, inflation_radius); diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp index 70ab06bec..ae2aa7207 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp @@ -60,12 +60,14 @@ class SweepAndTiniestQueue : public CopyMeshBroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisisons. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + std::vector& candidates) const override + [[noreturn]]; /// @brief Find the candidate edge-vertex collisisons. /// @param[out] candidates The candidate edge-vertex collisisons. void detect_edge_vertex_candidates( - std::vector& candidates) const override; + std::vector& candidates) const override + [[noreturn]]; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisisons. @@ -80,7 +82,7 @@ class SweepAndTiniestQueue : public CopyMeshBroadPhase { /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. void detect_edge_face_candidates( - std::vector& candidates) const override; + std::vector& candidates) const override [[noreturn]]; protected: long to_edge_id(long id) const; @@ -128,12 +130,14 @@ class SweepAndTiniestQueueGPU : public CopyMeshBroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisisons. void detect_vertex_vertex_candidates( - std::vector& candidates) const override; + std::vector& candidates) const override + [[noreturn]]; /// @brief Find the candidate edge-vertex collisisons. /// @param[out] candidates The candidate edge-vertex collisisons. void detect_edge_vertex_candidates( - std::vector& candidates) const override; + std::vector& candidates) const override + [[noreturn]]; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisisons. @@ -148,7 +152,7 @@ class SweepAndTiniestQueueGPU : public CopyMeshBroadPhase { /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. void detect_edge_face_candidates( - std::vector& candidates) const override; + std::vector& candidates) const override [[noreturn]]; private: std::vector boxes; diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index 9dccf7ddc..370362014 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -13,11 +13,13 @@ namespace ipc { -bool implements_vertex_vertex(const BroadPhaseMethod method) -{ - return method != BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE - && method != BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE_GPU; -} +namespace { + bool implements_vertex_vertex(const BroadPhaseMethod method) + { + return method != BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE + && method != BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE_GPU; + } +} // namespace void Candidates::build( const CollisionMesh& mesh, diff --git a/src/ipc/candidates/edge_edge.cpp b/src/ipc/candidates/edge_edge.cpp index 8bf9c99c8..e64ff15f3 100644 --- a/src/ipc/candidates/edge_edge.cpp +++ b/src/ipc/candidates/edge_edge.cpp @@ -5,9 +5,9 @@ namespace ipc { -EdgeEdgeCandidate::EdgeEdgeCandidate(long edge0_id, long edge1_id) - : edge0_id(edge0_id) - , edge1_id(edge1_id) +EdgeEdgeCandidate::EdgeEdgeCandidate(long _edge0_id, long _edge1_id) + : edge0_id(_edge0_id) + , edge1_id(_edge1_id) { } diff --git a/src/ipc/candidates/edge_face.cpp b/src/ipc/candidates/edge_face.cpp index 03d723161..698bb2e64 100644 --- a/src/ipc/candidates/edge_face.cpp +++ b/src/ipc/candidates/edge_face.cpp @@ -2,9 +2,9 @@ namespace ipc { -EdgeFaceCandidate::EdgeFaceCandidate(long edge_id, long face_id) - : edge_id(edge_id) - , face_id(face_id) +EdgeFaceCandidate::EdgeFaceCandidate(long _edge_id, long _face_id) + : edge_id(_edge_id) + , face_id(_face_id) { } diff --git a/src/ipc/candidates/edge_vertex.cpp b/src/ipc/candidates/edge_vertex.cpp index ec9e2a6f6..5bef1b1a3 100644 --- a/src/ipc/candidates/edge_vertex.cpp +++ b/src/ipc/candidates/edge_vertex.cpp @@ -7,9 +7,9 @@ namespace ipc { -EdgeVertexCandidate::EdgeVertexCandidate(long edge_id, long vertex_id) - : edge_id(edge_id) - , vertex_id(vertex_id) +EdgeVertexCandidate::EdgeVertexCandidate(long _edge_id, long _vertex_id) + : edge_id(_edge_id) + , vertex_id(_vertex_id) { } diff --git a/src/ipc/candidates/face_vertex.cpp b/src/ipc/candidates/face_vertex.cpp index 8791e02ef..670fecd41 100644 --- a/src/ipc/candidates/face_vertex.cpp +++ b/src/ipc/candidates/face_vertex.cpp @@ -7,9 +7,9 @@ namespace ipc { -FaceVertexCandidate::FaceVertexCandidate(long face_id, long vertex_id) - : face_id(face_id) - , vertex_id(vertex_id) +FaceVertexCandidate::FaceVertexCandidate(long _face_id, long _vertex_id) + : face_id(_face_id) + , vertex_id(_vertex_id) { } diff --git a/src/ipc/candidates/vertex_vertex.cpp b/src/ipc/candidates/vertex_vertex.cpp index fb17de52c..93888e308 100644 --- a/src/ipc/candidates/vertex_vertex.cpp +++ b/src/ipc/candidates/vertex_vertex.cpp @@ -6,9 +6,9 @@ namespace ipc { -VertexVertexCandidate::VertexVertexCandidate(long vertex0_id, long vertex1_id) - : vertex0_id(vertex0_id) - , vertex1_id(vertex1_id) +VertexVertexCandidate::VertexVertexCandidate(long _vertex0_id, long _vertex1_id) + : vertex0_id(_vertex0_id) + , vertex1_id(_vertex1_id) { } diff --git a/src/ipc/ccd/additive_ccd.cpp b/src/ipc/ccd/additive_ccd.cpp index 39af9dde9..adce30f5e 100644 --- a/src/ipc/ccd/additive_ccd.cpp +++ b/src/ipc/ccd/additive_ccd.cpp @@ -143,7 +143,7 @@ bool point_point_ccd( return point_point_distance(x.head(dim), x.tail(dim)); }; - VectorMax12d x = stack(p0_t0, p1_t0); + const VectorMax12d x = stack(p0_t0, p1_t0); const VectorMax12d dx = stack(dp0, dp1); return additive_ccd( @@ -187,14 +187,14 @@ bool point_edge_ccd( return false; } - VectorMax12d x = stack(p_t0, e0_t0, e1_t0); - const VectorMax12d dx = stack(dp, de0, de1); - auto distance_squared = [dim](const VectorMax12d& x) { return point_edge_distance( x.head(dim), x.segment(dim, dim), x.tail(dim)); }; + const VectorMax12d x = stack(p_t0, e0_t0, e1_t0); + const VectorMax12d dx = stack(dp, de0, de1); + return additive_ccd( x, dx, distance_squared, max_disp_mag, toi, min_distance, tmax, conservative_rescaling); @@ -230,9 +230,6 @@ bool point_triangle_ccd( Eigen::Vector3d dt2 = t2_t1 - t2_t0; subtract_mean(dp, dt0, dt1, dt2); - VectorMax12d x = stack(p_t0, t0_t0, t1_t0, t2_t0); - const VectorMax12d dx = stack(dp, dt0, dt1, dt2); - const double max_disp_mag = dp.norm() + std::sqrt(std::max( { dt0.squaredNorm(), dt1.squaredNorm(), dt2.squaredNorm() })); @@ -245,6 +242,9 @@ bool point_triangle_ccd( x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); }; + const VectorMax12d x = stack(p_t0, t0_t0, t1_t0, t2_t0); + const VectorMax12d dx = stack(dp, dt0, dt1, dt2); + return additive_ccd( x, dx, distance_squared, max_disp_mag, toi, min_distance, tmax, conservative_rescaling); @@ -287,9 +287,6 @@ bool edge_edge_ccd( return false; } - VectorMax12d x = stack(ea0_t0, ea1_t0, eb0_t0, eb1_t0); - const VectorMax12d dx = stack(dea0, dea1, deb0, deb1); - const double min_distance_sq = min_distance * min_distance; auto distance_squared = [min_distance_sq](const VectorMax12d& x) { const auto& ea0 = x.head<3>(); @@ -308,6 +305,9 @@ bool edge_edge_ccd( return d_sq; }; + const VectorMax12d x = stack(ea0_t0, ea1_t0, eb0_t0, eb1_t0); + const VectorMax12d dx = stack(dea0, dea1, deb0, deb1); + return additive_ccd( x, dx, distance_squared, max_disp_mag, toi, min_distance, tmax, conservative_rescaling); diff --git a/src/ipc/ccd/additive_ccd.hpp b/src/ipc/ccd/additive_ccd.hpp index bef785e55..cd71c3c2b 100644 --- a/src/ipc/ccd/additive_ccd.hpp +++ b/src/ipc/ccd/additive_ccd.hpp @@ -128,7 +128,7 @@ bool edge_edge_ccd( bool additive_ccd( VectorMax12d x, const VectorMax12d& dx, - const std::function& distance_squared, + const std::function& distance_squared, const double max_disp_mag, double& toi, const double min_distance = 0.0, diff --git a/src/ipc/ccd/ccd.cpp b/src/ipc/ccd/ccd.cpp index 1e7e44a70..ee8813362 100644 --- a/src/ipc/ccd/ccd.cpp +++ b/src/ipc/ccd/ccd.cpp @@ -106,19 +106,19 @@ bool point_point_ccd_3D( const double adjusted_tolerance = std::min( INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance); - const auto ccd = [&](long max_iterations, double min_distance, - bool no_zero_toi, double& toi) -> bool { + const auto ccd = [&](long _max_iterations, double _min_distance, + bool no_zero_toi, double& _toi) -> bool { #ifdef IPC_TOOLKIT_WITH_CORRECT_CCD double output_tolerance; // NOTE: Use degenerate edge-edge return ticcd::edgeEdgeCCD( p0_t0, p0_t0, p1_t0, p1_t0, p0_t1, p0_t1, p1_t1, p1_t1, Eigen::Array3d::Constant(-1), // rounding error (auto) - min_distance, // minimum separation distance - toi, // time of impact + _min_distance, // minimum separation distance + _toi, // time of impact adjusted_tolerance, // delta tmax, // maximum time to check - max_iterations, // maximum number of iterations + _max_iterations, // maximum number of iterations output_tolerance, // delta_actual no_zero_toi); #else @@ -178,19 +178,19 @@ bool point_edge_ccd_3D( const double adjusted_tolerance = std::min( INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance); - const auto ccd = [&](long max_iterations, double min_distance, - bool no_zero_toi, double& toi) -> bool { + const auto ccd = [&](long _max_iterations, double _min_distance, + bool no_zero_toi, double& _toi) -> bool { #ifdef IPC_TOOLKIT_WITH_CORRECT_CCD double output_tolerance = tolerance; // NOTE: Use degenerate edge-edge bool is_impacting = ticcd::edgeEdgeCCD( p_t0, p_t0, e0_t0, e1_t0, p_t1, p_t1, e0_t1, e1_t1, Eigen::Array3d::Constant(-1), // rounding error (auto) - min_distance, // minimum separation distance - toi, // time of impact + _min_distance, // minimum separation distance + _toi, // time of impact adjusted_tolerance, // delta tmax, // maximum time to check - max_iterations, // maximum number of iterations + _max_iterations, // maximum number of iterations output_tolerance, // delta_actual no_zero_toi); if (adjusted_tolerance < output_tolerance && toi < CCD_SMALL_TOI) { @@ -264,18 +264,18 @@ bool edge_edge_ccd( const double adjusted_tolerance = std::min( INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance); - const auto ccd = [&](long max_iterations, double min_distance, - bool no_zero_toi, double& toi) -> bool { + const auto ccd = [&](long _max_iterations, double _min_distance, + bool no_zero_toi, double& _toi) -> bool { #ifdef IPC_TOOLKIT_WITH_CORRECT_CCD double output_tolerance; bool is_impacting = ticcd::edgeEdgeCCD( ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, Eigen::Array3d::Constant(-1), // rounding error (auto) - min_distance, // minimum separation distance - toi, // time of impact + _min_distance, // minimum separation distance + _toi, // time of impact adjusted_tolerance, // delta tmax, // maximum time to check - max_iterations, // maximum number of iterations + _max_iterations, // maximum number of iterations output_tolerance, // delta_actual no_zero_toi); if (adjusted_tolerance < output_tolerance && toi < CCD_SMALL_TOI) { @@ -327,18 +327,18 @@ bool point_triangle_ccd( const double adjusted_tolerance = std::min( INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance); - const auto ccd = [&](long max_iterations, double min_distance, - bool no_zero_toi, double& toi) -> bool { + const auto ccd = [&](long _max_iterations, double _min_distance, + bool no_zero_toi, double& _toi) -> bool { #ifdef IPC_TOOLKIT_WITH_CORRECT_CCD double output_tolerance; bool is_impacting = ticcd::vertexFaceCCD( p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, t1_t1, t2_t1, Eigen::Array3d::Constant(-1), // rounding error (auto) - min_distance, // minimum separation distance - toi, // time of impact + _min_distance, // minimum separation distance + _toi, // time of impact adjusted_tolerance, // delta tmax, // maximum time to check - max_iterations, // maximum number of iterations + _max_iterations, // maximum number of iterations output_tolerance, // delta_actual no_zero_toi); if (adjusted_tolerance < output_tolerance && toi < CCD_SMALL_TOI) { diff --git a/src/ipc/ccd/ccd.hpp b/src/ipc/ccd/ccd.hpp index f75416f81..517f2061e 100644 --- a/src/ipc/ccd/ccd.hpp +++ b/src/ipc/ccd/ccd.hpp @@ -183,6 +183,26 @@ bool point_triangle_ccd( const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS, const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING); +/// @brief Perform the CCD strategy outlined by Li et al. [2020]. +/// @param[in] ccd The continuous collision detection function. +/// @param[in] max_iterations The maximum number of iterations to perform. +/// @param[in] min_distance The minimum distance between the objects. +/// @param[in] initial_distance The initial distance between the objects. +/// @param[in] conservative_rescaling The conservative rescaling of the time of impact. +/// @param[out] toi Output time of impact. +/// @return True if a collision was detected, false otherwise. +bool ccd_strategy( + const std::function& ccd, + const long max_iterations, + const double min_distance, + const double initial_distance, + const double conservative_rescaling, + double& toi); + /// @brief Helper function to check if the initial distance is less than the minimum distance. /// @param[in] initial_distance The initial distance between the objects. /// @param[in] min_distance The minimum distance between the objects. diff --git a/src/ipc/ccd/nonlinear_ccd.cpp b/src/ipc/ccd/nonlinear_ccd.cpp index bbb59c292..0d42f9ffb 100644 --- a/src/ipc/ccd/nonlinear_ccd.cpp +++ b/src/ipc/ccd/nonlinear_ccd.cpp @@ -168,15 +168,15 @@ bool point_point_nonlinear_ccd( p0.max_distance_from_linear(t0, t1), p1.max_distance_from_linear(t0, t1)); }, - [&](const double ti0, const double ti1, const double min_distance, - const bool no_zero_toi, double& toi) { + [&](const double ti0, const double ti1, const double _min_distance, + const bool no_zero_toi, double& _toi) { double output_tolerance; return ticcd::edgeEdgeCCD( to_3D(p0(ti0)), to_3D(p0(ti0)), to_3D(p1(ti0)), to_3D(p1(ti0)), to_3D(p0(ti1)), to_3D(p0(ti1)), to_3D(p1(ti1)), to_3D(p1(ti1)), Eigen::Array3d::Constant(-1), // rounding error (auto) - min_distance, // minimum separation distance - toi, // time of impact + _min_distance, // minimum separation distance + _toi, // time of impact tolerance, // delta 1.0, // maximum time to check max_iterations, // maximum number of iterations @@ -207,15 +207,15 @@ bool point_edge_nonlinear_ccd( e0.max_distance_from_linear(t0, t1), e1.max_distance_from_linear(t0, t1)); }, - [&](const double ti0, const double ti1, const double min_distance, - const bool no_zero_toi, double& toi) { + [&](const double ti0, const double ti1, const double _min_distance, + const bool no_zero_toi, double& _toi) { double output_tolerance; return ticcd::edgeEdgeCCD( to_3D(p(ti0)), to_3D(p(ti0)), to_3D(e0(ti0)), to_3D(e1(ti0)), to_3D(p(ti1)), to_3D(p(ti1)), to_3D(e0(ti1)), to_3D(e1(ti1)), Eigen::Array3d::Constant(-1), // rounding error (auto) - min_distance, // minimum separation distance - toi, // time of impact + _min_distance, // minimum separation distance + _toi, // time of impact tolerance, // delta 1.0, // maximum time to check max_iterations, // maximum number of iterations @@ -249,20 +249,20 @@ bool edge_edge_nonlinear_ccd( eb0.max_distance_from_linear(t0, t1), eb1.max_distance_from_linear(t0, t1)); }, - [&](const double ti0, const double ti1, const double min_distance, - const bool no_zero_toi, double& toi) { + [&](const double ti0, const double ti1, const double _min_distance, + const bool no_zero_toi, double& _toi) { double output_tolerance; return ticcd::edgeEdgeCCD( ea0(ti0), ea1(ti0), eb0(ti0), eb1(ti0), // ea0(ti1), ea1(ti1), eb0(ti1), eb1(ti1), - Eigen::Array3d::Constant(-1), // rounding error (auto) - min_distance, // minimum separation distance - toi, // time of impact - tolerance, // delta - 1.0, // maximum time to check - max_iterations, // maximum number of iterations - output_tolerance, // delta_actual - no_zero_toi); // no zero toi + Eigen::Array3d::Constant(-1), // rounding error (auto) + _min_distance, // minimum separation distance + _toi, // time of impact + tolerance, // delta + 1.0, // maximum time to check + max_iterations, // maximum number of iterations + output_tolerance, // delta_actual + no_zero_toi); // no zero toi }, toi, tmax, min_sep_distance, conservative_rescaling); } @@ -289,20 +289,20 @@ bool point_triangle_nonlinear_ccd( t1.max_distance_from_linear(ti0, ti1), t2.max_distance_from_linear(ti0, ti1) }); }, - [&](const double ti0, const double ti1, const double min_distance, - const bool no_zero_toi, double& toi) { + [&](const double ti0, const double ti1, const double _min_distance, + const bool no_zero_toi, double& _toi) { double output_tolerance; return ticcd::vertexFaceCCD( p(ti0), t0(ti0), t1(ti0), t2(ti0), // p(ti1), t0(ti1), t1(ti1), t2(ti1), - Eigen::Array3d::Constant(-1), // rounding error (auto) - min_distance, // minimum separation distance - toi, // time of impact - tolerance, // delta - 1.0, // maximum time to check - max_iterations, // maximum number of iterations - output_tolerance, // delta_actual - no_zero_toi); // no zero toi + Eigen::Array3d::Constant(-1), // rounding error (auto) + _min_distance, // minimum separation distance + _toi, // time of impact + tolerance, // delta + 1.0, // maximum time to check + max_iterations, // maximum number of iterations + output_tolerance, // delta_actual + no_zero_toi); // no zero toi }, toi, tmax, min_distance, conservative_rescaling); } diff --git a/src/ipc/ccd/nonlinear_ccd.hpp b/src/ipc/ccd/nonlinear_ccd.hpp index 5d25d19db..bf201dc1f 100644 --- a/src/ipc/ccd/nonlinear_ccd.hpp +++ b/src/ipc/ccd/nonlinear_ccd.hpp @@ -29,10 +29,12 @@ class NonlinearTrajectory { #ifdef IPC_TOOLKIT_WITH_FILIB /// @brief A nonlinear trajectory with an implementation of the max_distance_from_linear function using interval arithmetic. -class IntervalNonlinearTrajectory : public NonlinearTrajectory { +class IntervalNonlinearTrajectory : virtual public NonlinearTrajectory { public: virtual ~IntervalNonlinearTrajectory() = default; + using NonlinearTrajectory::operator(); + /// @brief Compute the point's position over a time interval t virtual VectorMax3I operator()(const filib::Interval& t) const = 0; @@ -135,4 +137,28 @@ bool point_triangle_nonlinear_ccd( const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS, const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING); +/// @brief Perform conservative piecewise linear CCD of a nonlinear trajectories. +/// @param[in] distance Return the distance for a given time in [0, 1]. +/// @param[in] max_distance_from_linear Return the maximum distance from the linearized trajectory for a given time interval. +/// @param[in] linear_ccd Perform linear CCD on a given time interval. +/// @param[out] toi Output time of impact. +/// @param[in] tmax Maximum time to check for collision. +/// @param[in] min_distance Minimum separation distance between the objects. +/// @param[in] conservative_rescaling Conservative rescaling of the time of impact. +/// @return +bool conservative_piecewise_linear_ccd( + const std::function& distance, + const std::function& + max_distance_from_linear, + const std::function& linear_ccd, + double& toi, + const double tmax = 1.0, + const double min_distance = 0, + const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING); + } // namespace ipc diff --git a/src/ipc/collisions/collision_constraint.cpp b/src/ipc/collisions/collision_constraint.cpp index 18e6b24a7..bb485480d 100644 --- a/src/ipc/collisions/collision_constraint.cpp +++ b/src/ipc/collisions/collision_constraint.cpp @@ -5,9 +5,9 @@ namespace ipc { CollisionConstraint::CollisionConstraint( - const double weight, const Eigen::SparseVector& weight_gradient) - : weight(weight) - , weight_gradient(weight_gradient) + const double _weight, const Eigen::SparseVector& _weight_gradient) + : weight(_weight) + , weight_gradient(_weight_gradient) { } diff --git a/src/ipc/collisions/collision_constraints_builder.cpp b/src/ipc/collisions/collision_constraints_builder.cpp index 17141c69f..31ca5388a 100644 --- a/src/ipc/collisions/collision_constraints_builder.cpp +++ b/src/ipc/collisions/collision_constraints_builder.cpp @@ -10,10 +10,10 @@ namespace ipc { CollisionConstraintsBuilder::CollisionConstraintsBuilder( - const bool use_convergent_formulation, - const bool should_compute_weight_gradient) - : use_convergent_formulation(use_convergent_formulation) - , should_compute_weight_gradient(should_compute_weight_gradient) + const bool _use_convergent_formulation, + const bool _should_compute_weight_gradient) + : use_convergent_formulation(_use_convergent_formulation) + , should_compute_weight_gradient(_should_compute_weight_gradient) { } @@ -117,6 +117,7 @@ void CollisionConstraintsBuilder::add_edge_vertex_constraint( break; case PointEdgeDistanceType::AUTO: + default: assert(false); break; } @@ -215,6 +216,7 @@ void CollisionConstraintsBuilder::add_edge_edge_constraints( break; case EdgeEdgeDistanceType::AUTO: + default: assert(false); break; } @@ -290,6 +292,7 @@ void CollisionConstraintsBuilder::add_face_vertex_constraints( break; case PointTriangleDistanceType::AUTO: + default: assert(false); break; } diff --git a/src/ipc/collisions/edge_edge.cpp b/src/ipc/collisions/edge_edge.cpp index 17ebab7d0..c9614b065 100644 --- a/src/ipc/collisions/edge_edge.cpp +++ b/src/ipc/collisions/edge_edge.cpp @@ -7,37 +7,37 @@ namespace ipc { EdgeEdgeConstraint::EdgeEdgeConstraint( - const long edge0_id, - const long edge1_id, - const double eps_x, - const EdgeEdgeDistanceType dtype) - : EdgeEdgeCandidate(edge0_id, edge1_id) - , eps_x(eps_x) - , dtype(dtype) + const long _edge0_id, + const long _edge1_id, + const double _eps_x, + const EdgeEdgeDistanceType _dtype) + : EdgeEdgeCandidate(_edge0_id, _edge1_id) + , eps_x(_eps_x) + , dtype(_dtype) { } EdgeEdgeConstraint::EdgeEdgeConstraint( const EdgeEdgeCandidate& candidate, - const double eps_x, - const EdgeEdgeDistanceType dtype) + const double _eps_x, + const EdgeEdgeDistanceType _dtype) : EdgeEdgeCandidate(candidate) - , eps_x(eps_x) - , dtype(dtype) + , eps_x(_eps_x) + , dtype(_dtype) { } EdgeEdgeConstraint::EdgeEdgeConstraint( - const long edge0_id, - const long edge1_id, - const double eps_x, - const double weight, - const Eigen::SparseVector& weight_gradient, - const EdgeEdgeDistanceType dtype) - : EdgeEdgeCandidate(edge0_id, edge1_id) - , CollisionConstraint(weight, weight_gradient) - , eps_x(eps_x) - , dtype(dtype) + const long _edge0_id, + const long _edge1_id, + const double _eps_x, + const double _weight, + const Eigen::SparseVector& _weight_gradient, + const EdgeEdgeDistanceType _dtype) + : EdgeEdgeCandidate(_edge0_id, _edge1_id) + , CollisionConstraint(_weight, _weight_gradient) + , eps_x(_eps_x) + , dtype(_dtype) { } diff --git a/src/ipc/collisions/edge_vertex.hpp b/src/ipc/collisions/edge_vertex.hpp index 326a1ee21..5d1c09e45 100644 --- a/src/ipc/collisions/edge_vertex.hpp +++ b/src/ipc/collisions/edge_vertex.hpp @@ -16,12 +16,12 @@ class EdgeVertexConstraint : public EdgeVertexCandidate, } EdgeVertexConstraint( - const long edge_id, - const long vertex_id, - const double weight, - const Eigen::SparseVector& weight_gradient) - : EdgeVertexCandidate(edge_id, vertex_id) - , CollisionConstraint(weight, weight_gradient) + const long _edge_id, + const long _vertex_id, + const double _weight, + const Eigen::SparseVector& _weight_gradient) + : EdgeVertexCandidate(_edge_id, _vertex_id) + , CollisionConstraint(_weight, _weight_gradient) { } diff --git a/src/ipc/collisions/face_vertex.hpp b/src/ipc/collisions/face_vertex.hpp index e74d9fdf7..2ba2899d1 100644 --- a/src/ipc/collisions/face_vertex.hpp +++ b/src/ipc/collisions/face_vertex.hpp @@ -16,12 +16,12 @@ class FaceVertexConstraint : public FaceVertexCandidate, } FaceVertexConstraint( - const long face_id, - const long vertex_id, - const double weight, - const Eigen::SparseVector& weight_gradient) - : FaceVertexCandidate(face_id, vertex_id) - , CollisionConstraint(weight, weight_gradient) + const long _face_id, + const long _vertex_id, + const double _weight, + const Eigen::SparseVector& _weight_gradient) + : FaceVertexCandidate(_face_id, _vertex_id) + , CollisionConstraint(_weight, _weight_gradient) { } diff --git a/src/ipc/collisions/plane_vertex.cpp b/src/ipc/collisions/plane_vertex.cpp index 02990dd90..2356d57e6 100644 --- a/src/ipc/collisions/plane_vertex.cpp +++ b/src/ipc/collisions/plane_vertex.cpp @@ -5,12 +5,12 @@ namespace ipc { PlaneVertexConstraint::PlaneVertexConstraint( - const VectorMax3d& plane_origin, - const VectorMax3d& plane_normal, - const long vertex_id) - : plane_origin(plane_origin) - , plane_normal(plane_normal) - , vertex_id(vertex_id) + const VectorMax3d& _plane_origin, + const VectorMax3d& _plane_normal, + const long _vertex_id) + : plane_origin(_plane_origin) + , plane_normal(_plane_normal) + , vertex_id(_vertex_id) { } diff --git a/src/ipc/collisions/vertex_vertex.hpp b/src/ipc/collisions/vertex_vertex.hpp index 3c8ccf189..be3689d58 100644 --- a/src/ipc/collisions/vertex_vertex.hpp +++ b/src/ipc/collisions/vertex_vertex.hpp @@ -17,12 +17,12 @@ class VertexVertexConstraint : public VertexVertexCandidate, } VertexVertexConstraint( - const long vertex0_id, - const long vertex1_id, - const double weight, - const Eigen::SparseVector& weight_gradient) - : VertexVertexCandidate(vertex0_id, vertex1_id) - , CollisionConstraint(weight, weight_gradient) + const long _vertex0_id, + const long _vertex1_id, + const double _weight, + const Eigen::SparseVector& _weight_gradient) + : VertexVertexCandidate(_vertex0_id, _vertex1_id) + , CollisionConstraint(_weight, _weight_gradient) { } diff --git a/src/ipc/distance/point_point.cpp b/src/ipc/distance/point_point.cpp index 785aafedd..115be1bfc 100644 --- a/src/ipc/distance/point_point.cpp +++ b/src/ipc/distance/point_point.cpp @@ -1,4 +1,4 @@ -#include +#include "point_point.hpp" namespace ipc { diff --git a/src/ipc/friction/constraints/edge_edge.cpp b/src/ipc/friction/constraints/edge_edge.cpp index d36e356d5..20d8273b0 100644 --- a/src/ipc/friction/constraints/edge_edge.cpp +++ b/src/ipc/friction/constraints/edge_edge.cpp @@ -83,18 +83,18 @@ VectorMax3d EdgeEdgeFrictionConstraint::relative_velocity( } MatrixMax EdgeEdgeFrictionConstraint::relative_velocity_matrix( - const VectorMax2d& closest_point) const + const VectorMax2d& _closest_point) const { - assert(closest_point.size() == 2); - return edge_edge_relative_velocity_matrix(dim(), closest_point); + assert(_closest_point.size() == 2); + return edge_edge_relative_velocity_matrix(dim(), _closest_point); } MatrixMax EdgeEdgeFrictionConstraint::relative_velocity_matrix_jacobian( - const VectorMax2d& closest_point) const + const VectorMax2d& _closest_point) const { - assert(closest_point.size() == 2); - return edge_edge_relative_velocity_matrix_jacobian(dim(), closest_point); + assert(_closest_point.size() == 2); + return edge_edge_relative_velocity_matrix_jacobian(dim(), _closest_point); } } // namespace ipc diff --git a/src/ipc/friction/constraints/edge_vertex.cpp b/src/ipc/friction/constraints/edge_vertex.cpp index 3a2a4632a..1a94d4f90 100644 --- a/src/ipc/friction/constraints/edge_vertex.cpp +++ b/src/ipc/friction/constraints/edge_vertex.cpp @@ -55,11 +55,11 @@ VectorMax2d EdgeVertexFrictionConstraint::compute_closest_point( const VectorMax12d& positions) const { assert(positions.size() == ndof()); - VectorMax2d closest_point(1); - closest_point[0] = point_edge_closest_point( + VectorMax2d alpha(1); + alpha[0] = point_edge_closest_point( positions.head(dim()), positions.segment(dim(), dim()), positions.tail(dim())); - return closest_point; + return alpha; } MatrixMax @@ -85,19 +85,19 @@ VectorMax3d EdgeVertexFrictionConstraint::relative_velocity( } MatrixMax EdgeVertexFrictionConstraint::relative_velocity_matrix( - const VectorMax2d& closest_point) const + const VectorMax2d& _closest_point) const { - assert(closest_point.size() == 1); - return point_edge_relative_velocity_matrix(dim(), closest_point[0]); + assert(_closest_point.size() == 1); + return point_edge_relative_velocity_matrix(dim(), _closest_point[0]); } MatrixMax EdgeVertexFrictionConstraint::relative_velocity_matrix_jacobian( - const VectorMax2d& closest_point) const + const VectorMax2d& _closest_point) const { - assert(closest_point.size() == 1); + assert(_closest_point.size() == 1); return point_edge_relative_velocity_matrix_jacobian( - dim(), closest_point[0]); + dim(), _closest_point[0]); } } // namespace ipc diff --git a/src/ipc/friction/constraints/face_vertex.cpp b/src/ipc/friction/constraints/face_vertex.cpp index b79da218c..9d4f11e7c 100644 --- a/src/ipc/friction/constraints/face_vertex.cpp +++ b/src/ipc/friction/constraints/face_vertex.cpp @@ -82,19 +82,19 @@ VectorMax3d FaceVertexFrictionConstraint::relative_velocity( } MatrixMax FaceVertexFrictionConstraint::relative_velocity_matrix( - const VectorMax2d& closest_point) const + const VectorMax2d& _closest_point) const { - assert(closest_point.size() == 2); - return point_triangle_relative_velocity_matrix(dim(), closest_point); + assert(_closest_point.size() == 2); + return point_triangle_relative_velocity_matrix(dim(), _closest_point); } MatrixMax FaceVertexFrictionConstraint::relative_velocity_matrix_jacobian( - const VectorMax2d& closest_point) const + const VectorMax2d& _closest_point) const { - assert(closest_point.size() == 2); + assert(_closest_point.size() == 2); return point_triangle_relative_velocity_matrix_jacobian( - dim(), closest_point); + dim(), _closest_point); } } // namespace ipc diff --git a/src/ipc/friction/constraints/vertex_vertex.cpp b/src/ipc/friction/constraints/vertex_vertex.cpp index 632254097..8e06d559f 100644 --- a/src/ipc/friction/constraints/vertex_vertex.cpp +++ b/src/ipc/friction/constraints/vertex_vertex.cpp @@ -74,17 +74,17 @@ VectorMax3d VertexVertexFrictionConstraint::relative_velocity( MatrixMax VertexVertexFrictionConstraint::relative_velocity_matrix( - const VectorMax2d& closest_point) const + const VectorMax2d& _closest_point) const { - assert(closest_point.size() == 0); + assert(_closest_point.size() == 0); return point_point_relative_velocity_matrix(dim()); } MatrixMax VertexVertexFrictionConstraint::relative_velocity_matrix_jacobian( - const VectorMax2d& closest_point) const + const VectorMax2d& _closest_point) const { - assert(closest_point.size() == 0); + assert(_closest_point.size() == 0); return point_point_relative_velocity_matrix_jacobian(dim()); } diff --git a/src/ipc/ipc.cpp b/src/ipc/ipc.cpp index be796563a..d77900e69 100644 --- a/src/ipc/ipc.cpp +++ b/src/ipc/ipc.cpp @@ -53,10 +53,10 @@ double compute_collision_free_stepsize( if (broad_phase_method == BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE_GPU) { #ifdef IPC_TOOLKIT_WITH_CUDA - double min_distance = 0; // TODO + // TODO: Use correct min_distance const double step_size = ccd::gpu::compute_toi_strategy( vertices_t0, vertices_t1, mesh.edges(), mesh.faces(), - max_iterations, min_distance, tolerance); + max_iterations, /*min_distance=*/0, tolerance); if (step_size < 1.0) { return 0.8 * step_size; } diff --git a/src/ipc/utils/intersection.cpp b/src/ipc/utils/intersection.cpp index f7fd194b1..c6f991bae 100644 --- a/src/ipc/utils/intersection.cpp +++ b/src/ipc/utils/intersection.cpp @@ -14,90 +14,104 @@ namespace ipc { #ifdef IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION -bool is_edge_intersecting_triangle_rational( - const Eigen::Vector3d& e0_float, - const Eigen::Vector3d& e1_float, - const Eigen::Vector3d& t0_float, - const Eigen::Vector3d& t1_float, - const Eigen::Vector3d& t2_float) -{ - using namespace rational; - - typedef Eigen::Matrix - Vector3r; - - Vector3r e0, e1, t0, t1, t2; - - for (int d = 0; d < 3; ++d) { - e0[d] = e0_float[d]; - e1[d] = e1_float[d]; - - t0[d] = t0_float[d]; - t1[d] = t1_float[d]; - t2[d] = t2_float[d]; - } - - const Rational d = e0[0] * t0[1] * t1[2] - e0[0] * t0[1] * t2[2] - - e0[0] * t0[2] * t1[1] + e0[0] * t0[2] * t2[1] + e0[0] * t1[1] * t2[2] - - e0[0] * t1[2] * t2[1] - e0[1] * t0[0] * t1[2] + e0[1] * t0[0] * t2[2] - + e0[1] * t0[2] * t1[0] - e0[1] * t0[2] * t2[0] - e0[1] * t1[0] * t2[2] - + e0[1] * t1[2] * t2[0] + e0[2] * t0[0] * t1[1] - e0[2] * t0[0] * t2[1] - - e0[2] * t0[1] * t1[0] + e0[2] * t0[1] * t2[0] + e0[2] * t1[0] * t2[1] - - e0[2] * t1[1] * t2[0] - e1[0] * t0[1] * t1[2] + e1[0] * t0[1] * t2[2] - + e1[0] * t0[2] * t1[1] - e1[0] * t0[2] * t2[1] - e1[0] * t1[1] * t2[2] - + e1[0] * t1[2] * t2[1] + e1[1] * t0[0] * t1[2] - e1[1] * t0[0] * t2[2] - - e1[1] * t0[2] * t1[0] + e1[1] * t0[2] * t2[0] + e1[1] * t1[0] * t2[2] - - e1[1] * t1[2] * t2[0] - e1[2] * t0[0] * t1[1] + e1[2] * t0[0] * t2[1] - + e1[2] * t0[1] * t1[0] - e1[2] * t0[1] * t2[0] - e1[2] * t1[0] * t2[1] - + e1[2] * t1[1] * t2[0]; - if (d.sign() == 0) { - return true; - } - - // t is the parametric coordinate for the edge - const Rational t = - (e0[0] * t0[1] * t1[2] - e0[0] * t0[1] * t2[2] - e0[0] * t0[2] * t1[1] - + e0[0] * t0[2] * t2[1] + e0[0] * t1[1] * t2[2] - e0[0] * t1[2] * t2[1] - - e0[1] * t0[0] * t1[2] + e0[1] * t0[0] * t2[2] + e0[1] * t0[2] * t1[0] - - e0[1] * t0[2] * t2[0] - e0[1] * t1[0] * t2[2] + e0[1] * t1[2] * t2[0] - + e0[2] * t0[0] * t1[1] - e0[2] * t0[0] * t2[1] - e0[2] * t0[1] * t1[0] - + e0[2] * t0[1] * t2[0] + e0[2] * t1[0] * t2[1] - e0[2] * t1[1] * t2[0] - - t0[0] * t1[1] * t2[2] + t0[0] * t1[2] * t2[1] + t0[1] * t1[0] * t2[2] - - t0[1] * t1[2] * t2[0] - t0[2] * t1[0] * t2[1] - + t0[2] * t1[1] * t2[0]) - / d; - - if (t < 0 || t > 1) { - return false; +namespace { + bool is_edge_intersecting_triangle_rational( + const Eigen::Vector3d& e0_float, + const Eigen::Vector3d& e1_float, + const Eigen::Vector3d& t0_float, + const Eigen::Vector3d& t1_float, + const Eigen::Vector3d& t2_float) + { + using namespace rational; + + typedef Eigen::Matrix< + Rational, 3, 1, Eigen::ColMajor | Eigen::DontAlign> + Vector3r; + + Vector3r e0, e1, t0, t1, t2; + + for (int d = 0; d < 3; ++d) { + e0[d] = e0_float[d]; + e1[d] = e1_float[d]; + + t0[d] = t0_float[d]; + t1[d] = t1_float[d]; + t2[d] = t2_float[d]; + } + + const Rational d = e0[0] * t0[1] * t1[2] - e0[0] * t0[1] * t2[2] + - e0[0] * t0[2] * t1[1] + e0[0] * t0[2] * t2[1] + + e0[0] * t1[1] * t2[2] - e0[0] * t1[2] * t2[1] + - e0[1] * t0[0] * t1[2] + e0[1] * t0[0] * t2[2] + + e0[1] * t0[2] * t1[0] - e0[1] * t0[2] * t2[0] + - e0[1] * t1[0] * t2[2] + e0[1] * t1[2] * t2[0] + + e0[2] * t0[0] * t1[1] - e0[2] * t0[0] * t2[1] + - e0[2] * t0[1] * t1[0] + e0[2] * t0[1] * t2[0] + + e0[2] * t1[0] * t2[1] - e0[2] * t1[1] * t2[0] + - e1[0] * t0[1] * t1[2] + e1[0] * t0[1] * t2[2] + + e1[0] * t0[2] * t1[1] - e1[0] * t0[2] * t2[1] + - e1[0] * t1[1] * t2[2] + e1[0] * t1[2] * t2[1] + + e1[1] * t0[0] * t1[2] - e1[1] * t0[0] * t2[2] + - e1[1] * t0[2] * t1[0] + e1[1] * t0[2] * t2[0] + + e1[1] * t1[0] * t2[2] - e1[1] * t1[2] * t2[0] + - e1[2] * t0[0] * t1[1] + e1[2] * t0[0] * t2[1] + + e1[2] * t0[1] * t1[0] - e1[2] * t0[1] * t2[0] + - e1[2] * t1[0] * t2[1] + e1[2] * t1[1] * t2[0]; + if (d.sign() == 0) { + return true; + } + + // t is the parametric coordinate for the edge + const Rational t = (e0[0] * t0[1] * t1[2] - e0[0] * t0[1] * t2[2] + - e0[0] * t0[2] * t1[1] + e0[0] * t0[2] * t2[1] + + e0[0] * t1[1] * t2[2] - e0[0] * t1[2] * t2[1] + - e0[1] * t0[0] * t1[2] + e0[1] * t0[0] * t2[2] + + e0[1] * t0[2] * t1[0] - e0[1] * t0[2] * t2[0] + - e0[1] * t1[0] * t2[2] + e0[1] * t1[2] * t2[0] + + e0[2] * t0[0] * t1[1] - e0[2] * t0[0] * t2[1] + - e0[2] * t0[1] * t1[0] + e0[2] * t0[1] * t2[0] + + e0[2] * t1[0] * t2[1] - e0[2] * t1[1] * t2[0] + - t0[0] * t1[1] * t2[2] + t0[0] * t1[2] * t2[1] + + t0[1] * t1[0] * t2[2] - t0[1] * t1[2] * t2[0] + - t0[2] * t1[0] * t2[1] + t0[2] * t1[1] * t2[0]) + / d; + + if (t < 0 || t > 1) { + return false; + } + + // u is the first barycentric coordinate for the triangle + const Rational u = (-e0[0] * e1[1] * t0[2] + e0[0] * e1[1] * t2[2] + + e0[0] * e1[2] * t0[1] - e0[0] * e1[2] * t2[1] + - e0[0] * t0[1] * t2[2] + e0[0] * t0[2] * t2[1] + + e0[1] * e1[0] * t0[2] - e0[1] * e1[0] * t2[2] + - e0[1] * e1[2] * t0[0] + e0[1] * e1[2] * t2[0] + + e0[1] * t0[0] * t2[2] - e0[1] * t0[2] * t2[0] + - e0[2] * e1[0] * t0[1] + e0[2] * e1[0] * t2[1] + + e0[2] * e1[1] * t0[0] - e0[2] * e1[1] * t2[0] + - e0[2] * t0[0] * t2[1] + e0[2] * t0[1] * t2[0] + + e1[0] * t0[1] * t2[2] - e1[0] * t0[2] * t2[1] + - e1[1] * t0[0] * t2[2] + e1[1] * t0[2] * t2[0] + + e1[2] * t0[0] * t2[1] - e1[2] * t0[1] * t2[0]) + / d; + // v is the second barycentric coordinate for the triangle + const Rational v = (e0[0] * e1[1] * t0[2] - e0[0] * e1[1] * t1[2] + - e0[0] * e1[2] * t0[1] + e0[0] * e1[2] * t1[1] + + e0[0] * t0[1] * t1[2] - e0[0] * t0[2] * t1[1] + - e0[1] * e1[0] * t0[2] + e0[1] * e1[0] * t1[2] + + e0[1] * e1[2] * t0[0] - e0[1] * e1[2] * t1[0] + - e0[1] * t0[0] * t1[2] + e0[1] * t0[2] * t1[0] + + e0[2] * e1[0] * t0[1] - e0[2] * e1[0] * t1[1] + - e0[2] * e1[1] * t0[0] + e0[2] * e1[1] * t1[0] + + e0[2] * t0[0] * t1[1] - e0[2] * t0[1] * t1[0] + - e1[0] * t0[1] * t1[2] + e1[0] * t0[2] * t1[1] + + e1[1] * t0[0] * t1[2] - e1[1] * t0[2] * t1[0] + - e1[2] * t0[0] * t1[1] + e1[2] * t0[1] * t1[0]) + / d; + + return u >= 0 && u <= 1 && v >= 0 && v <= 1 && u + v <= 1; } - - // u is the first barycentric coordinate for the triangle - const Rational u = - (-e0[0] * e1[1] * t0[2] + e0[0] * e1[1] * t2[2] + e0[0] * e1[2] * t0[1] - - e0[0] * e1[2] * t2[1] - e0[0] * t0[1] * t2[2] + e0[0] * t0[2] * t2[1] - + e0[1] * e1[0] * t0[2] - e0[1] * e1[0] * t2[2] - e0[1] * e1[2] * t0[0] - + e0[1] * e1[2] * t2[0] + e0[1] * t0[0] * t2[2] - e0[1] * t0[2] * t2[0] - - e0[2] * e1[0] * t0[1] + e0[2] * e1[0] * t2[1] + e0[2] * e1[1] * t0[0] - - e0[2] * e1[1] * t2[0] - e0[2] * t0[0] * t2[1] + e0[2] * t0[1] * t2[0] - + e1[0] * t0[1] * t2[2] - e1[0] * t0[2] * t2[1] - e1[1] * t0[0] * t2[2] - + e1[1] * t0[2] * t2[0] + e1[2] * t0[0] * t2[1] - - e1[2] * t0[1] * t2[0]) - / d; - // v is the second barycentric coordinate for the triangle - const Rational v = - (e0[0] * e1[1] * t0[2] - e0[0] * e1[1] * t1[2] - e0[0] * e1[2] * t0[1] - + e0[0] * e1[2] * t1[1] + e0[0] * t0[1] * t1[2] - e0[0] * t0[2] * t1[1] - - e0[1] * e1[0] * t0[2] + e0[1] * e1[0] * t1[2] + e0[1] * e1[2] * t0[0] - - e0[1] * e1[2] * t1[0] - e0[1] * t0[0] * t1[2] + e0[1] * t0[2] * t1[0] - + e0[2] * e1[0] * t0[1] - e0[2] * e1[0] * t1[1] - e0[2] * e1[1] * t0[0] - + e0[2] * e1[1] * t1[0] + e0[2] * t0[0] * t1[1] - e0[2] * t0[1] * t1[0] - - e1[0] * t0[1] * t1[2] + e1[0] * t0[2] * t1[1] + e1[1] * t0[0] * t1[2] - - e1[1] * t0[2] * t1[0] - e1[2] * t0[0] * t1[1] - + e1[2] * t0[1] * t1[0]) - / d; - - return u >= 0 && u <= 1 && v >= 0 && v <= 1 && u + v <= 1; -} +} // namespace #endif bool is_edge_intersecting_triangle( diff --git a/tests/src/tests/barrier/test_barrier.cpp b/tests/src/tests/barrier/test_barrier.cpp index c5d04ca11..0c279d154 100644 --- a/tests/src/tests/barrier/test_barrier.cpp +++ b/tests/src/tests/barrier/test_barrier.cpp @@ -77,14 +77,14 @@ TEST_CASE("Barrier derivatives", "[barrier]") } SECTION("Barrier with physical units") { - barrier = [dhat](double d, double p_dhat) { - return dhat * normalized_barrier(d, p_dhat); + barrier = [dhat](double _d, double p_dhat) { + return dhat * normalized_barrier(_d, p_dhat); }; - barrier_gradient = [dhat](double d, double p_dhat) { - return dhat * normalized_barrier_gradient(d, p_dhat); + barrier_gradient = [dhat](double _d, double p_dhat) { + return dhat * normalized_barrier_gradient(_d, p_dhat); }; - barrier_hessian = [dhat](double d, double p_dhat) { - return dhat * normalized_barrier_hessian(d, p_dhat); + barrier_hessian = [dhat](double _d, double p_dhat) { + return dhat * normalized_barrier_hessian(_d, p_dhat); }; } @@ -96,7 +96,7 @@ TEST_CASE("Barrier derivatives", "[barrier]") Eigen::VectorXd fgrad(1); fd::finite_gradient( - d_vec, [&](const Eigen::VectorXd& d) { return barrier(d[0], dhat); }, + d_vec, [&](const Eigen::VectorXd& _d) { return barrier(_d[0], dhat); }, fgrad); Eigen::VectorXd grad(1); @@ -109,7 +109,9 @@ TEST_CASE("Barrier derivatives", "[barrier]") fd::finite_gradient( d_vec, - [&](const Eigen::VectorXd& d) { return barrier_gradient(d[0], dhat); }, + [&](const Eigen::VectorXd& _d) { + return barrier_gradient(_d[0], dhat); + }, fgrad); grad << barrier_hessian(d, dhat); diff --git a/tests/src/tests/ccd/collision_generator.cpp b/tests/src/tests/ccd/collision_generator.cpp index b6e6ef521..3a6631f3f 100644 --- a/tests/src/tests/ccd/collision_generator.cpp +++ b/tests/src/tests/ccd/collision_generator.cpp @@ -2,8 +2,8 @@ #include /* rand */ -TestImpactGenerator::TestImpactGenerator(size_t value, bool rigid) - : rigid(rigid) +TestImpactGenerator::TestImpactGenerator(size_t value, bool _rigid) + : rigid(_rigid) , current_i(0) , max_i(value) { diff --git a/tests/src/tests/ccd/test_nonlinear_ccd.cpp b/tests/src/tests/ccd/test_nonlinear_ccd.cpp index 3fae03e7d..3e46b56ad 100644 --- a/tests/src/tests/ccd/test_nonlinear_ccd.cpp +++ b/tests/src/tests/ccd/test_nonlinear_ccd.cpp @@ -12,12 +12,12 @@ using namespace ipc; class RotationalTrajectory : virtual public NonlinearTrajectory { public: RotationalTrajectory( - const VectorMax3d& point, - const VectorMax3d& center, - const double z_angular_velocity) - : center(center) - , point(point) - , z_angular_velocity(z_angular_velocity) + const VectorMax3d& _point, + const VectorMax3d& _center, + const double _z_angular_velocity) + : center(_center) + , point(_point) + , z_angular_velocity(_z_angular_velocity) { assert(point.size() == point.size()); } @@ -70,17 +70,14 @@ class IntervalRotationalTrajectory : public RotationalTrajectory, public IntervalNonlinearTrajectory { public: IntervalRotationalTrajectory( - const VectorMax3d& point, - const VectorMax3d& center, - const double z_angular_velocity) - : RotationalTrajectory(point, center, z_angular_velocity) + const VectorMax3d& _point, + const VectorMax3d& _center, + const double _z_angular_velocity) + : RotationalTrajectory(_point, _center, _z_angular_velocity) { } - VectorMax3d operator()(const double t) const override - { - return position(center, point, z_angular_velocity, t); - } + using RotationalTrajectory::operator(); VectorMax3I operator()(const filib::Interval& t) const override { @@ -96,7 +93,7 @@ class IntervalRotationalTrajectory : public RotationalTrajectory, class StaticTrajectory : public NonlinearTrajectory { public: - StaticTrajectory(const VectorMax3d& point) : point(point) { } + StaticTrajectory(const VectorMax3d& _point) : point(_point) { } VectorMax3d operator()(const double t) const override { return point; } diff --git a/tests/src/tests/distance/test_edge_edge.cpp b/tests/src/tests/distance/test_edge_edge.cpp index 2531e5844..098473b4b 100644 --- a/tests/src/tests/distance/test_edge_edge.cpp +++ b/tests/src/tests/distance/test_edge_edge.cpp @@ -13,6 +13,15 @@ using namespace ipc; +namespace { +double edge_edge_distance_stacked(const Eigen::VectorXd& x) +{ + assert(x.size() == 12); + return edge_edge_distance( + x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); +}; +} // namespace + TEST_CASE("Edge-edge distance", "[distance][edge-edge]") { double e0y = GENERATE(-10, -1, -1e-4, 0, 1e-4, 1, 10); @@ -254,15 +263,12 @@ TEST_CASE("Edge-edge distance gradient", "[distance][edge-edge][gradient]") const Vector12d grad = edge_edge_distance_gradient(e00, e01, e10, e11); - // Compute the gradient using finite differences Vector12d x; x << e00, e01, e10, e11; - auto f = [](const Eigen::VectorXd& x) { - return edge_edge_distance( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); - }; + + // Compute the gradient using finite differences Eigen::VectorXd fgrad; - fd::finite_gradient(x, f, fgrad); + fd::finite_gradient(x, edge_edge_distance_stacked, fgrad); CAPTURE(e0x, e0y, e0z, edge_edge_distance_type(e00, e01, e10, e11)); CHECK(fd::compare_gradient(grad, fgrad)); @@ -287,15 +293,12 @@ TEST_CASE( double distance = edge_edge_distance(e00, e01, e10, e11); const Vector12d grad = edge_edge_distance_gradient(e00, e01, e10, e11); - // Compute the gradient using finite differences Vector12d x; x << e00, e01, e10, e11; - auto f = [](const Eigen::VectorXd& x) { - return edge_edge_distance( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); - }; + + // Compute the gradient using finite differences Eigen::VectorXd fgrad; - fd::finite_gradient(x, f, fgrad); + fd::finite_gradient(x, edge_edge_distance_stacked, fgrad); CAPTURE(angle, (grad - fgrad).squaredNorm()); CHECK(distance == Catch::Approx(1.0)); diff --git a/tests/src/tests/distance/test_line_line.cpp b/tests/src/tests/distance/test_line_line.cpp index 08f1b8f73..53d850832 100644 --- a/tests/src/tests/distance/test_line_line.cpp +++ b/tests/src/tests/distance/test_line_line.cpp @@ -10,6 +10,15 @@ using namespace ipc; +namespace { +double line_line_distance_stacked(const Eigen::VectorXd& x) +{ + assert(x.size() == 12); + return line_line_distance( + x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); +} +} // namespace + TEST_CASE("Line-line distance", "[distance][line-line]") { double ya = GENERATE(take(10, random(-100.0, 100.0))); @@ -37,13 +46,7 @@ TEST_CASE("Line-line distance gradient", "[distance][line-line][gradient]") x << ea0, ea1, eb0, eb1; Eigen::VectorXd expected_grad; expected_grad.resize(grad.size()); - fd::finite_gradient( - x, - [](const Eigen::VectorXd& x) { - return line_line_distance( - x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); - }, - expected_grad); + fd::finite_gradient(x, line_line_distance_stacked, expected_grad); CAPTURE(ya, yb, (grad - expected_grad).norm()); bool is_grad_correct = fd::compare_gradient(grad, expected_grad); @@ -64,13 +67,7 @@ TEST_CASE("Line-line distance hessian", "[distance][line-line][hessian]") x << ea0, ea1, eb0, eb1; Eigen::MatrixXd expected_hess; expected_hess.resize(hess.rows(), hess.cols()); - fd::finite_hessian( - x, - [](const Eigen::VectorXd& x) { - return line_line_distance( - x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); - }, - expected_hess); + fd::finite_hessian(x, line_line_distance_stacked, expected_hess); CAPTURE(ya, yb, (hess - expected_hess).norm()); CHECK(fd::compare_hessian(hess, expected_hess, 1e-2)); diff --git a/tests/src/tests/distance/test_point_edge.cpp b/tests/src/tests/distance/test_point_edge.cpp index 92c0d7ce3..23a78bc3a 100644 --- a/tests/src/tests/distance/test_point_edge.cpp +++ b/tests/src/tests/distance/test_point_edge.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -10,9 +11,22 @@ using namespace ipc; -TEST_CASE("Point-edge distance", "[distance][point-edge][gradient][hessian]") +namespace { +template double point_edge_distance_stacked(const Eigen::VectorXd& x) +{ + assert(x.size() == 3 * dim); + return point_edge_distance( + x.head(), x.segment(dim), x.tail()); +} +} // namespace + +TEMPLATE_TEST_CASE_SIG( + "Point-edge distance", + "[distance][point-edge][gradient][hessian]", + ((int dim), dim), + 2, + 3) { - int dim = GENERATE(2, 3); double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10); VectorMax3d p = VectorMax3d::Zero(dim); p.y() = expected_distance; @@ -32,12 +46,8 @@ TEST_CASE("Point-edge distance", "[distance][point-edge][gradient][hessian]") // Compute the gradient using finite differences VectorMax9d x(3 * dim); x << p, e0, e1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_edge_distance( - x.head(dim), x.segment(dim, dim), x.tail(dim)); - }; Eigen::VectorXd fgrad; - fd::finite_gradient(x, f, fgrad); + fd::finite_gradient(x, point_edge_distance_stacked, fgrad); CHECK(fd::compare_gradient(grad, fgrad)); } @@ -48,24 +58,22 @@ TEST_CASE("Point-edge distance", "[distance][point-edge][gradient][hessian]") // Compute the gradient using finite differences VectorMax9d x(3 * dim); x << p, e0, e1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_edge_distance( - x.head(dim), x.segment(dim, dim), x.tail(dim)); - }; Eigen::MatrixXd fhess; - fd::finite_hessian(x, f, fhess); + fd::finite_hessian(x, point_edge_distance_stacked, fhess); CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } } -TEST_CASE( +TEMPLATE_TEST_CASE_SIG( "Point-edge distance all types (random edges)", - "[distance][point-edge][gradient][hessian]") + "[distance][point-edge][gradient][hessian]", + ((int dim), dim), + 2, + 3) { const double alpha = GENERATE(range(-1.0, 2.0, 0.1)); const double d = GENERATE(range(-10.0, 10.0, 1.0)); - const int dim = GENERATE(2, 3); const int n_random_edges = 20; for (int i = 0; i < n_random_edges; i++) { @@ -100,12 +108,9 @@ TEST_CASE( // Compute the gradient using finite differences VectorMax9d x(3 * dim); x << p, e0, e1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_edge_distance( - x.head(dim), x.segment(dim, dim), x.tail(dim)); - }; + Eigen::VectorXd fgrad; - fd::finite_gradient(x, f, fgrad); + fd::finite_gradient(x, point_edge_distance_stacked, fgrad); CHECK(fd::compare_gradient(grad, fgrad)); } @@ -116,22 +121,20 @@ TEST_CASE( // Compute the gradient using finite differences VectorMax9d x(3 * dim); x << p, e0, e1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_edge_distance( - x.head(dim), x.segment(dim, dim), x.tail(dim)); - }; Eigen::MatrixXd fhess; - fd::finite_hessian(x, f, fhess); + fd::finite_hessian(x, point_edge_distance_stacked, fhess); CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } } } -TEST_CASE( +TEMPLATE_TEST_CASE_SIG( "Point-edge distance all types", - "[distance][point-edge][gradient][hessian]") + "[distance][point-edge][gradient][hessian]", + ((int dim), dim), + 2, + 3) { - const int dim = GENERATE(2, 3); const double alpha = GENERATE(range(-1.0, 2.0, 0.1)); const double d = GENERATE(range(-10.0, 10.0, 1.0)); @@ -173,12 +176,8 @@ TEST_CASE( // Compute the gradient using finite differences VectorMax9d x(3 * dim); x << p, e0, e1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_edge_distance( - x.head(dim), x.segment(dim, dim), x.tail(dim)); - }; Eigen::VectorXd fgrad; - fd::finite_gradient(x, f, fgrad); + fd::finite_gradient(x, point_edge_distance_stacked, fgrad); CHECK(fd::compare_gradient(grad, fgrad)); } @@ -189,12 +188,8 @@ TEST_CASE( // Compute the gradient using finite differences VectorMax9d x(3 * dim); x << p, e0, e1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_edge_distance( - x.head(dim), x.segment(dim, dim), x.tail(dim)); - }; Eigen::MatrixXd fhess; - fd::finite_hessian(x, f, fhess); + fd::finite_hessian(x, point_edge_distance_stacked, fhess); CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } } diff --git a/tests/src/tests/distance/test_point_line.cpp b/tests/src/tests/distance/test_point_line.cpp index 707d134e9..8b03b0710 100644 --- a/tests/src/tests/distance/test_point_line.cpp +++ b/tests/src/tests/distance/test_point_line.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -12,6 +13,15 @@ using namespace ipc; +namespace { +template double point_line_distance_stacked(const Eigen::VectorXd& x) +{ + assert(x.size() == 3 * dim); + return point_line_distance( + x.head(), x.segment(dim), x.tail()); +} +} // namespace + TEST_CASE("Point-line distance", "[distance][point-line]") { int dim = GENERATE(2, 3); @@ -65,11 +75,12 @@ TEST_CASE("Point-line distance 2", "[distance][point-line]") } } -TEST_CASE("Point-line distance gradient", "[distance][point-line][gradient]") +TEMPLATE_TEST_CASE_SIG( + "Point-line distance gradient", + "[distance][point-line][gradient]", + ((int dim), dim), + 2) { - // int dim = GENERATE(2, 3); - int dim = 2; - double y_point = GENERATE(take(10, random(-10.0, 10.0))); VectorMax3d p = VectorMax3d::Zero(dim); p.y() = y_point; @@ -87,21 +98,20 @@ TEST_CASE("Point-line distance gradient", "[distance][point-line][gradient]") // Compute the gradient using finite differences VectorMax9d x(3 * dim); x << p, e0, e1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_line_distance( - x.head(dim), x.segment(dim, dim), x.tail(dim)); - }; Eigen::VectorXd fgrad; - fd::finite_gradient(x, f, fgrad); + fd::finite_gradient(x, point_line_distance_stacked, fgrad); CAPTURE(dim, y_point, y_line); CHECK(fd::compare_gradient(grad, fgrad)); } -TEST_CASE("Point-line distance hessian", "[distance][point-line][hessian]") +TEMPLATE_TEST_CASE_SIG( + "Point-line distance hessian", + "[distance][point-line][hessian]", + ((int dim), dim), + 2, + 3) { - int dim = GENERATE(2, 3); - double y_point = GENERATE(take(10, random(-10.0, 10.0))); VectorMax3d p = VectorMax3d::Zero(dim); p.y() = y_point; @@ -119,20 +129,19 @@ TEST_CASE("Point-line distance hessian", "[distance][point-line][hessian]") // Compute the gradient using finite differences VectorMax9d x(3 * dim); x << p, e0, e1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_line_distance( - x.head(dim), x.segment(dim, dim), x.tail(dim)); - }; Eigen::MatrixXd fhess; - fd::finite_hessian(x, f, fhess); + fd::finite_hessian(x, point_line_distance_stacked, fhess); CAPTURE(dim, y_point, y_line); CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } -TEST_CASE( +TEMPLATE_TEST_CASE_SIG( "Point-line distance hessian case 1", - "[distance][point-line][hessian][case1]") + "[distance][point-line][hessian][case1]", + ((int dim), dim), + 2, + 3) { Eigen::Vector3d p(-10.8386, 10, -3.91955); Eigen::Vector3d e0(0, 0, -1); @@ -143,11 +152,8 @@ TEST_CASE( // Compute the gradient using finite differences Vector9d x; x << p, e0, e1; - auto f = [](const Eigen::VectorXd& x) { - return point_line_distance(x.head(3), x.segment(3, 3), x.tail(3)); - }; Eigen::MatrixXd fhess; - fd::finite_hessian(x, f, fhess); + fd::finite_hessian(x, point_line_distance_stacked, fhess); CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } diff --git a/tests/src/tests/distance/test_point_plane.cpp b/tests/src/tests/distance/test_point_plane.cpp index 63f0c0dbf..6bf1691c8 100644 --- a/tests/src/tests/distance/test_point_plane.cpp +++ b/tests/src/tests/distance/test_point_plane.cpp @@ -9,6 +9,15 @@ using namespace ipc; +namespace { +double point_plane_distance_stacked(const Eigen::VectorXd& x) +{ + assert(x.size() == 12); + return point_plane_distance( + x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); +}; +} // namespace + TEST_CASE( "Point-plane distance and derivatives (dynamic plane)", "[distance][point-plane][gradient][hessian]") @@ -34,13 +43,7 @@ TEST_CASE( x_vec << p, t0, t1, t2; Eigen::VectorXd expected_grad; expected_grad.resize(grad.size()); - fd::finite_gradient( - x_vec, - [](const Eigen::VectorXd& x) { - return point_plane_distance( - x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); - }, - expected_grad); + fd::finite_gradient(x_vec, point_plane_distance_stacked, expected_grad); CAPTURE((grad - expected_grad).norm()); CHECK(fd::compare_gradient(grad, expected_grad)); @@ -53,13 +56,7 @@ TEST_CASE( x_vec << p, t0, t1, t2; Eigen::MatrixXd expected_hess; expected_hess.resize(hess.rows(), hess.cols()); - fd::finite_hessian( - x_vec, - [](const Eigen::VectorXd& x) { - return point_plane_distance( - x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>()); - }, - expected_hess); + fd::finite_hessian(x_vec, point_plane_distance_stacked, expected_hess); CAPTURE((hess - expected_hess).norm()); CHECK(fd::compare_hessian(hess, expected_hess, 5e-2)); diff --git a/tests/src/tests/distance/test_point_point.cpp b/tests/src/tests/distance/test_point_point.cpp index 6288fb83f..ec405c8e3 100644 --- a/tests/src/tests/distance/test_point_point.cpp +++ b/tests/src/tests/distance/test_point_point.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -9,6 +10,14 @@ using namespace ipc; +namespace { +template double point_point_distance_stacked(const Eigen::VectorXd& x) +{ + assert(x.size() == 2 * dim); + return point_point_distance(x.head(), x.tail()); +} +} // namespace + TEST_CASE("Point-point distance", "[distance][point-point]") { int dim = GENERATE(2, 3); @@ -26,9 +35,13 @@ TEST_CASE("Point-point distance", "[distance][point-point]") CHECK(distance == Catch::Approx(expected_distance * expected_distance)); } -TEST_CASE("Point-point distance gradient", "[distance][point-point][gradient]") +TEMPLATE_TEST_CASE_SIG( + "Point-point distance gradient", + "[distance][point-point][gradient]", + ((int dim), dim), + 2, + 3) { - int dim = GENERATE(2, 3); VectorMax3d p0 = VectorMax3d::Zero(dim); VectorMax3d p1 = VectorMax3d::Zero(dim); double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10); @@ -45,18 +58,19 @@ TEST_CASE("Point-point distance gradient", "[distance][point-point][gradient]") // Compute the gradient using finite differences VectorMax6d x(2 * dim); x << p0, p1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_point_distance(x.head(dim), x.tail(dim)); - }; Eigen::VectorXd fgrad; - fd::finite_gradient(x, f, fgrad); + fd::finite_gradient(x, point_point_distance_stacked, fgrad); CHECK(fd::compare_gradient(grad, fgrad)); } -TEST_CASE("Point-point distance hessian", "[distance][point-point][hessian]") +TEMPLATE_TEST_CASE_SIG( + "Point-point distance hessian", + "[distance][point-point][hessian]", + ((int dim), dim), + 2, + 3) { - int dim = GENERATE(2, 3); VectorMax3d p0 = VectorMax3d::Zero(dim); VectorMax3d p1 = VectorMax3d::Zero(dim); double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10); @@ -73,11 +87,8 @@ TEST_CASE("Point-point distance hessian", "[distance][point-point][hessian]") // Compute the gradient using finite differences VectorMax6d x(2 * dim); x << p0, p1; - auto f = [&dim](const Eigen::VectorXd& x) { - return point_point_distance_gradient(x.head(dim), x.tail(dim)); - }; Eigen::MatrixXd fhess; - fd::finite_jacobian(x, f, fhess); + fd::finite_hessian(x, point_point_distance_stacked, fhess); CAPTURE((hess - fhess).squaredNorm()); CHECK(fd::compare_hessian(hess, fhess)); diff --git a/tests/src/tests/distance/test_point_triangle.cpp b/tests/src/tests/distance/test_point_triangle.cpp index 002711c29..0ca6153fd 100644 --- a/tests/src/tests/distance/test_point_triangle.cpp +++ b/tests/src/tests/distance/test_point_triangle.cpp @@ -1,3 +1,5 @@ +#include + #include #include #include @@ -10,13 +12,14 @@ using namespace ipc; -inline Eigen::Vector2d -edge_normal(const Eigen::Vector2d& e0, const Eigen::Vector2d& e1) +namespace { +double point_triangle_distance_stacked(const Eigen::VectorXd& x) { - Eigen::Vector2d e = e1 - e0; - Eigen::Vector2d normal(-e.y(), e.x()); - return normal.normalized(); + assert(x.size() == 12); + return point_triangle_distance( + x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); } +} // namespace TEST_CASE("Point-triangle distance", "[distance][point-triangle]") { @@ -58,7 +61,7 @@ TEST_CASE("Point-triangle distance", "[distance][point-triangle]") { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); closest_point = (t1 - t0) * alpha + t0; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z())); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); p.x() = closest_point.x() + scale * perp.x(); @@ -68,7 +71,7 @@ TEST_CASE("Point-triangle distance", "[distance][point-triangle]") { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); closest_point = (t2 - t1) * alpha + t1; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z())); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); p.x() = closest_point.x() + scale * perp.x(); @@ -78,7 +81,7 @@ TEST_CASE("Point-triangle distance", "[distance][point-triangle]") { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); closest_point = (t0 - t2) * alpha + t2; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z())); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); p.x() = closest_point.x() + scale * perp.x(); @@ -127,7 +130,7 @@ TEST_CASE( { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); Eigen::Vector3d closest_point = (t1 - t0) * alpha + t0; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z())); // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); @@ -138,7 +141,7 @@ TEST_CASE( { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); Eigen::Vector3d closest_point = (t2 - t1) * alpha + t1; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z())); // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); @@ -149,7 +152,7 @@ TEST_CASE( { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); Eigen::Vector3d closest_point = (t0 - t2) * alpha + t2; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z())); // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); @@ -159,16 +162,12 @@ TEST_CASE( Vector12d x; x << p, t0, t1, t2; - // Compute the gradient using finite differences - auto f = [&](const Eigen::VectorXd& x) { - return point_triangle_distance( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); - }; const Vector12d grad = point_triangle_distance_gradient(p, t0, t1, t2); + // Compute the gradient using finite differences Eigen::VectorXd fgrad; - fd::finite_gradient(x, f, fgrad); + fd::finite_gradient(x, point_triangle_distance_stacked, fgrad); CHECK(fd::compare_gradient(grad, fgrad)); } @@ -207,7 +206,7 @@ TEST_CASE("Point-triangle distance hessian", "[distance][point-triangle][hess]") { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); Eigen::Vector3d closest_point = (t1 - t0) * alpha + t0; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z())); // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); @@ -218,7 +217,7 @@ TEST_CASE("Point-triangle distance hessian", "[distance][point-triangle][hess]") { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); Eigen::Vector3d closest_point = (t2 - t1) * alpha + t1; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z())); // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); @@ -229,7 +228,7 @@ TEST_CASE("Point-triangle distance hessian", "[distance][point-triangle][hess]") { double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); Eigen::Vector3d closest_point = (t0 - t2) * alpha + t2; - Eigen::Vector2d perp = edge_normal( + Eigen::Vector2d perp = tests::edge_normal( Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z())); // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); @@ -242,17 +241,12 @@ TEST_CASE("Point-triangle distance hessian", "[distance][point-triangle][hess]") Vector12d x; x << p, t0, t1, t2; - // Compute the gradient using finite differences - auto f = [&](const Eigen::VectorXd& x) { - return point_triangle_distance( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9), - dtype); - }; const Matrix12d hess = point_triangle_distance_hessian(p, t0, t1, t2); + // Compute the gradient using finite differences Eigen::MatrixXd fhess; - fd::finite_hessian(x, f, fhess); + fd::finite_hessian(x, point_triangle_distance_stacked, fhess); CAPTURE(dtype); CHECK(fd::compare_hessian(hess, fhess, 1e-2)); diff --git a/tests/src/tests/friction/test_closest_point.cpp b/tests/src/tests/friction/test_closest_point.cpp index 0547c635e..00497a312 100644 --- a/tests/src/tests/friction/test_closest_point.cpp +++ b/tests/src/tests/friction/test_closest_point.cpp @@ -35,10 +35,10 @@ TEST_CASE( Eigen::MatrixXd J_FD; fd::finite_jacobian( x, - [&](const Eigen::VectorXd& x) { + [](const Eigen::VectorXd& _x) { return point_triangle_closest_point( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), - x.segment<3>(9)); + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)); }, J_FD); @@ -65,10 +65,10 @@ TEST_CASE("Edge-edge closest point", "[friction][edge-edge][closest_point]") Eigen::MatrixXd J_FD; fd::finite_jacobian( x, - [&](const Eigen::VectorXd& x) { + [](const Eigen::VectorXd& _x) { return edge_edge_closest_point( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), - x.segment<3>(9)); + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)); }, J_FD); @@ -91,9 +91,9 @@ TEST_CASE("Point-edge closest point", "[friction][point-edge][closest_point]") Eigen::VectorXd J_FD; fd::finite_gradient( x, - [&](const Eigen::VectorXd& x) { + [](const Eigen::VectorXd& _x) { return point_edge_closest_point( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6)); + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6)); }, J_FD); @@ -118,9 +118,9 @@ TEST_CASE( Eigen::VectorXd J_FD; fd::finite_gradient( x, - [&](const Eigen::VectorXd& x) { + [](const Eigen::VectorXd& _x) { return point_edge_closest_point( - x.segment<2>(0), x.segment<2>(2), x.segment<2>(4)); + _x.segment<2>(0), _x.segment<2>(2), _x.segment<2>(4)); }, J_FD); diff --git a/tests/src/tests/friction/test_normal_force_magnitude.cpp b/tests/src/tests/friction/test_normal_force_magnitude.cpp index 38a70c777..5e1d63230 100644 --- a/tests/src/tests/friction/test_normal_force_magnitude.cpp +++ b/tests/src/tests/friction/test_normal_force_magnitude.cpp @@ -25,10 +25,9 @@ TEST_CASE( distance, distance_grad, dhat, barrier_stiffness); auto N = [dhat, barrier_stiffness](const Eigen::VectorXd& x) { - const double distance = point_triangle_distance( + const double d = point_triangle_distance( x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); - return compute_normal_force_magnitude( - distance, dhat, barrier_stiffness); + return compute_normal_force_magnitude(d, dhat, barrier_stiffness); }; Vector12d x; @@ -55,10 +54,9 @@ TEST_CASE( distance, distance_grad, dhat, barrier_stiffness); auto N = [dhat, barrier_stiffness](const Eigen::VectorXd& x) { - const double distance = edge_edge_distance( + const double d = edge_edge_distance( x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); - return compute_normal_force_magnitude( - distance, dhat, barrier_stiffness); + return compute_normal_force_magnitude(d, dhat, barrier_stiffness); }; Vector12d x; @@ -83,10 +81,9 @@ TEST_CASE( distance, distance_grad, dhat, barrier_stiffness); auto N = [dhat, barrier_stiffness](const Eigen::VectorXd& x) { - const double distance = point_edge_distance( + const double d = point_edge_distance( x.segment<3>(0), x.segment<3>(3), x.segment<3>(6)); - return compute_normal_force_magnitude( - distance, dhat, barrier_stiffness); + return compute_normal_force_magnitude(d, dhat, barrier_stiffness); }; Vector9d x; @@ -111,9 +108,8 @@ TEST_CASE( distance, distance_grad, dhat, barrier_stiffness); auto N = [dhat, barrier_stiffness](const Eigen::VectorXd& x) { - const double distance = point_point_distance(x.head<3>(), x.tail<3>()); - return compute_normal_force_magnitude( - distance, dhat, barrier_stiffness); + const double d = point_point_distance(x.head<3>(), x.tail<3>()); + return compute_normal_force_magnitude(d, dhat, barrier_stiffness); }; Vector6d x; @@ -137,10 +133,9 @@ TEST_CASE( distance, distance_grad, dhat, barrier_stiffness); auto N = [dhat, barrier_stiffness](const Eigen::VectorXd& x) { - const double distance = point_edge_distance( + const double d = point_edge_distance( x.segment<2>(0), x.segment<2>(2), x.segment<2>(4)); - return compute_normal_force_magnitude( - distance, dhat, barrier_stiffness); + return compute_normal_force_magnitude(d, dhat, barrier_stiffness); }; Vector6d x; @@ -164,9 +159,8 @@ TEST_CASE( distance, distance_grad, dhat, barrier_stiffness); auto N = [dhat, barrier_stiffness](const Eigen::VectorXd& x) { - const double distance = point_point_distance(x.head<2>(), x.tail<2>()); - return compute_normal_force_magnitude( - distance, dhat, barrier_stiffness); + const double d = point_point_distance(x.head<2>(), x.tail<2>()); + return compute_normal_force_magnitude(d, dhat, barrier_stiffness); }; Eigen::Vector4d x; diff --git a/tests/src/tests/friction/test_smooth_friction_mollifier.cpp b/tests/src/tests/friction/test_smooth_friction_mollifier.cpp index b96ce1dc0..5e16eb7cd 100644 --- a/tests/src/tests/friction/test_smooth_friction_mollifier.cpp +++ b/tests/src/tests/friction/test_smooth_friction_mollifier.cpp @@ -31,8 +31,8 @@ TEST_CASE("Smooth friction gradient", "[friction][mollifier]") Eigen::VectorXd fd_f1_over_x(1); fd::finite_gradient( X, - [&](const Eigen::VectorXd& X) { - return ipc::f0_SF(X[0], epsv_times_h); + [&](const Eigen::VectorXd& _X) { + return ipc::f0_SF(_X[0], epsv_times_h); }, fd_f1_over_x); // fd_f1_over_x /= x; @@ -50,8 +50,8 @@ TEST_CASE("Smooth friction gradient", "[friction][mollifier]") Eigen::VectorXd fd_f2(1); fd::finite_gradient( X, - [&](const Eigen::VectorXd& X) { - return ipc::f1_SF_over_x(X[0], epsv_times_h); + [&](const Eigen::VectorXd& _X) { + return ipc::f1_SF_over_x(_X[0], epsv_times_h); }, fd_f2); // fd_f2 /= x; diff --git a/tests/src/tests/friction/test_tangent_basis.cpp b/tests/src/tests/friction/test_tangent_basis.cpp index bfd86535c..4f8d6220c 100644 --- a/tests/src/tests/friction/test_tangent_basis.cpp +++ b/tests/src/tests/friction/test_tangent_basis.cpp @@ -33,10 +33,10 @@ TEST_CASE( Eigen::MatrixXd tmp; fd::finite_jacobian( x, - [](const Eigen::VectorXd& x) -> Eigen::VectorXd { + [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { return point_triangle_tangent_basis( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), - x.segment<3>(9)) + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)) .reshaped(); }, tmp); @@ -74,10 +74,10 @@ TEST_CASE( Eigen::MatrixXd tmp; fd::finite_jacobian( x, - [](const Eigen::VectorXd& x) -> Eigen::VectorXd { + [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { return edge_edge_tangent_basis( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), - x.segment<3>(9)) + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)) .reshaped(); }, tmp); @@ -114,9 +114,9 @@ TEST_CASE( Eigen::MatrixXd tmp; fd::finite_jacobian( x, - [](const Eigen::VectorXd& x) -> Eigen::VectorXd { + [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { return point_edge_tangent_basis( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6)) + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6)) .reshaped(); }, tmp); @@ -153,8 +153,8 @@ TEST_CASE( Eigen::MatrixXd tmp; fd::finite_jacobian( x, - [](const Eigen::VectorXd& x) -> Eigen::VectorXd { - return point_point_tangent_basis(x.head<3>(), x.tail<3>()) + [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + return point_point_tangent_basis(_x.head<3>(), _x.tail<3>()) .reshaped(); }, tmp); @@ -185,9 +185,9 @@ TEST_CASE( Eigen::MatrixXd J_fd; fd::finite_jacobian( x, - [](const Eigen::VectorXd& x) -> Eigen::VectorXd { + [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { return point_edge_tangent_basis( - x.segment<2>(0), x.segment<2>(2), x.segment<2>(4)); + _x.segment<2>(0), _x.segment<2>(2), _x.segment<2>(4)); }, J_fd); J_fd = J_fd.reshaped(); @@ -215,8 +215,8 @@ TEST_CASE( Eigen::MatrixXd J_fd; fd::finite_jacobian( x, - [](const Eigen::VectorXd& x) -> Eigen::VectorXd { - return point_point_tangent_basis(x.head<2>(), x.tail<2>()); + [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + return point_point_tangent_basis(_x.head<2>(), _x.tail<2>()); }, J_fd); J_fd = J_fd.reshaped(); From f95ab1c7a5606411030303bf1211ad47fb7c868a Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 22 Oct 2023 22:31:40 -0600 Subject: [PATCH 3/4] Fix [[noreturn]] --- .../broad_phase/sweep_and_tiniest_queue.hpp | 28 ++++++++----------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp index ae2aa7207..54106857f 100644 --- a/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp +++ b/src/ipc/broad_phase/sweep_and_tiniest_queue.hpp @@ -59,15 +59,13 @@ class SweepAndTiniestQueue : public CopyMeshBroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisisons. - void detect_vertex_vertex_candidates( - std::vector& candidates) const override - [[noreturn]]; + [[noreturn]] void detect_vertex_vertex_candidates( + std::vector& candidates) const override; /// @brief Find the candidate edge-vertex collisisons. /// @param[out] candidates The candidate edge-vertex collisisons. - void detect_edge_vertex_candidates( - std::vector& candidates) const override - [[noreturn]]; + [[noreturn]] void detect_edge_vertex_candidates( + std::vector& candidates) const override; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisisons. @@ -81,8 +79,8 @@ class SweepAndTiniestQueue : public CopyMeshBroadPhase { /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. - void detect_edge_face_candidates( - std::vector& candidates) const override [[noreturn]]; + [[noreturn]] void detect_edge_face_candidates( + std::vector& candidates) const override; protected: long to_edge_id(long id) const; @@ -129,15 +127,13 @@ class SweepAndTiniestQueueGPU : public CopyMeshBroadPhase { /// @brief Find the candidate vertex-vertex collisions. /// @param[out] candidates The candidate vertex-vertex collisisons. - void detect_vertex_vertex_candidates( - std::vector& candidates) const override - [[noreturn]]; + [[noreturn]] void detect_vertex_vertex_candidates( + std::vector& candidates) const override; /// @brief Find the candidate edge-vertex collisisons. /// @param[out] candidates The candidate edge-vertex collisisons. - void detect_edge_vertex_candidates( - std::vector& candidates) const override - [[noreturn]]; + [[noreturn]] void detect_edge_vertex_candidates( + std::vector& candidates) const override; /// @brief Find the candidate edge-edge collisions. /// @param[out] candidates The candidate edge-edge collisisons. @@ -151,8 +147,8 @@ class SweepAndTiniestQueueGPU : public CopyMeshBroadPhase { /// @brief Find the candidate edge-face intersections. /// @param[out] candidates The candidate edge-face intersections. - void detect_edge_face_candidates( - std::vector& candidates) const override [[noreturn]]; + [[noreturn]] void detect_edge_face_candidates( + std::vector& candidates) const override; private: std::vector boxes; From 4268d07dacf9a37c969ba33157589e9ce26b9c11 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 22 Oct 2023 22:59:44 -0600 Subject: [PATCH 4/4] Fix tests --- tests/src/tests/distance/test_point_line.cpp | 9 +++----- .../tests/distance/test_point_triangle.cpp | 21 +++++++++++++++---- 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/tests/src/tests/distance/test_point_line.cpp b/tests/src/tests/distance/test_point_line.cpp index 8b03b0710..c38e56b50 100644 --- a/tests/src/tests/distance/test_point_line.cpp +++ b/tests/src/tests/distance/test_point_line.cpp @@ -136,12 +136,9 @@ TEMPLATE_TEST_CASE_SIG( CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } -TEMPLATE_TEST_CASE_SIG( +TEST_CASE( "Point-line distance hessian case 1", - "[distance][point-line][hessian][case1]", - ((int dim), dim), - 2, - 3) + "[distance][point-line][hessian][case1]") { Eigen::Vector3d p(-10.8386, 10, -3.91955); Eigen::Vector3d e0(0, 0, -1); @@ -153,7 +150,7 @@ TEMPLATE_TEST_CASE_SIG( Vector9d x; x << p, e0, e1; Eigen::MatrixXd fhess; - fd::finite_hessian(x, point_line_distance_stacked, fhess); + fd::finite_hessian(x, point_line_distance_stacked<3>, fhess); CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } diff --git a/tests/src/tests/distance/test_point_triangle.cpp b/tests/src/tests/distance/test_point_triangle.cpp index 0ca6153fd..773d06b19 100644 --- a/tests/src/tests/distance/test_point_triangle.cpp +++ b/tests/src/tests/distance/test_point_triangle.cpp @@ -13,11 +13,14 @@ using namespace ipc; namespace { -double point_triangle_distance_stacked(const Eigen::VectorXd& x) +double point_triangle_distance_stacked( + const Eigen::VectorXd& x, + const PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) { assert(x.size() == 12); return point_triangle_distance( - x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9)); + x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9), + dtype); } } // namespace @@ -167,7 +170,12 @@ TEST_CASE( // Compute the gradient using finite differences Eigen::VectorXd fgrad; - fd::finite_gradient(x, point_triangle_distance_stacked, fgrad); + fd::finite_gradient( + x, + [](const Eigen::VectorXd& _x) { + return point_triangle_distance_stacked(_x); + }, + fgrad); CHECK(fd::compare_gradient(grad, fgrad)); } @@ -246,7 +254,12 @@ TEST_CASE("Point-triangle distance hessian", "[distance][point-triangle][hess]") // Compute the gradient using finite differences Eigen::MatrixXd fhess; - fd::finite_hessian(x, point_triangle_distance_stacked, fhess); + fd::finite_hessian( + x, + [dtype](const Eigen::VectorXd& _x) { + return point_triangle_distance_stacked(_x, dtype); + }, + fhess); CAPTURE(dtype); CHECK(fd::compare_hessian(hess, fhess, 1e-2));