lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Compute contact point in cg::separation

Browse source
This commit is contained in:
Nikita Lisitsa 2024-03-15 23:27:10 +03:00
parent 49eafc4806
commit 52969c718a
1 changed files with 60 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

69
libs/cg/include/psemek/cg/convex/separation.hpp
View file

@ -13,6 +13,8 @@ namespace psemek::cg
{
T distance = -std::numeric_limits<T>::infinity();
geom::vector<T, N> direction = geom::vector<T, N>::zero();
geom::point<T, N> contact_point = geom::point<T, N>::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<scalar_type, 2>;
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<scalar_type, 3>;
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<scalar_type>::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<scalar_type>::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;
}