Skip to content

Commit d8f646c

Browse files
committed
SupportSetFunc: simplify the ShapeSupportData struct
We only need the polygon representation, not the full support set.
1 parent 104b6f1 commit d8f646c

File tree

3 files changed

+66
-48
lines changed

3 files changed

+66
-48
lines changed

include/hpp/fcl/narrowphase/support_functions.h

+7-6
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,8 @@ struct HPP_FCL_DLLAPI ShapeSupportData {
8888
Vec3f last_dir = Vec3f::Zero();
8989

9090
// @brief Temporary set used to compute the convex-hull of a support set.
91-
SupportSet support_set = SupportSet(0);
91+
// Only used for ConvexBase and Box.
92+
SupportSet::Polygon polygon;
9293
};
9394

9495
/// @brief Triangle support function.
@@ -312,11 +313,11 @@ HPP_FCL_DLLAPI void getShapeSupportSet(const LargeConvex* convex,
312313

313314
/// @brief Computes the convex-hull of support_set. For now, this function is
314315
/// only needed for Box and ConvexBase.
315-
/// @param[in] Data which contains the support set which convex-hull we want to
316-
/// compute.
317-
/// @param[out] Convex-hull of the support set.
318-
HPP_FCL_DLLAPI void computeSupportSetConvexHull(
319-
SupportSet& support_set_data, SupportSet& support_set_cvx_hull);
316+
/// @param[in] cloud data which contains the 2d points of the support set which
317+
/// convex-hull we want to compute.
318+
/// @param[out] 2d points of the the support set's convex-hull.
319+
HPP_FCL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
320+
SupportSet::Polygon& cvx_hull);
320321

321322
} // namespace details
322323

src/contact_patch/contact_patch_solver.cpp

+3-5
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape,
6969
return details::getShapeSupportSetTpl<TriangleP, Options::NoSweptSphere>;
7070
case GEOM_BOX: {
7171
const size_t num_corners_box = 8;
72-
support_data.support_set.points().reserve(num_corners_box);
72+
support_data.polygon.reserve(num_corners_box);
7373
return details::getShapeSupportSetTpl<Box, Options::NoSweptSphere>;
7474
}
7575
case GEOM_SPHERE:
@@ -84,10 +84,8 @@ ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape,
8484
return details::getShapeSupportSetTpl<Cylinder, Options::NoSweptSphere>;
8585
case GEOM_CONVEX: {
8686
const ConvexBase* convex = static_cast<const ConvexBase*>(shape);
87-
if (support_data.support_set.points().capacity() <
88-
default_num_preallocated_supports) {
89-
support_data.support_set.points().reserve(
90-
default_num_preallocated_supports);
87+
if (support_data.polygon.capacity() < default_num_preallocated_supports) {
88+
support_data.polygon.reserve(default_num_preallocated_supports);
9189
}
9290
if ((size_t)(convex->num_points) >
9391
ConvexBase::num_vertices_large_convex_threshold) {

src/narrowphase/support_functions.cpp

+56-37
Original file line numberDiff line numberDiff line change
@@ -560,26 +560,30 @@ void getShapeSupportSet(const Box* box, SupportSet& support_set,
560560
const FCL_REAL x = box->halfSide[0];
561561
const FCL_REAL y = box->halfSide[1];
562562
const FCL_REAL z = box->halfSide[2];
563-
support_data.support_set.points().clear();
564-
support_data.support_set.direction = support_set.direction;
565-
support_data.support_set.tf = support_set.tf;
566563
const std::array<Vec3f, 8> corners = {
567564
Vec3f(x, y, z), Vec3f(-x, y, z), Vec3f(-x, -y, z), Vec3f(x, -y, z),
568565
Vec3f(x, y, -z), Vec3f(-x, y, -z), Vec3f(-x, -y, -z), Vec3f(x, -y, -z),
569566
};
570567

568+
SupportSet::Polygon& polygon = support_data.polygon;
569+
polygon.clear();
570+
const Transform3f& tf = support_set.tf;
571571
for (const Vec3f& corner : corners) {
572572
const FCL_REAL val = corner.dot(support_dir);
573573
if (support_value - val < tol) {
574574
if (_SupportOptions == SupportOptions::WithSweptSphere) {
575-
support_data.support_set.addPoint(corner + box->getSweptSphereRadius() *
576-
support_dir);
575+
const Vec2f p =
576+
tf.inverseTransform(corner +
577+
box->getSweptSphereRadius() * support_dir)
578+
.template head<2>();
579+
polygon.emplace_back(p);
577580
} else {
578-
support_data.support_set.addPoint(corner);
581+
const Vec2f p = tf.inverseTransform(corner).template head<2>();
582+
polygon.emplace_back(p);
579583
}
580584
}
581585
}
582-
computeSupportSetConvexHull(support_data.support_set, support_set);
586+
computeSupportSetConvexHull(polygon, support_set.points());
583587
}
584588
getShapeSupportSetTplInstantiation(Box);
585589

@@ -804,54 +808,64 @@ void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set,
804808
const FCL_REAL support_value = support_dir.dot(support);
805809

806810
const std::vector<Vec3f>& points = *(convex->points);
807-
support_data.support_set.points().clear();
808-
support_data.support_set.direction = support_set.direction;
809-
support_data.support_set.tf = support_set.tf;
811+
SupportSet::Polygon& polygon = support_data.polygon;
812+
polygon.clear();
813+
const Transform3f& tf = support_set.tf;
810814
for (const Vec3f& point : points) {
811815
const FCL_REAL dot = support_dir.dot(point);
812816
if (support_value - dot <= tol) {
813817
if (_SupportOptions == SupportOptions::WithSweptSphere) {
814-
support_data.support_set.addPoint(
815-
point + convex->getSweptSphereRadius() * support_dir);
818+
const Vec2f p =
819+
tf.inverseTransform(point +
820+
convex->getSweptSphereRadius() * support_dir)
821+
.template head<2>();
822+
polygon.emplace_back(p);
816823
} else {
817-
support_data.support_set.addPoint(point);
824+
const Vec2f p = tf.inverseTransform(point).template head<2>();
825+
polygon.emplace_back(p);
818826
}
819827
}
820828
}
821829

822-
computeSupportSetConvexHull(support_data.support_set, support_set);
830+
computeSupportSetConvexHull(polygon, support_set.points());
823831
}
824832

825833
// ============================================================================
826834
template <int _SupportOptions>
827-
void convexSupportSetRecurse(const ConvexBase* convex, const size_t vertex_idx,
828-
const Vec3f& support_dir,
829-
const FCL_REAL support_value,
830-
ShapeSupportData& support_data, FCL_REAL tol) {
831-
std::vector<int8_t>& visited = support_data.visited;
835+
void convexSupportSetRecurse(
836+
const std::vector<Vec3f>& points,
837+
const std::vector<ConvexBase::Neighbors>& neighbors,
838+
const FCL_REAL swept_sphere_radius, const size_t vertex_idx,
839+
const Vec3f& support_dir, const FCL_REAL support_value,
840+
const Transform3f& tf, std::vector<int8_t>& visited,
841+
SupportSet::Polygon& polygon, FCL_REAL tol) {
842+
HPP_FCL_UNUSED_VARIABLE(swept_sphere_radius);
843+
832844
if (visited[vertex_idx]) {
833845
return;
834846
}
835847

836-
const std::vector<Vec3f>& points = *(convex->points);
837-
const std::vector<ConvexBase::Neighbors>& neighbors = *(convex->neighbors);
838848
visited[vertex_idx] = true;
839849
const Vec3f& point = points[vertex_idx];
840850
const FCL_REAL val = point.dot(support_dir);
841851
if (support_value - val <= tol) {
842852
if (_SupportOptions == SupportOptions::WithSweptSphere) {
843-
support_data.support_set.addPoint(point + convex->getSweptSphereRadius() *
844-
support_dir);
853+
const Vec2f p =
854+
tf.inverseTransform(point + swept_sphere_radius * support_dir)
855+
.template head<2>();
856+
polygon.emplace_back(p);
857+
845858
} else {
846-
support_data.support_set.addPoint(point);
859+
const Vec2f p = tf.inverseTransform(point).template head<2>();
860+
polygon.emplace_back(p);
847861
}
848862

849863
const ConvexBase::Neighbors& point_neighbors = neighbors[vertex_idx];
850864
for (int i = 0; i < point_neighbors.count(); ++i) {
851865
const size_t neighbor_index = (size_t)(point_neighbors[i]);
852-
convexSupportSetRecurse<_SupportOptions>(convex, neighbor_index,
853-
support_dir, support_value,
854-
support_data, tol);
866+
convexSupportSetRecurse<_SupportOptions>(
867+
points, neighbors, swept_sphere_radius, neighbor_index, support_dir,
868+
support_value, tf, visited, polygon, tol);
855869
}
856870
}
857871
}
@@ -868,17 +882,24 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set,
868882
convex, support_dir, support, hint, support_data);
869883
const FCL_REAL support_value = support.dot(support_dir);
870884

871-
support_data.support_set.points().clear();
872-
support_data.support_set.direction = support_set.direction;
873-
support_data.support_set.tf = support_set.tf;
885+
const std::vector<Vec3f>& points = *(convex->points);
886+
const std::vector<ConvexBase::Neighbors>& neighbors = *(convex->neighbors);
887+
const FCL_REAL swept_sphere_radius = convex->getSweptSphereRadius();
888+
std::vector<int8_t>& visited = support_data.visited;
874889
// `visited` is guaranteed to be of right size due to previous call to convex
875890
// log support function.
876891
std::fill(support_data.visited.begin(), support_data.visited.end(), false);
892+
893+
SupportSet::Polygon& polygon = support_data.polygon;
894+
polygon.clear();
895+
const Transform3f& tf = support_set.tf;
896+
877897
const size_t vertex_idx = (size_t)(hint);
878-
convexSupportSetRecurse<_SupportOptions>(convex, vertex_idx, support_dir,
879-
support_value, support_data, tol);
898+
convexSupportSetRecurse<_SupportOptions>(
899+
points, neighbors, swept_sphere_radius, vertex_idx, support_dir,
900+
support_value, tf, visited, polygon, tol);
880901

881-
computeSupportSetConvexHull(support_data.support_set, support_set);
902+
computeSupportSetConvexHull(polygon, support_set.points());
882903
}
883904

884905
// ============================================================================
@@ -921,10 +942,8 @@ void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set,
921942
getShapeSupportSetTplInstantiation(LargeConvex);
922943

923944
// ============================================================================
924-
HPP_FCL_DLLAPI void computeSupportSetConvexHull(
925-
SupportSet& support_set, SupportSet& support_set_cvx_hull) {
926-
SupportSet::Polygon& cloud = support_set.points();
927-
SupportSet::Polygon& cvx_hull = support_set_cvx_hull.points();
945+
HPP_FCL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
946+
SupportSet::Polygon& cvx_hull) {
928947
cvx_hull.clear();
929948

930949
if (cloud.size() <= 2) {

0 commit comments

Comments
 (0)