diff --git a/libs/cg/include/psemek/cg/convex/separation.hpp b/libs/cg/include/psemek/cg/convex/separation.hpp index 391d4afc..588804f2 100644 --- a/libs/cg/include/psemek/cg/convex/separation.hpp +++ b/libs/cg/include/psemek/cg/convex/separation.hpp @@ -13,6 +13,8 @@ namespace psemek::cg { T distance = -std::numeric_limits::infinity(); geom::vector direction = geom::vector::zero(); + geom::point contact_point = geom::point::zero(); + int contact_point_body = 0; separation_info & operator |= (separation_info const & i); }; @@ -47,9 +49,10 @@ namespace psemek::cg using result_type = separation_info; - auto process_edges = [](auto const & vs1, auto const & es1, auto const & vs2) + auto process_edges = [](auto const & vs1, auto const & es1, auto const & vs2, int contact_body) { result_type result; + result.contact_point_body = contact_body; for (auto const & e : es1) { @@ -60,7 +63,8 @@ namespace psemek::cg for (auto const & v : vs2) { auto const d = geom::dot(geom::homogeneous(edge_result.direction), geom::homogeneous(v - vs1[e[0]])); - edge_result.distance = std::min(edge_result.distance, d); + if (geom::make_min(edge_result.distance, d)) + edge_result.contact_point = v; } result |= edge_result; @@ -68,14 +72,17 @@ namespace psemek::cg break; } + if (contact_body == 0) + result.direction = -result.direction; + return result; }; result_type result; - result |= process_edges(vs1, es1, vs2); + result |= process_edges(vs1, es1, vs2, 1); if (EarlyExit && result.distance > 0) return result; - result |= process_edges(vs2, es2, vs1); + result |= process_edges(vs2, es2, vs1, 0); return result; } @@ -88,6 +95,9 @@ namespace psemek::cg auto const & fs1 = faces(b1); auto const & fs2 = faces(b2); + auto const & es1 = edges(b1); + auto const & es2 = edges(b2); + auto const & eds1 = edge_directions(b1); auto const & eds2 = edge_directions(b2); @@ -95,9 +105,10 @@ namespace psemek::cg using result_type = separation_info; - auto process_faces = [](auto const & vs1, auto const & fs1, auto const & vs2) + auto process_faces = [](auto const & vs1, auto const & fs1, auto const & vs2, int contact_body) { result_type result; + result.contact_point_body = contact_body; for (auto const & f : fs1) { @@ -110,7 +121,8 @@ namespace psemek::cg for (auto const & v : vs2) { auto const d = geom::dot(face_result.direction, v - face_p); - face_result.distance = std::min(d, face_result.distance); + if (geom::make_min(face_result.distance, d)) + face_result.contact_point = v; } result |= face_result; @@ -118,6 +130,9 @@ namespace psemek::cg break; } + if (contact_body == 0) + result.direction = -result.direction; + return result; }; @@ -171,13 +186,49 @@ namespace psemek::cg }; result_type result; - result |= process_faces(vs1, fs1, vs2); + result |= process_faces(vs1, fs1, vs2, 1); if (EarlyExit && result.distance > 0) return result; - result |= process_faces(vs2, fs2, vs1); + result |= process_faces(vs2, fs2, vs1, 0); if (EarlyExit && result.distance > 0) return result; - result |= process_edges(vs1, eds1, vs2, eds2); + + auto edge_result = process_edges(vs1, eds1, vs2, eds2); + if (geom::make_max(result.distance, edge_result.distance)) + { + result = edge_result; + + auto e1 = es1[0]; + auto e2 = es2[0]; + + { + auto max_d = geom::limits::min(); + for (auto const & e : es1) + { + auto d = geom::dot(geom::homogeneous(vs1[e[0]]), geom::homogeneous(result.direction)) + geom::dot(geom::homogeneous(vs1[e[1]]), geom::homogeneous(result.direction)); + if (geom::make_max(max_d, d)) + e1 = e; + } + } + + { + auto min_d = geom::limits::max(); + for (auto const & e : es2) + { + auto d = geom::dot(geom::homogeneous(vs2[e[0]]), geom::homogeneous(result.direction)) + geom::dot(geom::homogeneous(vs2[e[1]]), geom::homogeneous(result.direction)); + if (geom::make_min(min_d, d)) + e2 = e; + } + } + + result.contact_point_body = 0; + + auto m = geom::cross(result.direction, vs2[e2[1]] - vs2[e2[0]]); + auto v = geom::dot(geom::homogeneous(vs2[e2[0]]), geom::homogeneous(m)); + auto t = (v - geom::dot(geom::homogeneous(vs1[e1[0]]), geom::homogeneous(m))) / geom::dot(geom::homogeneous(vs1[e1[1]] - vs1[e1[0]]), geom::homogeneous(m)); + result.contact_point = geom::lerp(vs1[e1[0]], vs1[e2[1]], geom::clamp(t, geom::interval(scalar_type(0), scalar_type(1)))); + } + return result; }