@@ -560,26 +560,30 @@ void getShapeSupportSet(const Box* box, SupportSet& support_set,
560
560
const FCL_REAL x = box->halfSide [0 ];
561
561
const FCL_REAL y = box->halfSide [1 ];
562
562
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 ;
566
563
const std::array<Vec3f, 8 > corners = {
567
564
Vec3f (x, y, z), Vec3f (-x, y, z), Vec3f (-x, -y, z), Vec3f (x, -y, z),
568
565
Vec3f (x, y, -z), Vec3f (-x, y, -z), Vec3f (-x, -y, -z), Vec3f (x, -y, -z),
569
566
};
570
567
568
+ SupportSet::Polygon& polygon = support_data.polygon ;
569
+ polygon.clear ();
570
+ const Transform3f& tf = support_set.tf ;
571
571
for (const Vec3f& corner : corners) {
572
572
const FCL_REAL val = corner.dot (support_dir);
573
573
if (support_value - val < tol) {
574
574
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);
577
580
} else {
578
- support_data.support_set .addPoint (corner);
581
+ const Vec2f p = tf.inverseTransform (corner).template head <2 >();
582
+ polygon.emplace_back (p);
579
583
}
580
584
}
581
585
}
582
- computeSupportSetConvexHull (support_data. support_set , support_set);
586
+ computeSupportSetConvexHull (polygon , support_set. points () );
583
587
}
584
588
getShapeSupportSetTplInstantiation (Box);
585
589
@@ -804,54 +808,64 @@ void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set,
804
808
const FCL_REAL support_value = support_dir.dot (support);
805
809
806
810
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 ;
810
814
for (const Vec3f& point : points) {
811
815
const FCL_REAL dot = support_dir.dot (point);
812
816
if (support_value - dot <= tol) {
813
817
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);
816
823
} else {
817
- support_data.support_set .addPoint (point);
824
+ const Vec2f p = tf.inverseTransform (point).template head <2 >();
825
+ polygon.emplace_back (p);
818
826
}
819
827
}
820
828
}
821
829
822
- computeSupportSetConvexHull (support_data. support_set , support_set);
830
+ computeSupportSetConvexHull (polygon , support_set. points () );
823
831
}
824
832
825
833
// ============================================================================
826
834
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
+
832
844
if (visited[vertex_idx]) {
833
845
return ;
834
846
}
835
847
836
- const std::vector<Vec3f>& points = *(convex->points );
837
- const std::vector<ConvexBase::Neighbors>& neighbors = *(convex->neighbors );
838
848
visited[vertex_idx] = true ;
839
849
const Vec3f& point = points[vertex_idx];
840
850
const FCL_REAL val = point.dot (support_dir);
841
851
if (support_value - val <= tol) {
842
852
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
+
845
858
} else {
846
- support_data.support_set .addPoint (point);
859
+ const Vec2f p = tf.inverseTransform (point).template head <2 >();
860
+ polygon.emplace_back (p);
847
861
}
848
862
849
863
const ConvexBase::Neighbors& point_neighbors = neighbors[vertex_idx];
850
864
for (int i = 0 ; i < point_neighbors.count (); ++i) {
851
865
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);
855
869
}
856
870
}
857
871
}
@@ -868,17 +882,24 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set,
868
882
convex, support_dir, support, hint, support_data);
869
883
const FCL_REAL support_value = support.dot (support_dir);
870
884
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 ;
874
889
// `visited` is guaranteed to be of right size due to previous call to convex
875
890
// log support function.
876
891
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
+
877
897
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);
880
901
881
- computeSupportSetConvexHull (support_data. support_set , support_set);
902
+ computeSupportSetConvexHull (polygon , support_set. points () );
882
903
}
883
904
884
905
// ============================================================================
@@ -921,10 +942,8 @@ void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set,
921
942
getShapeSupportSetTplInstantiation (LargeConvex);
922
943
923
944
// ============================================================================
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) {
928
947
cvx_hull.clear ();
929
948
930
949
if (cloud.size () <= 2 ) {
0 commit comments