diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h index 5ebd39936..09a86f52e 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h @@ -496,12 +496,12 @@ bool coneHalfspaceIntersect(const Cone& s1, const Transform3& tf1, } //============================================================================== +// TODO(SeanCurtis-TRI): This is generally unused in FCL. Consider killing it. template bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, Vector3* contact_points, S* penetration_depth, Vector3* normal) { -// TODO(SeanCurtis-TRI): This is generally unused in FCL. Consider killing it. Halfspace new_s2 = transform(s2, tf2); Vector3 v; @@ -551,7 +551,7 @@ bool convexHalfspaceIntersect(const Convex& convex_C, Halfspace half_space_C = transform(half_space_H, X_FC.inverse() * X_FH); Vector3 p_CV_deepest; - S minimum_distance = std::numeric_limits::max(); + S min_signed_distance = std::numeric_limits::max(); // TODO: Once we have an efficient "support vector" implementation for Convex // (necessary to make GJK run faster with convex), this could benefit by @@ -559,21 +559,22 @@ bool convexHalfspaceIntersect(const Convex& convex_C, // That would also make computing normal_C cheaper; it could just be the // product: X_FC.linear().transpose() * X_FH.linear() * half_space_H.n. for (const auto& p_CV : convex_C.getVertices()) { - const S distance = half_space_C.signedDistance(p_CV); - if (distance < minimum_distance) { - minimum_distance = distance; + const S signed_distance = half_space_C.signedDistance(p_CV); + if (signed_distance < min_signed_distance) { + min_signed_distance = signed_distance; p_CV_deepest = p_CV; - if (distance <= 0 && contacts == nullptr) return true; + if (signed_distance <= 0 && contacts == nullptr) return true; } } - const bool intersecting = minimum_distance <= 0; + const bool intersecting = min_signed_distance <= 0; if (intersecting && contacts) { const Vector3 normal_F = X_FH.linear() * half_space_H.n; const Vector3 p_FV = X_FC * p_CV_deepest; - // NOTE: penetration depth is defined as negative signed distance. - const S depth = -minimum_distance; + // NOTE: penetration depth is defined as the negative of signed distance. + // So, the depth reported here will always be non-negative. + const S depth = -min_signed_distance; contacts->emplace_back(-normal_F, p_FV + normal_F * (0.5 * depth), depth); } diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h index 900c8a0ba..6bee9daad 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h @@ -131,13 +131,23 @@ bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, /// transforms to a common frame F. /// /// If the two geometries are intersecting and `contacts` is not `nullptr`, the -/// new ContactPoint.normal will be antiparallel to the half space normal -/// (expressed in Frame F). We define the point P to be a point of `convex_C` -/// that most deeply penetrates into the half space. It is not guaranteed to be -/// unique. If it is not unique, it will be arbitrarily selected from the set of -/// all such points. The ContactPoint.penetration_depth value is the depth of P. -/// ContactPoint.pos is defined as the point between P and the nearest point on -/// the boundary of the half space, measured and exppressed in F. +/// new ContactPoint.normal will point in the opposite direction as the half +/// space normal (expressed in Frame F) -- it points _into_ the half space. We +/// define the point P to be a point of `convex_C` that most deeply penetrates +/// into the half space. It is not guaranteed to be unique. If it is not unique, +/// it will be arbitrarily selected from the set of all such points. The +/// ContactPoint.penetration_depth value is the depth of P. ContactPoint.pos is +/// defined as the point between P and the nearest point on the boundary of the +/// half space, measured and expressed in F. +/// +/// ContactPoint is documented to report contact position in the world frame W. +/// This function will only truly satisfy that requirement if F = W. It is the +/// responsibility of the caller to understand the semantics of F and confirm +/// that it satisfies that requirement. +/// +/// This makes use of the +/// [Drake monogram notation](http://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html) +/// to describe quantities (particularly the poses of shapes). /// /// @param convex_C The convex mesh. Its vertex positions are measured and /// expressed in Frame C. diff --git a/test/narrowphase/detail/primitive_shape_algorithm/test_half_space_convex.cpp b/test/narrowphase/detail/primitive_shape_algorithm/test_half_space_convex.cpp index 01f846fd9..d5c8ec3e7 100644 --- a/test/narrowphase/detail/primitive_shape_algorithm/test_half_space_convex.cpp +++ b/test/narrowphase/detail/primitive_shape_algorithm/test_half_space_convex.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018. Toyota Research Institute + * Copyright (c) 2020. Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -34,7 +34,7 @@ /** @author Sean Curtis (sean@tri.global) (2020) */ -// Tests the custom half space-convex collision test. +// Tests the Halfspace-Convex primitive collision test. // TODO(SeanCurtis-TRI): Currently, there are no half space-X distance primitive // implementations. @@ -62,7 +62,7 @@ using std::string; using std::vector; /* - Creates a unit tetrahedron as a Convex geometry. + Creates a simple tetrahedron as a Convex geometry. +z │ │ @@ -79,8 +79,8 @@ using std::vector; +x The tet is defined in geometry frame G, shown above with vertex positions - marked. The final convex mesh is expressed in tetrahedral frame G. It - relationship to T is given by X_TG. + marked. The final tetrahedral convex mesh is expressed in frame T. The + relationship to G is given by X_TG. */ template Convex MakeTetrahedron(const Transform3& X_TG) { @@ -107,9 +107,9 @@ Convex MakeTetrahedron(const Transform3& X_TG) { } /* Specifies the data necessary for a single test and the expected results. - A Convex shape (the tetrahedron returned by MakeTetrahedron) is posed in - the half space frame H (such that the half space lies on the Hz = 0 plane - with the normal pointing in the Hz direction). */ + The Convex tetrahedron is defined in frame T, the Halfspace is defined in frame + H, and both are posed relative to the configuration frame C with X_CT and X_CH, + respectively. */ template struct Configuration { /* Convenience constructor for creating an expected _non-colliding_ @@ -163,11 +163,11 @@ Transform3 MakeTransform(const AngleAxis& angle_axis, } template -vector> GetConfigurations() { +aligned_vector> GetConfigurations() { const Transform3 I = Transform3::Identity(); const S kPi = constants::pi(); - vector> configurations; + aligned_vector> configurations; // Vanilla configuration -- the un-rotated tet floats slightly above the // plane. @@ -175,8 +175,8 @@ vector> GetConfigurations() { "simple non-colliding", MakeTransform(AngleAxis::Identity(), Vector3{0, 0, S(0.1)}), I); - // Simple penetration - the tet is rotated so the point is down and penetrates - // the plane. + // Simple penetration - the tet is rotated so the point V3 is down and + // penetrates the plane. const AngleAxis R_CT_point_down{kPi, Vector3::UnitX()}; configurations.emplace_back( "simple penetration", @@ -184,34 +184,37 @@ vector> GetConfigurations() { Vector3{0, 0, -1}, Vector3{0, 0, S(-0.25)}); // Orient the half-space so it is not axis aligned and does not pass through - // the origin. Then position the tet so that one point is near penetration + // the origin. Then position the tet so that the point V3 is near penetration // and submit *two* configurations: one in collision, one not. - Transform3 X_CH = + const Transform3 X_CH = MakeTransform(AngleAxis{kPi / 5, Vector3{1, 2, 3}.normalized()}, Vector3{S(0.25), S(0.5), S(0.75)}); - // Steal the orientation from the previous configuration so that vertex 3 - // is pointing downwards into the half space. + // Steal the orientation from the previous configuration so that V3 is + // pointing downwards into the half space. const AngleAxis R_HT_point_down = R_CT_point_down; - configurations.emplace_back( - "non-trivial half space, non-colliding", - X_CH * MakeTransform(R_HT_point_down, Vector3{0, 0, S(1.01)}), X_CH); + const Transform3 X_HT_separated = + MakeTransform(R_HT_point_down, Vector3{0, 0, S(1.01)}); + configurations.emplace_back("non-trivial half space, non-colliding", + X_CH * X_HT_separated, X_CH); // We pose the tet relative to the half plane, and the transform it again into // the configuration frame. Offset of 0.5 in the z-direction gives us a // penetration depth of 0.5. - const Transform3 X_CT = - X_CH * MakeTransform(R_HT_point_down, Vector3{0, 0, S(0.5)}); + const Transform3 X_HT_colliding = + MakeTransform(R_HT_point_down, Vector3{0, 0, S(0.5)}); + const Transform3 X_CT_collding = X_CH * X_HT_colliding; const Vector3 Hz_C = X_CH.linear().col(2); - // By construction, we're colliding vertex 3 (0, 0, 1). So, let's find where - // it is in this configuration. + // By construction, we're colliding V3 (0, 0, 1). So, let's find where it is + // in this configuration. const Vector3 p_TV3 = Vector3::UnitZ(); - const Vector3 p_CV3 = X_CT * p_TV3; + const Vector3 p_CV3 = X_CT_collding * p_TV3; const S depth(0.5); - configurations.emplace_back("non-trivial half space, colliding", X_CT, X_CH, - depth, -Hz_C, p_CV3 + Hz_C * S(0.5) * depth); + configurations.emplace_back("non-trivial half space, colliding", + X_CT_collding, X_CH, depth, -Hz_C, + p_CV3 + Hz_C * S(0.5) * depth); return configurations; } @@ -250,7 +253,7 @@ void TestCollision(const Convex& convex_T, const Transform3& X_WT, against non-trivial transforms). */ template void EvalCollisionForTestConfiguration(const Configuration& config, S eps) { - vector> X_WCs{}; + aligned_vector> X_WCs{}; X_WCs.emplace_back(Transform3::Identity()); X_WCs.emplace_back( MakeTransform(AngleAxis{constants::pi() / 7, @@ -259,8 +262,8 @@ void EvalCollisionForTestConfiguration(const Configuration& config, S eps) { for (const auto& X_WC : X_WCs) { // For a given configuration, we can set it up two ways: - // X_CT and X_CH multiply mesh_T and half_space_H, respectively, or - // identity multiplies mesh_C and half_space_C. + // X_CT and X_CH multiply convex_T and half_space_H, respectively, or + // identity multiplies convex_C and half_space_C. // We'll do both; their answers should be the same. This will allow us to // easily confirm that all of the values contribute. @@ -294,7 +297,7 @@ void EvalCollisionForTestConfiguration(const Configuration& config, S eps) { // Simply evaluate collision on each of the given configurations. template -void QueryCollision(const vector>& configs) { +void QueryCollision(const aligned_vector>& configs) { // Half space collision is clean enough that even under ugly rotations, we // still get good precision. const S eps = 2 * constants::eps();