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(); +}