From 8fb2ce0e3463ab7e431d4e5afee05724d45c2eeb Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 31 Jul 2018 10:46:04 -0700 Subject: [PATCH] Add custom sphere-box collision and distance tests By default, the GJK solver was being used for performing distance queries between box's 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-box 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_box.cpp) - Borrows the CompareMatrices functionality from Drake and adds it to FCL. - Adds the custom sphere-box collision (sphere_box.h and sphere_box-inl.h) - Adds *extensive* unit tests on the custom algorithm. - Ties the custom algorithm into the libccd and indep GJK solvers. - Remove a useless conflicting test from test_fcl_collision (it's only useless in retrospect). And its formulation can't help but become corrupt. - Update *other* tests that otherwise depend on box-sphere collision. --- CHANGELOG.md | 5 + .../narrowphase/detail/gjk_solver_indep-inl.h | 43 +- .../detail/gjk_solver_libccd-inl.h | 43 +- .../sphere_box-inl.h | 231 ++++++ .../primitive_shape_algorithm/sphere_box.h | 142 ++++ .../primitive_shape_algorithm/sphere_box.cpp | 60 ++ test/CMakeLists.txt | 1 + test/eigen_matrix_compare.h | 150 ++++ test/narrowphase/detail/CMakeLists.txt | 1 + .../primitive_shape_algorithm/CMakeLists.txt | 8 + .../test_sphere_box.cpp | 749 ++++++++++++++++++ test/test_fcl_collision.cpp | 102 --- test/test_fcl_geometric_shapes.cpp | 33 +- test/test_fcl_sphere_box.cpp | 216 +++++ 14 files changed, 1670 insertions(+), 114 deletions(-) create mode 100644 include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h create mode 100644 include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h create mode 100644 src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp create mode 100644 test/eigen_matrix_compare.h create mode 100644 test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt create mode 100644 test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp create mode 100644 test/test_fcl_sphere_box.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 7f2bebd6d..710400e79 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ * Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156) +* Narrowphase + + * Add custom sphere-box collision and distance algorithms for both solvers: [#316](https://github.com/flexible-collision-library/fcl/pull/316) + * Distance * Added distance request option for computing exact negative distance: [#172](https://github.com/flexible-collision-library/fcl/pull/172) @@ -26,6 +30,7 @@ * Added CMake targets for generating API documentation: [#174](https://github.com/flexible-collision-library/fcl/pull/174) * Enabled build with SSE option by default: [#159](https://github.com/flexible-collision-library/fcl/pull/159) * Added missing copyright headers: [#149](https://github.com/flexible-collision-library/fcl/pull/149) + * Added test utility for performing equality between Eigen matrix-types (`CompareMatrices` in `test/eign_matrix_compare.h`): [#316](https://github.com/flexible-collision-library/fcl/pull/316) ### FCL 0.5.0 (2016-07-19) diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index e75236202..4cd8482c5 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -49,6 +49,7 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk.h" #include "fcl/narrowphase/detail/convexity_based_algorithm/epa.h" #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_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" @@ -181,7 +182,7 @@ bool GJKSolver_indep::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | O | | | | | | O | O | | +// | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -246,6 +247,8 @@ FCL_GJK_INDEP_SHAPE_INTERSECT(Box, detail::boxBoxIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) + 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) @@ -670,7 +673,7 @@ bool GJKSolver_indep::shapeSignedDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | | | | | | | | | | +// | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -689,6 +692,42 @@ bool GJKSolver_indep::shapeSignedDistance( // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +//============================================================================== +template +struct ShapeDistanceIndepImpl, Box> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Box& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Sphere> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Box& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceIndepImpl, Capsule> diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 7f0a60b82..745583ef7 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -46,6 +46,7 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h" #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_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" @@ -177,7 +178,7 @@ bool GJKSolver_libccd::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | O | | | | | | O | O | | +// | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -242,6 +243,8 @@ FCL_GJK_LIBCCD_SHAPE_INTERSECT(Box, detail::boxBoxIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) + 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) @@ -651,7 +654,7 @@ bool GJKSolver_libccd::shapeDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | | | | | | | | | | +// | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -670,6 +673,42 @@ bool GJKSolver_libccd::shapeDistance( // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Box> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Box& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Sphere> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Box& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceLibccdImpl, Capsule> diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h new file mode 100644 index 000000000..53cdace4e --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h @@ -0,0 +1,231 @@ +/* + * 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_SPHEREBOX_INL_H +#define FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" + +namespace fcl { +namespace detail { + +extern template FCL_EXPORT bool +sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + std::vector>* contacts); + +//============================================================================== + +extern template FCL_EXPORT bool +sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + double* distance, Vector3* p_FSb, + Vector3* p_FBs); + +//============================================================================== + +// Helper function for box-sphere queries. Given a box defined in its canonical +// frame B (i.e., aligned to the axes and centered on the origin) and a query +// point Q, determines point N, the point *inside* the box nearest to Q. Note: +// this is *not* the nearest point on the surface of the box; if Q is inside +// the box, then the nearest point is Q itself. +// @param size The size of the box to query against. +// @param p_BQ The position vector from frame B's origin to the query +// point Q measured and expressed in frame B. +// @param[out] p_BN_ptr A position vector from frame B's origin to the point N +// measured and expressed in frame B. +// @returns true if the nearest point is a different point than the query point. +// @pre P_BN_ptr must point to a valid Vector3 instance. +template +bool nearestPointInBox(const Vector3& size, const Vector3& p_BQ, + Vector3* p_BN_ptr) { + assert(p_BN_ptr != nullptr); + Vector3& p_BN = *p_BN_ptr; + // Clamp the point to the box. If we do *any* clamping we know the center was + // outside. If we did *no* clamping, the point is inside the box. + const Vector3 half_size = size / 2; + // The nearest point on the box (N) to Q measured in frame B. + bool clamped = false; + for (int i = 0; i < 3; ++i) { + p_BN(i) = p_BQ(i); + if (p_BQ(i) < -half_size(i)) { + clamped = true; + p_BN(i) = -half_size(i); + } + if (p_BQ(i) > half_size(i)) { + clamped = true; + p_BN(i) = half_size(i); + } + } + return clamped; +} +//============================================================================== + +template +FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, + std::vector>* contacts) { + const S r = sphere.radius; + // Find the sphere center C in the box's frame. + const Transform3 X_BS = X_FB.inverse() * X_FS; + const Vector3 p_BC = X_BS.translation(); + + // Find N, the nearest point *inside* the box to the sphere center C (measured + // and expressed in frame B) + Vector3 p_BN; + bool N_is_not_C = nearestPointInBox(box.side, p_BC, &p_BN); + + // Compute the position vector from the nearest point N to the sphere center + // C in the common box frame B. If the center is inside the box, this will be + // the zero vector. + Vector3 p_CN_B = p_BN - p_BC; + S squared_distance = p_CN_B.squaredNorm(); + // The nearest point to the sphere is *farther* than radius, they are *not* + // penetrating. + if (squared_distance > r * r) + return false; + + // Now we know they are colliding. + + if (contacts != nullptr) { + // Return values have been requested. + S depth{0}; + Vector3 n_SB_B; // Normal pointing from sphere into box (in box's frame) + Vector3 p_BP; // Contact position (P) in the box frame. + // 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 (N_is_not_C && squared_distance > eps * eps) { + // The center is on the outside. Normal direction is from C to N (computed + // above) and penetration depth is r - |p_BN - p_BC|. The contact position + // is 1/2 the penetration distance in the normal direction from p_BN. + S distance = sqrt(squared_distance); + n_SB_B = p_CN_B / distance; + depth = r - distance; + p_BP = p_BN + n_SB_B * (depth * 0.5); + } else { + // The center is inside. The sphere center projects onto all faces. The + // face that is closest defines the normal direction. The penetration + // depth is the distance to that face + radius. The position is the point + // midway between the projection point, and the point opposite the sphere + // center in the *negative* normal direction. + Vector3 half_size = box.side / 2; + S min_distance = + std::numeric_limits::Real>::infinity(); + int min_axis = -1; + for (int i = 0; i < 3; ++i) { + S dist = p_BC(i) >= 0 ? half_size(i) - p_BC(i) : p_BC(i) + half_size(i); + // To be closer, the face has to be more than epsilon closer. + if (dist + eps < min_distance) { + min_distance = dist; + min_axis = i; + } + } + n_SB_B << 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 box; and the penetration is *into* + // the +z face (so points in the -z direction). The same logic applies to + // all other directions. + n_SB_B(min_axis) = p_BC(min_axis) >= 0 ? -1 : 1; + depth = min_distance + r; + p_BP = p_BC + n_SB_B * ((r - min_distance) / 2); + } + contacts->emplace_back(X_FB.linear() * n_SB_B, X_FB * p_BP, depth); + } + return true; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereBoxDistance(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, S* distance, + Vector3* p_FSb, Vector3* p_FBs) { + // Find the sphere center C in the box's frame. + const Transform3 X_BS = X_FB.inverse() * X_FS; + const Vector3 p_BC = X_BS.translation(); + const S r = sphere.radius; + + // Find N, the nearest point *inside* the box to the sphere center C (measured + // and expressed in frame B) + Vector3 p_BN; + bool N_is_not_C = nearestPointInBox(box.side, p_BC, &p_BN); + + if (N_is_not_C) { + // If N is not C, 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 + // C in the frame B. + Vector3 p_NC_B = p_BC - p_BN; + S squared_distance = p_NC_B.squaredNorm(); + if (squared_distance > r * r) { + // The distance to the nearest point is greater than the radius, we have + // proven separation. + S d{-1}; + if (distance || p_FBs || p_FSb) + d = sqrt(squared_distance); + if (distance != nullptr) + *distance = d - r; + if (p_FBs != nullptr) + *p_FBs = X_FB * p_BN; + if (p_FSb != nullptr) { + const Vector3 p_BSb = (p_NC_B / d) * (d - r) + p_BN; + *p_FSb = X_FB * p_BSb; + } + 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_SPHEREBOX_INL_H diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h new file mode 100644 index 000000000..87fabf8cd --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h @@ -0,0 +1,142 @@ +/* + * 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_SPHEREBOX_H +#define FCL_NARROWPHASE_DETAIL_SPHEREBOX_H + +#include "fcl/geometry/shape/box.h" +#include "fcl/geometry/shape/sphere.h" +#include "fcl/narrowphase/contact_point.h" + +namespace fcl { + +namespace detail { + +/** @name Custom box-sphere proximity algorithms + + These functions provide custom algorithms for analyzing the relationship + between a sphere and a box. + + 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 box 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. + +//@{ + +// NOTE: The handling of the discontinuities in normal and position was +// implicitly defined in the spherebox test in test_fcl_geometric_shapes.cpp. It +// provided backwards compatibility to that test. + +/** Detect collision between the sphere and box. If colliding, return + characterization of collision in the provided vector. + + The reported depth is guaranteed to be continuous with respect to the relative + pose of the two objects. The normal and contact position are guaranteed to be + continuous while the sphere center lies *outside* the box. However, if the + sphere center lies *inside* the box, 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 multiple faces). In this case, the faces are + arbitrarily prioritized and the face with the highest priority is used to + compute normal and position. The priority order is +x, -x, +y, -y, +z, and -z. + For example: + + - If the center is near the edge between the -y and +z faces, the normal will + be defined w.r.t. to the -y face. + - If the center is near the corner of the +x, +y, & +z faces, the +x face + will be used. + + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param box The box geometry. + @param X_FB The pose of the box B 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 sphereBoxIntersect(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, + std::vector>* contacts); + +/** Evaluate the minimum separating distance between a sphere and box. 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 box The box geometry. + @param X_FB The pose of the box B in the common frame F. + @param distance[out] (optional) The separating distance between the box + and sphere. Set to -1 if the shapes are penetrating. + @param p_FSb[out] (optional) The closest point on the *sphere* to the box + measured and expressed in frame F. + @param p_FBs[out] (optional) The closest point on the *box* 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 sphereBoxDistance(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, S* distance, + Vector3* p_FSb, Vector3* p_FBs); + +//@} + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h" + +#endif // FCL_NARROWPHASE_DETAIL_SPHEREBOX_H diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp new file mode 100644 index 000000000..291ad3063 --- /dev/null +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp @@ -0,0 +1,60 @@ +/* + * 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_box-inl.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +template bool +sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + std::vector>* contacts); + +//============================================================================== + +template bool +sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + double* distance, Vector3* p_FSb, + Vector3* p_FBs); + +} // namespace detail +} // namespace fcl diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index b21c05adb..36d435629 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -59,6 +59,7 @@ set(tests test_fcl_shape_mesh_consistency.cpp test_fcl_signed_distance.cpp test_fcl_simple.cpp + test_fcl_sphere_box.cpp test_fcl_sphere_capsule.cpp test_fcl_sphere_sphere.cpp ) diff --git a/test/eigen_matrix_compare.h b/test/eigen_matrix_compare.h new file mode 100644 index 000000000..11e88474a --- /dev/null +++ b/test/eigen_matrix_compare.h @@ -0,0 +1,150 @@ +/* + * 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. + */ + +// This code was taken from Drake. +// https://github.com/RobotLocomotion/drake/blob/master/common/test_utilities/eigen_matrix_compare.h + +#ifndef FCL_EIGEN_MATRIX_COMPARE_H +#define FCL_EIGEN_MATRIX_COMPARE_H + +#include +#include +#include + +#include +#include + +namespace fcl { + +enum class MatrixCompareType { absolute, relative }; + +/** + * Compares two matrices to determine whether they are equal to within a certain + * threshold. + * + * @param m1 The first matrix to compare. + * @param m2 The second matrix to compare. + * @param tolerance The tolerance for determining equivalence. + * @param compare_type Whether the tolerance is absolute or relative. + * @return true if the two matrices are equal based on the specified tolerance. + */ +template +::testing::AssertionResult CompareMatrices( + const Eigen::MatrixBase& m1, + const Eigen::MatrixBase& m2, double tolerance = 0.0, + MatrixCompareType compare_type = MatrixCompareType::absolute) { + if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) { + return ::testing::AssertionFailure() + << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols() + << " vs. " << m2.rows() << " x " << m2.cols() << ")"; + } + + for (int ii = 0; ii < m1.rows(); ii++) { + for (int jj = 0; jj < m1.cols(); jj++) { + // First handle the corner cases of positive infinity, negative infinity, + // and NaN + const auto both_positive_infinity = + m1(ii, jj) == std::numeric_limits::infinity() && + m2(ii, jj) == std::numeric_limits::infinity(); + + const auto both_negative_infinity = + m1(ii, jj) == -std::numeric_limits::infinity() && + m2(ii, jj) == -std::numeric_limits::infinity(); + + using std::isnan; + const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj)); + + if (both_positive_infinity || both_negative_infinity || both_nan) + continue; + + // Check for case where one value is NaN and the other is not + if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) || + (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) { + return ::testing::AssertionFailure() << "NaN missmatch at (" << ii + << ", " << jj << "):\nm1 =\n" + << m1 << "\nm2 =\n" + << m2; + } + + // Determine whether the difference between the two matrices is less than + // the tolerance. + using std::abs; + const auto delta = abs(m1(ii, jj) - m2(ii, jj)); + + if (compare_type == MatrixCompareType::absolute) { + // Perform comparison using absolute tolerance. + + if (delta > tolerance) { + return ::testing::AssertionFailure() + << "Values at (" << ii << ", " << jj + << ") exceed tolerance: " << m1(ii, jj) << " vs. " + << m2(ii, jj) << ", diff = " << delta + << ", tolerance = " << tolerance << "\nm1 =\n" + << m1 << "\nm2 =\n" + << m2 << "\ndelta=\n" + << (m1 - m2); + } + } else { + // Perform comparison using relative tolerance, see: + // http://realtimecollisiondetection.net/blog/?p=89 + using std::max; + const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj))); + const auto relative_tolerance = + tolerance * max(decltype(max_value){1}, max_value); + + if (delta > relative_tolerance) { + return ::testing::AssertionFailure() + << "Values at (" << ii << ", " << jj + << ") exceed tolerance: " << m1(ii, jj) << " vs. " + << m2(ii, jj) << ", diff = " << delta + << ", tolerance = " << tolerance + << ", relative tolerance = " << relative_tolerance + << "\nm1 =\n" + << m1 << "\nm2 =\n" + << m2 << "\ndelta=\n" + << (m1 - m2); + } + } + } + } + + return ::testing::AssertionSuccess() << "m1 =\n" + << m1 + << "\nis approximately equal to m2 =\n" + << m2; +} + +} // namespace fcl + +#endif // FCL_EIGEN_MATRIX_COMPARE_H diff --git a/test/narrowphase/detail/CMakeLists.txt b/test/narrowphase/detail/CMakeLists.txt index c46f5f81e..8c6534c8e 100644 --- a/test/narrowphase/detail/CMakeLists.txt +++ b/test/narrowphase/detail/CMakeLists.txt @@ -1 +1,2 @@ add_subdirectory(convexity_based_algorithm) +add_subdirectory(primitive_shape_algorithm) diff --git a/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt b/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt new file mode 100644 index 000000000..8e908cf78 --- /dev/null +++ b/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt @@ -0,0 +1,8 @@ +set(tests + test_sphere_box.cpp +) + +# Build all the tests +foreach(test ${tests}) + add_fcl_test(${test}) +endforeach(test) diff --git a/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp b/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp new file mode 100644 index 000000000..a10d5dd67 --- /dev/null +++ b/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp @@ -0,0 +1,749 @@ +/* + * 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-box tests: distance and collision. + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h" + +#include + +#include + +#include "eigen_matrix_compare.h" +#include "fcl/geometry/shape/box.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. +template +struct Eps { + using Real = typename constants::Real; + static Real value() { return 16 * constants::eps(); } +}; + +// NOTE: The version of Eigen in travis CI seems to be using code that when +// evaluating: X_FB.inverse() * X_FS ends up doing the equivalent of multiplying +// two 4x4 matrices (rather than exploiting the compact affine representation). +// As such, it leaks a slightly more error into the computation and this extra +// padding accounts for CI peculiarity. +template <> +struct Eps { + using Real = constants::Real; + static Real value() { return 20 * constants::eps(); } +}; + +// Utility function for evaluating points inside boxes. Tests various +// configurations of points and boxes. +template void NearestPointInBox() { + // Picking sizes that are *not* powers of two and *not* uniform in size. + Box box{S(0.6), S(1.2), S(3.6)}; + Vector3 p_BN; + Vector3 p_BQ; + + // Case: query point at origin. + p_BQ << 0, 0, 0; + bool N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_FALSE(N_is_not_C); + EXPECT_TRUE(CompareMatrices(p_BN, p_BQ, 0, MatrixCompareType::absolute)); + + Vector3 half_size = box.side * 0.5; + // Per-octant tests: + for (S x : {-1, 1}) { + for (S y : {-1, 1}) { + for (S z : {-1, 1}) { + Vector3 quadrant{x, y, z}; + // Case: point inside (no clamped values). + p_BQ = quadrant.cwiseProduct(half_size * 0.5); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_FALSE(N_is_not_C); + EXPECT_TRUE( + CompareMatrices(p_BN, p_BQ, 0, MatrixCompareType::absolute)); + + // For each axis: + for (int axis : {0, 1, 2}) { + // Case: one direction clamped. + Vector3 scale{0.5, 0.5, 0.5}; + scale(axis) = 1.5; + p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale)); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_TRUE(N_is_not_C); + for (int i : {0, 1, 2}) { + if (i == axis) + EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i)); + else + EXPECT_EQ(p_BN(i), p_BQ(i)); + } + + // Case: One direction unclamped. + scale << 1.5, 1.5, 1.5; + scale(axis) = 0.5; + p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale)); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_TRUE(N_is_not_C); + for (int i : {0, 1, 2}) { + if (i == axis) + EXPECT_EQ(p_BN(i), p_BQ(i)); + else + EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i)); + } + + // Case: query point on face in axis direction -- unclamped. + scale << 0.5, 0.5, 0.5; + scale(axis) = 1.0; + p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale)); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_FALSE(N_is_not_C); + EXPECT_TRUE( + CompareMatrices(p_BN, p_BQ, 0, MatrixCompareType::absolute)); + } + + // Case: external point in Voronoi region of corner (all axes clamped). + p_BQ = quadrant.cwiseProduct(half_size * 1.5); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_TRUE(N_is_not_C); + for (int i : {0, 1, 2}) + EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i)); + } + } + } +} + +// Defines the test configuration for a single test. It includes the geometry +// and the pose of the sphere in the box's frame B. 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 box 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 Vector3& half_size_in, + S radius, const Vector3& p_BSo_in, bool colliding) + : name(name_in), half_size(half_size_in), r(radius), p_BSo(p_BSo_in), + expected_colliding(colliding) {} + + // Descriptive name of the test configuration. + std::string name; + // The half size of the axis-aligned, origin-centered box. + Vector3 half_size; + // Radius of the sphere. + S r; + // Position of the sphere's center in the box frame. + Vector3 p_BSo; + + // 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 box, respectively, closest to the others measured + // and expressed in the box frame B. Only defined if separated. + Vector3 expected_p_BSb; + Vector3 expected_p_BBs; +}; + +// 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 box are uniformly +// scaled. +template +std::vector> GetUniformConfigurations() { + // Common configuration values + // Box and sphere dimensions. + const S w = 0.6; + const S d = 1.2; + const S h = 3.6; + const S r = 0.7; + const Vector3 half_size{w / 2, d / 2, h / 2}; + const bool collides = true; + + std::vector> configurations; + + { + // Case: Completely separated. Nearest point on the +z face. + const Vector3 p_BS{half_size(0) * S(0.5), half_size(1) * S(0.5), + half_size(2) + r * S(1.1)}; + configurations.emplace_back( + "Separated; nearest face +z", half_size, r, p_BS, !collides); + // Not colliding --> no collision values. + TestConfiguration& config = configurations.back(); + config.expected_distance = p_BS(2) - half_size(2) - r; + config.expected_p_BBs = Vector3{p_BS(0), p_BS(1), half_size(2)}; + config.expected_p_BSb = Vector3{p_BS(0), p_BS(1), p_BS(2) - r}; + } + + { + // Case: Sphere completely separated with center in vertex Voronoi region. + const Vector3 p_BS = half_size + Vector3{r, r, r} * S(1.25); + configurations.emplace_back( + "Separated; nearest +x, +y, +z corner", half_size, r, p_BS, !collides); + // Not colliding --> no collision values. + TestConfiguration& config = configurations.back(); + // position vector from sphere center (S) to nearest point on box (N). + const Vector3 r_SN = half_size - p_BS; + const S len_r_SN = r_SN.norm(); + config.expected_distance = len_r_SN - r; + config.expected_p_BBs = half_size; + config.expected_p_BSb = p_BS + r_SN * (r / len_r_SN); + } + + // Case: Intersection with the sphere center *outside* the box. + // Subcase: sphere in face voronoi region -- normal should be in face + // direction. + // Intersects the z+ face with a depth of half the radius and a normal in the + // -z direction. + { + const S target_depth = r * 0.5; + const Vector3 p_BS{half_size + Vector3{0, 0, r - target_depth}}; + configurations.emplace_back( + "Colliding: center outside, center projects onto +z face", half_size, r, + p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos = Vector3{p_BS(0), p_BS(1), (h - target_depth) / 2}; + + // Colliding; no distance values required. + } + + // Subcase: sphere in vertex Voronoi region -- normal should be in the + // direction from sphere center to box corner. + { + const S target_depth = r * 0.5; + const Vector3 n_SB_B = Vector3(-1, -2, -3).normalized(); + const Vector3 p_BS = half_size - n_SB_B * (r - target_depth); + configurations.emplace_back( + "Colliding: center outside, center nearest +x, +y, +z vertex", + half_size, r, p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = n_SB_B; + config.expected_pos = half_size + n_SB_B * (target_depth * 0.5); + + // Colliding; no distance values required. + } + + // Case: Intersection with the sphere center *inside* the box. We create six + // tests; one for each face of the box. The center will be closest to the + // target face. For the target face f, the normal should be in the -fₙ + // direction (fₙ = normal of face f), the penetration depth is + // radius plus distance to face, and the position is half the penetration + // depth from the face in the -fₙ direction. + // The distance to the face f will be some value less than the smallest half + // size to guarantee no confusion regarding different dimensions. + const std::string axis_name[] = {"x", "y", "z"}; + const S center_inset = half_size.minCoeff() * 0.5; + for (int axis = 0; axis < 3; ++axis) { + for (int sign : {-1, 1}) { + const Vector3 dir = sign * Vector3::Unit(axis); + const Vector3 p_BS = dir * (center_inset - half_size(axis)); + configurations.emplace_back( + "Colliding: center inside, center nearest " + + std::string(sign > 0 ? "+" : "-") + axis_name[axis] + " face", + half_size, r, p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = center_inset + r; + config.expected_normal = dir; + config.expected_pos = dir * ((r + center_inset) / 2 - half_size(axis)); + + // Colliding; no distance values required. + } + } + + // TODO(SeanCurtis-TRI): Consider pushing the point off the face by less than + // epsilon. + + // Sub-case: Sphere center lies on face. + { + const Vector3 p_BS{S(0.), S(0.), half_size(2)}; + configurations.emplace_back("Sphere center lies on +z face", half_size, r, + p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = r; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos << p_BS(0), p_BS(1), p_BS(2) - r / 2; + } + + // Sub-case: Sphere center lies on corner. + { + // Alias the half_size as the coordinate of the +x, +y, +z corner. + const Vector3& p_BS = half_size; + configurations.emplace_back("Sphere center lies on +x, +y, +z corner", + half_size, r, p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = r; + // Sphere center is equidistant to three faces (+x, +y, +z). As documented, + // in this case, the +x face is selected (by aribtrary priority) and the + // normal points *into* that face. + config.expected_normal = -Vector3::UnitX(); + config.expected_pos << p_BS(0) - r / 2, p_BS(1), p_BS(2); + } + + // Case: Sphere and box origins are coincident. + + // Coincident centers subcase: The box is a cube, so the all directions + // produce the same minimum dimension; normal should point in the -x + // direction. + { + configurations.emplace_back( + "Sphere and cube origins coincident", Vector3{10, 10, 10}, 5, + Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitX(); + config.expected_pos << 2.5, 0, 0; + } + + // Coincident centers subcase: Box height and depth are equal and smaller than + // width; the normal should point in the negative x-direction. + { + configurations.emplace_back( + "Sphere and box coincident - x & z are minimum dimension", + Vector3{10, 15, 10}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitX(); + config.expected_pos << 2.5, 0, 0; + } + + // Coincident centers subcase: Box width is the smallest dimension; the normal + // should point in the negative x-direction. + { + configurations.emplace_back( + "Sphere and box coincident - x is minimum dimension", + Vector3{10, 12, 14}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitX(); + config.expected_pos << 2.5, 0, 0; + } + + // Coincident centers subcase: Box height and depth are equal and smaller than + // width; the normal should point in the negative y-direction. + { + configurations.emplace_back( + "Sphere and box coincident - y & z are minimum dimension", + Vector3{15, 10, 10}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitY(); + config.expected_pos << 0, 2.5, 0; + } + + // Coincident centers subcase: Box depth is the smallest dimension; the normal + // should point in the negative y-direction. + { + configurations.emplace_back( + "Sphere and box coincident - y is minimum dimension", + Vector3{15, 10, 14}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitY(); + config.expected_pos << 0, 2.5, 0; + } + + // Coincident centers subcase: Box height is the smallest dimension; the + // normal should point in the negative z-direction. + { + configurations.emplace_back( + "Sphere and box coincident - z is minimum dimension", + Vector3{15, 12, 10}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos << 0, 0, 2.5; + } + + return configurations; +} + +// Returns a collection of configurations where sphere and box are scaled +// very differently. +template +std::vector> GetNonUniformConfigurations() { + std::vector> configurations; + + { + // Case: long "skinny" box and tiny sphere. Nearest feature is the +z face. + const Vector3 half_size(15, 1, 1); + const S r = 0.01; + { + // Subcase: colliding. + const Vector3 p_BS{half_size(0) * S(0.95), S(0.), + half_size(2) + r * S(0.5)}; + configurations.emplace_back("Long, skinny box collides with small sphere", + half_size, r, p_BS, true /* colliding */); + TestConfiguration& config = configurations.back(); + config.expected_normal = -Vector3::UnitZ(); + config.expected_depth = r - (p_BS(2) - half_size(2)); + config.expected_pos = + Vector3{p_BS(0), p_BS(1), + half_size(2) - config.expected_depth / 2}; + } + { + // Subcase: not-colliding. + const S distance = r * 0.1; + const Vector3 p_BS{half_size(0) * S(0.95), S(0.), + half_size(2) + r + distance}; + configurations.emplace_back( + "Long, skinny box *not* colliding with small sphere", half_size, + r, p_BS, false /* not colliding */); + TestConfiguration& config = configurations.back(); + config.expected_distance = distance; + config.expected_p_BSb = p_BS - Vector3{0, 0, r}; + config.expected_p_BBs << p_BS(0), p_BS(1), half_size(2); + } + } + + { + // Case: Large sphere collides with small box. Nearest feature is the +x, + // +y, +z corner. + const Vector3 half_size(0.1, 0.15, 0.2); + const S r = 10; + const Vector3 n_SB = Vector3{-1, -2, -3}.normalized(); + { + // Subcase: colliding. + S target_depth = half_size.minCoeff() * 0.5; + const Vector3 p_BS = half_size - n_SB * (r - target_depth); + configurations.emplace_back("Large sphere colliding with tiny box", + half_size, r, p_BS, true /* colliding */); + TestConfiguration& config = configurations.back(); + config.expected_normal = n_SB; + config.expected_depth = target_depth; + config.expected_pos = half_size + n_SB * (target_depth * 0.5); + } + { + // Subcase: not colliding. + S distance = half_size.minCoeff() * 0.1; + const Vector3 p_BS = half_size - n_SB * (r + distance); + configurations.emplace_back( + "Large sphere *not* colliding with tiny box", half_size, + r, p_BS, false /* not colliding */); + TestConfiguration& config = configurations.back(); + config.expected_distance = distance; + config.expected_p_BSb = p_BS + n_SB * r; + config.expected_p_BBs = half_size; + } + } + + 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 box's +// frame with an unrotated sphere. The parameters provide the test +// configuration, an pose of the box's frame 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_WB, + const Matrix3& R_SB, + S eps) { + // Set up the experiment from input parameters and test configuration. + Box box{config.half_size * 2}; + Sphere sphere{config.r}; + Transform3 X_BS = Transform3::Identity(); + X_BS.translation() = config.p_BSo; + X_BS.linear() = R_SB; + Transform3 X_WS = X_WB * X_BS; + + bool colliding = sphereBoxIntersect(sphere, X_WS, box, X_WB, nullptr); + EXPECT_EQ(colliding, config.expected_colliding) << config.name; + + std::vector> contacts; + colliding = sphereBoxIntersect(sphere, X_WS, box, X_WB, &contacts); + EXPECT_EQ(colliding, config.expected_colliding) << config.name; + if (config.expected_colliding) { + EXPECT_EQ(contacts.size(), 1u) << config.name; + const ContactPoint& contact = contacts[0]; + EXPECT_NEAR(contact.penetration_depth, config.expected_depth, eps) + << config.name; + EXPECT_TRUE(CompareMatrices(contact.normal, + X_WB.linear() * config.expected_normal, eps, + MatrixCompareType::absolute)) + << config.name; + EXPECT_TRUE(CompareMatrices(contact.pos, X_WB * 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 box'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_WB, + const Matrix3& R_SB, + S eps) { + // Set up the experiment from input parameters and test configuration. + Box box{config.half_size * 2}; + Sphere sphere{config.r}; + Transform3 X_BS = Transform3::Identity(); + X_BS.translation() = config.p_BSo; + X_BS.linear() = R_SB; + Transform3 X_WS = X_WB * X_BS; + + bool separated = sphereBoxDistance(sphere, X_WS, box, X_WB, 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_WSb{0, 0, 0}; + Vector3 p_WBs{0, 0, 0}; + + separated = + sphereBoxDistance(sphere, X_WS, box, X_WB, &distance, &p_WSb, &p_WBs); + 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_WSb, + X_WB * config.expected_p_BSb, eps, + MatrixCompareType::absolute)) + << config.name; + EXPECT_TRUE(CompareMatrices(p_WBs, + X_WB * config.expected_p_BBs, eps, + MatrixCompareType::absolute)) + << config.name; + } else { + EXPECT_EQ(distance, S(-1)) << config.name; + EXPECT_TRUE(CompareMatrices(p_WSb, Vector3::Zero(), 0, + MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(p_WBs, 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_BS = Matrix3::Identity()) { + // Evaluate all the configurations with the given box pose in frame F. + auto evaluate_all = [&R_BS, query_eval]( + const std::vector>& configs, + const Transform3& X_FB) { + for (const auto config : configs) { + query_eval(config, X_FB, R_BS, Eps::value()); + } + }; + + // Frame F is the box frame. + Transform3 X_FB = Transform3::Identity(); + evaluate_all(AppendLabel(configurations, "X_FB = I"), X_FB); + + // Simple arbitrary translation away from the origin. + X_FB.translation() << 1.3, 2.7, 6.5; + evaluate_all(AppendLabel(configurations, "X_FB is translation"), X_FB); + + std::string axis_name[] = {"x", "y", "z"}; + // 90 degree rotation around each axis. + for (int axis = 0; axis < 3; ++axis) { + std::string label = "X_FB is 90-degree rotation around " + axis_name[axis]; + AngleAxis angle_axis{constants::pi() / 2, Vector3::Unit(axis)}; + X_FB.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, label), X_FB); + } + + // Arbitrary orientation. + { + AngleAxis angle_axis{constants::pi() / 3, + Vector3{1, 2, 3}.normalized()}; + X_FB.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, "X_FB is arbitrary rotation"), + X_FB); + } + + // Near axis aligned. + { + AngleAxis angle_axis{constants::eps_12(), Vector3::UnitX()}; + X_FB.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, "X_FB is near identity"), + X_FB); + } +} + +// 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 box. +GTEST_TEST(SphereBoxPrimitiveTest, NearestPointInBox) { + NearestPointInBox(); + NearestPointInBox(); +} + +// Evaluates collision on all test configurations across multiple poses in the +// world frame - but the sphere rotation is always the identity. +GTEST_TEST(SphereBoxPrimitiveTest, 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(SphereBoxPrimitiveTest, CollisionWithSphereRotations) { + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates collision on a small set of configurations where the box and scale +// are of radically different scales - evaluation across multiple poses in the +// world frame. +GTEST_TEST(SphereBoxPrimitiveTest, 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(SphereBoxPrimitiveTest, 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(SphereBoxPrimitiveTest, DistanceWithSphereRotations) { + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +// Evaluates distance on a small set of configurations where the box and scale +// are of radically different scales - evaluation across multiple poses in the +// world frame. +GTEST_TEST(SphereBoxPrimitiveTest, 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_collision.cpp b/test/test_fcl_collision.cpp index ad68e62ff..86b373f1d 100644 --- a/test/test_fcl_collision.cpp +++ b/test/test_fcl_collision.cpp @@ -827,108 +827,6 @@ void test_mesh_mesh() } } -// Tests the tolerance value in the CollisionRequest, confirming that it -// propagates down to the solution. It creates a box and sphere and positions -// them so that they *barely* intersect and show that we get different answers -// when changing the collision request's gjk_tolerance value. -// -// The unit sphere is located at the origin. The box is rotated and positioned -// so that a single corner most deeply penetrates into the sphere from the +z -// direction. -// -// The point of this test is *not* to prove that the collision algorithm is -// correct. But merely to show that different tolerances produce different -// answers (i.e., the tolerances are getting through). The answer based on a -// smaller tolerance may or may not be a better answer; that is the subject of -// other unit tests. -// -// The test is formulated this way because in the disparity in behavior between -// the ccd and indep GJK implementations. They differ enough in response to -// tolerance, that a test that actually tested for convergence to "truth" -// became unwieldy. (Specifically, it seems the ccd implementation's answers -// would converge to an answer that differed from the *real* answer by 1e-8 or -// half the precision that I would've hoped for. -// TODO(SeanCurtis-TRI): Create test that confirms that smaller tolerances lead -// to better answers. -template -void CollisionRequestGjkToleranceTest() { - typedef typename Eigen::NumTraits::Real Real; - - using GeometryPtr_t = std::shared_ptr>; - - const S pi = fcl::constants::pi(); - const Real radius = Real(1.0); - const Real size = Real(1.0); - const Real depth = Real(1e-7); - const Vector3 contact_point{0, 0, radius - Real(0.5) * depth}; - - GeometryPtr_t box_geometry(new fcl::Box(size, size, size)); - // Rotate the box twice. The first time brings the edge into contact with the - // sphere. The second brings the corner into contact. Then position it so - // that the corner lies at the position (0, 0, Z), where Z = radius - depth. - Transform3 box_pose{AngleAxis(-pi / 4, Vector3::UnitY()) * - AngleAxis(pi / 4, Vector3::UnitX())}; - Vector3 corner{-size / 2, -size / 2, -size / 2}; - Vector3 rotated_corner = box_pose.linear() * corner; - box_pose.translation() << -rotated_corner(0), - -rotated_corner(1), -rotated_corner(2) + radius - depth; - rotated_corner = box_pose * corner; - // Confirm corner is where it is expected. - EXPECT_NEAR(rotated_corner(0), 0, Eigen::NumTraits::dummy_precision()); - EXPECT_NEAR(rotated_corner(1), 0, Eigen::NumTraits::dummy_precision()); - EXPECT_NEAR(rotated_corner(2), radius - depth, - Eigen::NumTraits::dummy_precision()); - fcl::CollisionObject box(box_geometry, box_pose); - - GeometryPtr_t sphere_geometry(new fcl::Sphere(1)); - fcl::CollisionObject sphere(sphere_geometry, Transform3::Identity()); - - auto test_tolerance = [&box, &sphere] (fcl::GJKSolverType solver_type) { - fcl::CollisionRequest request; - request.num_max_contacts = 1; - request.enable_contact = true; - request.gjk_solver_type = solver_type; - - // 1/4 of the Real's bits in precision. - const Real loose_tolerance = std::pow(Eigen::NumTraits::epsilon(), 0.25); - // 7/8 of the Real's bits in precision (7/8 = 0.875). - const Real tight_tolerance = - std::pow(Eigen::NumTraits::epsilon(), 0.875); - - request.gjk_tolerance = loose_tolerance; - fcl::CollisionResult result_loose; - fcl::collide(&box, &sphere, request, result_loose); - - request.gjk_tolerance = tight_tolerance; - fcl::CollisionResult result_tight; - fcl::collide(&box, &sphere, request, result_tight); - - if (result_loose.numContacts() == result_tight.numContacts()) { - // If there are *no* contacts reported, differences cannot be detected. - GTEST_ASSERT_EQ(1u, result_loose.numContacts()); - const Vector3& pos_loose = result_loose.getContact(0).pos; - const Vector3& pos_tight = result_tight.getContact(0).pos; - // Doing literal bit-wise comparisons. Different bits is sufficient - // evidence to prove difference. - bool is_same = pos_loose(0) == pos_tight(0) && - pos_loose(1) == pos_tight(1) && - pos_loose(2) == pos_tight(2); - EXPECT_FALSE(is_same); - } - // If the number of contacts don't match, then *clearly* the tests have - // produced different results and the tolerance made a difference. - }; - - test_tolerance(fcl::GJKSolverType::GST_INDEP); - test_tolerance(fcl::GJKSolverType::GST_LIBCCD); -} - -GTEST_TEST(FCL_COLLISION, CollisionRequestGjkTolerance) { - CollisionRequestGjkToleranceTest(); - // NOTE: FCL doesn't build for float. -// CollisionRequestGjkToleranceTest(); -} - GTEST_TEST(FCL_COLLISION, OBB_Box_test) { // test_OBB_Box_test(); diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 0ad349b1f..52e92115a 100755 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -834,6 +834,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_boxbox) template void test_shapeIntersection_spherebox() { + // A collection of bool constants to make reading lists of bool easier. + const bool collides = true; + const bool check_position = true; + const bool check_depth = true; + const bool check_normal = true; + const bool check_opposite_normal = false; + Sphere s1(20); Box s2(5, 5, 5); @@ -847,36 +854,46 @@ void test_shapeIntersection_spherebox() tf1 = Transform3::Identity(); tf2 = Transform3::Identity(); - // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0). + // NOTE: The centers of the box and sphere are coincident. The documented + // convention is that the normal is aligned with the smallest dimension of + // the box, pointing in the negative direction of that axis. *This* test is + // the driving basis for that definition. contacts.resize(1); contacts[0].normal << -1, 0, 0; - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, check_normal); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, !check_normal); tf1 = Transform3::Identity(); tf2 = Transform3(Translation3(Vector3(22.5, 0, 0))); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + // As documented in sphere_box.h, touching is considered collision, so this + // should produce a collision. + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, !check_normal); tf1 = transform; tf2 = transform * Transform3(Translation3(Vector3(22.501, 0, 0))); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, !collides); tf1 = Transform3::Identity(); tf2 = Transform3(Translation3(Vector3(22.4, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, check_normal); tf1 = transform; tf2 = transform * Transform3(Translation3(Vector3(22.4, 0, 0))); contacts.resize(1); contacts[0].normal = transform.linear() * Vector3(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-4); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, check_normal, + !check_opposite_normal, 1e-4); } GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherebox) diff --git a/test/test_fcl_sphere_box.cpp b/test/test_fcl_sphere_box.cpp new file mode 100644 index 000000000..5b4d6c53d --- /dev/null +++ b/test/test_fcl_sphere_box.cpp @@ -0,0 +1,216 @@ +/* + * 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_box.cpp for an example. + +// This collides a box with a sphere. The box is long and skinny with size +// (w, d, h) and its geometric frame is aligned with the world frame. +// The sphere has radius r and is positioned at (sx, sy, sz) with an identity +// orientation. In this configuration, the sphere penetrates the box slightly +// on its face that faces in the +z direction. The contact is *fully* contained +// in that face. (As illustrated below.) +// +// Side view +// z small sphere +// ┆ ↓ +// ┏━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━◯━━━━━━┓ ┬ +// ╂┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄╂ x h +// ┗━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┛ ┴ +// ┆ +// +// ├────────────────── w ──────────────────┤ +// +// Top view +// y small sphere +// ┆ ↓ +// ┏━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┓ ┬ +// ╂┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄┄◯┄┄┄┄┄┄╂ x d +// ┗━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┛ ┴ +// ┆ +// +// Properties of expected outcome: +// - One contact *should* be reported, +// - Penetration depth δ should be: radius - (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 LargeBoxSmallSphereTest(fcl::GJKSolverType solver_type) { + using fcl::Vector3; + using Real = typename fcl::constants::Real; + const Real eps = fcl::constants::eps(); + + // Configure geometry. + + // Box and sphere dimensions. + const Real w = 0.115; + const Real h = 0.0025; + const Real d = 0.01; + // The magnitude of the box's extents along each axis. + const Vector3 half_size{w / 2, d / 2, h / 2}; + const Real r = 0.0015; + auto sphere_geometry = std::make_shared>(r); + auto box_geometry = std::make_shared>(w, d, h); + + // Poses of the geometry. + fcl::Transform3 X_WB = 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 * 0.5; + // Sphere center's height (position in z). + const Real sz = h / 2 + r - target_depth; + const Real pz = h / 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 box(box_geometry, X_WB); + + // 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 box for a fixed penetration + // depth. Note: the +z face extends in the +/- x and y directions up to the + // distance half_size. Notes on the selected samples: + // - We've picked positions such that the *whole* sphere projects onto the + // +z face (e.g., half_size(i) - r). 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 x_values{ + -half_size(0) + r, -half_size(0) * S(0.5), -eps, 0, eps, + half_size(0) * S(0.5), half_size(0) - r}; + const std::vector y_values{ + -half_size(1) + r, -half_size(1) * S(0.5), -eps, 0, eps, + half_size(1) * S(0.5), half_size(1) - r}; + for (Real sx : x_values) { + for (Real sy : y_values ) { + // 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, &box, expected_depth, expected_normal, + expected_pos, eps); + evaluate_collision(&box, &sphere, expected_depth, -expected_normal, + expected_pos, eps); + } + } +} + +GTEST_TEST(FCL_SPHERE_BOX, LargBoxSmallSphere_ccd) { + LargeBoxSmallSphereTest(fcl::GJKSolverType::GST_LIBCCD); + LargeBoxSmallSphereTest(fcl::GJKSolverType::GST_LIBCCD); +} + +GTEST_TEST(FCL_SPHERE_BOX, LargBoxSmallSphere_indep) { + LargeBoxSmallSphereTest(fcl::GJKSolverType::GST_INDEP); + LargeBoxSmallSphereTest(fcl::GJKSolverType::GST_INDEP); +} + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}