From b40385f08bece00bf6390eb3be4ff590194e720f Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Mon, 3 Feb 2020 08:56:17 -0800 Subject: [PATCH 1/2] Whitespace reformat This simply wraps all of the text to be better compliant with the google style guide. It (mostly) fits within 80 columns. Particularly the code examples. --- README.md | 114 +++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 79 insertions(+), 35 deletions(-) diff --git a/README.md b/README.md index 94a53fddd..294c0a663 100644 --- a/README.md +++ b/README.md @@ -4,17 +4,26 @@ Linux / OS X [![Build Status](https://travis-ci.org/flexible-collision-library/f Windows [![Build status](https://ci.appveyor.com/api/projects/status/do1k727uu6e8uemf/branch/master?svg=true)](https://ci.appveyor.com/project/flexible-collision-library/fcl) Coverage [![Coverage Status](https://coveralls.io/repos/github/flexible-collision-library/fcl/badge.svg?branch=master)](https://coveralls.io/github/flexible-collision-library/fcl?branch=master) -FCL is a library for performing three types of proximity queries on a pair of geometric models composed of triangles. - - Collision detection: detecting whether the two models overlap, and optionally, all of the triangles that overlap. - - Distance computation: computing the minimum distance between a pair of models, i.e., the distance between the closest pair of points. - - Tolerance verification: determining whether two models are closer or farther than a tolerance distance. - - Continuous collision detection: detecting whether the two moving models overlap during the movement, and optionally, the time of contact. - - Contact information: for collision detection and continuous collision detection, the contact information (including contact normals and contact points) can be returned optionally. +FCL is a library for performing three types of proximity queries on a pair of +geometric models composed of triangles. + - Collision detection: detecting whether the two models overlap, and + optionally, all of the triangles that overlap. + - Distance computation: computing the minimum distance between a pair of + models, i.e., the distance between the closest pair of points. + - Tolerance verification: determining whether two models are closer or farther + than a tolerance distance. + - Continuous collision detection: detecting whether the two moving models + overlap during the movement, and optionally, the time of contact. + - Contact information: for collision detection and continuous collision + detection, the contact information (including contact normals and contact + points) can be returned optionally. FCL has the following features - C++ interface - - Compilable for either linux or win32 (both makefiles and Microsoft Visual projects can be generated using cmake) - - No special topological constraints or adjacency information required for input models – all that is necessary is a list of the model's triangles + - Compilable for either linux or win32 (both makefiles and Microsoft Visual + projects can be generated using cmake) + - No special topological constraints or adjacency information required for + input models – all that is necessary is a list of the model's triangles - Supported different object shapes: + box + sphere @@ -26,16 +35,24 @@ FCL has the following features + half-space + plane + mesh - + octree (optional, octrees are represented using the octomap library http://octomap.github.com) + + octree (optional, octrees are represented using the octomap library + http://octomap.github.com) ## Installation -Before compiling FCL, please make sure Eigen and libccd (for collision checking between convex objects and is available here https://github.com/danfis/libccd) are installed. For libccd, make sure to compile from github version instead of the zip file from the webpage, because one bug fixing is not included in the zipped version. +Before compiling FCL, please make sure Eigen and libccd (for collision checking +between convex objects and is available here https://github.com/danfis/libccd) +are installed. For libccd, make sure to compile from github version instead of +the zip file from the webpage, because one bug fixing is not included in the +zipped version. -Some optional libraries need to be installed for some optional capability of FCL. For octree collision, please install the octomap library from http://octomap.github.com. +Some optional libraries need to be installed for some optional capability of +FCL. For octree collision, please install the octomap library from +http://octomap.github.com. -CMakeLists.txt is used to generate makefiles in Linux or Visual studio projects in windows. In command line, run +CMakeLists.txt is used to generate makefiles in Linux or Visual studio projects +in windows. In command line, run ``` cmake mkdir build cd build @@ -43,10 +60,13 @@ cmake .. ``` Next, in linux, use make to compile the code. -In windows, there will generate a visual studio project and then you can compile the code. +In windows, there will generate a visual studio project and then you can compile +the code. ## Interfaces -Before starting the proximity computation, we need first to set the geometry and transform for the objects involving in computation. The geometry of an object is represented as a mesh soup, which can be set as follows: +Before starting the proximity computation, we need first to set the geometry and +transform for the objects involving in computation. The geometry of an object is +represented as a mesh soup, which can be set as follows: ```cpp // set mesh triangles and vertice indices @@ -54,7 +74,8 @@ std::vector vertices; std::vector triangles; // code to set the vertices and triangles ... -// BVHModel is a template class for mesh geometry, for default OBBRSS template is used +// BVHModel is a template class for mesh geometry, for default OBBRSS template +// is used typedef BVHModel Model; std::shared_ptr geom = std::make_shared(); // add the mesh data into the BVHModel structure @@ -77,7 +98,8 @@ pose.translation() = T; ``` -Given the geometry and the transform, we can also combine them together to obtain a collision object instance and here is an example: +Given the geometry and the transform, we can also combine them together to +obtain a collision object instance and here is an example: ```cpp //geom and tf are the geometry and the transform of the object std::shared_ptr> geom = ... @@ -86,7 +108,12 @@ Transform3f tf = ... CollisionObjectf* obj = new CollisionObjectf(geom, tf); ``` -Once the objects are set, we can perform the proximity computation between them. All the proximity queries in FCL follow a common pipeline: first, set the query request data structure and then run the query function by using request as the input. The result is returned in a query result data structure. For example, for collision checking, we first set the CollisionRequest data structure, and then run the collision function: +Once the objects are set, we can perform the proximity computation between them. +All the proximity queries in FCL follow a common pipeline: first, set the query +request data structure and then run the query function by using request as the +input. The result is returned in a query result data structure. For example, for +collision checking, we first set the CollisionRequest data structure, and then +run the collision function: ```cpp // Given two objects o1 and o2 CollisionObjectf* o1 = ... @@ -99,7 +126,9 @@ CollisionResult result; collide(o1, o2, request, result); ``` -By setting the collision request, the user can easily choose whether to return contact information (which is slower) or just return binary collision results (which is faster). +By setting the collision request, the user can easily choose whether to return +contact information (which is slower) or just return binary collision results +(which is faster). For distance computation, the pipeline is almost the same: @@ -116,7 +145,9 @@ DistanceResult result; distance(o1, o2, request, result); ``` -For continuous collision, FCL requires the goal transform to be provided (the initial transform is included in the collision object data structure). Beside that, the pipeline is almost the same as distance/collision: +For continuous collision, FCL requires the goal transform to be provided (the +initial transform is included in the collision object data structure). Beside +that, the pipeline is almost the same as distance/collision: ```cpp // Given two objects o1 and o2 @@ -125,7 +156,8 @@ CollisionObjectf* o2 = ... // The goal transforms for o1 and o2 Transform3f tf_goal_o1 = ... Transform3f tf_goal_o2 = ... -// set the continuous collision request structure, here we just use the default setting +// set the continuous collision request structure, here we just use the default +// setting ContinuousCollisionRequest request; // result will be returned via the continuous collision result structure ContinuousCollisionResult result; @@ -133,31 +165,43 @@ ContinuousCollisionResult result; continuousCollide(o1, tf_goal_o1, o2, tf_goal_o2, request, result); ``` -FCL supports broadphase collision/distance between two groups of objects and can avoid the n square complexity. For collision, broadphase algorithm can return all the collision pairs. For distance, it can return the pair with the minimum distance. FCL uses a CollisionManager structure to manage all the objects involving the collision or distance operations. +FCL supports broadphase collision/distance between two groups of objects and can +avoid the n square complexity. For collision, broadphase algorithm can return +all the collision pairs. For distance, it can return the pair with the minimum +distance. FCL uses a CollisionManager structure to manage all the objects +involving the collision or distance operations. ```cpp // Initialize the collision manager for the first group of objects. // FCL provides various different implementations of CollisionManager. -// Generally, the DynamicAABBTreeCollisionManager would provide the best performance. +// Generally, the DynamicAABBTreeCollisionManager would provide the best +// performance. BroadPhaseCollisionManagerf* manager1 = new DynamicAABBTreeCollisionManagerf(); // Initialize the collision manager for the second group of objects. BroadPhaseCollisionManagerf* manager2 = new DynamicAABBTreeCollisionManagerf(); -// To add objects into the collision manager, using BroadPhaseCollisionManager::registerObject() function to add one object +// To add objects into the collision manager, using +// BroadPhaseCollisionManager::registerObject() function to add one object std::vector objects1 = ... for(std::size_t i = 0; i < objects1.size(); ++i) manager1->registerObject(objects1[i]); -// Another choose is to use BroadPhaseCollisionManager::registerObjects() function to add a set of objects +// Another choose is to use BroadPhaseCollisionManager::registerObjects() +// function to add a set of objects std::vector objects2 = ... manager2->registerObjects(objects2); -// In order to collect the information during broadphase, CollisionManager requires two settings: +// In order to collect the information during broadphase, CollisionManager +// 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. +// 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; -// Setup the managers, which is related with initializing the broadphase acceleration structure according to objects input +// Setup the managers, which is related with initializing the broadphase +// acceleration structure according to objects input manager1->setup(); manager2->setup(); // Examples for various queries @@ -181,8 +225,8 @@ manager2->distance(objects1[0], &distance_data, test::defaultDistanceFunction); For more examples, please refer to the test folder: - test_fcl_collision.cpp: provide examples for collision test - test_fcl_distance.cpp: provide examples for distance test -- test_fcl_broadphase.cpp: provide examples for broadphase collision/distance test +- test_fcl_broadphase.cpp: provide examples for broadphase collision/distance + test - test_fcl_frontlist.cpp: provide examples for frontlist collision acceleration -#- test_fcl_global_penetration.cpp: provide examples for global penetration depth test -#- test_fcl_xmldata.cpp: provide examples for more global penetration depth test based on xml data -- test_fcl_octomap.cpp: provide examples for collision/distance computation between octomap data and other data types. +- test_fcl_octomap.cpp: provide examples for collision/distance computation + between octomap data and other data types. From 8502edde73f91bd8f802b98a25c96260943d743b Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Mon, 3 Feb 2020 09:25:19 -0800 Subject: [PATCH 2/2] 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 9adae03cf..c789a2bcc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -36,7 +36,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 //==============================================================================