From dd104c5f060c7dff15ae973fe55b35fbe76b7402 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Sat, 4 Aug 2018 15:47:11 -0700 Subject: [PATCH] Add custom sphere-cylinder collision and distance tests By default, the GJK solver was being used for performing distance queries between cylinders and spheres. For small features, the answer was being dominated by the iterative tolerance and producing wildly problematic values. The logical thing to do is to perform sphere-cylinder collisions using knowledge of the primitives. This commit adds the following: - A new test illustrating the error of GJK is used (see test_fcl_sphere_cylinder.cpp) - Adds the custom sphere-cylinder collision/distance (sphere_cylinder.h and sphere_cylinder-inl.h) - Adds *extensive* unit tests on the custom algorithm. - Ties the custom algorithm into the libccd and indep GJK solvers. --- CHANGELOG.md | 1 + .../narrowphase/detail/gjk_solver_indep-inl.h | 43 +- .../detail/gjk_solver_libccd-inl.h | 43 +- .../sphere_cylinder-inl.h | 270 +++++ .../sphere_cylinder.h | 137 +++ .../sphere_cylinder.cpp | 63 ++ test/CMakeLists.txt | 1 + .../primitive_shape_algorithm/CMakeLists.txt | 1 + .../test_sphere_cylinder.cpp | 936 ++++++++++++++++++ test/test_fcl_sphere_cylinder.cpp | 229 +++++ 10 files changed, 1720 insertions(+), 4 deletions(-) create mode 100644 include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h create mode 100644 include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h create mode 100644 src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp create mode 100644 test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp create mode 100644 test/test_fcl_sphere_cylinder.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index f0788d60a..1e4074f97 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ * Narrowphase * Add custom sphere-box collision and distance algorithms for both solvers: [#316](https://github.com/flexible-collision-library/fcl/pull/316) + * Add custom sphere-cylinder collision and distance algorithms for both solvers: [#321](https://github.com/flexible-collision-library/fcl/pull/321) * Distance diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index 4cd8482c5..3e386737a 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -51,6 +51,7 @@ #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h" @@ -184,7 +185,7 @@ bool GJKSolver_indep::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | O | O | O | +// | sphere |/////| O | | O | | O | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -249,6 +250,8 @@ FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleInters FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect) + FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect) @@ -675,7 +678,7 @@ bool GJKSolver_indep::shapeSignedDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | | | O | +// | sphere |/////| O | | O | | O | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -764,6 +767,42 @@ struct ShapeDistanceIndepImpl, Sphere> } }; +//============================================================================== +template +struct ShapeDistanceIndepImpl, Cylinder> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Cylinder& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Sphere> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Cylinder& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceIndepImpl, Sphere> diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 745583ef7..4e6903215 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -48,6 +48,7 @@ #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h" @@ -180,7 +181,7 @@ bool GJKSolver_libccd::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | O | O | O | +// | sphere |/////| O | | O | | O | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | O | O | TODO | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -245,6 +246,8 @@ FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleInter FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect) + FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect) @@ -656,7 +659,7 @@ bool GJKSolver_libccd::shapeDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | | | O | +// | sphere |/////| O | | O | | O | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -745,6 +748,42 @@ struct ShapeDistanceLibccdImpl, Sphere> } }; +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Cylinder> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Cylinder& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Sphere> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Cylinder& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceLibccdImpl, Sphere> diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h new file mode 100644 index 000000000..f13c91e5a --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h @@ -0,0 +1,270 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Toyota Research Institute. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H +#define FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" + +namespace fcl { +namespace detail { + +extern template FCL_EXPORT bool +sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +//============================================================================== + +extern template FCL_EXPORT bool +sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + double* distance, Vector3* p_FSc, + Vector3* p_FCs); + +//============================================================================== + +// Helper function for cylinder-sphere queries. Given a cylinder defined in its +// canonical frame C (i.e., centered on the origin with cylinder's height axis +// aligned to the Cz axis) and a query point Q, determines point N, the point +// *inside* the cylinder nearest to Q. Note: this is *not* necessarily the +// nearest point on the surface of the cylinder; if Q is inside the cylinder, +// then the nearest point is Q itself. +// @param height The height of the cylinder. +// @param radius The radius of the cylinder. +// @param p_CQ The position vector from frame C's origin to the query +// point Q measured and expressed in frame C. +// @param[out] p_CN_ptr A position vector from frame C's origin to the point N +// measured and expressed in frame C. +// @returns true if the nearest point is a different point than the query point. +// @pre p_CN_ptr must point to a valid Vector3 instance. +template +bool nearestPointInCylinder(const S& height, const S& radius, + const Vector3& p_CQ, Vector3* p_CN_ptr) { + assert(p_CN_ptr != nullptr); + Vector3& p_CN = *p_CN_ptr; + p_CN = p_CQ; + + bool clamped = false; + + // Linearly clamp along the cylinders height axis. + const S half_height = height / 2; + if (p_CQ(2) > half_height) { + clamped = true; + p_CN(2) = half_height; + } else if (p_CQ(2) < -half_height) { + clamped = true; + p_CN(2) = -half_height; + } + + // Clamp according to the circular cross section. + Vector2 r_CQ{p_CQ(0), p_CQ(1)}; + S squared_distance = r_CQ.dot(r_CQ); + if (squared_distance > radius * radius) { + // The query point lies outside the *circular* extent of the cylinder. + clamped = true; + r_CQ *= radius / sqrt(squared_distance); + p_CN(0) = r_CQ(0); + p_CN(1) = r_CQ(1); + } + + return clamped; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereCylinderIntersect( + const Sphere& sphere, const Transform3& X_FS, + const Cylinder& cylinder, const Transform3& X_FC, + std::vector>* contacts) { + const S& r_s = sphere.radius; + // Find the sphere center So (abbreviated as S) in the cylinder's frame. + const Transform3 X_CS = X_FC.inverse() * X_FS; + const Vector3 p_CS = X_CS.translation(); + + // Find N, the nearest point *inside* the cylinder to the sphere center S + // (measure and expressed in frame C). + Vector3 p_CN; + bool S_is_outside = nearestPointInCylinder(cylinder.lz, cylinder.radius, p_CS, + &p_CN); + + // Compute the position vector from the sphere center S to the nearest point N + // in the cylinder frame C. If the center is inside the cylinder, this will + // be the zero vector. + Vector3 p_SN_C = p_CN - p_CS; + S p_SN_squared_dist = p_SN_C.squaredNorm(); + // The nearest point to the sphere is *farther* than radius; they are *not* + // penetrating. + if (p_SN_squared_dist > r_s * r_s) + return false; + + // Now we know they are colliding. + + if (contacts != nullptr) { + // Return values have been requested. + S depth{0}; + // Normal pointing from sphere into cylinder (in cylinder's frame) + Vector3 n_SC_C; + // Contact position (P) in the cylinder frame. + Vector3 p_CP; + + // We want to make sure that differences exceed machine precision -- we + // don't want normal and contact position dominated by noise. However, + // because we apply an arbitrary rigid transform to the sphere's center, we + // lose bits of precision. For an arbitrary non-identity transform, 4 bits + // is the maximum possible precision loss. So, we only consider a point to + // be outside the box if it's distance is at least that epsilon. + // Furthermore, in finding the *near* face, a better candidate must be more + // than this epsilon closer to the sphere center (see the test in the + // else branch). + auto eps = 16 * constants::eps(); + if (S_is_outside && p_SN_squared_dist > eps * eps) { + // The sphere center is *measurably outside* the cylinder. There are three + // possibilities: nearest point lies on the cap face, cap edge, or barrel. + // In all three cases, the normal points from the nearest point to the + // sphere center. Penetration depth is the radius minus the distance + // between the pair of points. And the contact point is simply half the + // depth from the nearest point in the normal direction. + + // Distance from closest point (N) to sphere center (S). + S d_NS = sqrt(p_SN_squared_dist); + n_SC_C = p_SN_C / d_NS; + depth = r_s - d_NS; + p_CP = p_CN + n_SC_C * (depth * 0.5); + } else { + // The center is inside. It's either nearer the barrel or the end face + // (with preference for the end face). + const S& h = cylinder.lz; + S face_distance = p_CS(2) >= 0 ? h / 2 - p_CS(2) : p_CS(2) + h / 2; + // For the barrel to be picked over the face, it must be more than + // epsilon closer to the sphere center. + + // The direction from the sphere to the nearest point on the barrel on + // the z = 0 plane. + Vector2 n_SB_xy = Vector2(p_CS(0), p_CS(1)); + S d_CS_xy = n_SB_xy.norm(); + S barrel_distance = cylinder.radius - d_CS_xy; + if (barrel_distance < face_distance - eps) { + // Closest to the barrel. + if (d_CS_xy > eps) { + // Normal towards barrel + n_SC_C << -n_SB_xy(0) / d_CS_xy, -n_SB_xy(1) / d_CS_xy, 0; + depth = r_s + barrel_distance; + p_CP = p_CS + n_SC_C * ((r_s - barrel_distance) / 2); + } else { + // Sphere center is on the central spine of the cylinder, as per + // documentation, assume we have penetration coming in from the +x + // direction. + n_SC_C = -Vector3::UnitX(); + depth = r_s + cylinder.radius; + p_CP = p_CS + n_SC_C * ((r_s - barrel_distance) / 2); + } + } else { + // Closest to the face. + n_SC_C << 0, 0, 0; + // NOTE: This sign *may* seem counter-intuitive. A center nearest the +z + // face produces a normal in the -z direction; this is because the + // normal points from the sphere and into the cylinder; and the + // penetration is *into* the +z face (so points in the -z direction). + n_SC_C(2) = p_CS(2) >= 0 ? -1 : 1; + depth = face_distance + r_s; + p_CP = p_CS + n_SC_C * ((r_s - face_distance) / 2); + } + } + contacts->emplace_back(X_FC.linear() * n_SC_C, X_FC * p_CP, depth); + } + return true; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, S* distance, + Vector3* p_FSc, Vector3* p_FCs) { + // Find the sphere center S in the cylinder's frame. + const Transform3 X_CS = X_FC.inverse() * X_FS; + const Vector3 p_CS = X_CS.translation(); + const S r_s = sphere.radius; + + // Find N, the nearest point *inside* the cylinder to the sphere center S + // (measured and expressed in frame C). + Vector3 p_CN; + bool S_is_outside = nearestPointInCylinder(cylinder.lz, cylinder.radius, p_CS, + &p_CN); + + if (S_is_outside) { + // If N is not S, we know the sphere center is *outside* the box (but we + // don't know yet if the they are completely separated). + + // Compute the position vector from the nearest point N to the sphere center + // S in the frame C. + Vector3 p_NS_C = p_CS - p_CN; + S p_NS_squared_dist = p_NS_C.squaredNorm(); + if (p_NS_squared_dist > r_s * r_s) { + // The distance to the nearest point is greater than the sphere radius; + // we have proven separation. + S d{-1}; + if (distance || p_FCs || p_FSc) + d = sqrt(p_NS_squared_dist); + if (distance != nullptr) + *distance = d - r_s; + if (p_FCs != nullptr) + *p_FCs = X_FC * p_CN; + if (p_FSc != nullptr) { + const Vector3 p_CSc = p_CS - (p_NS_C * r_s / d); + *p_FSc = X_FC * p_CSc; + } + return true; + } + } + + // We didn't *prove* separation, so we must be in penetration. + if (distance != nullptr) *distance = -1; + return false; +} + +} // namespace detail +} // namespace fcl + +#endif // FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h new file mode 100644 index 000000000..8ca3ef904 --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h @@ -0,0 +1,137 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Toyota Research Institute. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H +#define FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H + +#include "fcl/geometry/shape/cylinder.h" +#include "fcl/geometry/shape/sphere.h" +#include "fcl/narrowphase/contact_point.h" + +namespace fcl { + +namespace detail { + +/** @name Custom cylinder-sphere proximity algorithms + + These functions provide custom algorithms for analyzing the relationship + between a sphere and a cylinder. + + These functions make 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). + + Both shapes must be posed in a common frame (notated as F). This common frame + is typically the world frame W. Regardless, if the optional output data is + returned, it will be reported in this common frame F. + + The functions can be executed in one of two ways: to perform a strict boolean + query (is colliding/is separated) or to get data which characterizes the + colliding or separating state (e.g., penetration depth vs separating distance). + The difference in usage is defined by whether the optional out parameters + are null or non-null. In the documentation, these are labeled "(optional)". + + For these functions, if the sphere and cylinder are detected to be *touching* + this is considered a collision. As such, a collision query would report true + and a separating distance query would report false. + */ + +// NOTE: the choice to consider touching contact as collision is predicated on +// the implementation in sphere-sphere contact. + +//@{ + +/** Detect collision between the sphere and cylinder. If colliding, return + characterization of collision in the provided vector. + + The reported depth is guaranteed to be continuous. The normal and + contact position are guaranteed to be continuous with respect to the relative + pose of the two shapes while the sphere center lies *outside* the cylinder. + However, if the sphere center lies *inside* the cylinder, there are regions of + discontinuity in both normal and contact position. This is due to the fact that + there is not necessarily a *unique* characterization of penetration depth + (i.e., the sphere center may be equidistant to both a cap face as well as the + barrel. This ambiguity is resolved through an arbitrary prioritization. + Computing the normal relative to the end face is preferred to the barrel. If + the barrel is closer than the end face, but it doesn't provide a unique + solution, the +x direction is assumed to be the contact direction (yielding a + contact normal in the -x direction.) + + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param cylinder The cylinder geometry. + @param X_FC The pose of the cylinder C in the common frame F. + @param contacts[out] (optional) If the shapes collide, the contact point data + will be appended to the end of this vector. + @return True if the objects are colliding (including touching). + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +/** Evaluate the minimum separating distance between a sphere and cylinder. If + separated, the nearest points on each shape will be returned in frame F. + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param cylinder The cylinder geometry. + @param X_FC The pose of the cylinder C in the common frame F. + @param distance[out] (optional) The separating distance between the cylinder + and sphere. Set to -1 if the shapes are penetrating. + @param p_FSc[out] (optional) The closest point on the *sphere* to the + cylinder measured and expressed in frame F. + @param p_FCs[out] (optional) The closest point on the *cylinder* to the + sphere measured and expressed in frame F. + @return True if the objects are separated. + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, S* distance, + Vector3* p_FSc, Vector3* p_FCs); + +//@} + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" + +#endif // FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp new file mode 100644 index 000000000..d9e87d599 --- /dev/null +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp @@ -0,0 +1,63 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of CNRS-LAAS and AIST nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis (sean@tri.global) (2018) */ + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template bool +sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +//============================================================================== + +template bool +sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + double* distance, Vector3* p_FSc, + Vector3* p_FCs); + +} // namespace detail +} // namespace fcl diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 36d435629..1ae1ea5ce 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -61,6 +61,7 @@ set(tests test_fcl_simple.cpp test_fcl_sphere_box.cpp test_fcl_sphere_capsule.cpp + test_fcl_sphere_cylinder.cpp test_fcl_sphere_sphere.cpp ) diff --git a/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt b/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt index 8e908cf78..af5fb5455 100644 --- a/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt +++ b/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt @@ -1,5 +1,6 @@ set(tests test_sphere_box.cpp + test_sphere_cylinder.cpp ) # Build all the tests diff --git a/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp b/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp new file mode 100644 index 000000000..38c6d162d --- /dev/null +++ b/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp @@ -0,0 +1,936 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of CNRS-LAAS and AIST nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis (sean@tri.global) (2018) */ + +// Tests the custom sphere-cylinder tests: distance and collision. + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" + +#include +#include + +#include + +#include "eigen_matrix_compare.h" +#include "fcl/geometry/shape/cylinder.h" +#include "fcl/geometry/shape/sphere.h" + +namespace fcl { +namespace detail { +namespace { + +// In the worst case (with arbitrary frame orientations) it seems like I'm +// losing about 4 bits of precision in the solution (compared to performing +// the equivalent query without any rotations). This encodes that bit loss to +// an epsilon value appropriate to the scalar type. +// +// TODO(SeanCurtis-TRI): These eps values are *not* optimal. They are the result +// of a *number* of issues. +// 1. Generally, for float the scalar must be at least 20 * ε. The arbitrary +// rotation *really* beats up on the precision. +// 2. CI uses Eigen 3.2.0. The threshold must be 22 * ε for the tests to pass. +// This is true, even for doubles. Later versions (e.g., 3.2.92, aka +// 3.3-beta1) can pass with a tolerance of 16 * ε. +// 3. Mac CI requires another bump in the multiplier for floats. So, floats here +// are 24. +// Upgrade the Eigen version so that this tolerance can be reduced. +template +struct Eps { + using Real = typename constants::Real; + static Real value() { return 22 * constants::eps(); } +}; + +template <> +struct Eps { + using Real = typename constants::Real; + static Real value() { return 24 * constants::eps(); } +}; + +// Utility function for evaluating points inside cylinders. Tests various +// configurations of points and cylinders. +template void NearestPointInCylinder() { + // Picking sizes that are *not* powers of two and *not* uniform in size. + const S r = 0.6; + const S h = 1.8; + Vector3 p_CN; + Vector3 p_CQ; + + // Case: query point at origin. + p_CQ << 0, 0, 0; + bool N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_FALSE(N_is_not_S) << "point at origin"; + EXPECT_TRUE(CompareMatrices(p_CN, p_CQ, 0, MatrixCompareType::absolute)) + << "point at origin"; + + // Per cylinder-half tests (i.e., above and below the z = 0 plane). + for (S z_sign : {-1, 1}) { + for (const auto& dir : {Vector3(1, 0, 0), + Vector3(0, 1, 0), + Vector3(1, 1, 0).normalized(), + Vector3(-1, 2, 0).normalized(), + Vector3(1, -2, 0).normalized(), + Vector3(-2, -3, 0).normalized()}) { + const Vector3 z_offset_internal{0, 0, h * S(0.5) * z_sign}; + const Vector3 z_offset_external{0, 0, h * S(1.5) * z_sign}; + const Vector3 radial_offset_internal = dir * (r * S(0.5)); + const Vector3 radial_offset_external = dir * (r * S(1.5)); + + using std::to_string; + + std::stringstream ss; + ss << "dir: " << dir.transpose() << ", z: " << z_sign; + const std::string parameters = ss.str(); + // Case: point inside (no clamped values). + p_CQ = radial_offset_internal + z_offset_internal; + N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_FALSE(N_is_not_S) << "Sphere at origin - " << parameters; + EXPECT_TRUE( + CompareMatrices(p_CN, p_CQ, 0, MatrixCompareType::absolute)) + << "Sphere at origin - " << parameters; + + // Case: clamped only by the barrel. + p_CQ = radial_offset_external + z_offset_internal; + N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_TRUE(N_is_not_S) + << "Clamped by barrel - " << parameters; + const Vector3 point_on_barrel = z_offset_internal + dir * r; + EXPECT_NEAR(point_on_barrel(0), p_CN(0), Eps::value()) + << "Clamped by barrel - " << parameters; + EXPECT_NEAR(point_on_barrel(1), p_CN(1), Eps::value()) + << "Clamped by barrel - " << parameters; + EXPECT_EQ(p_CQ(2), p_CN(2)) + << "Clamped by barrel - " << parameters; + + // Case: clamped only by the end face. + p_CQ = radial_offset_internal + z_offset_external; + N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_TRUE(N_is_not_S) << "Clamped by end face - " << parameters; + EXPECT_EQ(p_CQ(0), p_CN(0)) << "Clamped by end face - " << parameters; + EXPECT_EQ(p_CQ(1), p_CN(1)) << "Clamped by end face - " << parameters; + EXPECT_EQ(0.5 * h * z_sign, p_CN(2)) << "Clamped by end face - " << parameters; + + // Case: clamped by both end face and barrel. + p_CQ = radial_offset_external + z_offset_external; + N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_TRUE(N_is_not_S) << "Fully clamped - " << parameters; + EXPECT_NEAR(point_on_barrel(0), p_CN(0), Eps::value()) + << "Fully clamped - " << parameters; + EXPECT_NEAR(point_on_barrel(1), p_CN(1), Eps::value()) + << "Fully clamped - " << parameters; + EXPECT_EQ(0.5 * h * z_sign, p_CN(2)) << "Fully clamped - " << parameters; + } + } +} + +// Defines the test configuration for a single test. It includes the geometry +// and the pose of the sphere in the cylinder's frame C. It also includes the +// expected answers in that same frame. It does not include those quantities +// that vary from test invocation to invocation (e.g., the pose of the cylinder +// in the world frame or the *orientation* of the sphere). +// +// Collision and distance are complementary queries -- two objects in collision +// have no defined distance because they are *not* separated and vice versa. +// These configurations allow for the test of the complementarity property. +template +struct TestConfiguration { + TestConfiguration(const std::string& name_in, const S& r_c_in, + const S& h_c_in, const S& r_s_in, + const Vector3 &p_CSo_in, bool colliding) + : name(name_in), + cylinder_half_len(h_c_in / 2), + r_c(r_c_in), + r_s(r_s_in), + p_CSo(p_CSo_in), + expected_colliding(colliding) {} + + // Descriptive name of the test configuration. + std::string name; + // Half the length of the cylinder along the z-axis. + S cylinder_half_len; + // Radius of the cylinder. + S r_c; + // Radius of the sphere. + S r_s; + // Position of the sphere's center in the cylinder frame. + Vector3 p_CSo; + + // Indicates if this test configuration is expected to be in collision. + bool expected_colliding{false}; + + // Collision values; only valid if expected_colliding is true. + S expected_depth{-1}; + Vector3 expected_normal; + Vector3 expected_pos; + + // Distance values; only valid if expected_colliding is false. + S expected_distance{-1}; + // The points on sphere and cylinder, respectively, closest to the other shape + // measured and expressed in the cylinder frame C. Only defined if separated. + Vector3 expected_p_CSc; + Vector3 expected_p_CCs; +}; + +// Utility for creating a copy of the input configurations and appending more +// labels to the configuration name -- aids in debugging. +template +std::vector> AppendLabel( + const std::vector>& configurations, + const std::string& label) { + std::vector> configs; + for (const auto& config : configurations) { + configs.push_back(config); + configs.back().name += " - " + label; + } + return configs; +} + +// Returns a collection of configurations where sphere and cylinder are simliar +// in size. +template +std::vector> GetUniformConfigurations() { + std::vector> configurations; + + // Common configuration values + // Cylinder and sphere dimensions. + const S h_c = 0.6; + const S half_h_c = h_c / 2; + const S r_c = 1.2; + const S r_s = 0.7; + const bool collides = true; + + // Collection of directions parallel to the z = 0 plane. Used for sampling + // queries in various directions around the barrel. Note: for the tests to be + // correct, these normals must all have unit length and a zero z-component. + std::vector> barrel_directions{ + Vector3{1, 0, 0}, + Vector3{0, 1, 0}, + Vector3(1, S(0.5), 0).normalized(), + Vector3(-1, S(0.5), 0).normalized(), + Vector3(-1, -S(0.5), 0).normalized(), + Vector3(1, -S(0.5), 0).normalized()}; + + { + // Case: Completely separated. Nearest point on the +z face. + const Vector3 p_CS{r_c * S(0.25), r_c * S(0.25), + half_h_c + r_s * S(1.1)}; + configurations.emplace_back( + "Separated; nearest face +z", r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = p_CS(2) - half_h_c - r_s; + config.expected_p_CCs = Vector3{p_CS(0), p_CS(1), half_h_c}; + config.expected_p_CSc = Vector3{p_CS(0), p_CS(1), p_CS(2) - r_s}; + } + + { + // Case: Sphere completely separated with center in barrel Voronoi region. + const S target_distance = 0.1; + const Vector3 n_SC = Vector3{-1, -1, 0}.normalized(); + const Vector3 p_CS = Vector3{0, 0, half_h_c * S(0.5)} - + n_SC * (r_s + r_c + target_distance); + configurations.emplace_back( + "Separated; near barrel", r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_distance; + config.expected_p_CCs = -n_SC * r_c + Vector3{0, 0, p_CS(2)}; + config.expected_p_CSc = p_CS + n_SC * r_s; + } + + { + // Case: Sphere completely separated with center in *edge* Voronoi region. + const S target_distance = .1; + const Vector3 n_SC = Vector3{-1, -1, -1}.normalized(); + const Vector3 p_CCs = Vector3{0, 0, half_h_c} + + Vector3{-n_SC(0), -n_SC(1), 0}.normalized() * r_c; + const Vector3 p_CS = p_CCs - n_SC * (r_s + target_distance); + configurations.emplace_back( + "Separated; near barrel edge", r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_distance; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS + n_SC * r_s; + } + + using std::min; + const S target_depth = min(r_c, r_s) * S(0.25); + + // Case contact - sphere center outside cylinder. + // Sub-cases intersection *only* through cap face; one test for each face. + for (S sign : {S(-1), S(1)}) { + const Vector3 n_SC = Vector3::UnitZ() * -sign; + const Vector3 p_CCs = Vector3{r_c * S(0.25), r_c * S(0.25), + half_h_c * sign}; + const Vector3 p_CS = p_CCs - n_SC * (r_s - target_depth); + configurations.emplace_back( + "Colliding external sphere center; cap face in " + + (sign < 0 ? std::string("-z") : std::string("+z")), + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = n_SC; + config.expected_pos = p_CCs + n_SC * (target_depth / 2); + // Colliding; no distance values required. + } + + // Sub-cases intersection *only* through barrel. Sampled in multiple + // directions. + for (const Vector3& n_CS : barrel_directions) { + const Vector3 p_CCs = Vector3{0, 0, half_h_c * S(.1)} + + n_CS * r_c; + const Vector3 p_CS = p_CCs + n_CS * (r_s - target_depth); + std::stringstream ss; + ss << "Colliding external sphere center; barrel from sphere center in" + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (target_depth / 2); + // Colliding; no distance values required. + } + + // Sub-cases intersection through edge. + for (S sign : {S(-1), S(1)}) { + // Projection of vector from cylinder center to sphere center on the z=0 + // plane (and then normalized). + for (const Vector3& n_CS_xy : barrel_directions) { + const Vector3 p_CCs = Vector3{0, 0, sign * half_h_c} + + n_CS_xy * r_c; + const Vector3 n_CS = p_CCs.normalized(); + const Vector3 p_CS = p_CCs + n_CS * (r_s - target_depth); + std::stringstream ss; + ss << "Colliding external sphere center; edge from sphere center in" + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (target_depth / 2); + // Colliding; no distance values required. + } + } + + // Case contact - sphere center *inside* cylinder. + + // Sub-cases: sphere is unambiguously closest to end face. One test for each + // end face. + for (S sign : {S(-1), S(1)}) { + // Distance from sphere center S to face F. + const S d_SF = 0.1; + const Vector3 n_SC = Vector3::UnitZ() * -sign; + const Vector3 p_CCs = Vector3{r_c * S(0.25), r_c * S(0.25), + half_h_c * sign}; + const Vector3 p_CS = p_CCs + n_SC * d_SF; + configurations.emplace_back( + "Colliding internal sphere center; cap face in " + + (sign < 0 ? std::string("-z") : std::string("+z")), + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = d_SF + r_s; + config.expected_normal = n_SC; + config.expected_pos = p_CCs + n_SC * (config.expected_depth / 2); + // Colliding; no distance values required. + } + + // Sub-cases: sphere is unambiguously closest to barrel; sampling multiple + // directions. + for (const Vector3& n_CS : barrel_directions) { + // Distance from sphere center S to point B on barrel. + const S d_SB = 0.1; + const Vector3 p_CCs = Vector3{0, 0, half_h_c * S(.1)} + + n_CS * r_c; + const Vector3 p_CS = p_CCs - n_CS * d_SB; + std::stringstream ss; + ss << "Colliding internal sphere center; barrel from sphere located in " + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s + d_SB; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (config.expected_depth / 2); + // Colliding; no distance values required. + } + + // Case contact - sphere center is *near* error-dominated regions + + // Sub-case: Sphere center is within epsilon *outside* of end face. + // Numerically, this is processed as if the center were inside the cylinder. + // For face contact, there's no difference. This test subsumes the test where + // the center lies *on* the surface of the cylinder. + for (S sign : {S(-1), S(1)}) { + // Distance from sphere center S to face F. + const S d_SF = Eps::value() / 2; + const Vector3 n_SC = Vector3::UnitZ() * -sign; + const Vector3 p_CCs = Vector3{r_c * S(0.25), r_c * S(0.25), + half_h_c * sign}; + const Vector3 p_CS = p_CCs - n_SC * d_SF; + configurations.emplace_back( + "Colliding sphere center outside by ε; cap face in " + + (sign < 0 ? std::string("-z") : std::string("+z")), + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s; + config.expected_normal = n_SC; + config.expected_pos = p_CCs + n_SC * (config.expected_depth / 2); + // Colliding; no distance values required. + } + + // Sub-case: Sphere center is within epsilon *outside* of barrel. + // Numerically, this is processed as if the center were inside the cylinder. + // For barrel contact, there's no difference. This test subsumes the test + // where the center lies *on* the surface of the cylinder. + for (const Vector3& n_CS : barrel_directions) { + // Distance from sphere center S to point B on barrel. + const S d_SB = Eps::value() / 2; + const Vector3 p_CCs = Vector3{0, 0, half_h_c * S(.1)} + + n_CS * r_c; + const Vector3 p_CS = p_CCs - n_CS * d_SB; + std::stringstream ss; + ss << "Colliding sphere center outside by ε; barrel from sphere located in " + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (config.expected_depth / 2); + // Colliding; no distance values required. + } + + // Sub-case: Sphere center is within epsilon *outside* of edge. + // Numerically, this is processed as if the center were inside the cylinder. + // If the center is in the Voronoi region of the edge, the reported normal + // will be either the face or the barrel -- whichever is closer. In this + // configuration, it is the face normal. This test subsumes the test where + // the center lies *on* the surface of the cylinder. + for (S sign : {S(-1), S(1)}) { + // Projection of vector from cylinder center to sphere center on the z=0 + // plane (and then normalized). + const S d_SC = Eps::value() / 2; + for (const Vector3& n_CS_xy : barrel_directions) { + const Vector3 p_CCs = Vector3{0, 0, sign * half_h_c} + + n_CS_xy * r_c; + const Vector3 n_CS = p_CCs.normalized(); + const Vector3 p_CS = p_CCs + n_CS * d_SC; + std::stringstream ss; + ss << "Colliding sphere center outside by ε; edge from sphere center in" + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s; + // NOTE: Epsilon *outside* is considered inside so the normal direction + // will be either face or barrel -- and, in this case, it's face. + config.expected_normal = -sign * Vector3::UnitZ(); + config.expected_pos = p_CCs + config.expected_normal * (r_s / 2); + // Colliding; no distance values required. + } + } + + { + // Sub-case: Sphere center is on origin - face is closer. It should prefer + // the +z face. + const Vector3 p_CS = Vector3::Zero(); + // Guarantee that the barrel is farther than the face. + const S big_radius = h_c * 2; + configurations.emplace_back( + "Collision with sphere at origin; face nearest", big_radius, h_c, r_s, + p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s + h_c / 2; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos = Vector3{0, 0, h_c / 2 - config.expected_depth / 2}; + // Colliding; no distance values required. + } + + { + // Sub-case: Sphere center is on origin - barrel is closer. + const Vector3 p_CS = Vector3::Zero(); + // Guarantee that the barrel is closer than the face. + const S big_height = r_c * 4; + configurations.emplace_back( + "Collision with sphere at origin; barrel nearest", r_c, big_height, r_s, + p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s + r_c; + config.expected_normal = -Vector3::UnitX(); + config.expected_pos = Vector3{r_c - config.expected_depth / 2, 0, 0}; + // Colliding; no distance values required. + } + + + return configurations; +} + +// Returns a collection of configurations where sphere and cylinder are scaled +// very differently. +template +std::vector> GetNonUniformConfigurations() { + std::vector> configurations; + + // Case: Large, flat cylinder and tiny sphere. + { + const S r_c = 9; + const S h_c = 0.1; + const S r_s = 0.025; + const bool collides = true; + const S target_depth = r_s / 2; + + // Sub-case: Colliding -- contact with +z face. + { + // Colliding sub-case. + const Vector3 p_CCs = Vector3(1, 2, 0).normalized() * (r_c - r_s) + + Vector3::UnitZ() * (h_c / 2); + const Vector3 p_CS{p_CCs + Vector3::UnitZ() * (r_s - target_depth)}; + configurations.emplace_back( + "Collision large disk, small sphere - contact at face", + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos = p_CCs - Vector3::UnitZ() * (target_depth / 2); + // Colliding; no distance values required. + } + + // Sub-case: Separated -- nearest feature +z face. + { + // Separated sub-case. + const Vector3 p_CCs = Vector3(1, 2, 0).normalized() * (r_c - r_s) + + Vector3::UnitZ() * (h_c / 2); + const Vector3 p_CS{p_CCs + + Vector3::UnitZ() * (r_s + target_depth)}; + configurations.emplace_back( + "Separation large disk, small sphere - nearest +z face", + r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_depth; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS - Vector3::UnitZ() * r_s; + } + + // Sub-case: Colliding -- contact with barrel. + const Vector3 n_CS = Vector3(1, 2, 0).normalized(); + const Vector3 p_CCs = n_CS * r_c + Vector3::UnitZ() * (r_s * 0.1); + { + // Colliding sub-case. + const Vector3 p_CS{p_CCs + n_CS * (r_s - target_depth)}; + configurations.emplace_back( + "Collision large disk, small sphere - contact at barrel", + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (target_depth / 2); + // Colliding; no distance values required. + } + + // Sub-case: Separated -- nearest feature is barrel. + { + // Separated sub-case. + const Vector3 p_CS{p_CCs + n_CS * (r_s + target_depth)}; + configurations.emplace_back( + "Separation large disk, small sphere - nearest barrel", + r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_depth; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS - n_CS * r_s; + } + } + + // Case: Large sphere and *tiny* cylinder. + { + const S r_c = 0.025; + const S h_c = 0.1; + const S r_s = 9; + const bool collides = true; + const S target_depth = r_c / 2; + + // Sub-case -- nearest feature is +z face. + { + const Vector3 p_CCs = + Vector3(1, 2, 0).normalized() * (r_c * S(0.5)) + + Vector3::UnitZ() * (h_c / 2); + + // Sub-case: Colliding. + { + const Vector3 + p_CS{p_CCs + Vector3::UnitZ() * (r_s - target_depth)}; + configurations.emplace_back( + "Collision large sphere, small disk - contact at face", + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos = p_CCs - Vector3::UnitZ() * (target_depth / 2); + // Colliding; no distance values required. + } + + // Subsub-case: Separated + { + const Vector3 p_CS{p_CCs + + Vector3::UnitZ() * (r_s + target_depth)}; + configurations.emplace_back( + "Separation large sphere, small disk - nearest +z face", + r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_depth; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS - Vector3::UnitZ() * r_s; + } + } + + // Sub-case: Nearest feature is barrel + { + const Vector3 n_CS = Vector3(1, 2, 0).normalized(); + const Vector3 p_CCs = n_CS * r_c + Vector3::UnitZ() * (h_c * 0.1); + + // Subsub-case: Colliding. + { + const Vector3 p_CS{p_CCs + n_CS * (r_s - target_depth)}; + configurations.emplace_back( + "Collision large sphere, small disk - contact at barrel", + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (target_depth / 2); + // Colliding; no distance values required. + } + + // Subsub-case: Separated . + { + const Vector3 p_CS{p_CCs + n_CS * (r_s + target_depth)}; + configurations.emplace_back( + "Separation large sphere, small disk - nearest barrel", + r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_depth; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS - n_CS * r_s; + } + } + } + + return configurations; +} + +template +using EvalFunc = +std::function &, const Transform3 &, + const Matrix3 &, S)>; + +// This evaluates an instance of a test configuration and confirms the results +// match the expected data. The test configuration is defined in the cylinder's +// frame with an unrotated sphere. The parameters provide the test parameters: +// the sphere orientation and the cylinder's pose in the world frame. +// +// Evaluates the collision query twice. Once as the boolean "is colliding" test +// and once with the collision characterized with depth, normal, and position. +template +void EvalCollisionForTestConfiguration(const TestConfiguration& config, + const Transform3& X_WC, + const Matrix3& R_CS, + S eps) { + // Set up the experiment from input parameters and test configuration. + Cylinder cylinder(config.r_c, config.cylinder_half_len * S(2)); + Sphere sphere{config.r_s}; + Transform3 X_CS = Transform3::Identity(); + X_CS.translation() = config.p_CSo; + X_CS.linear() = R_CS; + Transform3 X_WS = X_WC * X_CS; + + bool colliding = sphereCylinderIntersect(sphere, X_WS, cylinder, X_WC, + nullptr); + EXPECT_EQ(config.expected_colliding, colliding) << config.name; + + std::vector> contacts; + colliding = sphereCylinderIntersect(sphere, X_WS, cylinder, X_WC, &contacts); + EXPECT_EQ(colliding, config.expected_colliding) << config.name; + if (config.expected_colliding) { + EXPECT_EQ(1u, contacts.size()) << config.name; + const ContactPoint &contact = contacts[0]; + EXPECT_NEAR(config.expected_depth, contact.penetration_depth, eps) + << config.name; + EXPECT_TRUE(CompareMatrices(contact.normal, + X_WC.linear() * config.expected_normal, eps, + MatrixCompareType::absolute)) + << config.name; + EXPECT_TRUE(CompareMatrices(contact.pos, X_WC * config.expected_pos, eps, + MatrixCompareType::absolute)) + << config.name; + } else { + EXPECT_EQ(contacts.size(), 0u) << config.name; + } +} + +// This evaluates an instance of a test configuration and confirms the results +// match the expected data. The test configuration is defined in the cylinder's +// frame with an unrotated sphere. The parameters provide the test +// configuration. +// +// Evaluates the distance query twice. Once as the boolean "is separated" test +// and once with the separation characterized with distance and surface points. +template +void EvalDistanceForTestConfiguration(const TestConfiguration& config, + const Transform3& X_WC, + const Matrix3& R_CS, + S eps) { + // Set up the experiment from input parameters and test configuration. + Cylinder cylinder(config.r_c, config.cylinder_half_len * S(2)); + Sphere sphere{config.r_s}; + Transform3 X_CS = Transform3::Identity(); + X_CS.translation() = config.p_CSo; + X_CS.linear() = R_CS; + Transform3 X_WS = X_WC * X_CS; + + bool separated = sphereCylinderDistance(sphere, X_WS, cylinder, X_WC, + nullptr, nullptr, nullptr); + EXPECT_NE(separated, config.expected_colliding) << config.name; + + // Initializing this to -2, to confirm that a non-colliding scenario sets + // distance to -1. + S distance{-2}; + Vector3 p_WSc{0, 0, 0}; + Vector3 p_WCs{0, 0, 0}; + + separated = sphereCylinderDistance(sphere, X_WS, cylinder, X_WC, &distance, + &p_WSc, &p_WCs); + EXPECT_NE(separated, config.expected_colliding) << config.name; + if (!config.expected_colliding) { + EXPECT_NEAR(distance, config.expected_distance, eps) + << config.name; + EXPECT_TRUE(CompareMatrices(p_WSc, + X_WC * config.expected_p_CSc, eps, + MatrixCompareType::absolute)) + << config.name; + EXPECT_TRUE(CompareMatrices(p_WCs, + X_WC * config.expected_p_CCs, eps, + MatrixCompareType::absolute)) + << config.name; + } else { + EXPECT_EQ(distance, S(-1)) << config.name; + EXPECT_TRUE(CompareMatrices(p_WSc, Vector3::Zero(), 0, + MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(p_WCs, Vector3::Zero(), 0, + MatrixCompareType::absolute)); + } +} + +// This test defines the transforms for performing the single collision test. +template +void QueryWithVaryingWorldFrames( + const std::vector>& configurations, + EvalFunc query_eval, const Matrix3 &R_CS = Matrix3::Identity()) { + // Evaluate all the configurations with the given cylinder pose in frame F. + auto evaluate_all = [&R_CS, query_eval]( + const std::vector>& configs, + const Transform3& X_FC) { + for (const auto config : configs) { + query_eval(config, X_FC, R_CS, Eps::value()); + } + }; + + // Frame F is coincident and aligned with the cylinder frame. + Transform3 X_FC = Transform3::Identity(); + evaluate_all(AppendLabel(configurations, "X_FC = I"), X_FC); + + // Simple arbitrary translation away from the origin. + X_FC.translation() << 1.3, 2.7, 6.5; + evaluate_all(AppendLabel(configurations, "X_FC is translation"), X_FC); + + std::string axis_name[] = {"x", "y", "z"}; + // 90 degree rotation around each axis. + for (int axis = 0; axis < 3; ++axis) { + std::string label = "X_FC is 90-degree rotation around " + axis_name[axis]; + AngleAxis angle_axis{constants::pi() / 2, Vector3::Unit(axis)}; + X_FC.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, label), X_FC); + } + + // Arbitrary orientation. + { + AngleAxis angle_axis{constants::pi() / 3, + Vector3{1, 2, 3}.normalized()}; + X_FC.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, "X_FC is arbitrary rotation"), + X_FC); + } + + // Near axis aligned. + { + AngleAxis angle_axis{constants::eps_12(), Vector3::UnitX()}; + X_FC.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, "X_FC is near identity"), + X_FC); + } +} + +// Runs all test configurations across multiple poses in the world frame -- +// changing the orientation of the sphere -- should have no affect on the +// results. +template +void QueryWithOrientedSphere( + const std::vector>& configurations, + EvalFunc query_eval) { + + std::string axis_name[] = {"x", "y", "z"}; + + // 90 degree rotation around each axis. + for (int axis = 0; axis < 3; ++axis) { + AngleAxis angle_axis{constants::pi() / 2, Vector3::Unit(axis)}; + std::string label = "sphere rotate 90-degrees around " + axis_name[axis]; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } + + // Arbitrary orientation. + { + AngleAxis angle_axis{constants::pi() / 3, + Vector3{1, 2, 3}.normalized()}; + std::string label = "sphere rotated arbitrarily"; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } + + // Near axis aligned. + { + AngleAxis angle_axis{constants::eps_12(), Vector3::UnitX()}; + std::string label = "sphere rotated near axes"; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } +} + +//====================================================================== + +// Tests the helper function that finds the closest point in the cylinder. +GTEST_TEST(SphereCylinderPrimitiveTest, NearestPointInCylinder) { + NearestPointInCylinder(); + NearestPointInCylinder(); +} + +// Evaluates collision on all test configurations across multiple poses in the +// world frame - but the sphere rotation is always the identity. +GTEST_TEST(SphereCylinderPrimitiveTest, CollisionAcrossVaryingWorldFrames) { + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithVaryingWorldFrames( + GetUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates collision on all test configurations across multiple poses in the +// world frame - the sphere is rotated arbitrarily. +GTEST_TEST(SphereCylinderPrimitiveTest, CollisionWithSphereRotations) { + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates collision on a small set of configurations where the cylinder and +// sphere are of radically different scales - evaluation across multiple poses +// in the world frame. +GTEST_TEST(SphereCylinderPrimitiveTest, CollisionIncompatibleScales) { + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithVaryingWorldFrames( + GetNonUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates distance on all test configurations across multiple poses in the +// world frame - but the sphere rotation is always the identity. +GTEST_TEST(SphereCylinderPrimitiveTest, DistanceAcrossVaryingWorldFrames) { + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +// Evaluates distance on all test configurations across multiple poses in the +// world frame - the sphere is rotated arbitrarily. +GTEST_TEST(SphereCylinderPrimitiveTest, DistanceWithSphereRotations) { + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +// Evaluates distance on a small set of configurations where the cylinder and +// sphere are of radically different scales - evaluation across multiple poses +// in the world frame. +GTEST_TEST(SphereCylinderPrimitiveTest, DistanceIncompatibleScales) { + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +} // namespace +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char *argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_sphere_cylinder.cpp b/test/test_fcl_sphere_cylinder.cpp new file mode 100644 index 000000000..55c21f8be --- /dev/null +++ b/test/test_fcl_sphere_cylinder.cpp @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of CNRS-LAAS and AIST nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis (2018) */ + +#include + +#include +#include + +#include "eigen_matrix_compare.h" +#include "fcl/narrowphase/collision_object.h" +#include "fcl/narrowphase/distance.h" + +// TODO(SeanCurtis-TRI): Modify this test so it can be re-used for the distance +// query -- such that the sphere is slightly separated instead of slightly +// penetrating. See test_sphere_cylinder.cpp for an example. + +// This collides a cylinder with a sphere. The cylinder is disk-like with a +// large radius (r_c) and small height (h)c) and its geometric frame is aligned +// with the world frame. The sphere has radius r_s and is positioned at +// (sx, sy, sz) with an identity orientation. In this configuration, the sphere +// penetrates the cylinder slightly on the top face near the edge. The contact +// is *fully* contained in that face. (As illustrated below.) +// +// Side view +// z small sphere +// ┆ ↓ +// ┏━━━━━━━━━━━━┿━━━━◯━━━━━━┓ ┬ +// ┄┄┄┄┄┄╂┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄╂┄ x h_c +// ┗━━━━━━━━━━━━┿━━━━━━━━━━━┛ ┴ +// ┆ +// +// ├──── r_c───┤ +// +// Top view +// y +// ┆ +// ******* small sphere ┬ +// ** ┆ **╱ │ +// * ┆ ◯ * │ +// * ┆ * │ +// * ┆ * r_c +// * ┆ * │ +// * ┆ * │ +// * ┆ * │ +// ┄┄┄┄┄┄┄*┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄*┄┄┄┄ x ┴ +// * ┆ * +// * ┆ * +// * ┆ * +// * ┆ * +// * ┆ * +// * ┆ * +// ** ┆ ** +// ******* +// ┆ +// Properties of expected outcome: +// - One contact *should* be reported, +// - Penetration depth δ should be: r_s - (sz - h / 2), +// - Contact point should be at: [sx, sy, h / 2 - δ / 2], and +// - Contact normal should be [0, 0, -1] (pointing from sphere into box). +// +// NOTE: Orientation of the sphere should *not* make a difference and is not +// tested here; it relies on the sphere-box primitive algorithm unit tests to +// have robustly tested that. +// +// This test *fails* if GJK is used to evaluate the collision. It passes if the +// custom sphere-box algorithm is used, and, therefore, its purpose is to +// confirm that the custom algorithm is being applied. It doesn't exhaustively +// test sphere-box collision. It merely confirms the tested primitive algorithm +// has been wired up correctly. +template +void LargeCylinderSmallSphereTest(fcl::GJKSolverType solver_type) { + using fcl::Vector3; + using Real = typename fcl::constants::Real; + const Real eps = fcl::constants::eps(); + + // Configure geometry. + + // Cylinder and sphere dimensions. + const Real r_c = 9; + const Real h_c = 0.0025; + const Real r_s = 0.0015; + + auto sphere_geometry = std::make_shared>(r_s); + auto cylinder_geometry = std::make_shared>(r_c, h_c); + + // Poses of the cylinder in the world frame. + fcl::Transform3 X_WC = fcl::Transform3::Identity(); + + // Compute multiple sphere locations. All at the same height to produce a + // fixed, expected penetration depth of half of its radius. The reported + // position of the contact will have the x- and y- values of the sphere + // center, but be half the target_depth below the +z face, i.e., + // pz = (h / 2) - (target_depth / 2) + const Real target_depth = r_s * 0.5; + // Sphere center's height (position in z). + const Real sz = h_c / 2 + r_s - target_depth; + const Real pz = h_c / 2 - target_depth / 2; + // This transform will get repeated modified in the nested for loop below. + fcl::Transform3 X_WS = fcl::Transform3::Identity(); + + fcl::CollisionObject sphere(sphere_geometry, X_WS); + fcl::CollisionObject cylinder(cylinder_geometry, X_WC); + + // Expected results. (Expected position is defined inside the for loop as it + // changes with each new position of the sphere.) + const S expected_depth = target_depth; + // This normal direction assumes the *sphere* is the first collision object. + // If the order is reversed, the normal must be likewise reversed. + const Vector3 expected_normal = -Vector3::UnitZ(); + + // Set up the collision request. + fcl::CollisionRequest collision_request(1 /* num contacts */, + true /* enable_contact */); + collision_request.gjk_solver_type = solver_type; + + // Sample around the surface of the +z face on the disk for a fixed + // penetration depth. Note: the +z face is a disk with radius r_c. Notes on + // the selected samples: + // - We've picked positions such that the *whole* sphere projects onto the + // +z face. This *guarantees* that the contact is completely contained in + // the +z face so there is no possible ambiguity in the results. + // - We've picked points out near the boundaries, in the middle, and *near* + // zero without being zero. The GJK algorithm can actually provide the + // correct result at the (eps, eps) sample points. We leave those sample + // points in to confirm no degradation. + const std::vector r_values{0, eps, r_c / 2, r_c - r_s}; + const S pi = fcl::constants::pi(); + const std::vector theta_values{0, pi/2, pi, 3 * pi / 4}; + + for (const Real r : r_values) { + for (const Real theta : theta_values ) { + // Don't just evaluate at nice, axis-aligned directions. Pick some number + // that can't be perfectly represented. + const Real angle = theta + pi / 7; + const Real sx = cos(angle) * r; + const Real sy = sin(angle) * r; + // Repose the sphere. + X_WS.translation() << sx, sy, sz; + sphere.setTransform(X_WS); + + auto evaluate_collision = [&collision_request, &X_WS]( + const fcl::CollisionObject* s1, const fcl::CollisionObject* s2, + S expected_depth, const Vector3& expected_normal, + const Vector3& expected_pos, Real eps) { + // Compute collision. + fcl::CollisionResult collision_result; + std::size_t contact_count = + fcl::collide(s1, s2, collision_request, collision_result); + + // Test answers + if (contact_count == collision_request.num_max_contacts) { + std::vector> contacts; + collision_result.getContacts(contacts); + GTEST_ASSERT_EQ(contacts.size(), collision_request.num_max_contacts); + + const fcl::Contact& contact = contacts[0]; + EXPECT_NEAR(contact.penetration_depth, expected_depth, eps) + << "Sphere at: " << X_WS.translation().transpose(); + EXPECT_TRUE(fcl::CompareMatrices(contact.normal, + expected_normal, + eps, + fcl::MatrixCompareType::absolute)) + << "Sphere at: " << X_WS.translation().transpose(); + EXPECT_TRUE(fcl::CompareMatrices( + contact.pos, expected_pos, eps, fcl::MatrixCompareType::absolute)) + << "Sphere at: " << X_WS.translation().transpose(); + } else { + EXPECT_TRUE(false) << "No contacts reported for sphere at: " + << X_WS.translation().transpose(); + } + }; + + Vector3 expected_pos{sx, sy, pz}; + evaluate_collision(&sphere, &cylinder, expected_depth, expected_normal, + expected_pos, eps); + evaluate_collision(&cylinder, &sphere, expected_depth, -expected_normal, + expected_pos, eps); + } + } +} + +GTEST_TEST(FCL_SPHERE_CYLINDER, LargCylinderSmallSphere_ccd) { + LargeCylinderSmallSphereTest(fcl::GJKSolverType::GST_LIBCCD); + LargeCylinderSmallSphereTest(fcl::GJKSolverType::GST_LIBCCD); +} + +GTEST_TEST(FCL_SPHERE_CYLINDER, LargBoxSmallSphere_indep) { + LargeCylinderSmallSphereTest(fcl::GJKSolverType::GST_INDEP); + LargeCylinderSmallSphereTest(fcl::GJKSolverType::GST_INDEP); +} + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}