From 651bb99bb2c44f7893d76d5adcff38cfdc51f585 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Mon, 3 Feb 2020 09:25:19 -0800 Subject: [PATCH] Create default distance/collision callbacks in fcl namespace We had default callbacks defined in the test namespace. The main readme.md was recommending using them. This moves them directly into fcl namespace so that user code doesn't have to pull in test code. --- CHANGELOG.md | 7 +- README.md | 31 +-- .../broadphase/default_broadphase_callbacks.h | 219 ++++++++++++++++++ test/test_fcl_broadphase_collision_1.cpp | 9 +- test/test_fcl_broadphase_collision_2.cpp | 9 +- test/test_fcl_broadphase_distance.cpp | 13 +- test/test_fcl_octomap_collision.cpp | 21 +- test/test_fcl_octomap_cost.cpp | 13 +- test/test_fcl_octomap_distance.cpp | 21 +- test/test_fcl_utility.h | 137 ----------- 10 files changed, 287 insertions(+), 193 deletions(-) create mode 100644 include/fcl/broadphase/default_broadphase_callbacks.h diff --git a/CHANGELOG.md b/CHANGELOG.md index 7226fb169..81f390b50 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -34,7 +34,12 @@ * Added distance request option for computing exact negative distance: [#172](https://github.com/flexible-collision-library/fcl/pull/172) * Build/Test/Misc - + * Default callback functions for broadphase collision managers have been moved + out of `fcl::test` and into `fcl` namespace (with a corresponding name + change, e.g., `defaultDistanceFunction` --> `DefaultDistanceFunction`). + [#438](https://github.com/flexible-collision-library/fcl/pull/438) + * This includes the removal of the stub function + `defaultContinuousDistanceFunction()`. * Added version check for Visual Studio in CMake (VS2015 or greater required): [#189](https://github.com/flexible-collision-library/fcl/pull/189) * 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) diff --git a/README.md b/README.md index 294c0a663..338895488 100644 --- a/README.md +++ b/README.md @@ -191,34 +191,35 @@ manager2->registerObjects(objects2); // requires two settings: // a) a callback to collision or distance; // b) an intermediate data to store the information generated during the -// broadphase computation -// For a), FCL's test framework provides default callbacks for both collision -// and distance. For b), FCL uses the CollisionData structure for collision and -// DistanceData structure for distance. CollisionData/DistanceData is just a -// container from the test framework including both the -// CollisionRequest/DistanceRequest and CollisionResult/DistanceResult -// structures mentioned above. -CollisionData collision_data; -DistanceData distance_data; +// broadphase computation. +// For convenience, FCL provides default callbacks to satisfy a) and a +// corresponding call back data to satisfy b) for both collision and distance +// queries. For collision use DefaultCollisionCallback and DefaultCollisionData +// and for distance use DefaultDistanceCallback and DefaultDistanceData. +// The default collision/distance data structs are simply containers which +// include the request and distance structures for each query type as mentioned +// above. +DefaultCollisionData collision_data; +DefaultDistanceData distance_data; // Setup the managers, which is related with initializing the broadphase // acceleration structure according to objects input manager1->setup(); manager2->setup(); // Examples for various queries // 1. Collision query between two object groups and get collision numbers -manager2->collide(manager1, &collision_data, test::defaultCollisionFunction); +manager2->collide(manager1, &collision_data, DefaultCollisionFunction); int n_contact_num = collision_data.result.numContacts(); // 2. Distance query between two object groups and get the minimum distance -manager2->distance(manager1, &distance_data, test::defaultDistanceFunction); +manager2->distance(manager1, &distance_data, DefaultDistanceFunction); double min_distance = distance_data.result.min_distance; // 3. Self collision query for group 1 -manager1->collide(&collision_data, test::defaultCollisionFunction); +manager1->collide(&collision_data, DefaultCollisionFunction); // 4. Self distance query for group 1 -manager1->distance(&distance_data, test::defaultDistanceFunction); +manager1->distance(&distance_data, DefaultDistanceFunction); // 5. Collision query between one object in group 1 and the entire group 2 -manager2->collide(objects1[0], &collision_data, test::defaultCollisionFunction); +manager2->collide(objects1[0], &collision_data, DefaultCollisionFunction); // 6. Distance query between one object in group 1 and the entire group 2 -manager2->distance(objects1[0], &distance_data, test::defaultDistanceFunction); +manager2->distance(objects1[0], &distance_data, DefaultDistanceFunction); ``` diff --git a/include/fcl/broadphase/default_broadphase_callbacks.h b/include/fcl/broadphase/default_broadphase_callbacks.h new file mode 100644 index 000000000..1c9b4fec5 --- /dev/null +++ b/include/fcl/broadphase/default_broadphase_callbacks.h @@ -0,0 +1,219 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, 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 the copyright holder 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) */ + +#ifndef FCL_BROADPHASE_DEFAULTBROADPHASECALLBACKS_H +#define FCL_BROADPHASE_DEFAULTBROADPHASECALLBACKS_H + +#include "fcl/narrowphase/collision.h" +#include "fcl/narrowphase/collision_request.h" +#include "fcl/narrowphase/collision_result.h" +#include "fcl/narrowphase/continuous_collision.h" +#include "fcl/narrowphase/continuous_collision_request.h" +#include "fcl/narrowphase/continuous_collision_result.h" +#include "fcl/narrowphase/distance.h" +#include "fcl/narrowphase/distance_request.h" +#include "fcl/narrowphase/distance_result.h" + +namespace fcl { + +/// @brief Collision data for use with the DefaultCollisionFunction. It stores +/// the collision request and the result given by collision algorithm (and +/// stores the conclusion of whether further evaluation of the broadphase +/// collision manager has been deemed unnecessary). +template +struct DefaultCollisionData { + CollisionRequest request; + CollisionResult result; + + /// If `true`, requests that the broadphase evaluation stop. + bool done{false}; +}; + +/// @brief Provides a simple callback for the collision query in the +/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and +/// points to an instance of DefaultCollisionData. It simply invokes the +/// `collide()` method on the culled pair of geometries and stores the results +/// in the data's CollisionResult instance. +/// +/// This callback will cause the broadphase evaluation to stop if: +/// - the collision requests _disables_ cost _and_ +/// - the collide() reports a collision for the culled pair, _and_ +/// - we've reported the number of contacts requested in the CollisionRequest. +/// +/// For a given instance of DefaultCollisionData, if broadphase evaluation has +/// already terminated (i.e., DefaultCollisionFunction() returned `true`), +/// subsequent invocations with the same instance of DefaultCollisionData will +/// return immediately, requesting termination of broadphase evaluation (i.e., +/// return `true`). +/// +/// @param o1 The first object in the culled pair. +/// @param o2 The second object in the culled pair. +/// @param data A non-null pointer to a DefaultCollisionData instance. +/// @return `true` if the broadphase evaluation should stop. +/// @tparam S The scalar type with which the computation will be performed. +template +bool DefaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, + void* data) { + assert(data != nullptr); + auto* collision_data = static_cast*>(data); + const CollisionRequest& request = collision_data->request; + CollisionResult& result = collision_data->result; + + if(collision_data->done) return true; + + collide(o1, o2, request, result); + + if (!request.enable_cost && result.isCollision() && + result.numContacts() >= request.num_max_contacts) { + collision_data->done = true; + } + + return collision_data->done; +} + + +/// @brief Collision data for use with the DefaultContinuousCollisionFunction. +/// It stores the collision request and the result given by the collision +/// algorithm (and stores the conclusion of whether further evaluation of the +/// broadphase collision manager has been deemed unnecessary). +template +struct DefaultContinuousCollisionData { + ContinuousCollisionRequest request; + ContinuousCollisionResult result; + + /// If `true`, requests that the broadphase evaluation stop. + bool done{false}; +}; + + +/// @brief Provides a simple callback for the continuous collision query in the +/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and +/// points to an instance of DefaultContinuousCollisionData. It simply invokes +/// the `collide()` method on the culled pair of geometries and stores the +/// results in the data's ContinuousCollisionResult instance. +/// +/// This callback will never cause the broadphase evaluation to terminate early. +/// However, if the `done` member of the DefaultContinuousCollisionData is set +/// to true, this method will simply return without doing any computation. +/// +/// For a given instance of DefaultContinuousCollisionData, if broadphase +/// evaluation has already terminated (i.e., +/// DefaultContinuousCollisionFunction() returned `true`), subsequent +/// invocations with the same instance of DefaultCollisionData will return +/// immediately, requesting termination of broadphase evaluation (i.e., return +/// `true`). +/// +/// @param o1 The first object in the culled pair. +/// @param o2 The second object in the culled pair. +/// @param data A non-null pointer to a DefaultCollisionData instance. +/// @return True if the broadphase evaluation should stop. +/// @tparam S The scalar type with which the computation will be performed. +template +bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1, + ContinuousCollisionObject* o2, + void* data) { + assert(data != nullptr); + auto* cdata = static_cast*>(data); + + if (cdata->done) return true; + + const ContinuousCollisionRequest& request = cdata->request; + ContinuousCollisionResult& result = cdata->result; + collide(o1, o2, request, result); + + return cdata->done; +} + +/// @brief Distance data for use with the DefaultDistanceFunction. It stores +/// the distance request and the result given by distance algorithm (and +/// stores the conclusion of whether further evaluation of the broadphase +/// collision manager has been deemed unnecessary). +template +struct DefaultDistanceData { + DistanceRequest request; + DistanceResult result; + + /// If `true`, requests that the broadphase evaluation stop. + bool done{false}; +}; + +/// @brief Provides a simple callback for the distance query in the +/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and +/// points to an instance of DefaultDistanceData. It simply invokes the +/// `distance()` method on the culled pair of geometries and stores the results +/// in the data's DistanceResult instance. +/// +/// This callback will cause the broadphase evaluation to stop if: +/// - The distance is less than or equal to zero (i.e., the pair is in +/// contact). +/// +/// For a given instance of DefaultDistanceData, if broadphase evaluation has +/// already terminated (i.e., DefaultDistanceFunction() returned `true`), +/// subsequent invocations with the same instance of DefaultDistanceData will +/// simply report the previously reported minimum distance and request +/// termination of broadphase evaluation (i.e., return `true`). +/// +/// @param o1 The first object in the culled pair. +/// @param o2 The second object in the culled pair. +/// @param data A non-null pointer to a DefaultDistanceData instance. +/// @param dist The distance computed by distance(). +/// @return `true` if the broadphase evaluation should stop. +/// @tparam S The scalar type with which the computation will be performed. +template +bool DefaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, + void* data, S& dist) { + assert(data != nullptr); + auto* cdata = static_cast*>(data); + const DistanceRequest& request = cdata->request; + DistanceResult& result = cdata->result; + + if (cdata->done) { + dist = result.min_distance; + return true; + } + + distance(o1, o2, request, result); + + dist = result.min_distance; + + if (dist <= 0) return true; // in collision or in touch + + return cdata->done; +} + +} // namespace fcl + +#endif // FCL_BROADPHASE_DEFAULTBROADPHASECALLBACKS_H diff --git a/test/test_fcl_broadphase_collision_1.cpp b/test/test_fcl_broadphase_collision_1.cpp index 6e77c43a0..3185ea05e 100644 --- a/test/test_fcl_broadphase_collision_1.cpp +++ b/test/test_fcl_broadphase_collision_1.cpp @@ -45,6 +45,7 @@ #include "fcl/broadphase/broadphase_interval_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "fcl/broadphase/default_broadphase_callbacks.h" #include "fcl/broadphase/detail/sparse_hash_table.h" #include "fcl/broadphase/detail/spatial_hash.h" #include "fcl/geometry/geometric_shape_to_BVH_model.h" @@ -447,7 +448,7 @@ void broad_phase_update_collision_test(S env_scale, std::size_t env_size, std::s ts[i].push_back(timers[i].getElapsedTime()); } - std::vector> self_data(managers.size()); + std::vector> self_data(managers.size()); for(size_t i = 0; i < managers.size(); ++i) { if(exhaustive) self_data[i].request.num_max_contacts = 100000; @@ -457,7 +458,7 @@ void broad_phase_update_collision_test(S env_scale, std::size_t env_size, std::s for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); - managers[i]->collide(&self_data[i], test::defaultCollisionFunction); + managers[i]->collide(&self_data[i], DefaultCollisionFunction); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } @@ -488,7 +489,7 @@ void broad_phase_update_collision_test(S env_scale, std::size_t env_size, std::s for(size_t i = 0; i < query.size(); ++i) { - std::vector> query_data(managers.size()); + std::vector> query_data(managers.size()); for(size_t j = 0; j < query_data.size(); ++j) { if(exhaustive) query_data[j].request.num_max_contacts = 100000; @@ -498,7 +499,7 @@ void broad_phase_update_collision_test(S env_scale, std::size_t env_size, std::s for(size_t j = 0; j < query_data.size(); ++j) { timers[j].start(); - managers[j]->collide(query[i], &query_data[j], test::defaultCollisionFunction); + managers[j]->collide(query[i], &query_data[j], DefaultCollisionFunction); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); } diff --git a/test/test_fcl_broadphase_collision_2.cpp b/test/test_fcl_broadphase_collision_2.cpp index 52cf4173c..93af5f1d6 100644 --- a/test/test_fcl_broadphase_collision_2.cpp +++ b/test/test_fcl_broadphase_collision_2.cpp @@ -45,6 +45,7 @@ #include "fcl/broadphase/broadphase_interval_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "fcl/broadphase/default_broadphase_callbacks.h" #include "fcl/broadphase/detail/sparse_hash_table.h" #include "fcl/broadphase/detail/spatial_hash.h" #include "fcl/geometry/geometric_shape_to_BVH_model.h" @@ -252,7 +253,7 @@ void broad_phase_collision_test(S env_scale, std::size_t env_size, std::size_t q ts[i].push_back(timers[i].getElapsedTime()); } - std::vector> self_data(managers.size()); + std::vector> self_data(managers.size()); for(size_t i = 0; i < managers.size(); ++i) { if(exhaustive) self_data[i].request.num_max_contacts = 100000; @@ -262,7 +263,7 @@ void broad_phase_collision_test(S env_scale, std::size_t env_size, std::size_t q for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); - managers[i]->collide(&self_data[i], test::defaultCollisionFunction); + managers[i]->collide(&self_data[i], DefaultCollisionFunction); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } @@ -292,7 +293,7 @@ void broad_phase_collision_test(S env_scale, std::size_t env_size, std::size_t q for(size_t i = 0; i < query.size(); ++i) { - std::vector> query_data(managers.size()); + std::vector> query_data(managers.size()); for(size_t j = 0; j < query_data.size(); ++j) { if(exhaustive) query_data[j].request.num_max_contacts = 100000; @@ -302,7 +303,7 @@ void broad_phase_collision_test(S env_scale, std::size_t env_size, std::size_t q for(size_t j = 0; j < query_data.size(); ++j) { timers[j].start(); - managers[j]->collide(query[i], &query_data[j], test::defaultCollisionFunction); + managers[j]->collide(query[i], &query_data[j], DefaultCollisionFunction); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); } diff --git a/test/test_fcl_broadphase_distance.cpp b/test/test_fcl_broadphase_distance.cpp index 38acaa237..971e271bf 100644 --- a/test/test_fcl_broadphase_distance.cpp +++ b/test/test_fcl_broadphase_distance.cpp @@ -45,6 +45,7 @@ #include "fcl/broadphase/broadphase_interval_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "fcl/broadphase/default_broadphase_callbacks.h" #include "fcl/broadphase/detail/sparse_hash_table.h" #include "fcl/broadphase/detail/spatial_hash.h" #include "fcl/geometry/geometric_shape_to_BVH_model.h" @@ -379,12 +380,12 @@ void broad_phase_self_distance_test(S env_scale, std::size_t env_size, bool use_ } - std::vector> self_data(managers.size()); + std::vector> self_data(managers.size()); for(size_t i = 0; i < self_data.size(); ++i) { timers[i].start(); - managers[i]->distance(&self_data[i], test::defaultDistanceFunction); + managers[i]->distance(&self_data[i], DefaultDistanceFunction); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); // std::cout << self_data[i].result.min_distance << " "; @@ -457,8 +458,8 @@ void broad_phase_distance_test(S env_scale, std::size_t env_size, std::size_t qu for(std::size_t i = 0; i < candidates.size(); ++i) { - test::CollisionData query_data; - manager->collide(candidates[i], &query_data, test::defaultCollisionFunction); + DefaultCollisionData query_data; + manager->collide(candidates[i], &query_data, DefaultCollisionFunction); if(query_data.result.numContacts() == 0) query.push_back(candidates[i]); else @@ -524,11 +525,11 @@ void broad_phase_distance_test(S env_scale, std::size_t env_size, std::size_t qu for(size_t i = 0; i < query.size(); ++i) { - std::vector> query_data(managers.size()); + std::vector> query_data(managers.size()); for(size_t j = 0; j < managers.size(); ++j) { timers[j].start(); - managers[j]->distance(query[i], &query_data[j], test::defaultDistanceFunction); + managers[j]->distance(query[i], &query_data[j], DefaultDistanceFunction); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); // std::cout << query_data[j].result.min_distance << " "; diff --git a/test/test_fcl_octomap_collision.cpp b/test/test_fcl_octomap_collision.cpp index fb81d8f97..3c711884a 100644 --- a/test/test_fcl_octomap_collision.cpp +++ b/test/test_fcl_octomap_collision.cpp @@ -47,6 +47,7 @@ #include "fcl/broadphase/broadphase_interval_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "fcl/broadphase/default_broadphase_callbacks.h" #include "fcl/geometry/geometric_shape_to_BVH_model.h" #include "test_fcl_utility.h" #include "fcl_resources/config.h" @@ -192,10 +193,10 @@ void octomap_collision_test_BVH(std::size_t n, bool exhaustive, double resolutio CollisionObject obj1(m1_ptr, tf); CollisionObject obj2(tree_ptr, tf); - test::CollisionData cdata; + DefaultCollisionData cdata; if(exhaustive) cdata.request.num_max_contacts = 100000; - test::defaultCollisionFunction(&obj1, &obj2, &cdata); + DefaultCollisionFunction(&obj1, &obj2, &cdata); std::vector*> boxes; @@ -207,9 +208,9 @@ void octomap_collision_test_BVH(std::size_t n, bool exhaustive, double resolutio manager->registerObjects(boxes); manager->setup(); - test::CollisionData cdata2; + DefaultCollisionData cdata2; if(exhaustive) cdata2.request.num_max_contacts = 100000; - manager->collide(&obj1, &cdata2, test::defaultCollisionFunction); + manager->collide(&obj1, &cdata2, DefaultCollisionFunction); for(std::size_t j = 0; j < boxes.size(); ++j) delete boxes[j]; @@ -245,7 +246,7 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive, manager->registerObjects(env); manager->setup(); - test::CollisionData cdata; + DefaultCollisionData cdata; if(exhaustive) cdata.request.num_max_contacts = 100000; else cdata.request.num_max_contacts = num_max_contacts; @@ -254,11 +255,11 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive, timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; - manager->collide(&tree_obj, &cdata, test::defaultCollisionFunction); + manager->collide(&tree_obj, &cdata, DefaultCollisionFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); - test::CollisionData cdata3; + DefaultCollisionData cdata3; if(exhaustive) cdata3.request.num_max_contacts = 100000; else cdata3.request.num_max_contacts = num_max_contacts; @@ -267,7 +268,7 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive, timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; - manager->collide(&tree_obj, &cdata3, test::defaultCollisionFunction); + manager->collide(&tree_obj, &cdata3, DefaultCollisionFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); @@ -290,12 +291,12 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive, t2.push_back(timer2.getElapsedTime()); - test::CollisionData cdata2; + DefaultCollisionData cdata2; if(exhaustive) cdata2.request.num_max_contacts = 100000; else cdata2.request.num_max_contacts = num_max_contacts; timer2.start(); - manager->collide(manager2, &cdata2, test::defaultCollisionFunction); + manager->collide(manager2, &cdata2, DefaultCollisionFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); diff --git a/test/test_fcl_octomap_cost.cpp b/test/test_fcl_octomap_cost.cpp index 350e35750..5e34183d2 100644 --- a/test/test_fcl_octomap_cost.cpp +++ b/test/test_fcl_octomap_cost.cpp @@ -47,6 +47,7 @@ #include "fcl/broadphase/broadphase_interval_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "fcl/broadphase/default_broadphase_callbacks.h" #include "fcl/geometry/geometric_shape_to_BVH_model.h" #include "test_fcl_utility.h" #include "fcl_resources/config.h" @@ -109,7 +110,7 @@ void octomap_cost_test(S env_scale, std::size_t env_size, std::size_t num_max_co manager->registerObjects(env); manager->setup(); - test::CollisionData cdata; + DefaultCollisionData cdata; cdata.request.enable_cost = true; cdata.request.num_max_cost_sources = num_max_cost_sources; @@ -118,11 +119,11 @@ void octomap_cost_test(S env_scale, std::size_t env_size, std::size_t num_max_co timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; - manager->collide(&tree_obj, &cdata, test::defaultCollisionFunction); + manager->collide(&tree_obj, &cdata, DefaultCollisionFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); - test::CollisionData cdata3; + DefaultCollisionData cdata3; cdata3.request.enable_cost = true; cdata3.request.num_max_cost_sources = num_max_cost_sources; @@ -131,7 +132,7 @@ void octomap_cost_test(S env_scale, std::size_t env_size, std::size_t num_max_co timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; - manager->collide(&tree_obj, &cdata3, test::defaultCollisionFunction); + manager->collide(&tree_obj, &cdata3, DefaultCollisionFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); @@ -153,12 +154,12 @@ void octomap_cost_test(S env_scale, std::size_t env_size, std::size_t num_max_co timer2.stop(); t2.push_back(timer2.getElapsedTime()); - test::CollisionData cdata2; + DefaultCollisionData cdata2; cdata2.request.enable_cost = true; cdata3.request.num_max_cost_sources = num_max_cost_sources; timer2.start(); - manager->collide(manager2, &cdata2, test::defaultCollisionFunction); + manager->collide(manager2, &cdata2, DefaultCollisionFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); diff --git a/test/test_fcl_octomap_distance.cpp b/test/test_fcl_octomap_distance.cpp index 3a5e6880d..7929e85e9 100644 --- a/test/test_fcl_octomap_distance.cpp +++ b/test/test_fcl_octomap_distance.cpp @@ -48,6 +48,7 @@ #include "fcl/broadphase/broadphase_interval_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "fcl/broadphase/default_broadphase_callbacks.h" #include "test_fcl_utility.h" @@ -195,10 +196,10 @@ void octomap_distance_test_BVH(std::size_t n, double resolution) CollisionObject obj1(m1_ptr, tf); CollisionObject obj2(tree_ptr, tf); - test::DistanceData cdata; + DefaultDistanceData cdata; cdata.request.enable_nearest_points = true; S dist1 = std::numeric_limits::max(); - test::defaultDistanceFunction(&obj1, &obj2, &cdata, dist1); + DefaultDistanceFunction(&obj1, &obj2, &cdata, dist1); std::vector*> boxes; @@ -210,8 +211,8 @@ void octomap_distance_test_BVH(std::size_t n, double resolution) manager->registerObjects(boxes); manager->setup(); - test::DistanceData cdata2; - manager->distance(&obj1, &cdata2, test::defaultDistanceFunction); + DefaultDistanceData cdata2; + manager->distance(&obj1, &cdata2, DefaultDistanceFunction); S dist2 = cdata2.result.min_distance; for(std::size_t j = 0; j < boxes.size(); ++j) @@ -247,24 +248,24 @@ void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, boo manager->registerObjects(env); manager->setup(); - test::DistanceData cdata; + DefaultDistanceData cdata; test::TStruct t1; test::Timer timer1; timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; - manager->distance(&tree_obj, &cdata, test::defaultDistanceFunction); + manager->distance(&tree_obj, &cdata, DefaultDistanceFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); - test::DistanceData cdata3; + DefaultDistanceData cdata3; test::TStruct t3; test::Timer timer3; timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; - manager->distance(&tree_obj, &cdata3, test::defaultDistanceFunction); + manager->distance(&tree_obj, &cdata3, DefaultDistanceFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); @@ -288,10 +289,10 @@ void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, boo t2.push_back(timer2.getElapsedTime()); - test::DistanceData cdata2; + DefaultDistanceData cdata2; timer2.start(); - manager->distance(manager2, &cdata2, test::defaultDistanceFunction); + manager->distance(manager2, &cdata2, DefaultDistanceFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h index 937b100ef..8982889e1 100644 --- a/test/test_fcl_utility.h +++ b/test/test_fcl_utility.h @@ -164,79 +164,6 @@ struct DistanceRes Vector3 p2; }; -/// @brief Collision data stores the collision request and the result given by collision algorithm. -template -struct CollisionData -{ - CollisionData() - { - done = false; - } - - /// @brief Collision request - CollisionRequest request; - - /// @brief Collision result - CollisionResult result; - - /// @brief Whether the collision iteration can stop - bool done; -}; - - -/// @brief Distance data stores the distance request and the result given by distance algorithm. -template -struct DistanceData -{ - DistanceData() - { - done = false; - } - - /// @brief Distance request - DistanceRequest request; - - /// @brief Distance result - DistanceResult result; - - /// @brief Whether the distance iteration can stop - bool done; - -}; - -/// @brief Continuous collision data stores the continuous collision request and result given the continuous collision algorithm. -template -struct ContinuousCollisionData -{ - ContinuousCollisionData() - { - done = false; - } - - /// @brief Continuous collision request - ContinuousCollisionRequest request; - - /// @brief Continuous collision result - ContinuousCollisionResult result; - - /// @brief Whether the continuous collision iteration can stop - bool done; -}; - -/// @brief Default collision callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. -template -bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_); - -/// @brief Default distance callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. also return dist, i.e. the bmin distance till now -template -bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, S& dist); - -template -bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_); - -template -bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, S& dist); - std::string getNodeTypeName(NODE_TYPE node_type); std::string getGJKSolverName(GJKSolverType solver_type); @@ -608,70 +535,6 @@ void generateEnvironmentsMesh(std::vector*>& env, S env_scale } } -//============================================================================== -template -bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_) -{ - auto* cdata = static_cast*>(cdata_); - const auto& request = cdata->request; - auto& result = cdata->result; - - if(cdata->done) return true; - - collide(o1, o2, request, result); - - if(!request.enable_cost && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts)) - cdata->done = true; - - return cdata->done; -} - -//============================================================================== -template -bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, S& dist) -{ - auto* cdata = static_cast*>(cdata_); - const DistanceRequest& request = cdata->request; - DistanceResult& result = cdata->result; - - if(cdata->done) { dist = result.min_distance; return true; } - - distance(o1, o2, request, result); - - dist = result.min_distance; - - if(dist <= 0) return true; // in collision or in touch - - return cdata->done; -} - -//============================================================================== -template -bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_) -{ - auto* cdata = static_cast*>(cdata_); - const ContinuousCollisionRequest& request = cdata->request; - ContinuousCollisionResult& result = cdata->result; - - if(cdata->done) return true; - - collide(o1, o2, request, result); - - return cdata->done; -} - -//============================================================================== -template -bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, S& dist) -{ - FCL_UNUSED(o1); - FCL_UNUSED(o2); - FCL_UNUSED(cdata_); - FCL_UNUSED(dist); - - return true; -} - #if FCL_HAVE_OCTOMAP //==============================================================================