diff --git a/libs/cg/include/psemek/cg/convex/separation.hpp b/libs/cg/include/psemek/cg/convex/separation.hpp index f80b43ae..965e4bc8 100644 --- a/libs/cg/include/psemek/cg/convex/separation.hpp +++ b/libs/cg/include/psemek/cg/convex/separation.hpp @@ -34,7 +34,7 @@ namespace psemek::cg namespace detail { - template + template auto separation_2d(Body1 const & b1, Body2 const & b2) { auto const & vs1 = vertices(b1); @@ -64,6 +64,8 @@ namespace psemek::cg } result |= edge_result; + if (EarlyExit && result.distance > 0) + break; } return result; @@ -71,11 +73,13 @@ namespace psemek::cg result_type result; result |= process_edges(vs1, es1, vs2); + if (EarlyExit && result.distance > 0) + return result; result |= process_edges(vs2, es2, vs1); return result; } - template + template auto separation_3d(Body1 const & b1, Body2 const & b2) { auto const & vs1 = vertices(b1); @@ -110,6 +114,8 @@ namespace psemek::cg } result |= face_result; + if (EarlyExit && result.distance > 0) + break; } return result; @@ -155,6 +161,9 @@ namespace psemek::cg } result |= edge_result; + + if (EarlyExit && result.distance > 0) + return result; } } @@ -163,7 +172,11 @@ namespace psemek::cg result_type result; result |= process_faces(vs1, fs1, vs2); + if (EarlyExit && result.distance > 0) + return result; result |= process_faces(vs2, fs2, vs1); + if (EarlyExit && result.distance > 0) + return result; result |= process_edges(vs1, eds1, vs2, eds2); return result; } @@ -181,11 +194,34 @@ namespace psemek::cg if constexpr (dim1 == 2) { - return detail::separation_2d(b1, b2); + return detail::separation_2d(b1, b2); } else if constexpr (dim1 == 3) { - return detail::separation_3d(b1, b2); + return detail::separation_3d(b1, b2); + } + else + { + return; + } + } + + template + auto intersect(Body1 const & b1, Body2 const & b2) + { + constexpr auto dim1 = dimension; + constexpr auto dim2 = dimension; + + static_assert(dim1 == dim2); + static_assert(dim1 == 2 || dim1 == 3); + + if constexpr (dim1 == 2) + { + return detail::separation_2d(b1, b2).distance <= 0; + } + else if constexpr (dim1 == 3) + { + return detail::separation_3d(b1, b2).distance <= 0; } else {