diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index f0b81d59b..4123f707c 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h @@ -1010,7 +1010,7 @@ static int __ccdEPA(const void *obj1, const void *obj2, static void extractClosestPoints(ccd_simplex_t* simplex, ccd_vec3_t* p1, ccd_vec3_t* p2, ccd_vec3_t* p) { - int simplex_size = ccdSimplexSize(simplex); + const int simplex_size = ccdSimplexSize(simplex); assert(simplex_size <= 3); if (simplex_size == 1) { @@ -1151,19 +1151,33 @@ static void extractClosestPoints(ccd_simplex_t* simplex, } } - +// Computes the distance between two non-penetrating convex objects, `obj1` and +// `obj2` based on the warm-started witness simplex, returning the distance and +// nearest points on each object. +// @param obj1 The first convex object. +// @param obj2 The second convex object. +// @param ccd The libccd configuration. +// @param simplex A witness to the objects' separation generated by the GJK +// algorithm. NOTE: the simplex is not necessarily sufficiently refined to +// report the actual distance and may be further refined in this method. +// @param p1 If the objects are non-penetrating, the point on the surface of +// obj1 closest to obj2 (expressed in the world frame). +// @param p2 If the objects are non-penetrating, the point on the surface of +// obj2 closest to obj1 (expressed in the world frame). +// @returns The minimum distance between the two objects. If they are +// penetrating, -1 is returned. static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t* simplex, ccd_vec3_t* p1, ccd_vec3_t* p2) { - unsigned long iterations; - ccd_support_t last; // last support point - ccd_vec3_t dir; // direction vector - ccd_real_t dist, last_dist = CCD_REAL_MAX; + ccd_real_t last_dist = CCD_REAL_MAX; - for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) - { + for (unsigned long iterations = 0UL; iterations < ccd->max_iterations; + ++iterations) { + ccd_vec3_t closest_p; // The point on the simplex that is closest to the + // origin. + ccd_real_t dist; // get a next direction vector // we are trying to find out a point on the minkowski difference // that is nearest to the origin, so we obtain a point on the @@ -1171,7 +1185,7 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, // the origin if (ccdSimplexSize(simplex) == 1) { - ccdVec3Copy(&dir, &ccdSimplexPoint(simplex, 0)->v); + ccdVec3Copy(&closest_p, &ccdSimplexPoint(simplex, 0)->v); dist = ccdVec3Len2(&ccdSimplexPoint(simplex, 0)->v); dist = CCD_SQRT(dist); } @@ -1180,7 +1194,7 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, &ccdSimplexPoint(simplex, 0)->v, &ccdSimplexPoint(simplex, 1)->v, - &dir); + &closest_p); dist = CCD_SQRT(dist); } else if(ccdSimplexSize(simplex) == 3) @@ -1189,26 +1203,29 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, &ccdSimplexPoint(simplex, 0)->v, &ccdSimplexPoint(simplex, 1)->v, &ccdSimplexPoint(simplex, 2)->v, - &dir); + &closest_p); dist = CCD_SQRT(dist); } else { // ccdSimplexSize(&simplex) == 4 - dist = simplexReduceToTriangle(simplex, last_dist, &dir); + dist = simplexReduceToTriangle(simplex, last_dist, &closest_p); } // check whether we improved for at least a minimum tolerance if ((last_dist - dist) < ccd->dist_tolerance) { - extractClosestPoints(simplex, p1, p2, &dir); + extractClosestPoints(simplex, p1, p2, &closest_p); return dist; } // point direction towards the origin + ccd_vec3_t dir; // direction vector + ccdVec3Copy(&dir, &closest_p); ccdVec3Scale(&dir, -CCD_ONE); ccdVec3Normalize(&dir); // find out support point + ccd_support_t last; // last support point __ccdSupport(obj1, obj2, &dir, ccd, &last); // record last distance @@ -1221,7 +1238,7 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, dist = CCD_SQRT(dist); if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance) { - extractClosestPoints(simplex, p1, p2, &dir); + extractClosestPoints(simplex, p1, p2, &closest_p); return last_dist; } @@ -1327,81 +1344,28 @@ static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, co } -/// change the libccd distance to add two closest points +// Computes the distance between two non-penetrating convex objects, `obj1` and +// `obj2`, returning the distance and +// nearest points on each object. +// @param obj1 The first convex object. +// @param obj2 The second convex object. +// @param ccd The libccd configuration. +// @param p1 If the objects are non-penetrating, the point on the surface of +// obj1 closest to obj2 (expressed in the world frame). +// @param p2 If the objects are non-penetrating, the point on the surface of +// obj2 closest to obj1 (expressed in the world frame). +// @returns The minimum distance between the two objects. If they are +// penetrating, -1 is returned. +// @note Unlike _ccdDist function, this function does not need a warm-started +// simplex as the input argument. static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_vec3_t* p1, ccd_vec3_t* p2) { - unsigned long iterations; ccd_simplex_t simplex; - ccd_support_t last; // last support point - ccd_vec3_t dir; // direction vector - ccd_real_t dist, last_dist; - // first find an intersection if (__ccdGJK(obj1, obj2, ccd, &simplex) == 0) return -CCD_ONE; - last_dist = CCD_REAL_MAX; - - for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) { - // get a next direction vector - // we are trying to find out a point on the minkowski difference - // that is nearest to the origin, so we obtain a point on the - // simplex that is nearest and try to exapand the simplex towards - // the origin - if (ccdSimplexSize(&simplex) == 1){ - ccdVec3Copy(&dir, &ccdSimplexPoint(&simplex, 0)->v); - dist = ccdVec3Len2(&ccdSimplexPoint(&simplex, 0)->v); - dist = CCD_SQRT(dist); - }else if (ccdSimplexSize(&simplex) == 2){ - dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, - &ccdSimplexPoint(&simplex, 0)->v, - &ccdSimplexPoint(&simplex, 1)->v, - &dir); - dist = CCD_SQRT(dist); - }else if(ccdSimplexSize(&simplex) == 3){ - dist = ccdVec3PointTriDist2(ccd_vec3_origin, - &ccdSimplexPoint(&simplex, 0)->v, - &ccdSimplexPoint(&simplex, 1)->v, - &ccdSimplexPoint(&simplex, 2)->v, - &dir); - dist = CCD_SQRT(dist); - }else{ // ccdSimplexSize(&simplex) == 4 - dist = simplexReduceToTriangle(&simplex, last_dist, &dir); - } - - // check whether we improved for at least a minimum tolerance - if ((last_dist - dist) < ccd->dist_tolerance) - { - extractClosestPoints(&simplex, p1, p2, &dir); - return dist; - } - - // point direction towards the origin - ccdVec3Scale(&dir, -CCD_ONE); - ccdVec3Normalize(&dir); - - // find out support point - __ccdSupport(obj1, obj2, &dir, ccd, &last); - - // record last distance - last_dist = dist; - - // check whether we improved for at least a minimum tolerance - // this is here probably only for a degenerate cases when we got a - // point that is already in the simplex - dist = ccdVec3Len2(&last.v); - dist = CCD_SQRT(dist); - if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance) - { - extractClosestPoints(&simplex, p1, p2, &dir); - return last_dist; - } - - // add a point to simplex - ccdSimplexAdd(&simplex, &last); - } - - return -CCD_REAL(1.); + return _ccdDist(obj1, obj2, ccd, &simplex, p1, p2); } } // namespace libccd_extension diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 825cfc753..cc599b137 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -59,6 +59,7 @@ set(tests test_fcl_signed_distance.cpp test_fcl_simple.cpp test_fcl_sphere_capsule.cpp + test_fcl_sphere_sphere.cpp ) if (FCL_HAVE_OCTOMAP) @@ -92,3 +93,5 @@ include_directories("${CMAKE_CURRENT_BINARY_DIR}") foreach(test ${tests}) add_fcl_test(${test}) endforeach(test) + +add_subdirectory(narrowphase) diff --git a/test/narrowphase/CMakeLists.txt b/test/narrowphase/CMakeLists.txt new file mode 100644 index 000000000..3943c3cfa --- /dev/null +++ b/test/narrowphase/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(detail) diff --git a/test/narrowphase/detail/CMakeLists.txt b/test/narrowphase/detail/CMakeLists.txt new file mode 100644 index 000000000..c46f5f81e --- /dev/null +++ b/test/narrowphase/detail/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(convexity_based_algorithm) diff --git a/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt new file mode 100644 index 000000000..65a6843b3 --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt @@ -0,0 +1,8 @@ +set(tests + test_gjk_libccd-inl.cpp +) + +# Build all the tests +foreach(test ${tests}) + add_fcl_test(${test}) +endforeach(test) diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp new file mode 100644 index 000000000..6bc7bbdb3 --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp @@ -0,0 +1,163 @@ +/* + * 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 Hongkai Dai*/ +#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" + +#include +#include + +#include "fcl/narrowphase/detail/gjk_solver_libccd.h" + +namespace fcl { +namespace detail { + +// Given two spheres, sphere 1 has radius1, and centered at point A, whose +// position is p_FA measured and expressed in frame F; sphere 2 has radius2, +// and centered at point B, whose position is p_FB measured and expressed in +// frame F. Computes the signed distance between the two spheres, together with +// the two closest points Na on sphere 1 and Nb on sphere 2, returns the +// position of Na and Nb expressed in frame F. +// We use the monogram notation on spatial vectors. The monogram notation is +// explained in +// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html +template +S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3& p_FA, + const Vector3& p_FB, Vector3* p_FNa, + Vector3* p_FNb) { + S min_distance = (p_FA - p_FB).norm() - radius1 - radius2; + const Vector3 p_AB_F = + p_FB - p_FA; // The vector AB measured and expressed + // in frame F. + *p_FNa = p_FA + p_AB_F.normalized() * radius1; + *p_FNb = p_FB - p_AB_F.normalized() * radius2; + return min_distance; +} + +template +void TestSphereToSphereGJKSignedDistance(S radius1, S radius2, + const Vector3& p_FA, + const Vector3& p_FB, S tol, + S min_distance_expected, + const Vector3& p_FNa_expected, + const Vector3& p_FNb_expected) { + // Test if GJKSignedDistance computes the right distance. Here we used sphere + // to sphere as the geometries. The distance between sphere and sphere should + // be computed using distance between primitives, instead of the GJK + // algorithm. But here we choose spheres for simplicity. + + fcl::Sphere s1(radius1); + fcl::Sphere s2(radius2); + fcl::Transform3 tf1, tf2; + tf1.setIdentity(); + tf2.setIdentity(); + tf1.translation() = p_FA; + tf2.translation() = p_FB; + void* o1 = GJKInitializer>::createGJKObject(s1, tf1); + void* o2 = GJKInitializer>::createGJKObject(s2, tf2); + + S dist; + Vector3 p1, p2; + GJKSolver_libccd gjkSolver; + bool res = GJKSignedDistance( + o1, detail::GJKInitializer>::getSupportFunction(), o2, + detail::GJKInitializer>::getSupportFunction(), + gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, + &p1, &p2); + + EXPECT_EQ(res, min_distance_expected >= 0); + + EXPECT_NEAR(dist, min_distance_expected, tol); + EXPECT_TRUE(p1.isApprox(p_FNa_expected, tol)); + EXPECT_TRUE(p2.isApprox(p_FNb_expected, tol)); + + GJKInitializer>::deleteGJKObject(o1); + GJKInitializer>::deleteGJKObject(o2); +} + +template +struct SphereSpecification { + SphereSpecification(S radius_, const Vector3& center_) + : radius{radius_}, center{center_} {} + S radius; + Vector3 center; +}; + +template +void TestNonCollidingSphereGJKSignedDistance(S tol) { + std::vector> spheres; + spheres.emplace_back(0.5, Vector3(0, 0, -1.2)); + spheres.emplace_back(0.5, Vector3(1.25, 0, 0)); + spheres.emplace_back(0.3, Vector3(-0.2, 0, 0)); + spheres.emplace_back(0.4, Vector3(-0.2, 0, 1.1)); + for (int i = 0; i < static_cast(spheres.size()); ++i) { + for (int j = i + 1; j < static_cast(spheres.size()); ++j) { + if ((spheres[i].center - spheres[j].center).norm() > + spheres[i].radius + spheres[j].radius) { + // Not in collision. + Vector3 p_FNa, p_FNb; + const S min_distance_expected = ComputeSphereSphereDistance( + spheres[i].radius, spheres[j].radius, spheres[i].center, + spheres[j].center, &p_FNa, &p_FNb); + TestSphereToSphereGJKSignedDistance( + spheres[i].radius, spheres[j].radius, spheres[i].center, + spheres[j].center, tol, min_distance_expected, p_FNa, p_FNb); + } else { + GTEST_FAIL() << "The two spheres collide." + << "\nSpheres[" << i << "] with radius " + << spheres[i].radius << ", centered at " + << spheres[i].center.transpose() << "\nSpheres[" << j + << "] with radius " << spheres[j].radius + << ", centered at " << spheres[j].center.transpose() + << "\n"; + } + } + } +} + +GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) { + // TODO(hongkai.dai@tri.global): By setting gjkSolver.distance_tolerance to + // the default value (1E-6), the tolerance we get on the closest points are + // only up to 1E-3. Should investigate why there is such a big difference. + TestNonCollidingSphereGJKSignedDistance(1E-3); + TestNonCollidingSphereGJKSignedDistance(1E-3); +} +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_sphere_sphere.cpp b/test/test_fcl_sphere_sphere.cpp new file mode 100644 index 000000000..66db15cc5 --- /dev/null +++ b/test/test_fcl_sphere_sphere.cpp @@ -0,0 +1,194 @@ +/* + * 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 Hongkai Dai */ + +#include +#include +#include + +#include +#include + +#include "fcl/narrowphase/collision_object.h" +#include "fcl/narrowphase/distance.h" +#include "fcl/narrowphase/distance_request.h" +#include "fcl/narrowphase/distance_result.h" + +// For two spheres 1, 2, sphere 1 has radius1, and is centered at point A, with +// coordinate p_FA in some frame F; sphere 2 has radius2, and is centered at +// point B, with coordinate p_FB in the same frame F. Compute the (optionally +// signed) distance between the two spheres. +template +S ComputeSphereSphereDistance(S radius1, S radius2, const fcl::Vector3& p_FA, + const fcl::Vector3& p_FB, + fcl::GJKSolverType solver_type, + bool enable_nearest_points, + bool enable_signed_distance, + fcl::DistanceResult* result) { + // Pose of the sphere expressed in the frame F. X_FA is the pose of sphere 1 + // in frame F, while X_FB is the pose of sphere 2 in frame F. + // We use monogram notation as in Drake, explained in + // http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html + fcl::Transform3 X_FA, X_FB; + X_FA.setIdentity(); + X_FB.setIdentity(); + X_FA.translation() = p_FA; + X_FB.translation() = p_FB; + + fcl::DistanceRequest request; + request.enable_nearest_points = enable_nearest_points; + request.enable_signed_distance = enable_signed_distance; + request.gjk_solver_type = solver_type; + + using CollisionGeometryPtr_t = std::shared_ptr>; + CollisionGeometryPtr_t sphere_geometry_1(new fcl::Sphere(radius1)); + CollisionGeometryPtr_t sphere_geometry_2(new fcl::Sphere(radius2)); + + fcl::CollisionObject sphere_1(sphere_geometry_1, X_FA); + fcl::CollisionObject sphere_2(sphere_geometry_2, X_FB); + const S min_distance = fcl::distance(&sphere_1, &sphere_2, request, *result); + return min_distance; +} + +template +struct SphereSpecification { + // @param p_FC_ the center of the sphere C measured and expressed in a frame F + SphereSpecification(S m_radius, const fcl::Vector3& p_FC_) + : radius(m_radius), p_FC(p_FC_) {} + S radius; + fcl::Vector3 p_FC; +}; + +template +void CheckSphereToSphereDistance(const SphereSpecification& sphere1, + const SphereSpecification& sphere2, + fcl::GJKSolverType solver_type, + bool enable_nearest_points, + bool enable_signed_distance, + S min_distance_expected, + const fcl::Vector3& p1_expected, + const fcl::Vector3& p2_expected, S tol) { + fcl::DistanceResult result; + const S min_distance = ComputeSphereSphereDistance( + sphere1.radius, sphere2.radius, sphere1.p_FC, sphere2.p_FC, solver_type, + enable_nearest_points, enable_signed_distance, &result); + EXPECT_NEAR(min_distance, min_distance_expected, tol); + EXPECT_NEAR(result.min_distance, min_distance_expected, tol); + EXPECT_TRUE(result.nearest_points[0].isApprox(p1_expected, tol)); + EXPECT_TRUE(result.nearest_points[1].isApprox(p2_expected, tol)); +} + +template +struct SphereSphereDistance { + SphereSphereDistance(const SphereSpecification& m_sphere1, + const SphereSpecification& m_sphere2) + : sphere1(m_sphere1), sphere2(m_sphere2) { + min_distance = + (sphere1.p_FC - sphere2.p_FC).norm() - sphere1.radius - sphere2.radius; + const fcl::Vector3 AB = (sphere1.p_FC - sphere2.p_FC).normalized(); + p_S1P1 = AB * -sphere1.radius; + p_S2P2 = AB * sphere2.radius; + } + SphereSpecification sphere1; + SphereSpecification sphere2; + S min_distance; + // The closest point P1 on sphere 1 is expressed in the sphere 1 body frame + // S1. + // The closest point P2 on sphere 2 is expressed in the sphere 2 body frame + // S2. + fcl::Vector3 p_S1P1; + fcl::Vector3 p_S2P2; +}; + +template +void TestSeparatingSpheres(S tol, fcl::GJKSolverType solver_type) { + std::vector> spheres; + spheres.emplace_back(0.5, fcl::Vector3(0, 0, -1)); // sphere 1 + spheres.emplace_back(0.6, fcl::Vector3(1.2, 0, 0)); // sphere 2 + spheres.emplace_back(0.4, fcl::Vector3(-0.3, 0, 0)); // sphere 3 + spheres.emplace_back(0.3, fcl::Vector3(0, 0, 1)); // sphere 4 + + for (int i = 0; i < static_cast(spheres.size()); ++i) { + for (int j = i + 1; j < static_cast(spheres.size()); ++j) { + SphereSphereDistance sphere_sphere_distance(spheres[i], spheres[j]); + bool enable_signed_distance = true; + CheckSphereToSphereDistance( + spheres[i], spheres[j], solver_type, true, enable_signed_distance, + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S1P1, + sphere_sphere_distance.p_S2P2, tol); + + // Now switch the order of sphere 1 with sphere 2 in calling + // fcl::distance function, and test again. + CheckSphereToSphereDistance( + spheres[j], spheres[i], solver_type, true, enable_signed_distance, + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S2P2, + sphere_sphere_distance.p_S1P1, tol); + + enable_signed_distance = false; + CheckSphereToSphereDistance( + spheres[i], spheres[j], solver_type, true, enable_signed_distance, + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S1P1, + sphere_sphere_distance.p_S2P2, tol); + // Now switch the order of sphere 1 with sphere 2 in calling + // fcl::distance function, and test again. + CheckSphereToSphereDistance( + spheres[j], spheres[i], solver_type, true, enable_signed_distance, + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S2P2, + sphere_sphere_distance.p_S1P1, tol); + + // TODO (hongkai.dai@tri.global): Test enable_nearest_points=false. + } + } +} + +GTEST_TEST(FCL_SPHERE_SPHERE, Separating_Spheres_INDEP) { + TestSeparatingSpheres(1E-14, fcl::GJKSolverType::GST_INDEP); +} + +GTEST_TEST(FCL_SPHERE_SPHERE, Separating_Spheres_LIBCCD) { + // TODO(hongkai.dai@tri.global): The accuracy of the closest point is only up + // to 1E-3, although gjkSolver::distance_tolerance is 1E-6. We should + // investigate the accuracy issue. + // Specifically, when setting `enable_signed_distance = true`, then except for + // the the pair of spheres (2, 3), the closest point between all other pairs + // fail to achieve tolerance 1E-6. When `enable_signed_distance = false`, then + // all pairs achieve tolerance 1E-6. + TestSeparatingSpheres(1E-3, fcl::GJKSolverType::GST_LIBCCD); +} +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}