Skip to content

Commit 1b13d7b

Browse files
SeanCurtis-TRISeanCurtis-TRI
authored
Update the convex-half space primitive collision algorithm (#469)
No primitive algorithm was registered for half space-convex intersection. There was an existing function defined, but with an incompatible interface. This introduces a new function (with compatible interface), fully documents it, tests it, and connects it in as a registered primitive function. There are also incidental formatting changes related to the effort of writing the new primitive algorithm implementation.
1 parent 43f9805 commit 1b13d7b

8 files changed

Lines changed: 490 additions & 45 deletions

File tree

‎include/fcl/geometry/shape/halfspace.h‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -50,9 +50,11 @@
5050
namespace fcl
5151
{
5252

53-
/// @brief Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d;
54-
/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
55-
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space
53+
/// @brief Half Space: this is equivalent to the Planed in ODE. The separation
54+
/// plane is defined as n * x = d. Points in the negative side of the
55+
/// separation plane (i.e. {x | n * x < d}) are inside the half space and points
56+
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are
57+
/// outside the half space.
5658
template <typename S_>
5759
class FCL_EXPORT Halfspace : public ShapeBase<S_>
5860
{

‎include/fcl/narrowphase/detail/gjk_solver_indep-inl.h‎

Lines changed: 24 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -181,27 +181,29 @@ bool GJKSolver_indep<S>::shapeIntersect(
181181

182182
// Shape intersect algorithms not using built-in GJK algorithm
183183
//
184-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
185-
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
186-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
187-
// | box | O | O | | | | | O | O | |
188-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
189-
// | sphere |/////| O | | O | | O | O | O | O |
190-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
191-
// | ellipsoid |/////|////////| | | | | O | O | |
192-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
193-
// | capsule |/////|////////|///////////| | | | O | O | |
194-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
195-
// | cone |/////|////////|///////////|/////////| | | O | O | |
196-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
197-
// | cylinder |/////|////////|///////////|/////////|//////| | O | O | |
198-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
199-
// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O |
200-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
201-
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O |
202-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
203-
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
204-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
184+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
185+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | convex |
186+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
187+
// | box | O | O | | | | | O | O | | |
188+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
189+
// | sphere |/////| O | | O | | O | O | O | O | |
190+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
191+
// | ellipsoid |/////|////////| | | | | O | O | | |
192+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
193+
// | capsule |/////|////////|///////////| | | | O | O | | |
194+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
195+
// | cone |/////|////////|///////////|/////////| | | O | O | | |
196+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
197+
// | cylinder |/////|////////|///////////|/////////|//////| | O | O | | |
198+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
199+
// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | |
200+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
201+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | O |
202+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
203+
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | |
204+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
205+
// | convex |/////|////////|///////////|/////////|//////|//////////|///////|////////////|//////////| |
206+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
205207

206208
#define FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
207209
template <typename S>\
@@ -259,6 +261,7 @@ FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersec
259261
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Capsule, Halfspace, detail::capsuleHalfspaceIntersect)
260262
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cylinder, Halfspace, detail::cylinderHalfspaceIntersect)
261263
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cone, Halfspace, detail::coneHalfspaceIntersect)
264+
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Convex, Halfspace, detail::convexHalfspaceIntersect)
262265

263266
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Plane, detail::spherePlaneIntersect)
264267
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Plane, detail::ellipsoidPlaneIntersect)

‎include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h‎

Lines changed: 24 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -177,27 +177,29 @@ bool GJKSolver_libccd<S>::shapeIntersect(
177177

178178
// Shape intersect algorithms not using libccd
179179
//
180-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
181-
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
182-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
183-
// | box | O | O | | | | | O | O | |
184-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
185-
// | sphere |/////| O | | O | | O | O | O | O |
186-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
187-
// | ellipsoid |/////|////////| | | | | O | O | TODO |
188-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
189-
// | capsule |/////|////////|///////////| | | | O | O | |
190-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
191-
// | cone |/////|////////|///////////|/////////| | | O | O | |
192-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
193-
// | cylinder |/////|////////|///////////|/////////|//////| | O | O | |
194-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
195-
// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O |
196-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
197-
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O |
198-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
199-
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
200-
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
180+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
181+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | convex |
182+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
183+
// | box | O | O | | | | | O | O | | |
184+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
185+
// | sphere |/////| O | | O | | O | O | O | O | |
186+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
187+
// | ellipsoid |/////|////////| | | | | O | O | TODO | |
188+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
189+
// | capsule |/////|////////|///////////| | | | O | O | | |
190+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
191+
// | cone |/////|////////|///////////|/////////| | | O | O | | |
192+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
193+
// | cylinder |/////|////////|///////////|/////////|//////| | O | O | | |
194+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
195+
// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | |
196+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
197+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | O |
198+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
199+
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | |
200+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
201+
// | convex |/////|////////|///////////|/////////|//////|//////////|///////|////////////|//////////| |
202+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
201203

202204
#define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
203205
template <typename S>\
@@ -255,6 +257,7 @@ FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceInterse
255257
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Capsule, Halfspace, detail::capsuleHalfspaceIntersect)
256258
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cylinder, Halfspace, detail::cylinderHalfspaceIntersect)
257259
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cone, Halfspace, detail::coneHalfspaceIntersect)
260+
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Convex, Halfspace, detail::convexHalfspaceIntersect)
258261

259262
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Plane, detail::spherePlaneIntersect)
260263
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Plane, detail::ellipsoidPlaneIntersect)

‎include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h‎

100755100644
Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,12 @@ bool convexHalfspaceIntersect(
101101
const Halfspace<double>& s2, const Transform3<double>& tf2,
102102
Vector3<double>* contact_points, double* penetration_depth, Vector3<double>* normal);
103103

104+
//==============================================================================
105+
extern template bool convexHalfspaceIntersect(
106+
const Convex<double>& convex_C, const Transform3<double>& X_FC,
107+
const Halfspace<double>& half_space_H, const Transform3<double>& X_FH,
108+
std::vector<ContactPoint<double>>* contacts);
109+
104110
//==============================================================================
105111
extern template
106112
bool halfspaceTriangleIntersect(
@@ -490,6 +496,7 @@ bool coneHalfspaceIntersect(const Cone<S>& s1, const Transform3<S>& tf1,
490496
}
491497

492498
//==============================================================================
499+
// TODO(SeanCurtis-TRI): This is generally unused in FCL. Consider killing it.
493500
template <typename S>
494501
bool convexHalfspaceIntersect(const Convex<S>& s1, const Transform3<S>& tf1,
495502
const Halfspace<S>& s2, const Transform3<S>& tf2,
@@ -500,6 +507,10 @@ bool convexHalfspaceIntersect(const Convex<S>& s1, const Transform3<S>& tf1,
500507
Vector3<S> v;
501508
S depth = std::numeric_limits<S>::max();
502509

510+
// Note: There are two issues with this for loop:
511+
// 1. We are transforming *every* vertex in the convex. That's a waste.
512+
// 2. If we don't report contact results, and we detect collision with the
513+
// first vertex, we still process all vertices. Also a waste.
503514
for (const auto& vertex : s1.getVertices())
504515
{
505516
Vector3<S> p = tf1 * vertex;
@@ -514,15 +525,62 @@ bool convexHalfspaceIntersect(const Convex<S>& s1, const Transform3<S>& tf1,
514525

515526
if(depth <= 0)
516527
{
528+
// Note: this value for contact_point only works because depth is really
529+
// signed distance, so negating the normal cancels the negation of the
530+
// "penetration depth".
517531
if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth);
532+
// TODO(SeanCurtis-TRI): This appears to have the wrong sign for depth.
533+
// We've actually computed *signed distance* which is -depth.
518534
if(penetration_depth) *penetration_depth = depth;
535+
// Note: This points *into* the half space. It is not clear this matches
536+
// any documented convention.
519537
if(normal) *normal = -new_s2.n;
520538
return true;
521539
}
522540
else
523541
return false;
524542
}
525543

544+
//==============================================================================
545+
template <typename S>
546+
bool convexHalfspaceIntersect(const Convex<S>& convex_C,
547+
const Transform3<S>& X_FC,
548+
const Halfspace<S>& half_space_H,
549+
const Transform3<S>& X_FH,
550+
std::vector<ContactPoint<S>>* contacts) {
551+
Halfspace<S> half_space_C = transform(half_space_H, X_FC.inverse() * X_FH);
552+
553+
Vector3<S> p_CV_deepest;
554+
S min_signed_distance = std::numeric_limits<S>::max();
555+
556+
// TODO: Once we have an efficient "support vector" implementation for Convex
557+
// (necessary to make GJK run faster with convex), this could benefit by
558+
// simply asking for the support vector in the negative normal direction.
559+
// That would also make computing normal_C cheaper; it could just be the
560+
// product: X_FC.linear().transpose() * X_FH.linear() * half_space_H.n.
561+
for (const auto& p_CV : convex_C.getVertices()) {
562+
const S signed_distance = half_space_C.signedDistance(p_CV);
563+
if (signed_distance < min_signed_distance) {
564+
min_signed_distance = signed_distance;
565+
p_CV_deepest = p_CV;
566+
if (signed_distance <= 0 && contacts == nullptr) return true;
567+
}
568+
}
569+
570+
const bool intersecting = min_signed_distance <= 0;
571+
572+
if (intersecting && contacts) {
573+
const Vector3<S> normal_F = X_FH.linear() * half_space_H.n;
574+
const Vector3<S> p_FV = X_FC * p_CV_deepest;
575+
// NOTE: penetration depth is defined as the negative of signed distance.
576+
// So, the depth reported here will always be non-negative.
577+
const S depth = -min_signed_distance;
578+
contacts->emplace_back(-normal_F, p_FV + normal_F * (0.5 * depth), depth);
579+
}
580+
581+
return intersecting;
582+
}
583+
526584
//==============================================================================
527585
template <typename S>
528586
bool halfspaceTriangleIntersect(const Halfspace<S>& s1, const Transform3<S>& tf1,

‎include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h‎

100755100644
Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,52 @@ bool convexHalfspaceIntersect(const Convex<S>& s1, const Transform3<S>& tf1,
119119
const Halfspace<S>& s2, const Transform3<S>& tf2,
120120
Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal);
121121

122+
/// @brief Reports whether a convex mesh and half space are intersecting.
123+
/// If `contacts` is not `nullptr` and the two geometries are intersecting,
124+
/// a new ContactPoint will be added to the data.
125+
///
126+
/// The two geometries are considered to be free of intersection if and only if
127+
/// all points in the Convex mesh lie strictly _outside_ the half space. Lying
128+
/// *on* the boundary of the half space is considered intersecting.
129+
///
130+
/// The two geometries are each defined in their own frames, but we have
131+
/// transforms to a common frame F.
132+
///
133+
/// If the two geometries are intersecting and `contacts` is not `nullptr`, the
134+
/// new ContactPoint.normal will point in the opposite direction as the half
135+
/// space normal (expressed in Frame F) -- it points _into_ the half space. We
136+
/// define the point P to be a point of `convex_C` that most deeply penetrates
137+
/// into the half space. It is not guaranteed to be unique. If it is not unique,
138+
/// it will be arbitrarily selected from the set of all such points. The
139+
/// ContactPoint.penetration_depth value is the depth of P. ContactPoint.pos is
140+
/// defined as the point halfway between P and the nearest point on the boundary
141+
/// of the half space, measured and expressed in F.
142+
///
143+
/// ContactPoint is documented to report contact position in the world frame W.
144+
/// This function will only truly satisfy that requirement if F = W. It is the
145+
/// responsibility of the caller to understand the semantics of F and confirm
146+
/// that it satisfies that requirement.
147+
///
148+
/// This makes use of the
149+
/// [Drake monogram notation](http://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html)
150+
/// to describe quantities (particularly the poses of shapes).
151+
///
152+
/// @param convex_C The convex mesh. Its vertex positions are measured and
153+
/// expressed in Frame C.
154+
/// @param X_FC The transform relating Frame C with the common frame F.
155+
/// @param half_space_H The half space. The position and orientation of its
156+
/// boundary plane are measured and expressed in Frame H.
157+
/// @param X_FH The transform relating Frame H with the common frame F.
158+
/// @param[out] contacts The optional accumulator for data characterizing the
159+
/// intersection.
160+
/// @return `true` if the two geometries are intersecting.
161+
/// @tparam S The computational scalar.
162+
template <typename S>
163+
FCL_EXPORT bool convexHalfspaceIntersect(
164+
const Convex<S>& convex_C, const Transform3<S>& X_FC,
165+
const Halfspace<S>& half_space_H, const Transform3<S>& X_FH,
166+
std::vector<ContactPoint<S>>* contacts);
167+
122168
template <typename S>
123169
FCL_EXPORT
124170
bool halfspaceTriangleIntersect(const Halfspace<S>& s1, const Transform3<S>& tf1,

0 commit comments

Comments
 (0)