Skip to content

Commit

Permalink
Update the convex-half space primitive collision algorithm
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI committed May 8, 2020
1 parent 43f9805 commit ec232b3
Show file tree
Hide file tree
Showing 8 changed files with 490 additions and 45 deletions.
8 changes: 5 additions & 3 deletions include/fcl/geometry/shape/halfspace.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,11 @@
namespace fcl
{

/// @brief Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d;
/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space
/// @brief Half Space: this is equivalent to the Planed in ODE. The separation
/// plane is defined as n * x = d. Points in the negative side of the
/// separation plane (i.e. {x | n * x < d}) are inside the half space and points
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are
/// outside the half space.
template <typename S_>
class FCL_EXPORT Halfspace : public ShapeBase<S_>
{
Expand Down
45 changes: 24 additions & 21 deletions include/fcl/narrowphase/detail/gjk_solver_indep-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,27 +181,29 @@ bool GJKSolver_indep<S>::shapeIntersect(

// Shape intersect algorithms not using built-in GJK algorithm
//
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | O | O | | | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | O | O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid |/////|////////| | | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | capsule |/////|////////|///////////| | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cone |/////|////////|///////////|/////////| | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cylinder |/////|////////|///////////|/////////|//////| | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | convex |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | box | O | O | | | | | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | sphere |/////| O | | O | | O | O | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | ellipsoid |/////|////////| | | | | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | capsule |/////|////////|///////////| | | | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | cone |/////|////////|///////////|/////////| | | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | cylinder |/////|////////|///////////|/////////|//////| | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | convex |/////|////////|///////////|/////////|//////|//////////|///////|////////////|//////////| |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+

#define FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
template <typename S>\
Expand Down Expand Up @@ -259,6 +261,7 @@ FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersec
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Capsule, Halfspace, detail::capsuleHalfspaceIntersect)
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cylinder, Halfspace, detail::cylinderHalfspaceIntersect)
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cone, Halfspace, detail::coneHalfspaceIntersect)
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Convex, Halfspace, detail::convexHalfspaceIntersect)

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Plane, detail::spherePlaneIntersect)
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Plane, detail::ellipsoidPlaneIntersect)
Expand Down
45 changes: 24 additions & 21 deletions include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,27 +177,29 @@ bool GJKSolver_libccd<S>::shapeIntersect(

// Shape intersect algorithms not using libccd
//
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | O | O | | | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | O | O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid |/////|////////| | | | | O | O | TODO |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | capsule |/////|////////|///////////| | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cone |/////|////////|///////////|/////////| | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cylinder |/////|////////|///////////|/////////|//////| | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | convex |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | box | O | O | | | | | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | sphere |/////| O | | O | | O | O | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | ellipsoid |/////|////////| | | | | O | O | TODO | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | capsule |/////|////////|///////////| | | | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | cone |/////|////////|///////////|/////////| | | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | cylinder |/////|////////|///////////|/////////|//////| | O | O | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+
// | convex |/////|////////|///////////|/////////|//////|//////////|///////|////////////|//////////| |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+----------+

#define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\
template <typename S>\
Expand Down Expand Up @@ -255,6 +257,7 @@ FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceInterse
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Capsule, Halfspace, detail::capsuleHalfspaceIntersect)
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cylinder, Halfspace, detail::cylinderHalfspaceIntersect)
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cone, Halfspace, detail::coneHalfspaceIntersect)
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Convex, Halfspace, detail::convexHalfspaceIntersect)

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Plane, detail::spherePlaneIntersect)
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Plane, detail::ellipsoidPlaneIntersect)
Expand Down
58 changes: 58 additions & 0 deletions include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,12 @@ bool convexHalfspaceIntersect(
const Halfspace<double>& s2, const Transform3<double>& tf2,
Vector3<double>* contact_points, double* penetration_depth, Vector3<double>* normal);

//==============================================================================
extern template bool convexHalfspaceIntersect(
const Convex<double>& convex_C, const Transform3<double>& X_FC,
const Halfspace<double>& half_space_H, const Transform3<double>& X_FH,
std::vector<ContactPoint<double>>* contacts);

//==============================================================================
extern template
bool halfspaceTriangleIntersect(
Expand Down Expand Up @@ -490,6 +496,7 @@ bool coneHalfspaceIntersect(const Cone<S>& s1, const Transform3<S>& tf1,
}

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

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

if(depth <= 0)
{
// Note: this value for contact_point only works because depth is really
// signed distance, so negating the normal cancels the negation of the
// "penetration depth".
if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth);
// TODO(SeanCurtis-TRI): This appears to have the wrong sign for depth.
// We've actually computed *signed distance* which is -depth.
if(penetration_depth) *penetration_depth = depth;
// Note: This points *into* the half space. It is not clear this matches
// any documented convention.
if(normal) *normal = -new_s2.n;
return true;
}
else
return false;
}

//==============================================================================
template <typename S>
bool convexHalfspaceIntersect(const Convex<S>& convex_C,
const Transform3<S>& X_FC,
const Halfspace<S>& half_space_H,
const Transform3<S>& X_FH,
std::vector<ContactPoint<S>>* contacts) {
Halfspace<S> half_space_C = transform(half_space_H, X_FC.inverse() * X_FH);

Vector3<S> p_CV_deepest;
S min_signed_distance = std::numeric_limits<S>::max();

// TODO: Once we have an efficient "support vector" implementation for Convex
// (necessary to make GJK run faster with convex), this could benefit by
// simply asking for the support vector in the negative normal direction.
// 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 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 (signed_distance <= 0 && contacts == nullptr) return true;
}
}

const bool intersecting = min_signed_distance <= 0;

if (intersecting && contacts) {
const Vector3<S> normal_F = X_FH.linear() * half_space_H.n;
const Vector3<S> p_FV = X_FC * p_CV_deepest;
// 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);
}

return intersecting;
}

//==============================================================================
template <typename S>
bool halfspaceTriangleIntersect(const Halfspace<S>& s1, const Transform3<S>& tf1,
Expand Down
46 changes: 46 additions & 0 deletions include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,52 @@ bool convexHalfspaceIntersect(const Convex<S>& s1, const Transform3<S>& tf1,
const Halfspace<S>& s2, const Transform3<S>& tf2,
Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal);

/// @brief Reports whether a convex mesh and half space are intersecting.
/// If `contacts` is not `nullptr` and the two geometries are intersecting,
/// a new ContactPoint will be added to the data.
///
/// The two geometries are considered to be free of intersection if and only if
/// all points in the Convex mesh lie strictly _outside_ the half space. Lying
/// *on* the boundary of the half space is considered intersecting.
///
/// The two geometries are each defined in their own frames, but we have
/// transforms to a common frame F.
///
/// If the two geometries are intersecting and `contacts` is not `nullptr`, the
/// 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 halfway 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.
/// @param X_FC The transform relating Frame C with the common frame F.
/// @param half_space_H The half space. The position and orientation of its
/// boundary plane are measured and expressed in Frame H.
/// @param X_FH The transform relating Frame H with the common frame F.
/// @param[out] contacts The optional accumulator for data characterizing the
/// intersection.
/// @return `true` if the two geometries are intersecting.
/// @tparam S The computational scalar.
template <typename S>
FCL_EXPORT bool convexHalfspaceIntersect(
const Convex<S>& convex_C, const Transform3<S>& X_FC,
const Halfspace<S>& half_space_H, const Transform3<S>& X_FH,
std::vector<ContactPoint<S>>* contacts);

template <typename S>
FCL_EXPORT
bool halfspaceTriangleIntersect(const Halfspace<S>& s1, const Transform3<S>& tf1,
Expand Down
Loading

0 comments on commit ec232b3

Please sign in to comment.