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